//---------------------------------------------------------------------------------------------------------------------------------------------------
// <copyright file="Mass.cs" company="DarkWynter Studios">
//     Copyright (C)2007 DarkWynter Studios.  All rights reserved.
// </copyright>
//---------------------------------------------------------------------------------------------------------------------------------------------------
// {Contact : darkwynter.com for licensing information
//---------------------------------------------------------------------------------------------------------------------------------------------------
#define HACK_MOVEMENT

namespace DarkWynter.Engine.Physics
{
    #region Using Statements
    using System;
    using Microsoft.Xna.Framework;

    using Globals;
    using GameObjects;
    using Utilities;    // Logging...
    #endregion


    /// <summary>
    /// Mass object associated with all Game Objects, which encapsulates physical properties into an invisible object.
    /// Mass processes it's own Updates, though Collision.cs is used to handle mass-to-mass interactions.
    /// Uses a right-handed coordinate system.
    /// </summary>
    public class Mass
    {
        /// <summary>
        /// Pointer from this mass back up to the game object associated with it
        /// </summary>
        public GameObject gameObjectPointer;

        // Position
        /// <summary>
        /// The objects center location
        /// </summary>
        public Vector3 currentPosition = new Vector3();
        /// <summary>
        /// Needed for updating the lookup map
        /// </summary>
        public Vector3 lastPosition = new Vector3();

        /// <summary>
        /// Stores current rotation
        /// </summary>
        public Quaternion currentRotation = new Quaternion();
        /// <summary>
        /// Straight ahead - normalized
        /// </summary>
        public Vector3 normalVector = new Vector3();
        /// <summary>
        /// Straight up  - normalized
        /// </summary>
        public Vector3 upVector = new Vector3();
        /// <summary>
        /// Straight to the right - normalized
        /// </summary>
        public Vector3 perpVector = new Vector3();

        // Motion
        /// <summary>
        /// The object's current acceleration
        /// </summary>
        public Vector3 acceleration = new Vector3();
        /// <summary>
        /// Un-Normalized Vector in the direction of object movement 
        /// </summary>
        public Vector3 velocity = new Vector3();
        /// <summary>
        /// Used to sum the forces on a GameObject during Update
        /// </summary>
        public Vector3 totalForce = new Vector3();

        // Spatial Volume
        /// <summary>
        /// Bounding volume of this mass
        /// </summary>
        public BoundingVolume boundingVolume = new BoundingVolume();
        /// <summary>
        /// Model Scale Factor
        /// </summary>
        public Vector3 scale = new Vector3(1.0f);
     
        
        /// <summary>
        /// Object height above the terrain. Based on model properties
        /// </summary>
        public float objectHeight;

        // Energy
        /// <summary>
        /// Initial Fire energy given to player
        /// </summary>
        public float energy = 0.0f;


        #region Attributes

        /// <summary>
        /// Tells us what type of an object his is
        /// </summary>
        public Enums_Engine.ObjectType objectType = new Enums_Engine.ObjectType();

        /// <summary>
        /// Temperature change check
        /// </summary>
        public bool isChanging = false;
        /// <summary>
        /// Motion check
        /// </summary>
        public bool isMoving = false;
        /// <summary>
        /// Have we completed all collisions on this mass
        /// </summary>
        public bool collisionChecked = false;
        /// <summary>
        /// Does the object need to be recycled
        /// </summary>
        public bool deadObject = false;

        // Properties
        /// <summary>
        /// Kilos
        /// </summary>
        public float mass = 0.0f;
        /// <summary>
        /// Used to calculate bonding factor to other game objects
        /// </summary>
        public float adhesion = 0.1f;
        /// <summary>
        /// Used to calculate bonding factor to like objects
        /// </summary>
        public float cohesion = 0.5f;
        /// <summary>
        /// Temperature (Celcius) at which a particle changes state
        /// </summary>
        public float stateChangeThreshold = 0;
        /// <summary>
        /// Coefficient Of Restitution for this object
        /// </summary>
        public float COR;
        /// <summary>
        /// Static Friction Coefficient of this object
        /// </summary>
        public float staticFrictionCoefficient;
        /// <summary>
        /// Dynamic Friction Coefficient of this object
        /// </summary>
        public float dynamicFrictionCoefficient;

        /// <summary>
        /// Amount of damage to be applied when this mass falls/hits the terrain
        /// </summary>
        public float fallingDamageMultiplier = 0.0f;
        /// <summary>
        /// Motion type of this object
        /// </summary>
        public Enums_Engine.MovementType movementType = Enums_Engine.MovementType.HOVER;
        /// <summary>
        /// Change in energy value
        /// </summary>
        private float newEnergy = 0.0f;
        /// <summary>
        /// Orientation of the Sensor
        /// </summary>
        public Vector3 sensorDirection = new Vector3();
        /// <summary>
        /// Used for falling damage calculations
        /// </summary>
        public float lastMaxHeight = 0;
        /// <summary>
        /// Number of times this object has collided with something
        /// </summary>
        public int numberOfCollision = 0;
        #endregion


        /// <summary>
        /// Constructor
        /// </summary>
        public Mass()
        {
            totalForce = Vector3.Zero;
            velocity = Vector3.Zero;
        }

        /// <summary>
        /// Reset mass to initial state
        /// </summary>
        public void ClearValues()
        {
            // Reset everything here
            // Called by constructor...
            objectType = Enums_Engine.ObjectType.NONE;
            objectHeight = 0;
            gameObjectPointer = null;

            // Orientation
            currentRotation = Quaternion.Identity;   // Stores current rotation information
            normalVector = Vector3.Zero;            // Straight ahead - normalized
            upVector = Vector3.Zero;                // Straight up  - normalized
            perpVector = Vector3.Zero;              // Straight to the right?(left?)
            numberOfCollision = 0;

            // Position
            currentPosition = Vector3.Zero;         // The objects center location
            lastPosition = Vector3.Zero;            // Needed for updating the lookup map

            // Motion Variables
            acceleration = Vector3.Zero;
            velocity = Vector3.Zero;                // Un-Normalized Vector in the direction of object movement 
            totalForce = Vector3.Zero;              // Used to sum the forces on a GameObject during Update

            // Reaction Properties
            mass = 1;                                  // Kilos
            scale = new Vector3(1.0f);                                 // Model Scale Factor
            adhesion = 0;                              // Used to calculate bonding factor to other game objects
            cohesion = 0;                              // Used to calculate bonding factor to like objects

            // Energy State
            energy = 0.0f;                             // Initial Fire energy given to player
            newEnergy = 0.0f;
            stateChangeThreshold = 0;                  // Temperature (Celcius) at which a particle changes state

            COR = 0.0f;                                       // Coefficient Of Restitution for this object
            staticFrictionCoefficient = 0.0f;                 // Static Friction Coefficient of this object
            dynamicFrictionCoefficient = 0.0f;                // Dynamic Friction Coefficient of this object

            isChanging = false;
            isMoving = false;
            collisionChecked = false;

            fallingDamageMultiplier = 0.0f;

            deadObject = false;
        }

        /// <summary>
        /// Update the mass's position
        /// </summary>
        public void Update()
        {
            // Signal collision that this object needs collision detection
            collisionChecked = false;


            // If mass is moving, update physics
            // .. else, set isMoving false to indicate no update needed
            if (totalForce != Vector3.Zero || velocity != Vector3.Zero)
            {
                isMoving = true;

                // Set last position to the old current position
                lastPosition = currentPosition;

                // Add force if below speed limit and motion type is Hover
                if (totalForce.Length() > Statics_Engine.PlayerSettings.MAX_HOVER_SPEED && movementType == Enums_Engine.MovementType.HOVER)
                {
                    totalForce.Normalize();
                    totalForce *= Statics_Engine.PlayerSettings.MAX_HOVER_SPEED;
                }

                // Acceleration calc
                acceleration = totalForce / mass;
                if (float.IsNaN(acceleration.X) || float.IsNaN(acceleration.Y) || float.IsNaN(acceleration.Z))
                {
                    System.Diagnostics.Debug.WriteLine("acceleration = NaN");
                    Logging.addNan();
                }

                // Force has been applied..  zero it out..
                totalForce = Vector3.Zero;

                // Movement according to time S = u*t + 1/2*g*t^2
                currentPosition = lastPosition + velocity * Statics_Engine.SystemSettings.dt + (0.5f * acceleration * Statics_Engine.SystemSettings.dt * Statics_Engine.SystemSettings.dt);
                if (float.IsNaN(currentPosition.X))
                {
                    System.Diagnostics.Debug.WriteLine("CurrentPosition = NaN");
                    Logging.addNan();
                    currentPosition = lastPosition;
                }

                // Calculate velocity
                if (movementType == Enums_Engine.MovementType.WALK && gameObjectPointer.heightDifference >= 0.0f)
                {
                    velocity = Vector3.Zero;
                }
                else
                {
                    velocity += acceleration * Statics_Engine.SystemSettings.dt;
                }

                // CLEAN: ?? set the height
                if (currentPosition.Y > lastMaxHeight)
                {
                    lastMaxHeight = currentPosition.Y;
                }

                // Ensure that mass is within level boundaries
                EnforceLevelBoundary();

            }
            else
            {
                isMoving = false;
            }

            // Check if new energy has been added to the object
            if (newEnergy != 0.0f)
            {
                // Call the energy update function
                UpdateEnergy();
            }
        }
        /// <summary>
        /// Boundary checking
        /// </summary>
        public void EnforceLevelBoundary()
        {
            // X = 0 plane
            if (currentPosition.X < 0.0f)
            {
                // Clamp the position
                currentPosition.X = 1.0f;
                lastPosition.X = 0.0f;

                AddForce(new Vector3(-2.0f * mass * velocity.X * COR / Statics_Engine.SystemSettings.dt, 0.0f, 0.0f));
            }

            // X = mapSize plane
            if (currentPosition.X > (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor)
            {
                // Clamp the position
                currentPosition.X = (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor - 1;
                lastPosition.X = (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor - 2;

                AddForce(new Vector3(-2.0f * mass * velocity.X * COR / Statics_Engine.SystemSettings.dt, 0.0f, 0.0f));
            }

            //// Y = 0 plane
            //if (currentPosition.Y < 0.0f)
            //{
            //    // Clamp the position
            //    currentPosition.Y = 1.0f;
            //    lastPosition.Y = 0.0f;

            //    AddForce(new Vector3(0.0f, -2.0f * mass * velocity.Y * COR / Statics.SystemSettings.dt, 0.0f));
            //}

            // Y = mapSize plane
            if (currentPosition.Y > (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor)
            {
                 // Clamp the position
                currentPosition.Y = (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor - 1;
                lastPosition.Y = (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor - 2;

                AddForce(new Vector3(0.0f, -2.0f * mass * velocity.Y * COR / Statics_Engine.SystemSettings.dt, 0.0f));
            }

            // Z = 0 plane
            if (currentPosition.Z < 0.0f)
            {
                // Clamp the position
                currentPosition.Z = 1.0f;
                lastPosition.Z = 0.0f;

                AddForce(new Vector3(0.0f, 0.0f, -2.0f * mass * velocity.Z * COR / Statics_Engine.SystemSettings.dt));
            }

            // Z = mapSize plane
            if (currentPosition.Z > (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor)
            {
                // Clamp the position
                currentPosition.Z = (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor - 1;
                lastPosition.Z = (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor - 2;

                AddForce(new Vector3(0.0f, 0.0f, -2.0f * mass * velocity.Z * COR / Statics_Engine.SystemSettings.dt));
            }

            if (float.IsNaN(currentPosition.X))
            {
                Logging.addNan();
                currentPosition.X = 1.0f;
                lastPosition.X = 1.0f;
            }

            if (float.IsNaN(currentPosition.Y))
            {
                Logging.addNan();
                currentPosition.Y = 1.0f;
                lastPosition.Y = 0.0f;
            }

            if (float.IsNaN(currentPosition.Z))
            {
                Logging.addNan();
                currentPosition.Z = 1.0f;
                lastPosition.Z = 0.0f;
            }

            /////////////////////////////////////////
            // this is a hack to fix the crashes due to lastPosition being invalid.. we really shouldn't have to do this
            // 
            // X = 0 plane
            if (lastPosition.X < 0.0f)
            {
                // Clamp the position
                lastPosition.X = 0.0f;
            }
            // X = mapSize plane
            if (lastPosition.X > (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor)
            {
                // Clamp the position
                lastPosition.X = (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor - 2;
            }
            // Y = 0 plane
            if (lastPosition.Y < 0.0f)
            {
                // Clamp the position
                lastPosition.Y = 0.0f;
            }
            // Y = mapSize plane
            if (lastPosition.Y > (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor)
            {
                // Clamp the position
                lastPosition.Y = (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor - 2;
            }

            // Z = 0 plane
            if (lastPosition.Z < 0.0f)
            {
                // Clamp the position
                lastPosition.Z = 0.0f;
            }
            // Z = mapSize plane
            if (lastPosition.Z > (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor)
            {
                // Clamp the position
                lastPosition.Z = (Statics_Engine.TerrainSettings.vertexMapSize - 1) * Statics_Engine.TerrainSettings.terrainScaleFactor - 2;
            }
        }

        /// <summary>
        /// Set this mass's position vectors
        /// </summary>
        /// <param name="position">Center position</param>
        /// <param name="reference">Reference position</param>
        public void SetPosition(Vector3 position, Vector3 reference)
        {
            normalVector = new Vector3(0, 0, 1); //reference - position;
            normalVector.Normalize();

            upVector = new Vector3(0.0f, 1.0f, 0.0f);
            perpVector = new Vector3(-1.0f, 0.0f, 0.0f);//Vector3.Cross(normalVector, upVector); //new Vector3(-1.0f, 0.0f, 0.0f);
            upVector = Vector3.Cross(perpVector, normalVector);

            // Look in Z+ direction
            currentRotation = -Quaternion.Identity;

            Vector2 unitHoriz = new Vector2(0, 1);
            Vector2 unitVert = new Vector2(1, 0);

            Vector3 N = reference - position;
            N.Normalize();

            Vector2 Nhoriz = new Vector2(N.X, N.Z);
            Vector2 Nvert = new Vector2(N.Z, N.Y);

            float angleHoriz = (float)Math.Acos((double)(Vector2.Dot(unitHoriz, Nhoriz)));
            float anglVert = 0.0f;// (float)Math.Acos((double)(Vector2.Dot(unitVert, Nvert)));

            //Determine the sign of the angle needed
            Vector3 cross = Vector3.Cross(Vector3.UnitZ, N);
            if (cross.Y <= 0)
            {
                angleHoriz = -Math.Abs(angleHoriz);
            }
            else
            {
                angleHoriz = Math.Abs(angleHoriz);
            }

            //passing negative because I am negating it again inside the function
            Rotate(anglVert, angleHoriz);

            //            perpVector = Vector3.Cross(normalVector, upVector);
            //upVector = Vector3.Cross(perpVector, normalVector);

            currentPosition = new Vector3() + position;
            lastPosition = position - normalVector;
        }
        /// <summary>
        /// Add an external force to be applied to the mass
        /// </summary>
        /// <param name="force">Force to apply</param>
        public void AddForce(Vector3 force)
        {
            totalForce += force;
            if (float.IsNaN(totalForce.X))
            {
                Logging.addNan();
                System.Diagnostics.Debug.WriteLine("AddForce.. totalForce = NaN");
                totalForce = Vector3.Zero;
            }
        }
        /// <summary>
        /// Add force caused due to friction
        /// </summary>
        /// <param name="totalFriction">Total frictional coefficients</param>
        /// <param name="perpendicularForce">Normal force</param>
        /// <param name="tangentialDirection">Tangential direction of motion</param>
        public void AddFriction(float totalFriction, float perpendicularForce, Vector3 tangentialDirection)
        {
            if (tangentialDirection.Length() != 0.0f)
            {
                tangentialDirection.Normalize();
            }

            totalForce += (totalFriction * Math.Abs(perpendicularForce)) * tangentialDirection;

            if (float.IsNaN(totalForce.X))
            {
                Logging.addNan();
                //throw new Exception("NaN Error.");
                System.Diagnostics.Debug.WriteLine("AddFriction.. totalForce = NaN");
                totalForce = Vector3.Zero;
            }
        }
        /// <summary>
        /// Function to add/remove energy from the object
        /// </summary>
        private void UpdateEnergy()
        {
            // Add newEnergy to energy and reset newEnergy
            energy += newEnergy;
            newEnergy = 0.0f;

            // Bounds checking
            if (energy > 1.0f)
            {
                energy = 1.0f;
            }
            else if (energy < -1.0f)
            {
                energy = -1.0f;
            }
        }
        /// <summary>
        /// Function to set the energy state of an object
        /// </summary>
        /// <param name="newEnergyValue">Energy state of object</param>
        private void SetEnergy(float newEnergyValue)
        {
            energy = newEnergyValue;
        }
        /// <summary>
        /// Add/Remove energy to the mass
        /// </summary>
        /// <param name="amount">Change in energy</param>
        public void ChangeEnergy(float amount)
        {
            newEnergy += amount;
        }
        /// <summary>
        /// Move the player based on the controller inputs
        /// </summary>
        /// <param name="dn">Force applied along the normal</param>
        /// <param name="dx">Force applied along the tangent</param>
        public void MoveObject(float dn, float dx)
        {
            if (movementType == Enums_Engine.MovementType.WALK)
            {
                float moveRatioN = dn * Statics_Engine.PlayerSettings.WALK_SPEED;
                float moveRatioX = dx * Statics_Engine.PlayerSettings.WALK_SPEED;

                //Statics.lookupMap.RemoveCurrent(this);

                sensorDirection = Vector3.Zero;
                if (dn != 0)
                {
                    sensorDirection += new Vector3(normalVector.X, 0, normalVector.Z) * moveRatioN;
                }
                if (dx != 0)
                {
                    sensorDirection += new Vector3(perpVector.X, 0, perpVector.Z) * moveRatioX;
                }
                lastPosition = currentPosition;
                currentPosition += sensorDirection;
                EnforceLevelBoundary();
            }
            else
            {
                float moveRatioN = dn * Statics_Engine.PlayerSettings.HOVER_SPEED;
                float moveRatioX = dx * Statics_Engine.PlayerSettings.HOVER_SPEED;

                if (Math.Abs(totalForce.X) + Math.Abs(totalForce.Z) < 5000.0f)
                {
                    // Add forward/backward force to mass object 
                    AddForce( normalVector * moveRatioN);

                    // Add left/right force to mass object
                    AddForce(perpVector * moveRatioX);
                }
            }
        }
        /// <summary>
        /// Force applied when the player hits something
        /// </summary>
        /// <param name="amount">Amount of rebound force</param>
        public void KickBack(float amount)
        {
            if (movementType == Enums_Engine.MovementType.WALK && amount != 0.0f)
            {
                //Statics.lookupMap.RemoveCurrent(this);

                lastPosition = currentPosition;
                currentPosition += new Vector3(-sensorDirection.X, 0, -sensorDirection.Z) * amount;
                EnforceLevelBoundary();
            }
            else if (movementType == Enums_Engine.MovementType.HOVER)
            {
                velocity += -velocity * amount;
            }
        }
        /// <summary>
        /// Rotate the coordinate system
        /// </summary>
        /// <param name="dVert">Vertical rotation</param>
        /// <param name="dHoriz">Horizontal rotation</param>
        public void Rotate(float dVert, float dHoriz)
        {
            Quaternion vertRotation = Quaternion.CreateFromAxisAngle(perpVector, dVert);
            Quaternion horizRotation = Quaternion.CreateFromAxisAngle(upVector, -dHoriz);
            Quaternion totalRotation;
            Vector3 newNormal = Vector3.Transform(normalVector, Matrix.CreateFromQuaternion(horizRotation * vertRotation));

            // Limit the rotation
            double angle1 = Math.Acos((double)(Vector3.Dot(normalVector, new Vector3(0, 1, 0))));
            double angle2 = Math.Acos((double)(Vector3.Dot(normalVector, newNormal)));
            double angle;
            if (dVert > 0)
            {
                angle = angle1 - angle2;
            }
            else
            {
                angle = angle1 + angle2;
            }

            // Limit vertical (up/down) rotation
            if (angle < Statics_Engine.PlayerSettings.PLAYER_ROTATION_CAP_UP || angle > Statics_Engine.PlayerSettings.PLAYER_ROTATION_CAP_DOWN)
            {
                //ignore the vertical rotation
                totalRotation = horizRotation;// *(-vertRotation);
            }
            else
            {
                //do both rotations
                totalRotation = horizRotation * vertRotation;
            }

            currentRotation = Quaternion.Normalize(totalRotation * currentRotation);
            normalVector = Vector3.Transform(normalVector, Matrix.CreateFromQuaternion(totalRotation));
            perpVector = Vector3.Transform(perpVector, Matrix.CreateFromQuaternion(totalRotation));
        }
    }
}

/* Legacy Code
        //Strafe object on the V (sideways) plane
        public void Strafe(float dx)
        {
            if (movementType == MovementType.WALK && dx != 0.0f)
            {
                Statics.lookupMap.RemoveCurrent(this);

                float moveRatio = dx / Statics.PlayerSettings.PLAYER_SPEED_ON_THE_GROUND;

                lastPosition = currentPosition;
                sensorDirection = new Vector3(perpVector.X, 0, perpVector.Z) * moveRatio * Statics.PlayerSettings.PLAYER_WALK_SPEED;
                currentPosition += sensorDirection;
                EnforceBoundary();
            }
            else
            {
                AddForce(perpVector * dx);
            }
        }
        //Move forwards or backwards along normal (does not restrict Y value)
        public void Move(float dn)
        {
            if (movementType == MovementType.WALK && dn != 0.0f)
            {
                Statics.lookupMap.RemoveCurrent(this);

                float moveRatio = dn / Statics.PlayerSettings.PLAYER_SPEED_ON_THE_GROUND;

                lastPosition = currentPosition;
                sensorDirection = new Vector3(normalVector.X, 0, normalVector.Z) * moveRatio * Statics.PlayerSettings.PLAYER_WALK_SPEED;
                currentPosition += sensorDirection;
                EnforceBoundary();
            }
            else
            {
                AddForce(normalVector * dn);
            }
        }

*/