/*
      Copyright (C) 2010 Bepu Entertainment LLC.

      This software source code is provided 'as-is', without 
      any express or implied warranty.  In no event will the authors be held 
      liable for any damages arising from the use of this software.

      Permission is granted to anyone to use this software for any purpose,
      including commercial applications, and to alter it and redistribute it
      freely, subject to the following restrictions:

      1. The origin of this software must not be misrepresented; you must not
         claim that you wrote the original software. If you use this software
         in a product, an acknowledgment in the product documentation would be
         appreciated but is not required.
      2. Altered source versions must be plainly marked as such, and must not be
         misrepresented as being the original software.
      3. This notice may not be removed or altered from any source distribution.

    Contact us at:
    contact@bepu-games.com
 */
using System;
using System.Diagnostics;
using BEPUphysics;
using BEPUphysics.Constraints;
using BEPUphysics.Entities;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Input;

namespace BEPUphysicsDemos.Demos
{
    /// <summary>
    /// For random tests and general fiddling.
    /// </summary>
    /// <remarks>
    /// This demo type is initially excluded from the main list in the DemosGame.
    /// To access it while playing the demos, add an entry to the demoTypes array for this TestDemo.
    /// </remarks>
    public class SuspensionCarDemo : StandardDemo
    {
        private readonly RevoluteMotor drivingMotor1;
        private readonly RevoluteMotor drivingMotor2;
        private readonly RevoluteMotor steeringMotor1;
        private readonly RevoluteMotor steeringMotor2;
        private float driveSpeed = 30;
        private float maximumTurnAngle = MathHelper.Pi * .3f;

        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public SuspensionCarDemo(DemosGame game)
            : base(game)
        {
            game.Camera.Position = new Vector3(0, 2, 15);
            game.Camera.Yaw = 0;
            game.Camera.Pitch = 0;

            Space.Add(new Box(new Vector3(0, -5, 0), 20, 1, 20));

            var body = new Box(new Vector3(0, 0, 0), 4, .5f, 5, 20);
            body.CenterOfMassOffset = new Vector3(0, -.8f, 0);
            Space.Add(body);

            AddWheel(new Vector3(-1.8f, -2, 2.1f), body, out drivingMotor1, out steeringMotor1);
            AddWheel(new Vector3(1.8f, -2, 2.1f), body, out drivingMotor1, out  steeringMotor1);
            //Just overwrite the driving motor/steering motor from the previous ones, these last two are the real ones we want.
            AddWheel(new Vector3(-1.8f, -2, -2.1f), body, out drivingMotor1, out steeringMotor1);
            AddWheel(new Vector3(1.8f, -2, -2.1f), body, out drivingMotor2, out steeringMotor2);

            int xLength = 256;
            int yLength = 256;

            float xSpacing = 8f;
            float ySpacing = 8f;
            var heights = new float[xLength,yLength];
            float x, y;
            for (int i = 0; i < xLength; i++)
            {
                for (int j = 0; j < yLength; j++)
                {
                    x = i - xLength / 2;
                    y = j - yLength / 2;
                    //heights[i, j] = (float)(x * y / 1000f);
                    heights[i, j] = (float) (10 * (Math.Sin(x / 8) + Math.Sin(y / 8)));
                    //heights[i, j] = 3 * (float)Math.Sin(x * y / 100f);
                    //heights[i, j] = (x * x * x * y - y * y * y * x) / 1000f;
                }
            }
            //Create the terrain.
            var terrain = new Terrain(new Vector3(-xLength * xSpacing / 2, -10, -yLength * ySpacing / 2));
            terrain.UseFaceNormalWithinAngle = (float) Math.PI / 2;
            terrain.SetData(heights, QuadFormats.LowerLeftUpperRight, xSpacing, ySpacing);
            Space.Add(terrain);

            game.ModelDrawer.Add(terrain);
            Space.SimulationSettings.CollisionResponse.Iterations = 1;
        }

        void AddWheel(Vector3 wheelOffset, Entity body, out RevoluteMotor drivingMotor, out RevoluteMotor steeringMotor)
        {
            var wheel = new Cylinder(body.CenterPosition + wheelOffset, .2f, .5f, 10);
            wheel.DynamicFriction = 1.5f;
            wheel.StaticFriction = 1.5f;
            wheel.OrientationQuaternion = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);

            //Preventing the occasional pointless collision pair can speed things up.
            body.CollisionRules.SpecificEntities.Add(wheel, CollisionRule.NoPair);

            //Connect the wheel to the body.
            var pointOnLineJoint = new PointOnLineJoint(body, wheel, wheel.CenterPosition, Vector3.Down, wheel.CenterPosition);
            var suspensionLimit = new LinearAxisLimit(body, wheel, wheel.CenterPosition, wheel.CenterPosition, Vector3.Down, -1, 0);
            //This linear axis motor will give the suspension its springiness by pushing the wheels outward.
            var suspensionSpring = new LinearAxisMotor(body, wheel, wheel.CenterPosition, wheel.CenterPosition, Vector3.Down);
            suspensionSpring.Settings.Mode = MotorMode.Servomechanism;
            suspensionSpring.Settings.Servo.Goal = 0;
            suspensionSpring.Settings.Servo.SpringSettings.StiffnessConstant = 200;
            suspensionSpring.Settings.Servo.SpringSettings.DampingConstant = 50;
            
            var swivelHingeAngularJoint = new SwivelHingeAngularJoint(body, wheel, Vector3.Up, Vector3.Right);
            //Motorize the wheel.
            drivingMotor = new RevoluteMotor(body, wheel, Vector3.Left);
            drivingMotor.Settings.VelocityMotor.Softness = .2f;
            //Let it roll when the user isn't giving specific commands.
            drivingMotor.IsActive = false;
            steeringMotor = new RevoluteMotor(body, wheel, Vector3.Up);
            steeringMotor.Settings.Mode = MotorMode.Servomechanism;
            //The constructor makes a guess about how to set up the constraint.
            //It can't always be right since it doesn't have all the information;
            //in this case, it chooses the basis and test axis incorrectly.
            //This leads to a 'flipping' behavior when the wheel is rolling
            //(the test axis is 'rolling' with the wheel, and passes over
            //a singularity which causes a flip).

            //To fix this, we configure the constraint directly.
            //The basis is aligned with how the wheel is set up; we choose 'up' as 
            //the motorized axis, and right/forward to define the angle measurement plane.
            //The test axis is set to be perpendicular to the wheel's rotation so that
            //it only measures the steering angle.

            //If you're curious, the angle measurement is just a Math.Atan2.
            //The current world test axis is dotted against the two plane axes (Right and Forward here).
            //This gives an x and y value.  These can be plugged into Atan2 just like when
            //you compute an angle on a normal 2d graph.
            steeringMotor.Basis.SetWorldAxes(Vector3.Up, Vector3.Right, Vector3.Forward);
            steeringMotor.TestAxis = Vector3.Right;


            //Add the wheel and connection to the space.
            Space.Add(wheel);
            Space.Add(pointOnLineJoint);
            Space.Add(suspensionLimit);
            Space.Add(suspensionSpring);
            Space.Add(swivelHingeAngularJoint);
            Space.Add(drivingMotor);
            Space.Add(steeringMotor);

        }

        /// <summary>
        /// Gets the name of the simulation.
        /// </summary>
        public override string Name
        {
            get { return "Suspension Car Demo"; }
        }

        public override void Update(float dt)
        {
            if (Game.KeyboardInput.IsKeyDown(Keys.P))
                Debug.WriteLine("Break.");

            if (Game.KeyboardInput.IsKeyDown(Keys.NumPad8))
            {
                //Go forward
                drivingMotor1.Settings.VelocityMotor.GoalVelocity = driveSpeed;
                drivingMotor2.Settings.VelocityMotor.GoalVelocity = driveSpeed;
                //The driving motors are disabled when no button is pressed, so need to turn it on.
                drivingMotor1.IsActive = true;
                drivingMotor2.IsActive = true;
            }
            else if (Game.KeyboardInput.IsKeyDown(Keys.NumPad5))
            {
                //Go backward
                drivingMotor1.Settings.VelocityMotor.GoalVelocity = -driveSpeed;
                drivingMotor2.Settings.VelocityMotor.GoalVelocity = -driveSpeed;
                //The driving motors are disabled when no button is pressed, so need to turn it on.
                drivingMotor1.IsActive = true;
                drivingMotor2.IsActive = true;
            }
            else
            {
                //Let it roll.
                drivingMotor1.IsActive = false;
                drivingMotor2.IsActive = false;
            }

            if (Game.KeyboardInput.IsKeyDown(Keys.NumPad4))
            {
                //Turn left
                steeringMotor1.Settings.Servo.Goal = maximumTurnAngle;
                steeringMotor2.Settings.Servo.Goal = maximumTurnAngle;
            }
            else if (Game.KeyboardInput.IsKeyDown(Keys.NumPad6))
            {
                //Turn right
                steeringMotor1.Settings.Servo.Goal = -maximumTurnAngle;
                steeringMotor2.Settings.Servo.Goal = -maximumTurnAngle;
            }
            else
            {
                //Face forward
                steeringMotor1.Settings.Servo.Goal = 0;
                steeringMotor2.Settings.Servo.Goal = 0;
            }

            base.Update(dt);
        }

        public override void DrawUI()
        {
            base.DrawUI();
        }
    }
}