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

namespace VisiMath.Collisions
{
    /// <summary>
    /// Divides space to two halves, used for portals
    /// </summary>
    public interface ISpatialDivider
    {
        /// <summary>
        /// returns true if spatial index is at least partially on the "inside" part of the space
        /// </summary>
        /// <param name="index">Spatial index</param>
        /// <returns>True if inside</returns>
        bool IsInside(ref ISpatialIndex index);

        /// <summary>
        /// returns true if spatial index is fully in the "inside" part of the space
        /// </summary>
        /// <param name="index">Spatial index</param>
        /// <returns>True if inside</returns>
        bool IsFullyInside(ref ISpatialIndex index);

        /// <summary>
        /// Returns true, if object moving from one position to another moved fully through the portal
        /// </summary>
        /// <param name="startpos">Starting position</param>
        /// <param name="endpos">Ending position</param>
        /// <returns>True if object moved</returns>
        bool MovedThrough(ref ISpatialIndex startpos, ref ISpatialIndex endpos);
    }

    public abstract class BaseSpatialDivider : ISpatialDivider
    {
        /// <summary>
        /// Returns true if AA box is fully inside halfspace
        /// </summary>
        /// <param name="box">AA box</param>
        /// <returns>True if inside</returns>
        public abstract bool IsFullyInside(AABox box);

        /// <summary>
        /// Returns true if AA box is at least partialy inside halfspace
        /// </summary>
        /// <param name="box">AA box</param>
        /// <returns>True if inside</returns>
        public abstract bool IsInside(AABox box);

        /// <summary>
        /// Returns true if sphere is fully inside halfspace
        /// </summary>
        /// <param name="sph">SPhere</param>
        /// <returns>True if inside</returns>
        public abstract bool IsFullyInside(Sphere sph);

        /// <summary>
        /// Returns true if sphere is at least partialy inside halfspace
        /// </summary>
        /// <param name="sph">SPhere</param>
        /// <returns>True if inside</returns>
        public abstract bool IsInside(Sphere sph);

        /// <summary>
        /// Returns true if point is inside halfspace
        /// </summary>
        /// <param name="point">point</param>
        /// <returns>True if inside</returns>
        public abstract bool IsInside(Vector3 point);

        /// <summary>
        /// Returns true if generic convex object is fully inside halfspace
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if inside</returns>
        public abstract bool IsFullyInside(IConvexObject convex);

        /// <summary>
        /// Returns true if generic convex object is at least partialy inside halfspace
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if inside</returns>
        public abstract bool IsInside(IConvexObject convex);

        /// <summary>
        /// returns true if spatial index is fully on the "inside" part of the space
        /// </summary>
        /// <param name="index">Spatial index</param>
        /// <returns>True if inside</returns>
        public virtual bool IsFullyInside(ref ISpatialIndex index)
        {
            AABox box = index as AABox;
            if (box != null)
                return IsFullyInside(box);

            Sphere sph = index as Sphere;
            if (sph != null)
                return IsFullyInside(sph);

            Point3D point = index as Point3D;
            if (point != null)
                return IsInside(point.GetPosition(0));

            LineSegment3D line = index as LineSegment3D;
            if (line != null)
            {
                return (IsInside(line.GetPosition(0)) && IsInside(line.GetPosition(1)));
            }

            return IsFullyInside(index as IConvexObject);
        }

        /// <summary>
        /// returns true if spatial index is at least partialy on the "inside" part of the space
        /// </summary>
        /// <param name="index">Spatial index</param>
        /// <returns>True if inside</returns>
        public virtual bool IsInside(ref ISpatialIndex index)
        {
            AABox box = index as AABox;
            if (box != null)
                return IsInside(box);

            Sphere sph = index as Sphere;
            if (sph != null)
                return IsInside(sph);

            Point3D point = index as Point3D;
            if (point != null)
                return IsInside(point.GetPosition(0));

            LineSegment3D line = index as LineSegment3D;
            if (line != null)
            {
                return (IsInside(line.GetPosition(0)) || IsInside(line.GetPosition(1)));
            }
            return IsInside(index as IConvexObject);
        }


        /// <summary>
        /// Returns true, if object moving from one position to another moved fully through the portal
        /// </summary>
        /// <param name="startpos">Starting position</param>
        /// <param name="endpos">Ending position</param>
        /// <returns>True if object moved</returns>
        public virtual bool MovedThrough(ref ISpatialIndex startpos, ref ISpatialIndex endpos)
        {
            if (IsInside(ref startpos) && (!IsFullyInside(ref endpos)))   //naive, but should work
                return true;
            return false;
        }    


    }


    /// <summary>
    /// Simplest and fastest space divider - uses plane aligned with axis
    /// Example: AlignedPlaneDivider(0, 10, -1) returns everything with all x coordinates lower then 10 as inside
    /// </summary>
    public class AlignedPlaneDivider : BaseSpatialDivider
    {
        int Axis;
        float Position;
        //float Sign;
        bool Negative;  //note, using sign would be probably better
        
        /// <summary>
        /// Simple plane divider
        /// </summary>
        /// <param name="axis">Axis to align with (0 = x, 1 = y, 2 = z)</param>
        /// <param name="position">Position on the axis</param>
        /// <param name="sign">-1 or 1, means direction of "inside"</param>
        public AlignedPlaneDivider(int axis, float position, float sign)
        {
            if ((Axis < 0) || (Axis > 2))
                throw new ArgumentException("Wrong axis specified for AlignedPlaneDivider");
            Axis = axis;
            //Sign = Math.Sign(sign);
            if (sign < 0)
                Negative = true;
            else Negative = false;
            Position = position;
        }

        /// <summary>
        /// Returns true if AA box is fully inside halfspace
        /// </summary>
        /// <param name="box">AA box</param>
        /// <returns>True if inside</returns>
        public override bool IsFullyInside(AABox box)
        {
            if (Negative)
            {
                if ((box.Minimum[Axis] > Position) || (box.Maximum[Axis] > Position))
                    return false;
            }
            else
            {
                if ((box.Minimum[Axis] < Position) || (box.Maximum[Axis] < Position))
                    return false;
            }
            return true;
        }

        /// <summary>
        /// Returns true if AA box is at least partialy inside halfspace
        /// </summary>
        /// <param name="box">AA box</param>
        /// <returns>True if inside</returns>
        public override bool IsInside(AABox box)
        {
            if (Negative)
            {
                if ((box.Minimum[Axis] < Position) || (box.Maximum[Axis] < Position))
                    return true;
            }
            else
            {
                if ((box.Minimum[Axis] > Position) || (box.Maximum[Axis] > Position))
                    return true;
            }
            return false;
        }

        /// <summary>
        /// Returns true if sphere is fully inside halfspace
        /// </summary>
        /// <param name="sph">SPhere</param>
        /// <returns>True if inside</returns>
        public override bool IsFullyInside(Sphere sph)
        {
            if (Negative)
            {
                if (sph.Center[Axis] + sph.Radius > Position)
                    return false;
            }
            else
            {
                if (sph.Center[Axis] - sph.Radius < Position)
                    return false;
            }
            return true;
        }

        /// <summary>
        /// Returns true if sphere is at least partialy inside halfspace
        /// </summary>
        /// <param name="sph">SPhere</param>
        /// <returns>True if inside</returns>
        public override bool IsInside(Sphere sph)
        {
            if (Negative)
            {
                if (sph.Center[Axis] - sph.Radius < Position)
                    return true;
            }
            else
            {
                if (sph.Center[Axis] + sph.Radius > Position)
                    return true;
            }
            return false;
        }

        /// <summary>
        /// Returns true if point is inside halfspace
        /// </summary>
        /// <param name="point">point</param>
        /// <returns>True if inside</returns>
        public override bool IsInside(Vector3 point)
        {
            if (Negative)
            {
                if (point[Axis] > Position)
                    return false;
            }
            else
            {
                if (point[Axis] < Position)
                    return false;
            }
            return true;
        }

        /// <summary>
        /// Returns true if generic convex object is fully inside halfspace
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if inside</returns>
        public override bool IsFullyInside(IConvexObject convex)
        {
            int vercount = convex.VertexCount;
            for (int i = 0; i < vercount; i++)
            {
                Vector3 vec;
                convex.GetVertex(i, out vec);
                if (Negative)
                {
                    if (vec[Axis] >= Position)
                        return false;
                }
                else
                    if (vec[Axis] <= Position)
                        return false;
            }
            return true;
        }

        /// <summary>
        /// Returns true if generic convex object is at least partialy inside halfspace
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if inside</returns>
        public override bool IsInside(IConvexObject convex)
        {
            int vercount = convex.VertexCount;
            for (int i = 0; i < vercount; i++)
            {
                Vector3 vec;
                convex.GetVertex(i, out vec);
                if (Negative)
                {
                    if (vec[Axis] <= Position)
                        return true;
                }
                else
                    if (vec[Axis] >= Position)
                        return true;
            }
            return true;
        }

    }

    /// <summary>
    /// Generic plane divider with simple plane (like in BinarySpace portioning)
    /// Objects is deemeed inside if it is in the negative halfspace or intersects the plane
    /// </summary>
    public class PlanarDivider : BaseSpatialDivider
    {
        Vector3 Normal;
        float Distance;
        
        /// <summary>
        /// Creates planar divider from plane equation
        /// </summary>
        /// <param name="normal">Normal of the plane</param>
        /// <param name="distance">Distance from origin</param>
        public PlanarDivider(Vector3 normal, float distance)
        {
            Normal = normal.NormalizedCopy();   //just to be sure
            Distance = distance;
        }

        /// <summary>
        /// Creates planar divider from three points on the plane. Points should be in CLOCKWISE order
        /// </summary>
        /// <param name="pos1">Pos1</param>
        /// <param name="pos2">Pos2</param>
        /// <param name="pos3">Pos3</param>
        public PlanarDivider(Vector3 pos1, Vector3 pos2, Vector3 pos3)
        {
            Normal = MathTools.CalcTriNormal(ref pos1, ref pos2, ref pos3);
            Distance = Normal.Dot(ref pos1);    //check!!!
        }


        /// <summary>
        /// Returns true if AA box is fully inside halfspace
        /// </summary>
        /// <param name="box">AA box</param>
        /// <returns>True if inside</returns>
        public override bool IsFullyInside(AABox box)
        {
            Vector3 center = box.Center;                
            Vector3 extents = box.HalfExtents;

            float r = extents.X * MathTools.Abs(Normal.X) + extents.Y * MathTools.Abs(Normal.Y) + extents.Z * MathTools.Abs(Normal.Z);
            if ((Normal.Dot(ref center) - Distance) < -r)
                return true;
            return false;
        }

        /// <summary>
        /// Returns true if AA box is at least partialy inside halfspace
        /// </summary>
        /// <param name="box">AA box</param>
        /// <returns>True if inside</returns>
        public override bool IsInside(AABox box)
        {
            Vector3 center = box.Center;
            Vector3 extents = box.HalfExtents;

            float r = extents.X * MathTools.Abs(Normal.X) + extents.Y * MathTools.Abs(Normal.Y) + extents.Z * MathTools.Abs(Normal.Z);
            if ((Normal.Dot(ref center) - Distance) <= r)
                return true;
            return false;
        }


        /// <summary>
        /// Returns true if sphere is fully inside halfspace
        /// </summary>
        /// <param name="sph">SPhere</param>
        /// <returns>True if inside</returns>
        public override bool IsFullyInside(Sphere sph)
        {
            if ((sph._Center.Dot(ref Normal) - Distance) < -sph.Radius)
                return true;
            return false;
        }

        /// <summary>
        /// Returns true if sphere is at least partialy inside halfspace
        /// </summary>
        /// <param name="sph">SPhere</param>
        /// <returns>True if inside</returns>
        public override bool IsInside(Sphere sph)
        {
            if ((sph._Center.Dot(ref Normal) - Distance) <= sph.Radius)
                return true;
            return false;
        }

        /// <summary>
        /// Returns true if point is inside halfspace
        /// </summary>
        /// <param name="point">point</param>
        /// <returns>True if inside</returns>
        public override bool IsInside(Vector3 point)
        {
            if (point.Dot(ref Normal) - Distance <= 0)
                return true;
            return false;
        }


        /// <summary>
        /// Returns true if generic convex object is fully inside halfspace
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if inside</returns>
        public override bool IsFullyInside(IConvexObject convex)
        {
            int vercount = convex.VertexCount;
            for (int i = 0; i < vercount; i++)
            {
                Vector3 vec;
                convex.GetVertex(i, out vec);
                if (vec.Dot(ref Normal) - Distance > 0)
                    return false;
            }
            return true;
        }


        /// <summary>
        /// Returns true if generic convex object is at least partialy inside halfspace
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if inside</returns>
        public override bool IsInside(IConvexObject convex)
        {
            int vercount = convex.VertexCount;
            for (int i = 0; i < vercount; i++)
            {
                Vector3 vec;
                convex.GetVertex(i, out vec);
                if (vec.Dot(ref Normal) - Distance <= 0)
                    return true;
            }
            return false;
        }

    }

    /// <summary>
    /// Uses Quad (must be planar!) as a portal
    /// Note, really usable only for movethrough check!
    /// </summary>
    public class QuadSpatialDivider : PlanarDivider
    {
        Vector3 Corner1, Corner2, Corner3, Corner4;
        
        /// <summary>
        /// Creates planar divider from three points on the plane
        /// </summary>
        /// <param name="pos1">Pos1</param>
        /// <param name="pos2">Pos2</param>
        /// <param name="pos3">Pos3</param>
        public QuadSpatialDivider(Vector3 pos1, Vector3 pos2, Vector3 pos3, Vector3 pos4) : base(pos1, pos2, pos3)
        {
            Corner1 = pos1;
            Corner2 = pos2;
            Corner3 = pos3;
            Corner4 = pos4;
        }

        /// <summary>
        /// Returns true, if object moving from one position to another moved through the portal.
        /// Note: Currently converts spatial indices to spheres to see if intersection with quad occured
        /// </summary>
        /// <param name="startpos">Starting position</param>
        /// <param name="endpos">Ending position</param>
        /// <returns>True if object moved</returns>
        public override bool MovedThrough(ref ISpatialIndex startpos, ref ISpatialIndex endpos)
        {
            if (!base.MovedThrough(ref startpos, ref endpos))
                return false;

            Sphere s1 = startpos.AsSphere;
            Sphere s2 = endpos.AsSphere;

            Vector3 linestart = s1.Center;
            Vector3 lineend = s2.Center;

            float radius = MathTools.Max(s1.Radius, s2.Radius);

            float t;

            //note: optimize!!!
            if (MathTools.IntersectionTriangleCapsule(ref Corner1, ref Corner2, ref Corner3, ref linestart, ref lineend, radius, out t))
                return true;
            if (MathTools.IntersectionTriangleCapsule(ref Corner3, ref Corner4, ref Corner1, ref linestart, ref lineend, radius, out t))
                return true;
            return false;
        }    
    }



    /// <summary>
    /// Interface for spatial portal linking two spatial portions
    /// </summary>
    /// <typeparam name="TObject">Type of object being held in cells</typeparam>
    public interface ISpatialPortal<TObject>
        where TObject : ISpatialyIndexable
    {
        /// <summary>
        /// Allowed collision flags - only objects of specified flag can pass
        /// </summary>
        UInt64 AllowedCollisions { get; } // = UInt64.MaxValue;

        /// <summary>
        /// Cell on the "left"
        /// </summary>
        ISpatialCell<TObject> LeftCell { get; }
        /// <summary>
        /// Cell on the "right"
        /// </summary>
        ISpatialCell<TObject> RightCell { get; }
        /// <summary>
        /// Portal in the left cell leading to right node
        /// </summary>
        ISpatialDivider LeftPortal { get; }
        /// <summary>
        /// Portal in the right cell reading to left node
        /// </summary>
        ISpatialDivider RightPortal { get; }
    }

}
