using System;
using System.Collections.Generic;
using System.Text;
using Math2D;
using Microsoft.DirectX;

// Aliases
using PhysicsShapes = FRB.Physics.PhysicsManager.PhysicsShapes;

namespace FRB.Physics
{
    public class PhysicsObject : AttachablePoly
    {
        const int LEFT = 0;
        const int RIGHT = 1;
        const int TOP = 2;
        const int BOTTOM = 3;

        private int LastCollisionSide = -1;

        public Point mCenterOfMass = new Point(0,0);
        public Point mCenterPoint = new Point(0, 0);
        
        public float mMass = 1f;
        public float mElasticCoefficient = 1f;
        public float mStaticFriction = 0;
        public float mKineticFriction = 0;

        public bool mMoveable = false;
        public bool mCollidable = true;

        public PhysicsShapes mShape;

        public float sclX = 0;
        public float sclY = 0;

        public float mRadius = 1;

        public int Layer = 0;

        private Vector3 mOldPosition = new Vector3(0, 0, 0);
        private Vector3 mOldVelocity = new Vector3(0, 0, 0);
        private Vector3 mOldAcceleration = new Vector3(0, 0, 0);

        static double differenceX;
        static double differenceY;

        private List<PhysicsForce> mConstantForces = new List<PhysicsForce>();
        private List<PhysicsForce> mImpulseForces = new List<PhysicsForce>();


        public PhysicsObject()
            : base(0)
        {
            SetShape(PhysicsShapes.Point);
        }


        public PhysicsObject(PhysicsShapes NewShape) 
            : base(0)
        {
            SetShape(NewShape);
        }

        public PhysicsObject(Sprite newSprite)
            : base(0)
        {

            SetShape(PhysicsShapes.Square);
            
            Point[] SquarePoint = new Point[4];
                       
            // Point 1
            SquarePoint[0].x = -newSprite.sclX;
            SquarePoint[0].y = newSprite.sclY;

            // Point 2
            SquarePoint[1].x = newSprite.sclX;
            SquarePoint[1].y = newSprite.sclY;

            // Point 3
            SquarePoint[2].x = -newSprite.sclX;
            SquarePoint[2].y = -newSprite.sclY;

            // Point 4
            SquarePoint[3].x = newSprite.sclX;
            SquarePoint[3].y = -newSprite.sclY;
          
            // Create Our Square
            SetPointsForVertices(SquarePoint);

            sclX = newSprite.sclX;
            sclY = newSprite.sclY;
        }

        public void InitializeObject()
        {



        }

        private void SetPointsForVertices(Point[] NewPoints)
        {
            // Add New Points
            for (int i = 0; i < NewPoints.Length; i++)
                AddPoint(NewPoints[i].x, NewPoints[i].y);
        }


        public void SetShape(PhysicsShapes NewShape)
        {
            switch (NewShape)
            {
                case PhysicsShapes.Polygon:
                    mShape = PhysicsShapes.Polygon;
                    break;
                case PhysicsShapes.Square:

                    mShape = PhysicsShapes.Square;


                    break;
                case PhysicsShapes.Circle:

                    mShape = PhysicsShapes.Circle;

                    break;
                case PhysicsShapes.Point:

                    // Setup Default Properties of a Point

                    mShape = PhysicsShapes.Point;
                    mCollidable = false;


                    break;
            }
        }



        public void UpdateObject()
        {
            if (DependentOn == null)
            {
                for (int i = 0; i < vertices; i++)
                {
                    p[i].x = position.X + relP[i].x;
                    p[i].y = position.Y + relP[i].y;
                }

                UpdateScl();
            }

            ApplyConstantForces();
        }


        public void StopAllMovement()
        {
            mOldVelocity = velocity;
            mOldAcceleration = acceleration;

            xVelocity = 0;
            yVelocity = 0;

            xAcceleration = 0;
            yAcceleration = 0;
        }

        public void StartFromOldMovement()
        {
            velocity = mOldVelocity;
            acceleration = mOldAcceleration;
        }


        public void ApplyConstantForces()
        {
            for (int i = 0; i < mConstantForces.Count; i++)
            {
                mConstantForces[i].UpdateForce(this);
            }
        }

        public void ApplyImpulseForces()
        {
            // Apply Impulse Forces
            for (int i = 0; i < mImpulseForces.Count; i++)
            {
                mImpulseForces[i].UpdateForce(this);
            }

            // Delete the List now that we've applyed them
            mImpulseForces.Clear();
        }

        public void FindCollisionForce(PhysicsObject cObject)
        {
            Vector3 mVector1 = new Vector3();
            Vector3 mVector2 = new Vector3();
            
            Vector3 mVector3 = new Vector3();
            Vector3 mVector4 = new Vector3();

            switch (cObject.mShape)
            {
                case PhysicsShapes.Square:


                    switch (LastCollisionSide)
                    {
                        case LEFT: 
                            break;
                        case RIGHT: 
                            break;
                        case TOP: 
                            break;
                        case BOTTOM: 
                            break;
                    }




                    mVector1.X = cObject.rotationMatrix.M21;
                    mVector1.Y = cObject.rotationMatrix.M22;
                    mVector1.Z = cObject.rotationMatrix.M23;

                    mVector2.X = this.rotationMatrix.M21;
                    mVector2.Y = this.rotationMatrix.M22;
                    mVector2.Z = this.rotationMatrix.M23;

                    mVector1.Normalize();

                    mVector3 = mVector2 - 2 * mVector1 * (Vector3.Dot(mVector1, mVector2));

                    velocity = mVector3 * ((((mElasticCoefficient + 1) * cObject.mMass * cObject.mOldVelocity.Length()) + (mOldVelocity.Length() * (mMass - (mElasticCoefficient * cObject.mMass)))) / (mMass + cObject.mMass));

                    cObject.velocity = mVector3 * ((((cObject.mElasticCoefficient + 1) * mMass * mOldVelocity.Length()) + (cObject.mOldVelocity.Length() * (cObject.mMass - (cObject.mElasticCoefficient * mMass)))) / (mMass + cObject.mMass));


                    break;

                case PhysicsShapes.Circle:

                    Vector3 Norm = new Vector3();

                    Vector3 Norm1 = new Vector3();
                    Vector3 Norm2 = new Vector3();

                    Vector3 Tang1 = new Vector3();
                    Vector3 Tang2 = new Vector3();

                    Norm = this.position - cObject.position;
                    Norm.Normalize();

                    Norm1 = (Vector3.Dot(velocity, -Norm)) * -Norm;
                    Norm2 = (Vector3.Dot(cObject.velocity, Norm)) * Norm;


                    if (Norm1.X == 0 && Norm1.Y == 0)
                        Norm1 = Norm2;
                    else if (Norm2.X == 0 && Norm2.Y == 0)
                        Norm2 = Norm1;

                    Tang1 = Norm1 - velocity;
                    Tang2 = Norm2 - cObject.velocity;

                    mVector1 = Tang2 + Norm2;
                    mVector2 = Tang1 + Norm1;

                    mVector1.Normalize();
                    mVector2.Normalize();

                    float MyVel1 = ((((mElasticCoefficient + 1) * cObject.mMass * -cObject.velocity.Length()) + (velocity.Length() * (mMass - (mElasticCoefficient * cObject.mMass)))) / (mMass + cObject.mMass));
                    float MyVel2 = ((((cObject.mElasticCoefficient + 1) * mMass * velocity.Length()) + (-cObject.velocity.Length() * (cObject.mMass - (cObject.mElasticCoefficient * mMass)))) / (mMass + cObject.mMass));

                    velocity = mVector1 * MyVel1;
                    cObject.velocity = mVector2 * MyVel2;

                    break;

            }
        }


        public PhysicsForce EnvironmentGravitationalForce(Vector3 Direction, float Acceleration)
        {
            PhysicsForce newConstantForcesArray = new PhysicsForce(PhysicsForce.ForceType.EnvironmentConstantGravitational);

            mConstantForces.Add(newConstantForcesArray);

            newConstantForcesArray.DirectionV = Direction;
            newConstantForcesArray.Acceleration = Acceleration;

            return newConstantForcesArray;
        }

        public PhysicsForce PositionalGravitationalForce(Vector3 Position, Vector3 Direction, float Acceleration)
        {
            PhysicsForce newConstantForcesArray = new PhysicsForce(PhysicsForce.ForceType.PositionalConstantGravitational);

            mConstantForces.Add(newConstantForcesArray);

            newConstantForcesArray.PositionV = Position;
            newConstantForcesArray.DirectionV = Direction;
            newConstantForcesArray.Acceleration = Acceleration;

            return newConstantForcesArray;
        }


        public bool PollygonCollisionAgainst(PhysicsObject cPoly)
        {
            // make sure that the two are touching or else you don't want to move this instance
            if (this.isTouching(cPoly))
            {
                Point[] pointsInside = this.GetPointsInside(cPoly);

                double longestDistanceSquared = double.NegativeInfinity;

                Point tempVector;
                Point vectorTo = new Point(0, 0);

                for (int i = 0; i < pointsInside.Length; i++)
                {
                    tempVector = pointsInside[i].VectorTo(cPoly);
                    if (tempVector.x * tempVector.x + tempVector.y * tempVector.y > longestDistanceSquared)
                    {
                        vectorTo = tempVector;
                        longestDistanceSquared = vectorTo.x * vectorTo.x + vectorTo.y * vectorTo.y;

                    }
                }

                pointsInside = cPoly.GetPointsInside(this);
                for (int i = 0; i < pointsInside.Length; i++)
                {
                    tempVector = pointsInside[i].VectorTo(this);
                    if (tempVector.x * tempVector.x + tempVector.y * tempVector.y > longestDistanceSquared)
                    {
                        vectorTo = tempVector;
                        longestDistanceSquared = vectorTo.x * vectorTo.x + vectorTo.y * vectorTo.y;

                        // We want to reverse the vector here because this instance is going to be moved.
                        vectorTo.x = -vectorTo.x;
                        vectorTo.y = -vectorTo.y;
                    }
                }

                topParent.x += (float)vectorTo.x;
                topParent.y += (float)vectorTo.y;

                return true;
            }

            return false;                      
            
        }

        public bool rectCollisionMoveRotation(PhysicsObject PhyObj1, PhysicsObject PhyObj2)
        {
            float tempX = PhyObj1.x;
            float tempY = PhyObj1.y;

            FRB.CommonFunctions.RotatePointAroundPoint(PhyObj2.x, PhyObj2.y, ref tempX, ref tempY, -PhyObj2.rotZ);


            if (tempX - PhyObj1.sclX < PhyObj2.x + PhyObj2.sclX &&
                tempX + PhyObj1.sclX > PhyObj2.x - PhyObj2.sclX &&
                tempY - PhyObj1.sclY < PhyObj2.y + PhyObj2.sclY &&
                tempY + PhyObj1.sclY > PhyObj2.y - PhyObj2.sclY )
            {// looks like we have a collision.  So let's move the object to the edge
                int sideOn = LEFT; // set it to this.  We'll change this if we need to
                double smallest = tempX + PhyObj1.sclX - (PhyObj2.x - PhyObj2.sclX);
                if (smallest < 0) smallest *= -1; // this gets us the absolute distance from the left

                double currentDistance = tempX - PhyObj1.sclX - (PhyObj2.x + PhyObj2.sclX);
                if (currentDistance < 0) currentDistance *= -1;
                if (currentDistance < smallest) { smallest = currentDistance; sideOn = RIGHT; }

                currentDistance = tempY - PhyObj1.sclY - (PhyObj2.y + PhyObj2.sclY);
                if (currentDistance < 0) currentDistance *= -1;
                if (currentDistance < smallest) { smallest = currentDistance; sideOn = TOP; }

                currentDistance = tempY + PhyObj1.sclY - (PhyObj2.y - PhyObj2.sclY);
                if (currentDistance < 0) currentDistance *= -1;
                if (currentDistance < smallest) { smallest = currentDistance; sideOn = BOTTOM; }

                LastCollisionSide = sideOn;

                switch (sideOn)
                {
                    case LEFT: tempX = PhyObj2.x - PhyObj1.sclX - PhyObj2.sclX ; break;
                    case RIGHT: tempX = PhyObj2.x + PhyObj1.sclX + PhyObj2.sclX; break;
                    case TOP: tempY = PhyObj2.y + PhyObj1.sclY + PhyObj2.sclY; break;
                    case BOTTOM: tempY = PhyObj2.y - PhyObj1.sclY - PhyObj2.sclY; break;
                }
                FRB.CommonFunctions.RotatePointAroundPoint(PhyObj2.x, PhyObj2.y, ref tempX, ref tempY, PhyObj2.rotZ);
                PhyObj1.x = tempX;
                PhyObj1.y = tempY;

                return true;
            }
            return false;
        }


        public bool circleCollisionMove(PhysicsObject PhyObj1, PhysicsObject PhyObj2)
        {
            differenceX = PhyObj2.x - PhyObj1.x;
            differenceY = PhyObj2.y - PhyObj1.y;

            double differenceSquared = differenceX * differenceX + differenceY * differenceY;
            double difference = Math.Pow(differenceSquared, .5);

            if (difference - PhyObj1.mRadius - PhyObj2.mRadius < 0)
            {
                double angle = (float)Math.Atan2(PhyObj1.y - PhyObj2.y, PhyObj1.x - PhyObj2.x);

                float distanceToMove = PhyObj1.mRadius + PhyObj2.mRadius;

                PhyObj1.x = PhyObj2.x + (float)Math.Cos(angle) * distanceToMove;
                PhyObj1.y = PhyObj2.y + (float)Math.Sin(angle) * distanceToMove;
                return true;
            }
            else
                return false;


        }



        public static char GetClosestSide(PhysicsObject ObjectToTest, PhysicsObject referenceObject)
        {
            char sideOn = 'l'; // set it to this.  We'll change this if we need to

            double smallest = Math.Abs(
                  ObjectToTest.x + ObjectToTest.sclX - (referenceObject.x - referenceObject.sclX));

            double currentDistance = Math.Abs(
                  ObjectToTest.x - ObjectToTest.sclX - (referenceObject.x + referenceObject.sclX));
           
            if (currentDistance < smallest) { smallest = currentDistance; sideOn = 'r'; }
           
            currentDistance = Math.Abs(
                ObjectToTest.y - ObjectToTest.sclY - (referenceObject.y + referenceObject.sclY));
            
            if (currentDistance < smallest) { smallest = currentDistance; sideOn = 't'; }
            
            currentDistance = Math.Abs(
                ObjectToTest.y + ObjectToTest.sclY - (referenceObject.y - referenceObject.sclY));
            
            if (currentDistance < smallest) { smallest = currentDistance; sideOn = 'b'; }

            return sideOn;
        }


        public bool CollisionCheck(PhysicsObject cObject)
        {
            switch (this.mShape)
            {
                case PhysicsShapes.Polygon:
                    break;
                case PhysicsShapes.Square:

                    switch (cObject.mShape)
                    {
                        case PhysicsShapes.Polygon:
                            break;
                        case PhysicsShapes.Square:

                            return rectCollisionMoveRotation(this, cObject);

                            break;
                        case PhysicsShapes.Circle:
                            break;
                        case PhysicsShapes.Point:
                            break;
                    }

                    break;
                case PhysicsShapes.Circle:

                    switch (cObject.mShape)
                    {
                        case PhysicsShapes.Polygon:
                            break;
                        case PhysicsShapes.Square:
                            break;
                        case PhysicsShapes.Circle:

                            return circleCollisionMove(this, cObject);

                            break;
                        case PhysicsShapes.Point:
                            break;
                    }



                    break;
                case PhysicsShapes.Point:
                    break;
            }
            
            return false;
        }

            

        



    }
}