using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using VisiMath;
using VisiMath.Collisions;
using VisiMath.Geometry;

namespace VisimBox.Physics
{
    public class PhysicalContact : CollisionInfo
    {
        //TODO : hold rigidbodies references only as WEAK references!!!

        //contact normal etc in collisioninfo already
        #region primary values

        /// <summary>
        /// Rigid bodies in contact
        /// </summary>
        public RigidBody[] Bodies = new RigidBody[2];

        /// <summary>
        /// First rigid body in contact
        /// </summary>
        public RigidBody First 
        {
            get { return Bodies[0]; } set {Bodies[0] = value; }
        }

        /// <summary>
        /// Second rigid body in contact
        /// </summary>
        public RigidBody Second 
        {
            get { return Bodies[1]; } set {Bodies[1] = value; }
        }
        /// <summary>
        /// Lateral friction coefficient
        /// </summary>
        public float Friction;

        /// <summary>
        /// Restitution coefficient
        /// </summary>
        public float Restitution;

        #endregion


        /// <summary>
        /// Rotational matrix converting coordinates in contact to world
        /// </summary>
        public Matrix3 ContactToWorld;

        /// <summary>
        /// Closing velocity
        /// </summary>
        public Vector3 ContactVelocity;

        /// <summary>
        /// change in velocity for resolving the contact
        /// </summary>
        public float DesiredDeltaV;

        /// <summary>
        /// World space position of contact relative to center of both bodies
        /// </summary>
        public Vector3[] RelativeContactPos = new Vector3[2];

        protected float DeltaVSpeedLimit = 0.25f;
        protected float AngularLimit = 0.2f;

        /// <summary>
        /// Sets data for this contact point
        /// </summary>
        /// <param name="first">First body</param>
        /// <param name="second">Second body</param>
        /// <param name="fr">Friction</param>
        /// <param name="res">Restitution</param>
        public void SetBodyData(RigidBody first, RigidBody second, float fr, float res)
        {
            First = first;
            Second = second;
            Friction = fr;
            Restitution = res;
        }


        /// <summary>
        /// Swaps bodies
        /// </summary>
        protected void SwapBodies()
        {
            if (Second == null)
                return;
            ContactNormal.Mul(-1);
            RigidBody tmp = First;
            First = Second; Second = tmp;
        }

        /// <summary>
        /// Awakes sleeping nodes
        /// </summary>
        public void SetAwake()
        {
            if (Second == null)
                return;
            
            bool firstsleep = First.Sleeping;
            bool secondsleep = Second.Sleeping;

            if (firstsleep | secondsleep)
            {
                if (firstsleep)
                    First.Sleeping = false;
                else Second.Sleeping = false;
            }
        }


        /// <summary>
        /// Applies velocity changes to bodies and returns used values
        /// </summary>
        /// <param name="linear1">Linear change for first body</param>
        /// <param name="linear2">Linear change for second body</param>
        /// <param name="angular1">Angular change for first body</param>
        /// <param name="angular2">Angular change for second body</param>
        public void ApplyVelocityChange(ref Vector3 velocityChange1, ref Vector3 velocityChange2,
                                 ref Vector3 rotationChange1, ref Vector3 rotationChange2)
        {
            Matrix3 InvertedInertialTensor1 = First.InvertedInertia;
            Matrix3 InvertedInertialTensor2;

            if ((Second != null) && (Second.InvertedMass != 0))
                InvertedInertialTensor2 = Second.InvertedInertia;
            else
                InvertedInertialTensor2 = Matrix3.Identitity;
            
            Vector3 impulseContact;

            if (Friction == 0)
                impulseContact = FrictionLessImpulse(ref InvertedInertialTensor1, ref InvertedInertialTensor2);
            else
                impulseContact = FrictionImpulse(ref InvertedInertialTensor1, ref InvertedInertialTensor2);

            ContactToWorld.Transform(ref impulseContact);

            //split impulse to linear and rotation

            rotationChange1 = RelativeContactPos[0].Cross(ref impulseContact);
            InvertedInertialTensor1.Transform(ref rotationChange1);
            velocityChange1.Assign(ref impulseContact, First.InvertedMass);

            First.AddVelocity(ref velocityChange1);
            First.AddAngularVelocity(ref rotationChange1);
            //Console.DebugLine("Velocity change " + velocityChange1.ToString());
            //Console.DebugLine("Rotation velocity change " + rotationChange1.ToString());

            if ((Second != null) && (Second.InvertedMass != 0))
            {
                //split impulse to linear and rotation
                rotationChange2 = RelativeContactPos[1].Cross(ref impulseContact);
                InvertedInertialTensor2.Transform(ref rotationChange2);
                velocityChange2.Assign(ref impulseContact, -Second.InvertedMass);

                Second.AddVelocity(ref velocityChange2);
                Second.AddAngularVelocity(ref rotationChange2);
                //Console.DebugLine("Velocity change " + velocityChange2.ToString());
                //Console.DebugLine("Rotation velocity change " + rotationChange2.ToString());
            }            
        }

        
        /// <summary>
        /// Applies position changes to bodies and returns used values
        /// </summary>
        /// <param name="linear1">Linear position change for first body</param>
        /// <param name="linear2">Linear position change for second body</param>
        /// <param name="angular1">Angular position change for first body</param>
        /// <param name="angular2">Angular position change for second body</param>
        public void ApplyPositionChange(ref Vector3 linear1, ref Vector3 linear2,
                                ref Vector3 angular1, ref Vector3 angular2)
        {
            float[] angularInertia = new float[2];
            float[] linearInertia = new float[2];

            float totalInertia = 0;

            //inertia of bodies in the direction of contact normal
            //calculate total inertia
            for (int i = 0; i < 2; i++)
            {
                if ((Bodies[i] != null) && (Bodies[i].InvertedMass != 0))
                {
                    Matrix3 invinertia = Bodies[i].InvertedInertia;

                    Vector3 angularinertiaW = RelativeContactPos[i].Cross(ref ContactNormal);
                    invinertia.Transform(ref angularinertiaW);
                    angularinertiaW = angularinertiaW.Cross(ref RelativeContactPos[i]);
                    angularInertia[i] = angularinertiaW.Dot(ref ContactNormal);
                    linearInertia[i] = Bodies[i].InvertedMass;
                                        
                    totalInertia += linearInertia[i] + angularInertia[i];
                }
            }

            float[] angularMove = new float[2];
            float[] linearMove = new float[2];

            for (int i = 0; i < 2; i++)
            {
                if ((Bodies[i] != null)&&(Bodies[i].InvertedMass != 0))
                {
                    float sign = (i == 0) ? 1 : -1;

                    angularMove[i] = sign * Penetration * (angularInertia[i] / totalInertia);
                    linearMove[i] = sign * Penetration * (linearInertia[i] / totalInertia);

                    //limit angular move for big mass and small inertia tensor

                    Vector3 projection = RelativeContactPos[i];
                    projection.AddScaled(ref ContactNormal, -(RelativeContactPos[i].Dot(ref ContactNormal)));

                    //aproximation for very small angles, should be sin(angularlimit) * projection.Length
                    float maxMag = AngularLimit * projection.Length;

                    if (angularMove[i] < -maxMag)
                    {
                        float total = angularMove[i] + linearMove[i];
                        angularMove[i] = -maxMag;
                        linearMove[i] = total - angularMove[i];
                    }
                    else if (angularMove[i] > maxMag)
                    {
                        float total = angularMove[i] + linearMove[i];
                        angularMove[i] = maxMag;
                        linearMove[i] = total - angularMove[i];
                    }

                    //calculate required rotation from angular move

                    if (angularMove[i] == 0)
                    {
                        if (i == 0)
                            angular1.Assign(0);
                        else
                            angular2.Assign(0);
                    }
                    else
                    {
                        Vector3 targetdir = RelativeContactPos[i].Cross(ref ContactNormal);
                        Matrix3 invinertia = Bodies[i].InvertedInertia;
                        invinertia.Transform(ref targetdir);
                        float change = angularMove[i] / angularInertia[i];
                        if (i == 0)
                            angular1.Assign(ref targetdir,  change);
                        else
                            angular2.Assign(ref targetdir, change);                        
                    }

                    //velocity change - movement along normal
                    if (i == 0)
                        linear1.Assign(ref ContactNormal, linearMove[i]);
                    else
                        linear2.Assign(ref ContactNormal, linearMove[i]);


                    //apply changes to bodies

                    Vector3 pos = Bodies[i].Position;

                    //Vector3 poschange = new Vector3(ref ContactNormal, linearMove[i]);
                    //Console.DebugLine("Position change " + poschange.ToString());
                    pos.AddScaled(ref ContactNormal, linearMove[i]);

                    Bodies[i].Position = pos;

                    Quaternion q = Bodies[i].Rotation;
                    if (i == 0)
                    {
                        q.AddVector(ref angular1);
                        //Console.DebugLine("Rotation change " + angular1.ToString());
                    }
                    else
                    {
                        q.AddVector(ref angular2);
                        //Console.DebugLine("Rotation change " + angular2.ToString());
                    }
                    Bodies[i].Rotation = q;

                    if (!Bodies[i].Sleeping)
                        Bodies[i].UpdateValues();

                }
            }
        }

        /// <summary>
        /// Calculate impulse without any friction
        /// </summary>
        /// <param name="invtens1"></param>
        /// <param name="invtens2"></param>
        /// <returns>Impulse vector</returns>
        protected Vector3 FrictionLessImpulse(ref Matrix3 invtens1, ref Matrix3 invtens2)
        {
            Vector3 deltavelWorld = RelativeContactPos[0].Cross(ref ContactNormal);
            invtens1.Transform(ref deltavelWorld);
            deltavelWorld = deltavelWorld.Cross(ref RelativeContactPos[0]);

            float deltav = deltavelWorld.Dot(ref ContactNormal);

            deltav += First.InvertedMass;

            if (Second != null)
            {
                deltavelWorld = RelativeContactPos[1].Cross(ref ContactNormal);
                invtens2.Transform(ref deltavelWorld);
                deltavelWorld = deltavelWorld.Cross(ref RelativeContactPos[1]);

                deltav += deltavelWorld.Dot(ref ContactNormal);

                deltav += Second.InvertedMass;                
            }

            return new Vector3(DesiredDeltaV / deltav, 0, 0);
        }

        /// <summary>
        /// Calculate impulse with friction
        /// </summary>
        /// <param name="invtens1"></param>
        /// <param name="invtens2"></param>
        /// <returns>Impulse vector</returns>
        protected Vector3 FrictionImpulse(ref Matrix3 invtens1, ref Matrix3 invtens2)
        {
            float inverseMass = First.InvertedMass;

            //cross product equivalent for rotational matrices
            Matrix3 ImpulseTorque = Matrix3.CreateSkewSymetric(ref RelativeContactPos[0]);

            Matrix3 deltaVelWorld = ImpulseTorque;

            deltaVelWorld.MultiplyBy(ref invtens1);
            deltaVelWorld.MultiplyBy(ref ImpulseTorque);
            deltaVelWorld.Multiply(-1);

            if (Second != null)
            {
                ImpulseTorque = Matrix3.CreateSkewSymetric(ref RelativeContactPos[0]);
                Matrix3 deltaVelWorld2 = ImpulseTorque;

                deltaVelWorld2.MultiplyBy(ref invtens2);
                deltaVelWorld2.MultiplyBy(ref ImpulseTorque);
                deltaVelWorld2.Multiply(-1);

                deltaVelWorld.Add(ref deltaVelWorld2);

                inverseMass += Second.InvertedMass;
            }

            Matrix3 deltaV = Matrix3.Transpose(ref ContactToWorld);
            deltaV.MultiplyBy(ref deltaVelWorld);
            deltaV.MultiplyBy(ref ContactToWorld);

            //add linear vel changes
            deltaV.m00 += inverseMass;
            deltaV.m11 += inverseMass;
            deltaV.m22 += inverseMass;

            Matrix3 impulseMat = Matrix3.Invert(ref deltaV);

            Vector3 impulseContact = new Vector3(DesiredDeltaV, -ContactVelocity.Y, -ContactVelocity.Z);

            impulseMat.Transform(ref impulseContact);

            //check friction

            float planarImpulse = MathTools.Sqrt(impulseContact.Y * impulseContact.Y + impulseContact.Z * impulseContact.Z);

            if (planarImpulse > impulseContact.X * Friction)
            {
                float y = impulseContact.Y/ planarImpulse;
                float z = impulseContact.Z / planarImpulse;
                float x = deltaV.m00 + deltaV.m10 *Friction*y + deltaV.m20 * Friction * z;

                impulseContact.Assign(DesiredDeltaV/x, y, z);
            }

            return impulseContact;            
        }

        /// <summary>
        /// Calculates needed Delta V
        /// </summary>
        /// <param name="time">Elapsed time</param>
        protected void CalculateDeltaV(float time)
        {

            float velocity = 0;

            DesiredDeltaV = -ContactVelocity.X;

            //limit restitution for very low velocities
            if (MathTools.Abs(ContactVelocity.X) < DeltaVSpeedLimit)
                return;


            if (!First.Sleeping)
            {
                Vector3 v = First.AccelerationLastFrame;
                v.Mul(time);
                velocity = v.Dot(ref ContactNormal);
            }

            if ((Second != null) && (!Second.Sleeping))
            {
                Vector3 v = Second.AccelerationLastFrame;
                v.Mul(time);
                velocity += v.Dot(ref ContactNormal);
            }

            DesiredDeltaV += Restitution * (ContactVelocity.X - velocity);
        }


        protected Vector3[] tangents = new Vector3[2];

        /// <summary>
        /// Updates Matrix ContactToWorld
        /// </summary>
        protected void CalcContactToWorld()
        {
            //is normal.z closer to x or y?
            if (MathTools.Abs(ContactNormal.X) > MathTools.Abs(ContactNormal.Y))
            {
                float scalefactor = 1.0f / MathTools.Sqrt(ContactNormal.Z * ContactNormal.Z + ContactNormal.X * ContactNormal.X);

                //new x axis is at right angle with world Y
                tangents[0].X = ContactNormal.Z * scalefactor;
                tangents[0].Y = 0;
                tangents[0].Z = -ContactNormal.X * scalefactor;

                tangents[1].X = ContactNormal.Y * tangents[0].X;
                tangents[1].Y = ContactNormal.Z * tangents[0].X - ContactNormal.X * tangents[0].Z;
                tangents[1].Z = -ContactNormal.Y * tangents[0].X;
            }
            else
            {
                float scalefactor = 1.0f / MathTools.Sqrt(ContactNormal.Z * ContactNormal.Z + ContactNormal.Y * ContactNormal.Y);
                //new x axis is at right angle with world Y
                tangents[0].X = 0;
                tangents[0].Y = -ContactNormal.Z * scalefactor;
                tangents[0].Z = ContactNormal.Y * scalefactor;

                tangents[1].X = ContactNormal.Y * tangents[0].Z - ContactNormal.Z * tangents[0].Y;
                tangents[1].Y = -ContactNormal.X * tangents[0].Z;
                tangents[1].Z = ContactNormal.X * tangents[0].Z;
            }

            ContactToWorld.Column0 = ContactNormal;
            ContactToWorld.Column1 = tangents[0];
            ContactToWorld.Column1 = tangents[1];
        }

        /// <summary>
        /// Calculates local velocity for body
        /// </summary>
        /// <param name="index">0 or 1 for first or second body</param>
        /// <param name="time">Elapsed time</param>
        /// <returns>Calculated velocity</returns>
        protected Vector3 LocalVelocity(int index, float time)
        {
            RigidBody body = Bodies[index];

            Vector3 vel = body.AngularVelocity.Cross(ref RelativeContactPos[index]);
            vel.Add(body.Velocity);

            ContactToWorld.TransformTranspose(ref vel);

            Vector3 accVel = body.AccelerationLastFrame;
            accVel.Mul(time);
            ContactToWorld.TransformTranspose(ref accVel);  //!!! transposed transformation?

            accVel.X = 0; //ignore component in contact normal 

            vel.Add(ref accVel); //planar velocity

            if ((vel.X == float.NaN) || (vel.Y == float.NaN) || (vel.Z == float.NaN))
            {
                Console.WriteLine("Failed local velocity calculation!");
            }

            return vel;
        }


        /// <summary>
        /// Calculate internal values
        /// </summary>
        /// <param name="time">elapsed time</param>
        public void CalcInternal(float time)
        {
            if (First == null)
                SwapBodies();
            if (First == null)
                throw new ArgumentException("Can't solve contact for pair of null bodies!");            
            CalcContactToWorld();

            RelativeContactPos[0] = ContactPoint1; RelativeContactPos[0].Substract(First.Position);
            ContactVelocity = LocalVelocity(0, time);
            if (Second != null)
            {
                RelativeContactPos[1] = ContactPoint1; RelativeContactPos[1].Substract(Second.Position);
                ContactVelocity -= LocalVelocity(1, time);
            }
            CalculateDeltaV(time);
        }


    }
}
