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

namespace VisiMath.Collisions
{

    /// <summary>
    /// Generic uniform 2D grid partioning system
    /// </summary>
    /// <typeparam name="TObject"></typeparam>
    /// <typeparam name="TGridCell"></typeparam>
    public class Grid2D<TObject, TGridCell> : HierarchicalSpatialCell<TObject, TGridCell>
        where TGridCell : ISpatialCell<TObject>
        where TObject : ISpatialyIndexable
    {
        int SizeX, SizeY;
        float ScaleX, ScaleY;
        float InvScaleX, InvScaleY;
        Vector3 Center;

        TGridCell[,] Cells;

        public Grid2D(int sizex, int sizey, Vector3 center, float scalex, float scaley) 
            : this(sizex, sizey, center, scalex, scaley, float.NegativeInfinity, float.PositiveInfinity)
        {
        }

        public Grid2D(int sizex, int sizey, Vector3 center, float scalex, float scaley, float floor, float ceiling)
        {
            Debug.Assert(sizex > 0, "Can't create grid with zero X size!");
            Debug.Assert(sizey > 0, "Can't create grid with zero X size!");
            Debug.Assert(scalex > 0, "Can't create grid with zero X size!");
            Debug.Assert(scaley > 0, "Can't create grid with zero X size!");
            Cells = new TGridCell[sizex, sizey];
            SizeX = sizex;
            SizeY = sizey;
            InvScaleX = 1 / scalex;
            InvScaleY = 1 / scaley;
            ScaleX = scalex;
            ScaleY = scaley;
            Center = center;

            Vector3 min = new Vector3(center.X - 0.5f * sizex * scalex * 0.5f, center.Y- 0.5f * sizey * scaley, floor);
            Vector3 max = new Vector3(center.X + 0.5f * sizex * scalex * 0.5f, center.Y + 0.5f * sizey * scaley, ceiling);
            MyIndex = new AABox(ref min, ref max);
        }

        #region ISpatialCell<Grid2D<TGridCell>> Members

        /*
        /// <summary>
        /// Returns true if spatial portion contains spatial index fully
        /// </summary>
        /// <param name="index">Spatial index</param>
        //public override bool ContainsFully(ref ISpatialIndex index)
        //note, optimize for 2d checking only?
        */

        /// <summary>
        /// Returns spatial portion which contains position
        /// </summary>
        /// <param name="position">Position vector</param>
        /// <returns>Spatial node containing object</returns>
        public override ISpatialCell<TObject> GetNode(ref Vector3 position)
        {
            int i, j;
            if (!TransformPosition(position, out i, out j))
                return this;
            return Cells[i, j];
        }

        /// <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 override ISpatialCell<TObject> GetNode(ref Vector3 center, float radius)
        {
            Vector3 min = new Vector3(ref center);
            min.X -= radius;
            min.Y -= radius;
            Vector3 max = new Vector3(ref center);
            max.X += radius;
            max.Y += radius;
            return GetNode(ref min, ref max);
        }

        /// <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 override ISpatialCell<TObject> GetNode(ref Vector3 minimum, ref Vector3 maximum)
        {
            int i1, j1;
            int i2, j2;

            bool rval1 = TransformPosition(minimum, out i1, out j1);
            bool rval2 = TransformPosition(maximum, out i2, out j2);

            if (rval1 && rval2 && (i1 == i2) && (j1 == j2))
                return Cells[i1, j1];

            return this;
        }

        /// <summary>
        /// Returns smallest spatial portion which fully contains spatial index
        /// </summary>
        /// <param name="index">Spatial index</param>
        public override ISpatialCell<TObject> GetNode(ref ISpatialIndex index)
        {
            AABox box = index.AsBox;
            return GetNode(ref box.Minimum, ref box.Maximum);
        }

        #endregion

        protected bool TransformPosition(Vector3 position, out int i, out int j)
        {
            position -= Center;
            position.X *= InvScaleX;
            position.Y *= InvScaleY;

            i = (int)position.X;
            j = (int)position.Y;

            if ((i < 0) || (i >= SizeX) ||
                (j < 0) || (j >= SizeY))
                return false;
            return true;
        }



        #region IHierarchicalSpatialCell<TObject> Members

        /// <summary>
        /// Number of the subnodes in indexer for hiearchical indexing
        /// </summary>
        public override int SubNodeCount
        {
            get { return SizeX * SizeY; }
        }

        /// <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 override ISpatialIndex GetSubNodeIndex(int index)
        {            
            int i, j;
            if (!GetNodeIndex(index, out i, out j))
                return null;

            return Cells[i, j].Index;   //no point in calculating it here
        }

        protected bool GetNodeIndex(int index, out int i, out int j)
        {
            j = index / SizeX;
            i = index % SizeX;

            if (j >= SizeY)
                return false;
            return true;
        }

        /// <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 override ISpatialCell<TObject> GetSubNode(int index)
        {
            int i, j;

            if (!GetNodeIndex(index, out i, out j))
                return null;
            return Cells[i, j];
        }

        /// <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 override ISpatialCell<TObject> GetSubNode(ref ISpatialIndex index)
        {
            return GetNode(ref index);  //!!! wrong probably
        }

        #endregion
    }
    
    
    public class Grid3D
    {
    }

    public class LinkedCells
    {
    }
}
