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

namespace VisiMath.Geometry
{
          
    public interface GeoOctTree
    {
        GeoOctTreeNode TopNode { get; set; }
    }



    public class GeoOctTreeNode
    {
        /// <summary>
        /// Quad tree subnodes
        /// </summary>
        GeoOctTreeNode[] Nodes;

        GeoOctTreeNode Parent;
        GeoOctTree Master;

        /// <summary>
        /// GeoObjects directly in this node
        /// </summary>
        List<IGeometry> GeoObjects;

        List<VisiMath.Geometry.ColTriangle> Triangles;

        Vector3 Corner;
        float Size;

        bool Empty = true;

        public GeoOctTreeNode(GeoOctTree master, GeoOctTreeNode parent, Vector3 corner, float size)
        {
            Master = master;
            Parent = parent;
            Nodes = new GeoOctTreeNode[8];
            GeoObjects = new List<IGeometry>();
            Triangles = new List<ColTriangle>();
            Corner = corner;
            Size = size;
        }

        /// <summary>
        /// Add all triangles in list to hierarchy (note, calls conversion routine)
        /// </summary>
        /// <param name="mesh">Indexed mesh</param>
        /// <param name="trans">Optional transformation for list</param>
        public void Add(IndexedMesh mesh, Transformation trans = null)
        {
            TriangleList tl = mesh.ToTriangles();
            Add(tl, trans);
        }

        /// <summary>
        /// Add all triangles in list to hierarchy
        /// </summary>
        /// <param name="tlist">Triangle list</param>
        /// <param name="trans">Optional transformation for list</param>
        public void Add(TriangleList tlist, Transformation trans = null)
        {
            Vector3[] vertices = tlist.GetAllPositions();
            if (trans != null)
                trans.TransMatrix.TransformFast(vertices);
            Add(vertices);
        }
        
        /// <summary>
        /// Adds triangles to hierarchy
        /// </summary>
        /// <param name="vertices"></param>
        public void Add(Vector3[] vertices)
        {
            //calc normals
            int total = vertices.Length / 3;
            for (int i = 0; i < total; i++)
            {
                ColTriangle ct = new ColTriangle();
                ct.Construct(ref vertices[i * 3 + 0], ref vertices[i * 3 + 1], ref vertices[i * 3 + 2]);
                AddTriangle(ref ct);
            }
        }


        /// <summary>
        /// Adds one triangle to hierarchy
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <param name="v3"></param>
        /// <param name="normal"></param>
        public void AddTriangle(ref ColTriangle ct)
        {
            Vector3 min = Vector3.Minimum(ref ct.Points[0], ref ct.Points[1], ref ct.Points[2]);
            Vector3 max = Vector3.Maximum(ref ct.Points[0], ref ct.Points[1], ref ct.Points[2]);

            GeoOctTreeNode node = GetNode(ref min, ref max);
            node.ForceAddTriangle(ref ct);
        }

        /// <summary>
        /// Adds one triangle directly to this node
        /// </summary>
        /// <param name="v1"></param>
        /// <param name="v2"></param>
        /// <param name="v3"></param>
        /// <param name="normal"></param>
        public void ForceAddTriangle(ref ColTriangle tri)
        {
            Triangles.Add(tri);
            Empty = false;
        }

        /// <summary>
        /// Checks if AAbox is fully contained in the node
        /// </summary>
        /// <param name="min">Minimal corner</param>
        /// <param name="max">Maximal corner</param>
        /// <returns>True if contained</returns>
        public bool ContainsFully(ref Vector3 min, ref Vector3 max)
        {
            return MathTools.ContainsFully(ref Corner, Size, ref min, ref max);
        }

        /// <summary>
        /// Checks if node contains sphere
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">radius of the sphere</param>
        /// <returns>true if contains sphere, false if it's out of the bounds</returns>
        public bool Contains(ref Vector3 center, float radius)
        {
            return MathTools.Intersects(ref center, radius, ref Corner, Size);
        }

        /// <summary>
        /// Adds collision of it's own geometry
        /// </summary>
        /// <param name="sph">sphere center</param>
        /// <param name="list">list with collisions</param>
        /// <param name="index">write index</param>
        /// <param name="limit">limit of collisions to write</param>
        /// <returns>number of generated contacts</returns>
        public int AddMyContactCollision(ref Vector3 center, float radius, List<CollisionInfo> list, int index, int limit = 0, bool dontcheck = false)
        {
            if (!dontcheck && (!Contains(ref center, radius)))
                return 0;
            
            if (limit == 0)
                limit = int.MaxValue;


            int rval = 0;

            for (int i = 0; i < Triangles.Count; i++)
            {
                if (rval > limit)
                    return rval;
                if (index == list.Count)
                    return rval;

                Vector3 vec;
                float penetr;

                if (Triangles[i].IntersectionSphere(ref center, radius, out vec, out penetr))
                {
                    CollisionInfo cinfo = list[index];
                    cinfo.ContactNormal = Triangles[i].Normal;
                    cinfo.ContactPoint1 = vec;
                    cinfo.Penetration = penetr;
                    cinfo.Index = i;
                    cinfo.UserData = Triangles[i].UserData;
                    rval++;
                    index++;
                }
            }
            return rval;
        }

        /// <summary>
        /// Adds all collision with sphere with the node and it's subnodes
        /// </summary>
        /// <param name="sph">sphere center</param>
        /// <param name="list">list with collisions</param>
        /// <param name="index">write index</param>
        /// <param name="limit">limit of collisions to write</param>
        /// <returns>number of generated contacts</returns>
        public int AddContactCollisionDownWard(ref Vector3 center, float radius, List<CollisionInfo> list, int index, int limit = 0)
        {
            if (!Contains(ref center, radius))
                return 0;

            int rval = 0;
            
            if (!Empty)
                rval = AddMyContactCollision(ref center, radius, list, index, limit);

            if (rval > limit)
                return rval;

            index += rval;
            limit -= rval;

            //next my subnodes

            for (int i = 0; i < 8; i++)
            {
                var node = Nodes[i];
                if (node != null)
                {
                    int rv = node.AddContactCollisionDownWard(ref center, radius, list, index, limit);
                    rval += rv;
                    index += rv;
                    limit -= rv;
                    if (rval > limit)
                        return rval;
                }
            }
            return rval;
        }

        /// <summary>
        /// Adds all collision with sphere with the tree
        /// </summary>
        /// <param name="sph">sphere center</param>
        /// <param name="list">list with collisions</param>
        /// <param name="index">write index</param>
        /// <param name="limit">limit of collisions to write</param>
        /// <returns>number of generated contacts</returns>
        public int AddContactCollision(ref Vector3 center, float radius, List<CollisionInfo> list, int index, int limit = 0)
        {           
            //first add all contacts with my own geometry and my subnodes
            if (limit == 0)
                limit = int.MaxValue;

            int rval = AddContactCollisionDownWard(ref center, radius, list, index, limit);
            if (rval > limit)
                return rval;

            limit -= rval;

            if (Parent == null)
                return rval; //i'm done
            //then my neighbours and boss
            
            //unfinished - solve for checks from lower nodes
            return rval;
        }

        public float MyRayCast(Vector3 Origin, Vector3 Direction)
        {
            float rval = float.PositiveInfinity;
            for (int i = 0; i < Triangles.Count; i++)
            {

                float newdst = Triangles[i].RayIntersection(ref Origin, ref Direction);
                    rval = MathTools.Min(rval, newdst);
            }
            return rval;
        }

        public bool ContainsPos(ref Vector3 origin)
        {
            if ((origin.X < Corner.X) || (origin.X>Corner.X+Size))
                return false;
            if ((origin.Y < Corner.Y) || (origin.Y > Corner.Y + Size))
                return false;
            if ((origin.Z < Corner.Z) || (origin.Z > Corner.Z + Size))
                return false;
            return true;
        }

        public GeoOctTreeNode GetNode(ref Vector3 position, bool downonly = false)
        {
            if (!ContainsPos(ref position))
            {
                if (!downonly)
                {
                    if (Parent != null)
                        return Parent.GetNode(ref position);
                }
                return null;
            }

            for (int i = 0; i < 8; i++)
            {
                var node = Nodes[i];
                if (node != null)
                {
                    var rval = node.GetNode(ref position, true);
                    if (rval != null)
                        return rval;
                }
            }
            return this;
        }

        public float RayCast(Vector3 Origin, Vector3 Direction, bool starting = true)
        {
            if (starting)
            {            
                //first locate node with origin
                var node = GetNode(ref Origin);
                if (node == null)
                    return float.PositiveInfinity;
                return node.RayCast(Origin, Direction, false);                
            }
                        
            if (!ContainsPos(ref Origin))
                return float.PositiveInfinity;
            
            
            float rval = float.PositiveInfinity;
            if (!Empty)
                rval = MyRayCast(Origin, Direction);
            if (rval < float.PositiveInfinity)
                return rval;

            if (Parent == null)
                return rval;

            Origin.AddScaled(ref Direction, Size);

            Parent.RayCast(Origin, Direction, true);
            return rval;

            /*
            //idiotic, should check which node is hit
            for (int i = 0; i < 8; i++)
            {
                var node = Nodes[i];
                if (node != null)
                {
                    float dst = node.RayCast(ray);
                    if (dst != float.NaN)
                    {
                        rval = MathTools.Min(rval, dst);
                    }
                }
            }
            return rval;
            */
        }



        /// <summary>
        /// Gets smallest node from hiearchy fully containing sphere
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius of the sphere</param>
        /// <param name="createnew">If true, creates node, otherwise returns best possible</param>
        /// <param name="sizelimit">Size limit for node creation. If zero, no limits</param>
        /// <param name="bosssizelimit">Size limit for node creation for higher level node. If zero, no limits</param>
        /// <param name="createnewboss">If true, creates new new parent node if needed, , otherwise returns best possible</param>
        /// <returns>Node</returns>
        public GeoOctTreeNode GetNode(ref Vector3 center, float radius, bool createnew = true, float sizelimit = 0, bool createnewboss = true, float bosssizelimit = 0)
        {
            Vector3 min = center;
            Vector3 max = center;
            max.Add(radius, radius, radius);
            min.Substract(radius, radius, radius);
            return GetNode(ref min, ref max, createnew, sizelimit, createnewboss, bosssizelimit);
        }

        /// <summary>
        /// Gets smallest node from hiearchy fully containing AAbox
        /// </summary>
        /// <param name="min">Minimal corner</param>
        /// <param name="max">Maximal corner</param>
        /// <param name="createnew">If true, creates node, otherwise returns best possible</param>
        /// <param name="sizelimit">Size limit for node creation. If zero, no limits</param>
        /// <param name="bosssizelimit">Size limit for node creation for higher level node. If zero, no limits</param>
        /// <param name="createnewboss">If true, creates new new parent node if needed, , otherwise returns best possible</param>
        /// <returns>Node</returns>
        public GeoOctTreeNode GetNode(ref Vector3 min, ref Vector3 max, bool createnew = true, float sizelimit = 0, bool createnewboss = true, float bosssizelimit = 0)
        {
            if (ContainsFully(ref min, ref max))
            {
                //node is inside me
                float half = Size / 2;
                for (int i = 0; i < 8; i++)
                {

                    Vector3 vec = Corner;
                    if (i > 3)
                        vec.Z=vec.Z+half;

                    switch (i%4)
                    {
                        case 1:
                            vec.X = vec.X+ half;
                            break;
                        case 2:
                            vec.Y = vec.Y+half;
                            break;
                        case 3:
                            vec.X = vec.X+half;
                            vec.Y= vec.Y+half;
                            break;
                    }

                    if (MathTools.ContainsFully(ref vec, half, ref min, ref max))
                    {
                        //it's in the subnode
                        if (Nodes[i] != null)
                            return Nodes[i].GetNode(ref min, ref max, createnew, sizelimit);
                        if (createnew && (half > sizelimit))
                        {
                            //create node
                            GeoOctTreeNode node = new GeoOctTreeNode(Master, this, vec, half);
                            Nodes[i] = node;
                            return node.GetNode(ref min, ref max, createnew, sizelimit);
                        }
                    }
                }
                //it really is inside me...
                return this;
            }
            else
            {
                //node is outside, put it higher

                if (Parent == null)
                {
                    if (createnewboss)
                    {
                        if ((bosssizelimit == 0) || (Size * 2 < bosssizelimit))
                        {


                            //ok, so we're no longer the boss...

                            Vector3 corner = Corner;

                            int targetnode = 0;

                            for (int i = 0; i < 3; i++)
                            {
                                if (min[i] < Corner[i])
                                {
                                    corner[i] -= Size;
                                    targetnode += 2 << i;
                                }
                            }

                            GeoOctTreeNode node = new GeoOctTreeNode(Master, null, corner, Size * 2);
                            node.Nodes[targetnode] = this;
                            Parent = node;

                            //and notify master
                            Master.TopNode = node;
                        }
                    }
                    else return this;   //no creating allowed, we'll keep it
                }
                return Parent.GetNode(ref min, ref max, createnew, sizelimit);
            }

        }

    }


}
