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

namespace VisiMath.Collisions
{
    /// <summary>
    /// Collision solver for various objects
    /// </summary>
    public static class CollisionSolver
    {
        private static GJK_EPASolver solver = new GJK_EPASolver();
        
        #region Distances

        public static class Distances
        {
            /// <summary>
            /// Calculates distances between oriented box and line segment
            /// </summary>
            /// <param name="box">Oriented box</param>
            /// <param name="p1">Segment point 1</param>
            /// <param name="p2">Segment point 2</param>
            /// <returns>Distance</returns>
            public static float OOBoxLineSegment(OOBox box, ref Vector3 p1, ref Vector3 p2)
            {                
                Matrix4 mat = Matrix4.Identitity;
                LineSegment3D ls = new LineSegment3D(ref p1, ref p2);

                return solver.ClosestDistance(box, ref mat, ls, ref mat);        //OPTIMIZE!!!
            }

            
            /// <summary>
            /// Calculates distances between oriented box and triangle
            /// </summary>
            /// <param name="box">Oriented box</param>
            /// <param name="p1">Triangle point 1</param>
            /// <param name="p2">Triangle point 2</param>
            /// <param name="p3">Triangle point 3</param>
            /// <returns>Distance</returns>
            public static float OOBoxTriangle(OOBox box, ref Vector3 p1, ref Vector3 p2, ref Vector3 p3)
            {
                Matrix4 mat = Matrix4.Identitity;
                TriangleShape ts = new TriangleShape(ref p1, ref p2, ref p3);

                return solver.ClosestDistance(box, ref mat, ts, ref mat);        //OPTIMIZE!!!
            }
        
        }       

        #endregion

        #region Closest Points

        public static class ClosestPoint
        {
            /// <summary>
            /// Calculates closest point on the inifinite line to quad
            /// </summary>
            /// <param name="origin">Origin of the line</param>
            /// <param name="normal">Normal of the line</param>
            /// <param name="quad">Quad vertices</param>
            /// <param name="onQuad">Closest point on the quad</param>
            /// <param name="t">parameter t for line to reach the closest point</param>
            public static void LineQuad(ref Vector3 origin, ref Vector3 normal, Vector3[] quad, out Vector3 onQuad, out float t)
            {
                //stupid quick hack, just checks distance to the corners (and probably wrongly)
                //Do properly!!!!
                float best = float.MaxValue;
                int bc = 0;

                for (int i = 0; i < 4; i++)
                {
                    Vector3 p = quad[i];
                    p.Substract(ref origin);
                    float tt = normal.Dot(ref p);
                    if (tt < best)
                    {
                        best = tt;
                        bc = i;
                    }

                }
                t = best;
                onQuad = quad[bc];
            }
            
            
            /// <summary>
            /// Calculates closest point on the inifinite line to oobox
            /// </summary>
            /// <param name="origin">Origin of the line</param>
            /// <param name="normal">Normal of the line</param>
            /// <param name="box">OObox</param>
            /// <param name="onbox">Closest point on the box</param>
            /// <param name="t">parameter t for line to reach the closest point</param>
            public static void LineOOBox(ref Vector3 origin, ref Vector3 normal, OOBox box, out Vector3 onbox, out float t)
            {
                //stupid bruteforce, OPTIMIZE!
                Vector3[] Corners = box.GetQuads();

                float best = float.MaxValue;
                t = 0;
                onbox = new Vector3();

                for (int i = 0; i < 6; i++)
                {
                    Vector3[] quad = new Vector3[4]{Corners[i*6], Corners[i*6+1], Corners[i*6+2], Corners[i*6 + 3]};
                    float tt;
                    Vector3 onb;
                    LineQuad(ref origin, ref normal, quad, out onb, out tt);

                    Vector3 check = new Vector3(ref origin, tt);

                    float dst = check.Distance(ref onb);

                    if (dst < best)
                    {
                        best = dst;
                        onbox = onb;
                        t = tt;
                    }
                }
            }

            /// <summary>
            /// Calculates closest point on the line to oobox
            /// </summary>
            /// <param name="origin">Origin of the line</param>
            /// <param name="target">End of the line</param>
            /// <param name="box">OObox</param>
            /// <param name="onbox">Closest point on the box</param>
            /// <param name="t">parameter t for line to reach the closest point</param>
            public static void LineSegmentOOBox(ref Vector3 origin, ref Vector3 target, OOBox box, out Vector3 onbox, out Vector3 online)
            {
                float t;
                Vector3 normal = Vector3.Substract(ref target, ref origin);
                float length = normal.Length;
                LineOOBox(ref origin, ref normal, box, out onbox, out t);
                MathTools.Clamp(ref t, 0, 1);
                online = new Vector3(ref origin, t * length);   //this might not work for all cases?                
            }
        }


        #endregion

        #region Line Intersections

        public static class LineTest
        {
            /// <summary>
            /// Returns SIGNED distance line must travel from origin to hit the box, or float.nan if no intersection
            /// </summary>
            /// <param name="origin">Origin of the line</param>
            /// <param name="normal">Normal of the line</param>
            /// <param name="box">OObox</param>
            /// <returns></returns>
            public static float LineOOBox(ref Vector3 origin, ref Vector3 normal, OOBox box)
            {
                //Stupid bruteforce, OPTIMIZE!!!
                Vector3[] Corners = box.GetQuads();
                
                float t;
                Vector3 inters;

                float best = float.NaN;

                for (int i = 0; i < 6; i++)
                {
                    if (MathTools.IntersectionRayTriangle(ref origin, ref normal, ref Corners[i * 6 + 0], ref Corners[i * 6 + 1], ref Corners[i * 6 + 2], out t, out inters))
                    {
                        if (t != float.NaN)
                        {
                            if (best == float.NaN)
                                best = t;
                            else best = MathTools.Min(t, best);
                        }
                    }

                    if (MathTools.IntersectionRayTriangle(ref origin, ref normal, ref Corners[i * 6 + 2], ref Corners[i * 6 + 3], ref Corners[i * 6 + 0], out t, out inters))
                    {
                        if (t != float.NaN)
                        {
                            if (best == float.NaN)
                                best = t;
                            else best = MathTools.Min(t, best);
                        }
                    }
                }
                return best;
            }
        }


        #endregion


        #region Quick intersection tests

        public static class QuickTest
        {

            /// <summary>
            /// Performs quick collision check for two generic convex objects
            /// </summary>
            /// <param name="obj1">Object 1</param>
            /// <param name="obj2">Object 2</param>
            /// <returns>True if objects collide</returns>
            public static bool ConvexObjects(IConvexObject obj1, IConvexObject obj2)
            {
                return solver.Collide(obj1, obj2);
            }

            /// <summary>
            /// Performs quick intersection test of two spatial indices using GJK solver
            /// </summary>
            /// <param name="ind1">Spatial index1</param>
            /// <param name="ind2">Spatial index2</param>
            /// <returns>True if intersection</returns>
            public static bool SpatialIndices(ref ISpatialIndex ind1, ref ISpatialIndex ind2)
            {
                return solver.Collide(ind1, ind2);
            }

            /// <summary>
            /// Quick check for two generic geometry objects with transformations
            /// </summary>
            /// <param name="obj1">object 1</param>
            /// <param name="mat1">transformation of object 1</param>
            /// <param name="obj2">object 2</param>
            /// <param name="mat2">transformation of object 2</param>
            /// <returns>True if objects collide</returns>
            public static bool Geometry(IGeometry obj1, ref Matrix4 mat1, IGeometry obj2, ref Matrix4 mat2)
            {
                IConvexObject ic1 = obj1 as IConvexObject;
                IConvexObject ic2 = obj2 as IConvexObject;

                //both convex, use gjk
                if ((ic1 != null) && (ic2 != null))
                {
                    return solver.Collide(ic1, ref mat1, ic2, ref mat2);
                }

                if (ic1 != null)
                {
                    return ConvexGeneric(ic1, ref mat1, obj2, ref mat2);
                }

                if (ic2 != null)
                {
                    return ConvexGeneric(ic2, ref mat2, obj1, ref mat1);
                }
                
                return false;
                //throw new NotImplementedException();
            }

            public static bool ConvexGeneric(IConvexObject obj1, ref Matrix4 mat1, IGeometry obj2, ref Matrix4 mat2)
            {
                return false;
                //throw new NotImplementedException();
            }



        }

        #endregion

        /*
        public static class Containment
        {
            public static bool PointInHull(ref Vector3 point, Vector3[] vertices)
            {
            }

            public static bool PointInHull(Vector3 point, ref ISpatialIndex hullindex)
            {
            }

            public static bool SphereInHull(ref Vector3 center, float radius, Vector3[] vertices)
            {
            }

            public static bool SphereInHull(Vector3 center, float radius, ref ISpatialIndex hull)            
            {
            }
        }
        */

        #region Collisions

        public static class Collision
        {
            public static bool Sphere(Sphere obj1, ref Matrix4 mat1, IGeometry obj2, ref Matrix4 mat2, ref CollisionInfo info)
            {
                Sphere sph = obj2 as Sphere;
                if (sph != null)
                {
                    //both spheres
                    Vector3 v1 = obj1.Center;
                    v1.Add(mat1.Translation);
                    Vector3 v2 = obj2.Center;
                    v2.Add(mat2.Translation);

                    float dst2 = v1.DistanceSquared(ref v2);

                    float rads = (obj1.Radius + sph.Radius) * (obj1.Radius + sph.Radius);
                    
                    if (dst2 > rads)
                        return false;

                    float dst = MathTools.Sqrt(dst2);

                    Vector3 normal = v1 - v2;
                    normal.Normalize();
                    info.ContactNormal = normal;
                    v1.AddScaled(normal, 0.5f); //may not work...
                    info.ContactPoint1 = v1;
                    v2.AddScaled(normal, -0.5f);
                    info.ContactPoint2 = v2;
                    info.Penetration = rads - dst2;
                    
                    return true;
                }

                IConvexObject ic1 = obj1 as IConvexObject;
                IConvexObject ic2 = obj2 as IConvexObject;

                //both convex, use gjk
                if ((ic1 != null) && (ic2 != null))
                {
                    return solver.Collide(ic1, ref mat1, ic2, ref mat2, ref info);
                }

                
                return false;
                //throw new NotImplementedException();
            }


            public static bool Geometry(IGeometry obj1, ref Matrix4 mat1, IGeometry obj2, ref Matrix4 mat2, ref CollisionInfo info)
            {
                //check if one of them is sphere
                Sphere sph = obj1 as Sphere;
                if (sph != null)
                {
                    return Sphere(sph, ref mat1, obj2, ref mat2, ref info);
                }
                sph = obj2 as Sphere;
                if (sph != null)
                {
                    return Sphere(sph, ref mat2, obj1, ref mat1, ref info);
                }

                IConvexObject ic1 = obj1 as IConvexObject;
                IConvexObject ic2 = obj2 as IConvexObject;

                //both convex, use gjk
                if ((ic1 != null) && (ic2 != null))
                {
                    return solver.Collide(ic1, ref mat1, ic2, ref mat2, ref info);
                }

                //one or both are nonconvex / containers

                return false;

                //throw new NotImplementedException();
            }


        }


        #endregion


    }
}
