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

namespace VisiMath.Geometry
{
    /// <summary>
    /// Optimized triangle with all values precalculated to speed up tests
    /// </summary>
    public struct ColTriangle
    {
        /// <summary>
        /// Points of the triangle
        /// </summary>
        public Vector3[] Points;
        /// <summary>
        /// Edge vectors - p2-p1, p3-p2, p3-p1
        /// </summary>
        public Vector3[] Edges;
        /// <summary>
        /// Normal vector
        /// </summary>
        public Vector3 Normal;
        /// <summary>
        /// Length of the edges
        /// </summary>
        public float[] Length;
        /// <summary>
        /// D parameter for plane equation
        /// </summary>
        public float D;       
        /// <summary>
        /// User data to store inside the triangle
        /// </summary>
        public object UserData;


        /// <summary>
        /// Reconstruct triangle from already set Points
        /// </summary>
        public void Reconstruct()
        {
            if (Points == null)
                throw new ArgumentException("Math Triangle is not initialized!");
            Edges[0] = Vector3.Substract(ref Points[1], ref Points[0]);
            Edges[1] = Vector3.Substract(ref Points[2], ref Points[1]);
            Edges[2] = Vector3.Substract(ref Points[2], ref Points[0]);

            Length[0] = Edges[0].LengthSquared;
            Length[1] = Edges[1].LengthSquared;
            Length[2] = Edges[2].LengthSquared;

            Normal = Edges[0].Cross(ref Edges[1]);
            Normal.Normalize();

            Vector3 invnormal = new Vector3(ref Normal, -1);    //??
            D = invnormal.Dot(ref Points[0]);
        }


        /// <summary>
        /// Constructs triagnle data from three points (must be in clockwise order!)
        /// </summary>
        /// <param name="p1"></param>
        /// <param name="p2"></param>
        /// <param name="p3"></param>
        public void Construct(Vector3 p1, Vector3 p2, Vector3 p3)
        {
            Construct(ref p1, ref p2, ref p3);
        }
       /// <summary>
       /// Constructs triagnle data from three points (must be in clockwise order!)
       /// </summary>
       /// <param name="p1"></param>
       /// <param name="p2"></param>
       /// <param name="p3"></param>
        public void Construct(ref Vector3 p1, ref Vector3 p2, ref Vector3 p3)
        {
            if (Points == null)
                Points = new Vector3[3];
            if (Length == null)
                Length = new float[3];
            if (Edges == null)
                Edges = new Vector3[3];

            Points[0] = p1;
            Points[1] = p2;
            Points[2] = p3;
            Reconstruct();
        }

        /// <summary>
        /// Calculates squared distance from the the point
        /// </summary>
        /// <param name="center">position</param>
        /// <returns>Squared Distance form the vector</returns>
        public float SquaredDistance(ref Vector3 center)
        {
            float dst1 = center.DistanceSquared(ref Points[0]);
            float dst2 = center.DistanceSquared(ref Points[1]);
            float dst3 = center.DistanceSquared(ref Points[2]);

            if ((Length[0] > dst1) && (Length[0] > dst2))
            {
                if ((Length[1] > dst2) && (Length[1] > dst3))
                {
                    if ((Length[2] > dst1) && (Length[2] > dst3))
                    {
                        return MathTools.Abs(Normal.Dot(ref center) - D);   //distance from the triangle plane
                    }
                }
            }
            if (dst1 < dst2)
                return dst1 < dst3 ? (dst1) : (dst3);
            return dst2 < dst3 ? (dst2) : (dst3);
        }

        /// <summary>
        /// Calculates distance from the the point
        /// </summary>
        /// <param name="vec">position</param>
        /// <returns>Distance form the vector</returns>
        public float Distance(ref Vector3 vec)
        {
            return MathTools.Sqrt(SquaredDistance(ref vec));
        }

        /// <summary>
        /// Calculates closest point on the triangle to point
        /// </summary>
        /// <param name="point">Point in space</param>
        /// <returns>Closest point on triangle</returns>
        public Vector3 ClosestPoint(ref Vector3 point)
        {
            Vector3 vec;
            ClosestPoint(ref point, out vec);
            return vec;
        }

        /// <summary>
        /// Calculates closest point on the triangle to point
        /// </summary>
        /// <param name="p">Point in space</param>
        /// <param name="vec">Closest point on triangle</param>
        public void ClosestPoint(ref Vector3 p, out Vector3 vec)
        {
            //adapted from Realtime collision detection
            //Vertex region outside A

            Vector3 ap = Vector3.Substract(ref p, ref Points[0]);

            float d1 = Edges[0].Dot(ref ap);
            float d2 = Edges[2].Dot(ref ap);

            if ((d1 <=0) && (d2<=0))
            {
                vec = Points[0];
                return;
            }

            //vertex region outside B

            Vector3 bp = Vector3.Substract(ref p, ref Points[1]);
            float d3 = Edges[0].Dot(ref bp);
            float d4 = Edges[2].Dot(ref bp);

            if ((d3 >= 0) && (d4 <= d3))
            {
                vec = Points[1];
                return;
            }

            //edge AB

            float vc = d1 * d4 - d2 * d3;
            if ((vc <= 0) && (d1 >= 0) && (d3 <= 0))
            {
                vec = new Vector3(ref Points[0], d1 / (d1 - d3));
                return;
            }

            //Vertex region outside C

            Vector3 cp = Vector3.Substract(ref p, ref Points[2]);

            float d5 = Edges[0].Dot(ref cp);
            float d6 = Edges[2].Dot(ref cp);

            if ((d6 >= 0) && (d5 <= d6))
            {
                vec = Points[2];
                return;
            }

            //Edge AC

            float vb = d5 * d2 - d1 * d6;
            if ((vb <= 0) && (d2 >= 0) && (d6 <= 0))
            {
                vec = Points[0];
                vec.AddScaled(ref Edges[2], d2 / (d2 - d6));
                return;
            }

            //Edge BC

            float va = d3 * d6 - d5 * d4;
            if ((va <= 0) && ((d4-d3) >= 0) && ((d5-d6) >= 0))
            {
                vec = Points[1];
                vec.AddScaled(ref Edges[1], (d4 - d3) / (d4 - d3 + d5 - d6));
                return;
            }

            //Inside face region

            float denom = 1.0f / (va + vb + vc);
            float v = vb * denom;
            float w = vc * denom;
            vec = Points[0];
            vec.AddScaled(ref Edges[0], v);
            vec.AddScaled(ref Edges[1], w);
        }

        /// <summary>
        /// Calculates ray intersection
        /// </summary>
        /// <param name="origin"></param>
        /// <param name="normal"></param>
        /// <returns></returns>
        public float RayIntersection(ref Vector3 origin, ref Vector3 normal)
        {
            float dst;
            Vector3 intersp;

            if (MathTools.IntersectionRayTriangle(ref origin, ref normal, ref Points[0], ref Points[1], ref Points[2], out dst, out intersp, true))
                return dst;
            return float.PositiveInfinity;
        }

        /// <summary>
        /// Calculates intersection of the sphere and triangle
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius</param>
        ///<param name="penetration">Penetration value</param>
        ///<param name="TriPoint">Point on the triangle</param>
        /// <returns>True if intersection exists</returns>
        public bool IntersectionSphere(ref Vector3 center, float radius, out Vector3 TriPoint, out float penetration)
        {
            TriPoint = ClosestPoint(ref center);
            penetration = 0;

            Vector3 norm = Vector3.Substract(ref center, ref TriPoint);
            float distsq = norm.LengthSquared - radius * radius;
            if (distsq > 0)
                return false;

            penetration = MathTools.Sqrt(-distsq);
            return true;
        }

        /// <summary>
        /// Gets supporting vertex in local coordinates
        /// </summary>
        /// <param name="vertex">Save vertex</param>
        /// <param name="direction">Direction vector</param>
        public void GetLocalSupportingVertex(ref Vector3 direction, out Vector3 vertex)
        {
            float d1 = direction.Dot(ref Points[0]);
            float d2 = direction.Dot(ref Points[1]);
            float d3 = direction.Dot(ref Points[2]);

            if (d1 > d2)
            {
                if (d1 > d3)
                {
                    vertex = Points[0];
                    return;
                }
                vertex = Points[2];
                return;
            }
            if (d2 > d3)
            {
                vertex = Points[1];
                return;
            }
            vertex = Points[2];
        }
    }

    /// <summary>
    /// Triangle convex shape using accelerated collision triangle
    /// </summary>
    public class TriangleShape : IConvexObject
    {
        /// <summary>
        /// Collision triangle
        /// </summary>
        public ColTriangle Triangle;



        /// <summary>
        /// Constructor from finished collision triangle
        /// </summary>
        /// <param name="tri">Collision triangle</param>
        public TriangleShape(ref ColTriangle tri)
        {
            Triangle = tri;
        }

        /// <summary>
        /// Constructor from list of positions
        /// </summary>
        /// <param name="tris">triangle vertices</param>
        public TriangleShape(Vector3[] tris)
        {
            Triangle = new ColTriangle();
            Triangle.Construct(ref tris[0], ref tris[1], ref tris[2]);
        }


        /// <summary>
        /// Constructor from three corners
        /// </summary>
        /// <param name="pos1">Triangle corner</param>
        /// <param name="pos2">Triangle corner</param>
        /// <param name="pos3">Triangle corner</param>
        public TriangleShape(Vector3 pos1, Vector3 pos2, Vector3 pos3) : this(ref pos1, ref pos2, ref pos3)
        {
        }

        /// <summary>
        /// Constructor from three corners
        /// </summary>
        /// <param name="pos1">Triangle corner</param>
        /// <param name="pos2">Triangle corner</param>
        /// <param name="pos3">Triangle corner</param>
        public TriangleShape(ref Vector3 pos1, ref Vector3 pos2, ref Vector3 pos3)
        {
            Triangle.Construct(ref pos1, ref pos2, ref pos3);
        }



        /// <summary>
        /// Gets supporting vertex in local coordinates
        /// </summary>
        /// <param name="vertex">Save vertex</param>
        /// <param name="direction">Direction vector</param>
        public void GetLocalSupportingVertex(ref Vector3 direction, out Vector3 vertex)
        {
            Triangle.GetLocalSupportingVertex(ref direction, out vertex);
        }

        /// <summary>
        /// Returns implicit (math expression) or explicit (vertex based) type of convex object
        /// </summary>
        public ConvexObjectType BaseType { get { return ConvexObjectType.Explicit; } }

        /// <summary>
        /// Returns number of vertices, valid for explicit object types
        /// </summary>
        public int VertexCount { get { return 3; } }

        /// <summary>
        /// Returns vertex with specified index, valid for explicit object types
        /// </summary>
        /// <param name="index">index of vertex</param>
        /// <param name="vertex">Vector to save vertex</param>
        public void GetVertex(int index, out Vector3 vertex)
        {
            vertex = Triangle.Points[index];
        }

        /// <summary>
        /// Returns true if convex object contains position
        /// </summary>
        /// <param name="vec">Position to check</param>
        /// <returns>True if inside</returns>
        public bool ContainsPosition(Vector3 vec)
        {
            return MathTools.PointInTriangle(ref Triangle.Points[0], ref Triangle.Points[1], ref Triangle.Points[2], ref vec);
        }


    }
}
