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

namespace VisiMath.Geometry
{
    /// <summary>
    /// Convex hull - indexed mesh containing convex polyhedron
    /// </summary>
    public class ConvexHull : IndexedMesh, IVolumetric, IConvexObject, ISpatialIndex
    {
        protected AABox BoundBox;
        protected Sphere BoundSphere;

        protected bool centervalid = false;
        protected bool boxvalid = false;
        protected bool spherevalid = false;

        protected Vector3 _center;


        /// <summary>
        /// Returns copy of the geometric data
        /// </summary>
        /// <returns>Full copy of the geometry</returns>
        public override IGeometry GetCopy()
        {
            return new ConvexHull(this);
        }

        /// <summary>
        /// Copy constructor
        /// </summary>
        /// <param name="source">Source hull</param>
        public ConvexHull(ConvexHull source) : base(source)
        {
            //notimplemented - copy recalc data
        }

        /// <summary>
        /// Constructs hull from indexed mesh
        /// </summary>
        /// <param name="mesh">Mesh, should contain convex object</param>
        public ConvexHull(IndexedMesh mesh)
            : base(mesh.Vertices, mesh.Indices)
        {
            Recalc();
        }

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

        protected void Recalc()
        {
            //notimplemented - calculate face and edge datas for colchecks and hill climbing
        }

        public override Vector3 Center
        {
            get
            {
                if (!centervalid)
                    CalcCenter();
                return _center;
            }
        }

        protected void CalcCenter()
        {
            _center = new Vector3();
            for (int i = 0; i < Vertices.Count; i++)
            {
                _center.Add(Vertices[i].Position);
            }
            _center.Divide(Vertices.Count);
            centervalid = true;
            //note, uses simple averaging, this center is not the center of the weight probably - need to check
        }

        protected void CalcBoundBox()
        {
            Vector3 min = new Vector3(float.MaxValue);
            Vector3 max = new Vector3(float.MaxValue);

            for (int i = 0; i < Vertices.Count; i++)
            {
                Vector3 tmp = Vertices[i].Position;
                min = MathTools.Min(ref min, ref tmp);
                max = MathTools.Max(ref max, ref tmp);
            }

            if (BoundBox == null)
                BoundBox = new AABox(ref min, ref max);
            else
            {
                BoundBox.Minimum = min;
                BoundBox.Maximum = max;
            }
            boxvalid = true;
        }

        protected void CalcBoundSphere()
        {
            Vector3 cen = Center;
            float distsq = 0;

            for (int i = 0; i < Vertices.Count; i++)
            {
                float newdst = Vertices[i].Position.DistanceSquared(ref cen);
                distsq = MathTools.Max(distsq, newdst);
            }

            if (BoundSphere == null)
                BoundSphere = new Sphere(cen, MathTools.Sqrt(distsq));
            else
            {
                BoundSphere._Center = cen;
                BoundSphere.Radius = MathTools.Sqrt(distsq);
            }

            spherevalid = true;
        }


        /// <summary>
        /// Vertex count
        /// </summary>
        public int VertexCount { get { return Vertices.Count; } }

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


        /// <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 = Vertices[index].Position;
        }



        #region IConvexObject Members


        /// <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 max = float.NegativeInfinity;
            vertex = new Vector3();

            //note bruteforce implementation, use hill climbing for fully formed convex hulls

            for (int i = 0; i < Vertices.Count; i++)
            {
                Vector3 vec = Vertices[i].Position;
                float dot = vec.Dot(ref direction);
                if (dot > max)
                {
                    max = dot;
                    vertex = vec;
                }
            }
        }


        #endregion

        #region IVolumetric Members

        public float Volume
        {
            //get { throw new NotImplementedException(); } //Volume of convex hull... damn...
            get { return 1; }
        }

        #endregion



        #region SpatialIndex
        /// <summary>
        /// Returns spatial index as box
        /// </summary>
        public AABox AsBox
        {
            get
            {
                if (!boxvalid)
                    CalcBoundBox();
                return BoundBox;
            }
        }
        /// <summary>
        /// Returns spatial index as sphere
        /// </summary>
        public Sphere AsSphere
        {
            get
            {
                if (!spherevalid)
                    CalcBoundSphere();
                return BoundSphere;
            }
        }
        /// <summary>
        /// returns spatial index as point (center)
        /// </summary>
        public Vector3 AsPoint { get { return Center; } }

        /// <summary>
        /// return true if index contains (intersects) another index
        /// </summary>
        /// <param name="index">Second spatial index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(ref ISpatialIndex index)
        {
            AABox box = index as AABox;
            if (box != null)
                return Contains(box);
            Sphere sph = index as Sphere;
            if (sph != null)
                return Contains(sph);
            Point3D point = index as Point3D;
            if (point != null)
                return Contains(point);
            LineSegment3D line = index as LineSegment3D;
            if (line != null)
                return Contains(line);

            return Contains(index as IConvexObject);
        }

        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Vector3 index)
        {
            return ContainsPosition(index);
        }
        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(ref Vector3 index)
        {
            return ContainsPosition(index);
        }
        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Point3D index)
        {
            return ContainsPosition(index.GetPosition(0));
        }

        /// <summary>
        /// Return true if index contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if intersection exists</returns>
        public bool ContainsLine(Vector3 line1, Vector3 line2)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, new LineSegment3D(ref line1, ref line2));
        }
        /// <summary>
        /// Return true if index contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if intersection exists</returns>
        public bool ContainsLine(ref Vector3 line1, ref Vector3 line2)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, new LineSegment3D(ref line1, ref line2));
        }
        /// <summary>
        /// Return true if index contains line
        /// </summary>
        /// <param name="index">line index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(LineSegment3D line)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, line);
        }

        /// <summary>
        /// Return true if index contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Vector3 center, float radius)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, new Sphere(center, radius));
        }
        /// <summary>
        /// Return true if index contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(ref Vector3 center, float radius)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, new Sphere(center, radius));
        }
        /// <summary>
        /// Return true if index contains sphere
        /// </summary>
        /// <param name="sphere">sphere index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Sphere sphere)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, sphere);
        }

        /// <summary>
        /// Return true if index contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if intersection exists</returns>
        public bool ContainsBox(Vector3 min, Vector3 max)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, new AABox(ref min, ref max));
        }
        /// <summary>
        /// Return true if index contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if intersection exists</returns>
        public bool ContainsBox(ref Vector3 min, ref Vector3 max)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, new AABox(ref min, ref max));
        }
        /// <summary>
        /// Return true if index contains axis alligned box
        /// </summary>
        /// <param name="box">AABox</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(AABox box)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, box);
        }

        /// <summary>
        /// Return true if index contains generic convex object
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(IConvexObject convex)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, convex);
        }


        /// <summary>
        /// return true if index fully contains another index
        /// </summary>
        /// <param name="index">Second spatial index</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(ref ISpatialIndex index)
        {
            AABox box = index as AABox;
            if (box != null)
                return ContainsFully(box);
            Sphere sph = index as Sphere;
            if (sph != null)
                return ContainsFully(sph);
            Point3D point = index as Point3D;
            if (point != null)
                return Contains(point);
            LineSegment3D line = index as LineSegment3D;
            if (line != null)
                return ContainsFully(line);

            return ContainsFully(index as IConvexObject);
        }

        /// <summary>
        /// Return true if index fully contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFullyLine(Vector3 line1, Vector3 line2)
        {
            return (ContainsPosition(line1) && ContainsPosition(line2));
        }
        /// <summary>
        /// Return true if index fully contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFullyLine(ref Vector3 line1, ref Vector3 line2)
        {
            return (ContainsPosition(line1) && ContainsPosition(line2));
        }
        /// <summary>
        /// Return true if index fully contains line
        /// </summary>
        /// <param name="line">Line segment</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(LineSegment3D line)
        {
            return (ContainsPosition(line.GetPosition(0)) && ContainsPosition(line.GetPosition(1)));
        }

        /// <summary>
        /// Return true if index fully contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(Vector3 center, float radius)
        {
            return ContainsFully(ref center, radius);
        }
        /// <summary>
        /// Return true if index fully contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(ref Vector3 center, float radius)
        {
            for (int i = 0; i < facecount; i++)
            {
                if (Planes[i].XYZ.Dot(ref center) - Planes[i].W > -radius)
                    return false;
            }
            return true;
        }
        /// <summary>
        /// Return true if index fully contains sphere
        /// </summary>
        /// <param name="sphere">sphere index</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(Sphere sphere)
        {
            return ContainsFully(ref sphere._Center, sphere.Radius);
        }

        /// <summary>
        /// Return true if index fully contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFullyBox(Vector3 min, Vector3 max)
        {
            return (ContainsPosition(min) && ContainsPosition(max));
        }
        /// <summary>
        /// Return true if index fully contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFullyBox(ref Vector3 min, ref Vector3 max)
        {
            return (ContainsPosition(min) && ContainsPosition(max));
        }
        /// <summary>
        /// Return true if index fully contains axis alligned box
        /// </summary>
        /// <param name="box">AABox</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(AABox box)
        {
            return (ContainsPosition(box.Minimum) && ContainsPosition(box.Maximum));
        }

        /// <summary>
        /// Return true if index FULLY contains generic convex object
        /// CUrrently works only for explicit convex objects!
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if object is fully contained in index</returns>
        public bool ContainsFully(IConvexObject convex)
        {
            int vcount = convex.VertexCount;
            for (int i = 0; i < vcount; i++)
            {
                Vector3 vec;
                convex.GetVertex(i, out vec);
                if (!ContainsPosition(vec))
                    return false;
            }
            return true;
        }




        #endregion


        protected int edgecount;
        protected int facecount;

        protected bool fullyformed = false;

        /// <summary>
        /// Edge count
        /// </summary>
        public int EdgeCount { get { return edgecount; } }
        /// <summary>
        /// Face count
        /// </summary>
        public int FaceCount { get { return facecount; } }

        protected Vector4[] Planes;
        protected int[,] Edges;
        protected int[][] Faces;


        /// <summary>
        /// Get edge vector with specified index
        /// </summary>
        /// <param name="index">Edge index</param>
        /// <param name="edge">Vector to store edge</param>
        public void GetEdge(int index, out Vector3 edge1, out Vector3 edge2)
        {
            edge1 = Vertices[Edges[index, 0]].Position;
            edge2 = Vertices[Edges[index, 1]].Position;
        }

        /// <summary>
        /// Returns normal of face
        /// </summary>
        /// <param name="index">index of face</param>
        /// <param name="norm">Vector to store normal</param>
        public void GetNormal(int index, out Vector3 norm)
        {
            norm = Planes[index].XYZ;
        }

        /// <summary>
        /// Returns normal of face
        /// </summary>
        /// <param name="index">index of face</param>
        /// <param name="norm">Vector to store normal</param>
        public void GetFacePlane(int index, out Vector3 norm, out float d)
        {
            norm = Planes[index].XYZ;
            d = Planes[index].W;
        }

        //unfinished
        /*
 
                 /// <summary>
                /// Quick test if convexhull intersects other convexhull
                /// </summary>
                /// <param name="other">Second convex hull</param>
                /// <returns>True if intersection existst</returns>
                public bool Intersects(ConvexHull other)
                {
                    //naive implementation


                    //transform both to same space

                    //test each vertex of q against all faces of p
            
                    //test each vertex of p against all faces of q

                    //intersect each edge of p against q

                    //intersect each edge of q against p
                }
 
         * 
         */


    }    
}
