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

namespace VisiMath.Geometry
{
    /// <summary>
    /// Object oriented box
    /// </summary>
    public class OOBox : BaseGeometry, IConvexObject
    {
        /// <summary>
        /// Center of the box
        /// </summary>
        public Vector3 _Center;
        
        /// <summary>
        /// Rotation of box expressed in three orientation normals
        /// </summary>
        public Vector3[] Axes = new Vector3[3];
        /// <summary>
        /// Halfsizes of the box
        /// </summary>
        public Vector3 HalfSize;

        /// <summary>
        /// Default constructor
        /// </summary>
        public OOBox()
        {
        }

        /// <summary>
        /// XML reader constructor
        /// </summary>
        /// <param name="reader">XML reader</param>
        public OOBox(System.Xml.XmlReader reader)
            : base(reader)
        {
        }


        /// <summary>
        /// Calculates closest point on the box to position
        /// </summary>
        /// <param name="point">Position</param>
        /// <param name="target">Vector to save closest point on the box</param>
        public void ClosestPoint(ref Vector3 point, out Vector3 target)
        {
            Vector3 d = point;
            d.Substract(ref _Center);

            target = _Center;

            //for each axis

            for (int i = 0; i < 3; i++)
            {
                //project D on axis
                float dist = d.Dot(ref Axes[i]);
                
                //if farther then box extends, clamp
                MathTools.Clamp(ref dist, HalfSize[i], -HalfSize[i]);
                target.AddScaled(ref Axes[i], dist);
            }            
        }

        /// <summary>
        /// Calculates closest point on the box to position
        /// </summary>
        /// <param name="point">Position</param>
        /// <returns>Vector to save closest point on the box</returns>
        public Vector3 ClosestPoint(ref Vector3 point)
        {
            Vector3 target;
            ClosestPoint(ref point, out target);
            return target;
        }

        /// <summary>
        /// Calculates closest point on the box to line
        /// </summary>
        /// <param name="l1">Line start</param>
        /// <param name="l2">Line end</param>
        /// <returns>Closest point on the box</returns>
        public Vector3 ClosestPointLine(ref Vector3 l1, ref Vector3 l2)
        {            
            Vector3 closest;
            Vector3 closest2;
            CollisionSolver.ClosestPoint.LineSegmentOOBox(ref l1, ref l2, this, out closest, out closest2);
            return closest;
        }

        /// <summary>
        /// Calculates squared distance between vector and object
        /// </summary>
        /// <param name="point">Positional vector</param>
        /// <returns>Distance</returns>
        public float SquaredDistance(ref Vector3 point)
        {
            Vector3 v = point;
            v.Substract(ref _Center);
            float rval = 0;
            for (int i = 0; i < 3; i++)
            {
                float excess = 0.0f;
                float d = v.Dot(ref Axes[i]);
                if (d < -HalfSize[i])
                    excess = d + HalfSize[i];
                else if (d > HalfSize[i])
                    excess = d - HalfSize[i];
                rval += excess * excess;
            }
            return rval;
        }

        /// <summary>
        /// Calculates distance between vector and object
        /// </summary>
        /// <param name="point">Positional vector</param>
        /// <returns>Distance</returns>
        public override float Distance(ref Vector3 point)
        {
            return MathTools.Sqrt(SquaredDistance(ref point));
        }

        /// <summary>
        /// Quick intersection test of two object oriented boxes
        /// Uses separating axes theorem
        /// </summary>
        /// <param name="another"></param>
        /// <returns></returns>
        public bool Intersects(OOBox another)
        {
            Matrix3 R = new Matrix3(), AbsR = new Matrix3();

            //compute rotational matrix to bring another box to my coordinates
            //and subexpressions (with epsilon for almost paralel edges with zero crossproduct
            //based on realtime col detection, page 103


            for (int i =0 ; i < 3; i++)
                for (int j = 0; j < 3; j++)
                {
                    float rv = Axes[i].Dot(ref another.Axes[j]);
                    R[i, j] = rv;
                    AbsR[i, j] = MathTools.Abs(rv) + MathTools.Epsilon;
                }

            Vector3 transvector = another._Center - _Center;

            float ra, rb;

            //note: optimize, use direct call for fixed matrix elements

            //test axes a0, a1, a2

            for (int i = 0; i < 3; i++)
            {
                ra = HalfSize[i];
                rb = another.HalfSize[0] * AbsR[1, 0] + another.HalfSize[1] * AbsR[1, 1] + another.HalfSize[2] * AbsR[1, 2];
                if (MathTools.Abs(transvector[i]) > ra + rb)
                    return false;
            }

            //test axes b0, b1, b2

            for (int i = 0; i < 3; i++)
            {
                ra = HalfSize[0] * AbsR[0, 1] + HalfSize[1] * AbsR[1, 1] + HalfSize[2] * AbsR[2, 1];
                rb = another.HalfSize[1];
                if (MathTools.Abs(transvector[0] * R[0,1] + transvector[1] * R[1,1] + transvector[2] * R[2,1])      
                     > ra + rb)
                    return false;
            }

            //test a0xb0

            ra = HalfSize[1] * AbsR[2, 0] + HalfSize[2] * AbsR[1, 0];
            rb = another.HalfSize[1] * AbsR[0, 2] + another.HalfSize[2] * AbsR[0, 1];

            if (MathTools.Abs(transvector[2] * R[1, 0] - transvector[1] * R[2, 0])
                 > ra + rb)
                return false;

            //test a0xb1

            ra = HalfSize[1] * AbsR[2, 1] + HalfSize[2] * AbsR[1, 1];
            rb = another.HalfSize[0] * AbsR[0, 2] + another.HalfSize[2] * AbsR[0, 0];

            if (MathTools.Abs(transvector[2] * R[1, 1] - transvector[1] * R[2, 1])
                 > ra + rb)
                return false;

            //test a0xb2

            ra = HalfSize[1] * AbsR[2, 2] + HalfSize[2] * AbsR[1, 2];
            rb = another.HalfSize[0] * AbsR[0, 1] + another.HalfSize[1] * AbsR[0, 0];

            if (MathTools.Abs(transvector[2] * R[1, 2] - transvector[1] * R[2, 2])
                 > ra + rb)
                return false;

            //test a1xb0

            ra = HalfSize[0] * AbsR[2, 0] + HalfSize[2] * AbsR[0, 0];
            rb = another.HalfSize[1] * AbsR[1, 2] + another.HalfSize[2] * AbsR[1, 1];

            if (MathTools.Abs(transvector[0] * R[2, 0] - transvector[2] * R[0, 0])
                 > ra + rb)
                return false;

            //test a1xb1

            ra = HalfSize[0] * AbsR[2, 1] + HalfSize[2] * AbsR[0, 1];
            rb = another.HalfSize[0] * AbsR[1, 2] + another.HalfSize[2] * AbsR[1, 0];

            if (MathTools.Abs(transvector[0] * R[2, 1] - transvector[2] * R[0, 1])
                 > ra + rb)
                return false;

            //test a1xb2

            ra = HalfSize[0] * AbsR[2, 2] + HalfSize[2] * AbsR[0, 2];
            rb = another.HalfSize[0] * AbsR[1, 1] + another.HalfSize[1] * AbsR[1, 0];

            if (MathTools.Abs(transvector[0] * R[2, 2] - transvector[2] * R[0, 2])
                 > ra + rb)
                return false;

            //test a2xb0

            ra = HalfSize[0] * AbsR[1, 0] + HalfSize[1] * AbsR[0, 0];
            rb = another.HalfSize[1] * AbsR[2, 2] + another.HalfSize[2] * AbsR[2, 1];

            if (MathTools.Abs(transvector[1] * R[0, 0] - transvector[0] * R[1, 0])
                 > ra + rb)
                return false;

            //test a2xb1

            ra = HalfSize[0] * AbsR[1, 1] + HalfSize[1] * AbsR[0, 1];
            rb = another.HalfSize[0] * AbsR[2, 2] + another.HalfSize[2] * AbsR[2, 0];

            if (MathTools.Abs(transvector[1] * R[0, 1] - transvector[0] * R[1, 1])
                 > ra + rb)
                return false;

            //test a2xb2

            ra = HalfSize[0] * AbsR[1, 2] + HalfSize[1] * AbsR[0, 2];
            rb = another.HalfSize[0] * AbsR[2, 1] + another.HalfSize[1] * AbsR[2, 0];

            if (MathTools.Abs(transvector[1] * R[0, 2] + transvector[0] * R[1, 2])
                 > ra + rb)
                return false;

            //axes separated, there's collision
            return true;
        }


        /// <summary>
        /// OOBox is not renderable primitive
        /// </summary>
        public override bool IsPrimitive
        {
            get { return false; }
        }

        /// <summary>
        /// Center of the box
        /// </summary>
        public override Vector3 Center
        {
            get { return _Center; }
        }

        /// <summary>
        /// No, it's not container
        /// </summary>
        public override bool IsContainer
        {
            get { return false; }
        }

        /// <summary>
        /// Checks if specified position is inside the object
        /// </summary>
        /// <param name="pos">Position to check</param>
        /// <returns>True if vector is inside the object</returns>
        public override bool ContainsPosition(Vector3 pos)
        {
            return (Distance(ref pos) <= 0);
        }

        /// <summary>
        /// Check for intersection and fills intersection info if intersection is closer to the origin of the ray then value before
        /// </summary>
        /// <param name="inters">Data structure with intersection info</param>
        /// <param name="obj">Ray object</param>
        /// <returns>True if there was an intersection and it was closer then previous stored</returns>
        public override bool GetIntersectionInfo(Ray obj, ref IIntersectionInfo inters)
        {
            float linedistance = CollisionSolver.LineTest.LineOOBox(ref obj.Origin, ref obj.Direction, this);
            if (float.IsNaN(linedistance) || linedistance < 0)
                return false;

            inters.Distance = linedistance;
            inters.GeoObject = this;
            inters.IntersectionPoint = new Vector3(obj.Origin, linedistance);
            return true;
        }

        /// <summary>
        /// Checks for intersection between object and moving sphere
        /// </summary>
        ///<param name="radius">Radius of the sphere</param>
        /// <param name="maxdistance">Maximal distance to travel</param>
        /// <param name="ray">Ray describing movement of the sphere</param>
        /// <returns>Distance to the intersection or -1 if no intersection exists</returns>
        public override float IntersectsMoving(float radius, Ray ray, float maxdistance)
        {
            float t;
            Vector3 closest;
            CollisionSolver.ClosestPoint.LineOOBox(ref ray.Origin, ref ray.Direction, this, out closest, out t);

            MathTools.Clamp(ref t, 0, maxdistance);
            Vector3 closest2 = new Vector3(ref ray.Origin, t);

            if (closest.DistanceSquared(ref closest2) - radius * radius <= 0)
                return t;
            return -1;
        }

        /// <summary>
        /// Returns closest distance to the ray
        /// </summary>
        /// <param name="ray">Ray to check distance to</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public override float Distance(Ray ray, bool frontOnly = false)
        {
            float t;
            Vector3 myclosest;
            CollisionSolver.ClosestPoint.LineOOBox(ref ray.Origin, ref ray.Direction, this, out myclosest, out t);

            if (t < 0)
                return float.NaN;
            Vector3 closest = new Vector3(ray.Origin, t);
            return Distance(ref closest);
        }

        /// <summary>
        /// Returns closest distance to the line segment.
        /// </summary>
        ///<param name="pos1">Start of the line</param>
        ///<param name="pos2">end of the line</param>
        /// <returns>Closest distance</returns>
        public override float DistanceLineSegment(ref Vector3 pos1, ref Vector3 pos2)
        {
            return CollisionSolver.Distances.OOBoxLineSegment(this, ref pos1, ref pos2);
        }

        /// <summary>
        /// Returns closest distance to the triangle.
        /// </summary>
        /// <param name="pos1">First corner</param>
        /// <param name="pos2">Second corner</param>
        /// <param name="pos3">Third corner</param>
        /// <returns>Distance</returns>
        public override float DistanceTriangle(ref Vector3 pos1, ref Vector3 pos2, ref Vector3 pos3)
        {
            return CollisionSolver.Distances.OOBoxTriangle(this, 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)
        {
            //stupid and bruteforce, OPTIMIZE THIS!!!!

            Vector3[] corners = GetCorners();

            float max = float.NegativeInfinity;
            vertex = new Vector3();

            for (int i = 0; i < corners.Length; i++)
            {
                float dot = corners[i].Dot(ref direction);
                if (dot > max)
                {
                    max = dot;
                    vertex = corners[i];
                }
            }
        }

        /// <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 8; } }

        /// <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)
        {
            //bruteforce, optimize!
            var corners = GetCorners();
            vertex = corners[index];
        }



        #region conversions


        /// <summary>
        /// Preferred primitive type for rendering
        /// </summary>
        public override PrimitiveTypes PreferredPrimitive
        {
            get { return PrimitiveTypes.TriangleList; }
        }

        /// <summary>
        /// Preferred vertex type for default conversion
        /// </summary>
        public override Type PreferredVertexType
        {
            get { return typeof(PositionVertex); }
        }

        /// <summary>
        /// Returns vertices of the object
        /// </summary>
        /// <returns>Corners of the box</returns>
        public Vector3[] GetCorners()
        {
            Vector3[] rval = new Vector3[8];

            for (int i = 0; i < 8; i++)
            {
                Vector3 corn = _Center;

                if (i % 2 == 0)
                    corn.AddScaled(ref Axes[0], - HalfSize.X);
                else
                    corn.AddScaled(ref Axes[0], HalfSize.X);
                if (i % 4 == 0)
                    corn.AddScaled(ref Axes[1], -HalfSize.Y);
                else
                    corn.AddScaled(ref Axes[1], HalfSize.Y);
                if (i % 8 == 0)
                    corn.AddScaled(ref Axes[2], -HalfSize.Z);
                else
                    corn.AddScaled(ref Axes[2], HalfSize.Z);
            }

            return rval;
        }

        public Vector3[] GetQuads()
        {
            Vector3[] corners = GetCorners();
            Vector3[] rval = new Vector3[24];

            rval[0] = new Vector3(corners[3]);
            rval[1] = new Vector3(corners[2]);
            rval[2] = new Vector3(corners[1]);
            rval[3] = new Vector3(corners[0]);

            rval[4] = new Vector3(corners[4]);
            rval[5] = new Vector3(corners[5]);
            rval[6] = new Vector3(corners[6]);
            rval[7] = new Vector3(corners[7]);

            rval[8] = new Vector3(corners[7]);
            rval[9] = new Vector3(corners[3]);
            rval[10] = new Vector3(corners[0]);
            rval[11] = new Vector3(corners[4]);

            rval[12] = new Vector3(corners[2]);
            rval[13] = new Vector3(corners[6]);
            rval[14] = new Vector3(corners[5]);
            rval[15] = new Vector3(corners[1]);

            rval[16] = new Vector3(corners[0]);
            rval[17] = new Vector3(corners[1]);
            rval[18] = new Vector3(corners[5]);
            rval[19] = new Vector3(corners[4]);

            rval[20] = new Vector3(corners[7]);
            rval[21] = new Vector3(corners[6]);
            rval[22] = new Vector3(corners[2]);
            rval[23] = new Vector3(corners[3]);

            return rval;
        }

        /// <summary>
        /// Convert object to list of points
        /// </summary>
        /// <returns>New geometry object containing point based definition of the object</returns>
        /// <param name="quality">
        /// Unused
        /// </param>
        /// <param name="vertexlimit">Unused.</param>
        /// <param name="vertextype">Required type of the vertex</param>
        public override Point3DList ToPoints(Type vertextype, float quality = 0, int vertexlimit = 0)
        {
            IVertexArray varray = VertexFactory.CreateArray(vertextype, 8);

            Vector3[] corners = GetCorners();

            for (int i =0 ;i < corners.Length; i++)
            {
                IVertex v = varray[i];
                v.Position = corners[i];
                varray[i] = v;
            }

            return new Point3DList(varray);
        }

        public QuadList ToQuadList(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            Vector3[] quads = GetQuads();

            IVertexArray varray = VertexFactory.CreateArray(VertexType, 24);


            bool normals = false;
            if (VertexFactory.HasNormal(VertexType))
                normals = true;
            bool txcoords = VertexFactory.HasTexCoord(VertexType);

            for (int i = 0; i < 24; i++)
            {
                IVertex ver = VertexFactory.CreateNewVertex(VertexType);
                if (normals)
                {
                    Vector3 norm;
                    if (i < 4)
                        norm = new Vector3(0, 0, -1);
                    else if (i < 8)
                        norm = new Vector3(0, 0, 1);
                    else if (i < 12)
                        norm = new Vector3(-1, 0, 0);
                    else if (i < 16)
                        norm = new Vector3(1, 0, 0);
                    else if (i < 20)
                        norm = new Vector3(0, -1, 0);
                    else
                        norm = new Vector3(0, 1, 0);
                    ver.Normal = norm;
                }

                if (txcoords)
                {
                    int j = i % 4;

                    switch (j)
                    {
                        case 0:
                            ver.TexCoord2 = new Vector2(0, 0);
                            break;
                        case 1:
                            ver.TexCoord2 = new Vector2(1, 0);
                            break;
                        case 2:
                            ver.TexCoord2 = new Vector2(1, 1);
                            break;
                        case 3:
                            ver.TexCoord2 = new Vector2(0, 1);
                            break;
                    }
                }

                ver.Position = quads[i];
                varray[i] = ver;

            }
            return new QuadList(varray);
        }

        /// <summary>
        /// Convert object to list of lines
        /// </summary>
        /// <returns>New geometry object containing edges of the object</returns>
        /// <param name="quality">
        /// unused
        /// </param>
        /// <param name="vertexlimit">unused.</param>
        /// <param name="vertextype">Required type of the vertex</param>
        public override LineList ToLines(Type vertextype, float quality = 0, int vertexlimit = 0)
        {
            return ToQuadList(vertextype).ToLines(); // optimize!!!
        }

        /// <summary>
        /// Convert object to list of triangles. Valid only for surface based objects, otherwise returns null.
        /// </summary>
        /// <returns>New geometry object containing triangle faces of the object surface.</returns>
        /// <param name="quality">
        /// unused
        /// </param>
        /// <param name="vertexlimit">unused.</param>
        /// <param name="vertextype">Required type of the vertex</param>
        public override TriangleList ToTriangles(Type vertextype, float quality = 0, int vertexlimit = 0)
        {
            return ToQuadList(vertextype).ToTriangles();// optimize!!!
        }

        /// <summary>
        /// Convert object to indexed triangle mesh. Valid only for surface based objects, otherwise returns null.
        /// </summary>
        /// <returns>New geometry object containing triangle faces of the object surface</returns>
        /// <param name="quality">
        /// unused
        /// </param>
        /// <param name="vertexlimit">unused.</param>
        /// <param name="vertextype">Required type of the vertex</param>
        public override IndexedMesh ToIndexedMesh(Type vertextype, float quality = 0, int vertexlimit = 0)
        {
            return ToQuadList(vertextype).ToIndexedMesh();// optimize!!!
        }

        #endregion

        /// <summary>
        /// Deserializes object from XML
        /// </summary>
        /// <param name="reader">XML reader</param>
        public override void ReadXml(System.Xml.XmlReader reader)
        {
            if (reader["type"] != this.GetType().Name)
                throw new ArgumentException("IGeometry:ReadXML - wrong xml node type!");

            while (reader.Read())
            {
                if (reader.IsStartElement())
                {
                    if (reader.Name == "Center")
                    {
                        _Center.ReadXml(reader);
                    }
                    else if (reader.Name == "HalfSize")
                    {
                        HalfSize.ReadXml(reader);
                    }
                    else if (reader.Name == "Axis0")
                    {
                        Axes[0].ReadXml(reader);
                    }
                    else if (reader.Name == "Axis1")
                    {
                        Axes[0].ReadXml(reader);
                    }
                    else if (reader.Name == "Axis2")
                    {
                        Axes[0].ReadXml(reader);
                    }
                }
                else break;
            }
        }

        /// <summary>
        /// Serializes object to XML
        /// </summary>
        /// <param name="writer">XML writer to save values</param>
        public override void WriteXml(System.Xml.XmlWriter writer)
        {
            writer.WriteAttributeString("type", this.GetType().Name);
            writer.WriteStartElement("Center");
            _Center.WriteXml(writer);
            writer.WriteEndElement();
            writer.WriteStartElement("HalfSize");
            HalfSize.WriteXml(writer);
            writer.WriteEndElement();
            for (int i = 0; i < 3; i++)
            {
                writer.WriteStartElement("Axis" + i.ToString());
                Axes[i].WriteXml(writer);
                writer.WriteEndElement();
            }
        }

        /// <summary>
        /// Returns copy of the geometric data
        /// </summary>
        /// <returns>Full copy of the geometry</returns>
        public override IGeometry GetCopy()
        {
            OOBox rval = new OOBox();
            rval._Center = this._Center;
            rval.Axes = new Vector3[3];
            for (int i = 0; i < Axes.Length; i++)
                rval.Axes[i] = Axes[i];
            rval.HalfSize = this.HalfSize;
            return rval;
        }

    }
}
