using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;


namespace CollisionExample
{
    public class OOBB
    {
        /// Center of our OOBB.  Note that when we translate the center point,
        /// we need to move the cached world positions of each point in our OOBB,
        /// as well as the cached positions of the AABB
        public Vector2 Center
        {
            get
            {
                return mCenter;
            }
            set
            {
                Vector2 diff = value - mCenter;
                mCenter = value;
                for (int i = 0; i < 4; i++)
                {
                    mPointsInWorldSpace[i] += diff;
                }
                mAABBPos = mAABBPos + diff;
                mAABB.Location = new Point((int)mAABBPos.X, (int)mAABBPos.Y);
            }
        }

        // Vector from center of OOBB to largest x,y point, in object space
        public Vector2 HalfExtents
        {
            get
            {
                return mHalfExtents;
            }
            set
            {
                mHalfExtents = value;
                ResetWorldPointsAndAABB();
            }
        }

        // Rotation of our OOBB, clockwise, in radians
        public double Rotation
        {
            get
            {
                return mRotation;
            }
            set
            {
                mRotation = value;
                ResetWorldPointsAndAABB();
            }
        }

        // Rotation of our OOBB, clockwise, in degrees
        public double RotationDegrees
        {
            get
            {
                return mRotation * (180.0 / Math.PI);
            }
            set
            {
                mRotation = value * Math.PI / 180.0;
                ResetWorldPointsAndAABB();
            }
        }

        // Smallest possible Axis Aligned Bounding Box that contains our box
        public Rectangle AABB { get { return mAABB; } }


        public OOBB(Vector2 center, Vector2 radius, double rotationRadians)
        {
            mCenter = center;
            mHalfExtents = radius;
            mRotation = rotationRadians;
            mPointsInWorldSpace = new Vector2[4];
            ResetWorldPointsAndAABB();
        }

        /// <summary>
        /// Returns true if this OOBB collides with the OOBB passed in.  Returns the
        /// Minimum Displacement Vector required to move *this* object to avoid penetration.
        /// </summary>
        /// <param name="otherObject">The object to test collision with</param>
        /// <param name="MTD">Out: Minimum Translation Displacement vector required to
        /// move this object away from otherObject to avoid penetration</param>
        /// <returns></returns>
        public bool Collide(OOBB otherObject, out Vector2 MTD)
        {
            MTD = Vector2.Zero;

            // First, we try the AABB bounding box collision, for a 
            // fast exit on objets that are far apart
            if (!AABB.Intersects(otherObject.AABB))
            {
                return false;
            }

            float minOverlapSize = float.MaxValue;
            bool useOtherObject = false;
            // We need to find separating axes perpendicular to the faces of each object.
            // This outer loop executes twice, once for each object we are considering
            //   (this and otherObject)
            for (int objectToUse = 0; objectToUse < 2; objectToUse++, useOtherObject = !useOtherObject)
            {

                //  We are dealing with rectangles, so each object has only two separating axes 
                //    (instead of one axis for each edge, as would be the case if no edges in our
                //     figure were parallel)
                //  If we wanted to extend this to arbitraty poygons, we would need to create a
                //  separating axis for each edge in the polygon
                for (int i = 0; i < 2; i++)
                {
                    Vector2 axisOfSeparation;

                    // We are again taking advantage of the fact that these are rectangles -- the axes 
                    // that we want are perpendicular to each egde, but since we are dealing in rectangles,
                    // we can just take an axis parallel to two adjoining edges (since the axis parallel
                    // to one edge is perpendicular to the the adjoining edge).  If we were doing arbitrary
                    // polygons, we would need to do something like:
                    //
                    // axisOfSeparation = mPointsInWorldSpace[i] - mPointsInWorldSpace[i + 1];
                    // axisOfSeparation = new Vector2(-axisOfSeparation.Y, axisOfSeparation.X);
                    //
                    // While that would also work for rectangles, it is an unnecessary calculation.
                    // (Also, if we were doing arbitrary polygons, we would need to examine all edges,
                    //  mPointsInWorldSpace[0] - mPointsInWorldSpace[1]; mPointsInWorldSpace[1] - mPointsInWorldSpace[2];
                    //  ... mPointsInWorldSpace[n-1] - mPointsInWorldSpace[n]; mPointsInWorldSpace[n] - mPointsInWorldSpace[0];
                    if (useOtherObject)
                        axisOfSeparation = otherObject.mPointsInWorldSpace[i] - otherObject.mPointsInWorldSpace[i + 1];
                    else
                        axisOfSeparation = mPointsInWorldSpace[i] - mPointsInWorldSpace[i + 1];
                    axisOfSeparation.Normalize();

                    float minProjO1, maxProjO1;
                    float minProj02, maxProjO2;
                    ProjectionRange(out minProjO1, out maxProjO1, axisOfSeparation, mPointsInWorldSpace);
                    ProjectionRange(out minProj02, out maxProjO2, axisOfSeparation, otherObject.mPointsInWorldSpace);

                    float overlapSize = FindOverlap(minProjO1, maxProjO1, minProj02, maxProjO2);
                    if (overlapSize == 0.0f)
                    {
                        MTD = Vector2.Zero;
                        return false;
                    }
                    if (overlapSize < minOverlapSize)
                    {
                        minOverlapSize = overlapSize;
                        MTD = axisOfSeparation * minOverlapSize;
                        // We want the MTD vector to be the vector that *this* object needs to move,
                        // not the vector that the *other* object needs to move.  If the projection of
                        // this object is less than the projection of the other object, then the projection
                        // vector points the wrong way, so we should flip it.
                        if (minProjO1 < minProj02)
                        {
                            MTD = -MTD;
                        }
                    }
                }
            }
            return true;
        }

        /// <summary>
        /// Rotate a point theta degrees (from positive x to positive y, clockwise in XNA
        /// screen space), given sin(theta) and cos(theta)
        /// </summary>
        /// <param name="point">Point to rotate</param>
        /// <param name="sintheta">sin(theta)</param>
        /// <param name="costheta">cos(theta)</param>
        /// <returns>Rotated point</returns>
        private Vector2 Rotate(Vector2 point, double sintheta, double costheta)
        {
            return new Vector2((float)(point.X * costheta - point.Y * sintheta),
                               (float)(point.X * sintheta + point.Y * costheta));
        }

        /// <summary>
        /// Recalculate the world positions of all points in our object, as well as 
        /// the bounding AABB.  This method need to be called whenever the rotation
        /// or half-extents are changed. 
        /// </summary>
        private void ResetWorldPointsAndAABB()
        {
            double cosTheta = Math.Cos(Rotation);
            double sinTheta = Math.Sin(Rotation);

            mPointsInWorldSpace[0] = Rotate(new Vector2(-HalfExtents.X, -HalfExtents.Y), sinTheta, cosTheta) + Center;
            mPointsInWorldSpace[1] = Rotate(new Vector2(HalfExtents.X, -HalfExtents.Y), sinTheta, cosTheta) + Center;
            mPointsInWorldSpace[2] = Rotate(new Vector2(HalfExtents.X, HalfExtents.Y), sinTheta, cosTheta) + Center;
            mPointsInWorldSpace[3] = Rotate(new Vector2(-HalfExtents.X, HalfExtents.Y), sinTheta, cosTheta) + Center;

            float minXValue = Math.Min(Math.Min(mPointsInWorldSpace[0].X, mPointsInWorldSpace[1].X),
                                           Math.Min(mPointsInWorldSpace[2].X, mPointsInWorldSpace[3].X));
            float minYValue = Math.Min(Math.Min(mPointsInWorldSpace[0].Y, mPointsInWorldSpace[1].Y),
                                           Math.Min(mPointsInWorldSpace[2].Y, mPointsInWorldSpace[3].Y));
            float maxXValue = Math.Max(Math.Max(mPointsInWorldSpace[0].X, mPointsInWorldSpace[1].X),
                               Math.Max(mPointsInWorldSpace[2].X, mPointsInWorldSpace[3].X));
            float maxYValue = Math.Max(Math.Max(mPointsInWorldSpace[0].Y, mPointsInWorldSpace[1].Y),
                                           Math.Max(mPointsInWorldSpace[2].Y, mPointsInWorldSpace[3].Y));

            mAABBPos = new Vector2(minXValue, minYValue);
            mAABB = new Rectangle((int)minXValue, (int)minYValue, (int)(maxXValue - minXValue), (int)(maxYValue - minYValue));
        }

        // Floating point position of our AABB.  While we represent out
        //  AABB as a rectangle (integer values), it is handy to keep track
        //  of the floating-point position of the upper-left corner of our AABB,
        //  so that we can move our object fractional amounts, and not have the
        //  rounding error for the AABB add up over time
        private Vector2 mAABBPos;
        private Rectangle mAABB;
        private Vector2 mCenter;
        private Vector2 mHalfExtents;
        private double mRotation;
        private Vector2[] mPointsInWorldSpace;


        /// <summary>
        /// Given an axis to project an object onto, and a list of points, calculate the range of 
        /// the projection onto the axis (that is, the minimum and maximum values after the projection)
        /// </summary>
        /// <param name="min">Out parameter:  Minimum projected value</param>
        /// <param name="max">Out parameter:  Maximum projected value</param>
        /// <param name="axis">Axis to project points onto</param>
        /// <param name="objectPoints">List of points to project</param>
        public void ProjectionRange(out float min, out float max, Vector2 axis, Vector2[] objectPoints)
        {
            min = float.MaxValue;
            max = float.MinValue;

            foreach (Vector2 point in objectPoints)
            {
                float proj = Vector2.Dot(axis, point);

                min = Math.Min(min, proj);
                max = Math.Max(max, proj);
            }
        }

        /// <summary>
        /// Given two ranges (denoted by the low and high value of each range),
        /// calculate the overlap of the two ranges.  Value = 0 if no overlap.
        /// </summary>
        /// <param name="low1">Low value of first range</param>
        /// <param name="high1">High value of first range</param>
        /// <param name="low2">Low value of second range</param>
        /// <param name="high2">High value of second range</param>
        /// <returns></returns>
        public float FindOverlap(float low1, float high1, float low2, float high2)
        {
            if ((high2 < low1) || (high1 < low2))
                return 0;
            return Math.Min(high1, high2) - Math.Max(low1, low2);
        }
    }

}