using UnityEngine;
using System.Collections;
using System.Collections.Generic;

static class PhysicsExtensionMethods
{
    public static bool IsInSameDirection(this Vector3 vector, Vector3 otherVector)
    {
        return Vector3.Dot(vector, otherVector) > 0;
    }

    public static bool IsInOppositeDirection(this Vector3 vector, Vector3 otherVector)
    {
        return Vector3.Dot(vector, otherVector) < 0;
    }
}

public interface IConvexRegion
{
    Vector3 GetFurthestPoint(Vector3 direction);
}

class Simplex
{
    List<Vector3> _vertices =
        new List<Vector3>();

    public int Count
    {
        get { return _vertices.Count; }
    }

    public Vector3 this[int i]
    {
        get { return _vertices[i]; }
    }

    public Simplex(params Vector3[] vertices)
    {
        for (int i = 0; i < vertices.Length; i++)
        {
            _vertices.Add(vertices[i]);
        }
    }

    public void Add(Vector3 vertex)
    {
        _vertices.Add(vertex);
    }

    public void Remove(Vector3 vertex)
    {
        _vertices.Remove(vertex);
    }
}

public class GJK
{
    public static bool Intersects(IConvexRegion regioneOne, IConvexRegion regionTwo)
    {
        Vector3 s = Support(regioneOne, regionTwo, Vector3.one);
        Simplex simplex = new Simplex(s);
        Vector3 d = -s;
        int maxIterations = 50;

        for (int i = 0; i < maxIterations; i++)
        {
            Vector3 a = Support(regioneOne, regionTwo, d);

            if (a.IsInOppositeDirection(d))
            {
                return false;
            }
            simplex.Add(a);
            if (ProcessSimplex(ref simplex, ref d))
            {
                return true;
            }
        }
        return true;
    }

    static bool ProcessSimplex(ref Simplex simplex, ref Vector3 direction)
    {
        if (simplex.Count == 2)
        {
            return ProcessLine(ref simplex, ref direction);
        }
        else if (simplex.Count == 3)
        {
            return ProcessTriangle(ref simplex, ref direction);
        }
        else
        {
            return ProcessTetrehedron(ref simplex, ref direction);
        }
    }

    static bool ProcessLine(ref Simplex simplex, ref Vector3 direction)
    {
        Vector3 a = simplex[1];
        Vector3 b = simplex[0];
        Vector3 ab = b - a;
        Vector3 aO = -a;

        Debug.DrawLine(a, b, Color.red);

        if (ab.IsInSameDirection(aO))
        {
            float dot = Vector3.Dot(ab, aO);
            float angle = (float)Mathf.Acos(dot / (ab.magnitude * aO.magnitude));
            direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
        }
        else
        {
            simplex.Remove(b);
            direction = aO;
        }
        return false;
    }

    static bool ProcessTriangle(ref Simplex simplex, ref Vector3 direction)
    {
        Vector3 a = simplex[2];
        Vector3 b = simplex[1];
        Vector3 c = simplex[0];
        Vector3 ab = b - a;
        Vector3 ac = c - a;
        Vector3 abc = Vector3.Cross(ab, ac);
        Vector3 aO = -a;
        Vector3 acNormal = Vector3.Cross(abc, ac);
        Vector3 abNormal = Vector3.Cross(ab, abc);


        Debug.DrawLine(a, b, Color.blue);
        Debug.DrawLine(b, c, Color.blue);
        Debug.DrawLine(a, c, Color.blue);

        if (acNormal.IsInSameDirection(aO))
        {
            if (ac.IsInSameDirection(aO))
            {
                simplex.Remove(b);
                direction = Vector3.Cross(Vector3.Cross(ac, aO), ac);
            }
            else
            {
                if (ab.IsInSameDirection(aO))
                {
                    simplex.Remove(c);
                    direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
                }
                else
                {
                    simplex.Remove(b);
                    simplex.Remove(c);
                    direction = aO;
                }
            }
        }
        else
        {
            if (abNormal.IsInSameDirection(aO))
            {
                if (ab.IsInSameDirection(aO))
                {
                    simplex.Remove(c);
                    direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
                }
                else
                {
                    simplex.Remove(b);
                    simplex.Remove(c);
                    direction = aO;
                }
            }
            else
            {
                if (abc.IsInSameDirection(aO))
                {
                    direction = Vector3.Cross(Vector3.Cross(abc, aO), abc);
                }
                else
                {
                    direction = Vector3.Cross(Vector3.Cross(-abc, aO), -abc);
                }
            }
        }
        return false;
    }

    static bool ProcessTetrehedron(ref Simplex simplex, ref Vector3 direction)
    {
        Vector3 a = simplex[3];
        Vector3 b = simplex[2];
        Vector3 c = simplex[1];
        Vector3 d = simplex[0];
        Vector3 ac = c - a;
        Vector3 ad = d - a;
        Vector3 ab = b - a;
        Vector3 bc = c - b;
        Vector3 bd = d - b;

        Vector3 acd = Vector3.Cross(ad, ac);
        Vector3 abd = Vector3.Cross(ab, ad);
        Vector3 abc = Vector3.Cross(ac, ab);

        Vector3 aO = -a;

        if (abc.IsInSameDirection(aO))
        {
            if (Vector3.Cross(abc, ac).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ac, aO), ac);
            }
            else if (Vector3.Cross(ab, abc).IsInSameDirection(aO))
            {
                simplex.Remove(c);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
            }
            else
            {
                simplex.Remove(d);
                direction = abc;
            }
        }
        else if (acd.IsInSameDirection(aO))
        {
            if (Vector3.Cross(acd, ad).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(c);
                direction = Vector3.Cross(Vector3.Cross(ad, aO), ad);
            }
            else if (Vector3.Cross(ac, acd).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ac, aO), ac);
            }
            else
            {
                simplex.Remove(b);
                direction = acd;
            }
        }
        else if (abd.IsInSameDirection(aO))
        {
            if (Vector3.Cross(abd, ab).IsInSameDirection(aO))
            {
                simplex.Remove(c);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
            }
            else if (Vector3.Cross(ad, abd).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(c);
                direction = Vector3.Cross(Vector3.Cross(ad, aO), ad);
            }
            else
            {
                simplex.Remove(c);
                direction = abd;
            }
        }
        else
        {
            Debug.DrawLine(a, b, Color.green);
            Debug.DrawLine(b, c, Color.green);
            Debug.DrawLine(c, d, Color.green);
            Debug.DrawLine(d, a, Color.green);
            return true;
        }

        Debug.DrawLine(a, b, Color.yellow);
        Debug.DrawLine(b, c, Color.yellow);
        Debug.DrawLine(c, d, Color.yellow);
        Debug.DrawLine(d, a, Color.yellow);
        return false;
    }

    static Vector3 Support(
        IConvexRegion regionOne,
        IConvexRegion regionTwo,
        Vector3 direction)
    {
        return regionOne.GetFurthestPoint(direction) -
            regionTwo.GetFurthestPoint(-direction);
    }
}


public class Sphere : IConvexRegion
{
    public Vector3 Center;
    public float Radius;

    public Sphere(Vector3 center, float radius)
    {
        Center = center;
        Radius = radius;
    }

    public Vector3 GetFurthestPoint(Vector3 direction)
    {
        if (direction != Vector3.zero)
        {
            direction.Normalize();
        }
        return Center + Radius * direction;
    }
}


public class Box : IConvexRegion
{
    public Vector3 Center;
    Vector3 _halfDimensions = Vector3.one;
    Quaternion _orientation = Quaternion.identity;

    public Vector3 Dimensions
    {
        get { return 2f * _halfDimensions; }
    }

    public Box(Vector3 center) : this(center, 1f, 1f, 1f)
    {
    }

    public Box(Vector3 center,float width,float height,float depth)
        : this(center, width, height, depth, Matrix4x4.identity)
    {
    }

    public Box(Vector3 center,float width,float height,float depth,Matrix4x4 rotationMatrix)
    {
        Center = center;
        _halfDimensions = new Vector3(width / 2f,height / 2f,depth / 2f);
        _orientation = variables.QuaternionFromMatrix(rotationMatrix);
    }

    public Vector3 GetFurthestPoint(Vector3 direction)
    {
        Vector3 halfHeight = _halfDimensions.y * Vector3.up;
        Vector3 halfWidth = _halfDimensions.x * Vector3.right;
        Vector3 halfDepth = _halfDimensions.z * Vector3.back;

        Vector3[] vertices = new Vector3[8];
        vertices[0] = halfWidth + halfHeight + halfDepth;
        vertices[1] = -halfWidth + halfHeight + halfDepth;
        vertices[2] = halfWidth - halfHeight + halfDepth;
        vertices[3] = halfWidth + halfHeight - halfDepth;
        vertices[4] = -halfWidth - halfHeight + halfDepth;
        vertices[5] = halfWidth - halfHeight - halfDepth;
        vertices[6] = -halfWidth + halfHeight - halfDepth;
        vertices[7] = -halfWidth - halfHeight - halfDepth;

        Matrix4x4 m = Matrix4x4.TRS(Center, _orientation, Vector3.one);

        Vector3 furthestPoint = m.MultiplyPoint(vertices[0]);
        float maxDot = Vector3.Dot(furthestPoint, direction);
        for (int i = 1; i < 8; i++)
        {
            Vector3 vertex = m.MultiplyPoint(vertices[i]);
            float dot = Vector3.Dot(vertex, direction);
            if (dot > maxDot)
            {
                maxDot = dot;
                furthestPoint = vertex;
            }
        }
        return furthestPoint;
    }
}

public class Capsue : IConvexRegion
{
    public Vector3[] innerPoints = new Vector3[2];
    public float radius;

    public Capsue(Vector3[] innerPoints) : this(innerPoints, 1f)
    {
    }

    public Capsue(Vector3[] innerPoints, float radius)
    {
        this.innerPoints = innerPoints;
        this.radius = radius;
    }

    public Vector3 GetFurthestPoint(Vector3 direction)
    {
        if (direction != Vector3.zero)
        {
            direction.Normalize();
        }
        return (Vector3.Dot(direction, innerPoints[1] - innerPoints[0]) >= .0f ? innerPoints[1] : innerPoints[0]) + radius * direction;
    }
}