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

namespace VisiMath.Collisions
{
    
    /// <summary>
    /// Basic spatial cell
    /// </summary>
    /// <typeparam name="TObject"></typeparam>
    public class SpatialCell<TObject> : ISpatialCell<TObject>
        where TObject : ISpatialyIndexable
    {
        #region Privates

        /// <summary>
        /// Spatial index of the cell
        /// </summary>
        protected ISpatialIndex MyIndex;
        /// <summary>
        /// Portals in the cell
        /// </summary>
        protected List<ISpatialPortal<TObject>> Portals;
        /// <summary>
        /// Objects contained in the cell
        /// </summary>
        protected List<TObject> MyObjects;

        /// <summary>
        /// Parent of the cell
        /// </summary>
        protected SpatialCell<TObject> _Owner;
        #endregion

        #region Properties
        /// <summary>
        /// Spatial index associated to this cell
        /// </summary>
        public virtual ISpatialIndex Index { get { return MyIndex; } }

        /// <summary>
        /// Owner of the spatial cell for hiearchical indexing
        /// </summary>
        public ISpatialCell<TObject> Owner { get { return _Owner; } }


        /// <summary>
        /// Number of portals to adjacent spatialcells
        /// </summary>
        public int PortalCount
        {
            get
            {
                if (Portals == null)
                    return 0;
                return Portals.Count;
            }
        }

        /// <summary>
        /// Number of the objects contained directly in the node
        /// </summary>
        public int ObjectCount
        {
            get
            {
                if (MyObjects == null)
                    return 0;
                return MyObjects.Count;
            }
        }


        #endregion


        #region Constructors

        public SpatialCell(SpatialCell<TObject> owner = null)
        {
            _Owner = owner;
            Portals = new List<ISpatialPortal<TObject>>();
            MyObjects = new List<TObject>();            
        }


        #endregion


        /// <summary>
        /// Returns true if spatial portion contains spatial index fully
        /// </summary>
        /// <param name="index">Spatial index</param>
        public virtual bool ContainsFully(ref ISpatialIndex index)
        {
            return MyIndex.ContainsFully(ref index);
        }

        /// <summary>
        /// Returns true if spatial portion contains spatial index at least partially
        /// </summary>
        /// <param name="index">Spatial index</param>
        public virtual bool Contains(ref ISpatialIndex index)
        {
            return MyIndex.ContainsFully(ref index);
        }

        /// <summary>
        /// Returns spatial portal with specified index (0 to PortalCount)
        /// </summary>
        /// <param name="index"></param>
        /// <returns></returns>
        public ISpatialPortal<TObject> GetPortal(int index)
        {
            if (index >= Portals.Count)
                return null;
            return Portals[index];
        }

        //test space
        /*
        protected delegate void PortalFunction(ISpatialPortal<TObject> portal);

        protected void ForEachPortal(PortalFunction p)
        {
            for (int i = 0; i < Portals.Count; i++)
                p(Portals[i]);
        }

        public void test()
        {
            ForEachPortal((portal) =>
                {
                });
        }
        */

        /// <summary>
        /// Returns spatial portion which contains position
        /// </summary>
        /// <param name="position">Position vector</param>
        /// <returns>Spatial node containing object</returns>
        public virtual ISpatialCell<TObject> GetNode(ref Vector3 position)
        {
            ISpatialIndex ind = GetPrefferedIndex(ref position);
            return GetNode(ref ind);
        }

        /// <summary>
        /// Returns smallest spatial portion which fully contains sphere
        /// </summary>
        /// <param name="center">Sphere center</param>
        /// <param name="radius">Radius of the sphere</param>
        /// <returns>Spatial node fully containing object</returns>
        public virtual ISpatialCell<TObject> GetNode(ref Vector3 center, float radius)
        {
            ISpatialIndex ind = GetPrefferedIndex(ref center, radius);
            return GetNode(ref ind);
        }

        /// <summary>
        /// Returns smallest spatial portion which fully contains bounding box
        /// </summary>
        /// <param name="minimum">Minimal corner of the box</param>
        /// <param name="maximum">Maximal corner of the box</param>
        /// <returns>Spatial node fully containing object</returns>
        public virtual ISpatialCell<TObject> GetNode(ref Vector3 minimum, ref Vector3 maximum)
        {
            ISpatialIndex ind = GetPrefferedIndex(ref minimum, ref maximum);
            return GetNode(ref ind);
        }
        
        /// <summary>
        /// Creates spatial index of prefered type 
        /// </summary>
        /// <param name="position">position</param>
        /// <returns>Prefered type of spatial index</returns>
        public virtual ISpatialIndex GetPrefferedIndex(ref Vector3 position)
        {
            return new Point3D(position);
        }

        /// <summary>
        /// Creates spatial index of prefered type 
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius of the sphere</param>
        /// <returns>Prefered type of spatial index</returns>
        public virtual ISpatialIndex GetPrefferedIndex(ref Vector3 center, float radius)
        {
            return new Sphere(center, radius);
        }

        /// <summary>
        /// Creates spatial index of prefered type 
        /// </summary>
        /// <param name="max">maximal corner</param>
        /// <param name="min">minimal corner</param>
        /// <returns>Prefered type of spatial index</returns>
        public virtual ISpatialIndex GetPrefferedIndex(ref Vector3 min, ref Vector3 max)
        {
            return new AABox(ref min, ref max);
        }


        /// <summary>
        /// Returns smallest spatial portion which fully contains spatial index
        /// </summary>
        /// <param name="index">Spatial index</param>
        public virtual ISpatialCell<TObject> GetNode(ISpatialIndex index)
        {
            return GetNode(ref index);
        }

        /// <summary>
        /// Returns smallest spatial portion which fully contains spatial index
        /// </summary>
        /// <param name="index">Spatial index</param>
        public virtual ISpatialCell<TObject> GetNode(ref ISpatialIndex index)
        {
            if (MyIndex.ContainsFully(ref index))
                return this;

            //try portals



            //if (!MyIndex.Contains(ref index))
            if (_Owner == null)
                return null;    //out of bounds
            return _Owner.GetNode(ref index);  //no contact, not my problem
        }

        /// <summary>
        /// Returns list of the objects directly in the node
        /// </summary>
        /// <param name="CollisionGroup">Collision group flags</param>
        /// <returns>List of objects in node with correct collision group</returns>
        public virtual IEnumerable<TObject> GetObjects(UInt64 CollisionGroup = UInt64.MaxValue)
        {
            int guess;
            if (CollisionGroup == UInt64.MaxValue)
                guess = MyObjects.Count;
            else guess = MyObjects.Count / 2;

            List<TObject> rval = new List<TObject>(guess);

            foreach (TObject t in MyObjects)
            {
                if ((t.CollisionGroup & CollisionGroup) != 0)
                    rval.Add(t);
            }
            return rval;            
        }

        /// <summary>
        /// Adds objects directly in the node with correct collision group to the list
        /// </summary>
        /// <param name="CollisionGroup">Collision group flags</param>
        /// <param name="list"> List of the objects</param>
        /// <param name="startindex">Starting index in the list</param>
        public virtual int GetObjects(List<TObject> list, int startindex, UInt64 CollisionGroup = UInt64.MaxValue)
        {
            int total = 0;
            foreach (TObject t in MyObjects)
            {
                if ((t.CollisionGroup & CollisionGroup) != 0)
                {
                    if (startindex < list.Count)
                        list[startindex] = t;
                    else
                        list.Add(t);
                    total++;
                    startindex++;
                }
            }
            return total;
        }

        /// <summary>
        /// Returns number of all objects in the node (and subnodes) with specified collision group
        /// </summary>
        /// <param name="CollisionGroup">Collision group flags</param>
        /// <returns>Number of object</returns>
        public virtual int GetObjectCount(UInt64 CollisionGroup = UInt64.MaxValue)
        {
            int total = 0;
            foreach (TObject t in MyObjects)
            {
                if ((t.CollisionGroup & CollisionGroup) != 0)
                {
                    total++;
                }
            }
            return total;
        }

        /// <summary>
        /// Returns number of all objects in the node (and subnodes) with specified collision group on speficied spatialindex
        /// </summary>
        /// <param name="CollisionGroup">Collision group flags</param>
        /// <param name="index">Spatial index to check</param>
        /// <returns>Number of object</returns>
        public virtual int GetObjectCount(ref ISpatialIndex index, UInt64 CollisionGroup = UInt64.MaxValue)
        {
            if (!Contains(ref index))
                return 0;

            int total = 0;
            foreach (TObject t in MyObjects)
            {
                if ((t.CollisionGroup & CollisionGroup) != 0)
                {
                    if (t.SpatialIndex.Contains(ref index))
                    {
                        total++;
                    }
                }
            }
            return total;
        
        }

        /// <summary>
        /// Adds object to spatialindexer (to any subnode unless forced adding is set), should set it's data (cell and spatial index)
        /// </summary>
        /// <param name="obj">Object to add</param>
        /// <param name="force">If true, will place object directly in the node, won't search for better fit</param>
        public virtual void AddObject(TObject obj, bool force = false)
        {
            if (!force)
            {
                ISpatialCell<TObject> node = GetNode(obj.SpatialIndex);
                if (node != this)
                    AddObject(obj, false);
                return;
            }
            MyObjects.Add(obj);
            obj.Cell = this;            
        }

        /// <summary>
        /// Adds object to spatial indexer on specified index (should move objects internal index to new index)
        /// </summary>
        /// <param name="obj">object to add</param>
        /// <param name="index">Index</param>
        /// <param name="force">If true, will place object directly in the node, won't search for better fit</param>
        public virtual void AddObject(TObject obj, ref ISpatialIndex index, bool force = false)
        {
            obj.SpatialIndex = index;
            AddObject(obj, force);
        }

        /// <summary>
        /// Removes objects from indexer (including subnodes)
        /// </summary>
        /// <param name="obj">Object to remove</param>
        /// <returns>True if object removed</returns>
        public virtual bool RemoveObject(TObject obj)
        {
            if (MyObjects == null)
                return false;
            return MyObjects.Remove(obj);            
        }


        /// <summary>
        /// Moves object to new index
        /// </summary>
        /// <param name="obj">Object to move</param>
        /// <param name="newindex">New spatial index</param>
        /// <returns>New spatial portion where the object was moved</returns>
        public virtual ISpatialCell<TObject> MoveObject(TObject obj, ref ISpatialIndex newindex)
        {
            if (ContainsFully(ref newindex))
            {
                obj.SpatialIndex = newindex;
                //mine!
                if (!MyObjects.Contains(obj))
                {
                    AddObject(obj, true);
                }
                return this;
            }
            ISpatialCell<TObject> node = GetNode(newindex);
            return node.MoveObject(obj, ref newindex);
        }

        /// <summary>
        /// Generates list of all current collisions in the node for specified collision group
        /// </summary>
        /// <param name="collision">Collision flags</param>
        /// <returns>List of colliding pairs</returns>
        public List<SpatialPair<TObject>> GenerateCollisions(UInt64 collision = UInt64.MaxValue)
        {
            var rval = new List<SpatialPair<TObject>>();
            int count = GenerateCollisions(rval, 0, collision);
            if (count == 0)
                return null;
            return rval;
        }

        /// <summary>
        /// Generates list of all current collisions in the node for specified collision group and adds it on the existing list at starting index
        /// </summary>
        /// <param name="collision">Collision flags</param>
        /// <param name="list">List to add collisions</param>
        /// <param name="startindex">Starting index to save collisions</param>
        /// <returns>Number of added collision pairs</returns>
        public virtual int GenerateCollisions(List<SpatialPair<TObject>> list, int startindex, UInt64 collision = UInt64.MaxValue)
        {
            int rval = 0;
            for (int i = 0; i < MyObjects.Count-1; i++)
            {
                if ((MyObjects[i].CollisionGroup & collision) == 0)
                    continue;

                for (int j = i + 1; j < MyObjects.Count; j++)
                {
                    if ((MyObjects[j].CollisionGroup & collision) == 0)
                        continue;

                    ISpatialIndex second = MyObjects[j].SpatialIndex;
                    if (MyObjects[i].SpatialIndex.Contains(ref second))
                    {
                        SpatialPair<TObject> pair = new SpatialPair<TObject>(MyObjects[i], MyObjects[j]);
                        if (startindex < list.Count - 1)
                        {
                            list[startindex] = pair;
                        }
                        else
                            list.Add(pair);
                        startindex++;
                        rval++;
                    }
                }
            }
            return rval;
        }



    }


    /// <summary>
    /// Hiearchical spatial cell which contains more sub nodes
    /// </summary>
    public abstract class HierarchicalSpatialCell<TObject, TSubCell> : SpatialCell<TObject>, IHierarchicalSpatialCell<TObject>
        where TObject : ISpatialyIndexable
    {
        ISpatialCell<TObject> _Root;

        /// <summary>
        /// Root of the spatial indexer (owner of the spatial cell list / tree)
        /// </summary>
        public ISpatialCell<TObject> Root { get { return _Root; } set { _Root = value; } }

        /// <summary>
        /// Number of the subnodes in indexer for hiearchical indexing
        /// </summary>
        public abstract int SubNodeCount { get; }

        /// <summary>
        /// Total number of the subnodes in indexer for hiearchical indexing
        /// </summary>
        public virtual int TotalSubNodeCount 
        { 
            get
            {
                int total = 0;
                int totsub = SubNodeCount;
                for (int i = 0; i < totsub; i++)
                {
                    ISpatialCell<TObject> cell = GetSubNode(i);

                    IHierarchicalSpatialCell<TObject> hier = cell as IHierarchicalSpatialCell<TObject>;

                    total++;

                    if (hier != null)
                    {
                        total += hier.TotalSubNodeCount;
                    }
                }
                return total;
            }
        }

        /// <summary>
        /// Returns spatial index for subnode
        /// </summary>
        /// <param name="index">index of the subnode (valid are 0 to SubNodeCount</param>
        /// <returns>Spatial index</returns>
        public virtual ISpatialIndex GetSubNodeIndex(int index)
        {
            ISpatialCell<TObject> cell = GetSubNode(index);
            if (cell == null)
                return null;
            return cell.Index;
        }

        /// <summary>
        /// Returns subnode with specified integer index
        /// </summary>
        /// <param name="index">index of the subnode (valid are 0 to SubNodeCount</param>
        /// <returns>Subnode</returns>
        public abstract ISpatialCell<TObject> GetSubNode(int index);

        /// <summary>
        /// Returns this or subnode with specified spatial index (unlike GetNode doesn't move up in hiearchy)
        /// </summary>
        /// <param name="index">Spatial index</param>
        /// <returns>Subnode containg index or this</returns>
        public virtual ISpatialCell<TObject> GetSubNode(ref ISpatialIndex index)
        {
            if (!Contains(ref index))
                return null;
            int subc = SubNodeCount;
            for (int i = 0; i < subc; i++)
            {
                var cell = GetSubNode(i);

                var hiercell = cell as IHierarchicalSpatialCell<TObject>;

                if (hiercell != null)
                {
                    ISpatialCell<TObject> rval = hiercell.GetSubNode(ref index);
                    if (rval != null)
                        return rval;
                }
                else
                {
                    if (cell.ContainsFully(ref index))
                        return cell;
                }
            }
            return null;
        }

        /// <summary>
        /// Returns number of all objects in the node (and subnodes) with specified collision group
        /// </summary>
        /// <param name="CollisionGroup">Collision group flags</param>
        /// <returns>Number of object</returns>
        public override int GetObjectCount(ulong CollisionGroup = UInt64.MaxValue)
        {
            int total = base.GetObjectCount(CollisionGroup);
            int subn = SubNodeCount;
            for (int i = 0; i < subn; i++)
            {
                var node = GetSubNode(i);
                total += node.GetObjectCount(CollisionGroup);
            }
            return total;
        }

        /// <summary>
        /// Returns number of all objects in the node (and subnodes) with specified collision group on speficied spatialindex
        /// </summary>
        /// <param name="CollisionGroup">Collision group flags</param>
        /// <param name="index">Spatial index to check</param>
        /// <returns>Number of object</returns>
        public override int GetObjectCount(ref ISpatialIndex index, ulong CollisionGroup = UInt64.MaxValue)
        {
            int total = base.GetObjectCount(ref index, CollisionGroup);
            int subn = SubNodeCount;
            for (int i = 0; i < subn; i++)
            {
                var node = GetSubNode(i);
                total += node.GetObjectCount(ref index, CollisionGroup);
            }
            return total;
        }

        /// <summary>
        /// Removes objects from indexer (including subnodes)
        /// </summary>
        /// <param name="obj">Object to remove</param>
        /// <returns>True if object removed</returns>
        public override bool RemoveObject(TObject obj)
        {
            if (base.RemoveObject(obj))
                return true;
            int subn = SubNodeCount;
            for (int i = 0; i < subn; i++)
            {
                var node = GetSubNode(i);
                if (node.RemoveObject(obj))        //note!!! optimize by searching the node by index or by value of the cell?
                    return true;
            }
            return false;
        }

        /// <summary>
        /// Adds object to spatialindexer (to any subnode), should set it's data (cell and spatial index)
        /// </summary>
        /// <param name="obj">Object to add</param>
        /// <param name="force">If true, will place object directly in the node, won't search for better fit</param>
        public override void AddObject(TObject obj, bool force = false)
        {
            if (force)
            {
                base.AddObject(obj, force);
                return;
            }

            var node = GetNode(obj.SpatialIndex);
            node.AddObject(obj, force);
        }

        /// <summary>
        /// Adds object to spatial indexer on specified index (should move objects internal index to new index)
        /// </summary>
        /// <param name="obj">object to add</param>
        /// <param name="index">Index</param>
        public override void AddObject(TObject obj, ref ISpatialIndex index, bool force = false)
        {
            if (!force)
            {
                ISpatialCell<TObject> node = GetNode(ref index);

                if ((node != this) && (node != null))
                {
                    node.AddObject(obj, ref index);
                    return;
                }
            }
            obj.SpatialIndex = index;
            AddObject(obj, true);
        }

        /// <summary>
        /// Moves object to new index
        /// </summary>
        /// <param name="obj">Object to move</param>
        /// <param name="newindex">New spatial index</param>
        /// <returns>New spatial portion where the object was moved</returns>
        public override ISpatialCell<TObject> MoveObject(TObject obj, ref ISpatialIndex newindex)
        {
            if (MyObjects.Contains(obj))
            {
                var node = GetNode(newindex);
                if (node != this)
                {
                    MyObjects.Remove(obj);
                    obj.SpatialIndex = newindex;
                    node.AddObject(obj, true);
                    return node;
                }
                return this;
            }
            else
            {
                var node = (obj.Cell as ISpatialCell<TObject>);
                return node.MoveObject(obj, ref newindex);
            }
        }
        
    }

}
