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

namespace VisimBox.Physics
{
    /// <summary>
    /// Brute force collision checker without any spatial optimization
    /// </summary>
    public class CollisionChecker : IContactGenerator
    {

        protected List<RigidBody> _RigidBodies;
        protected List<StaticCollision> _StaticCollisions;
        protected List<IGeometry> _SavedStatics;

        /// <summary>
        /// registered rigid bodies
        /// </summary>
        public List<RigidBody> RigidBodies { get { return _RigidBodies; } }

        /// <summary>
        /// registered static collisions (used by editor)
        /// </summary>
        public List<StaticCollision> StaticCollisions { get { return _StaticCollisions; } }
        
        //ConvexList static
        //Polygonsoup

        public CollisionChecker(int rigidbodies, int staticcols)
        {
            _RigidBodies = new List<RigidBody>(rigidbodies);
            _StaticCollisions = new List<StaticCollision>(staticcols);
            _SavedStatics = new List<IGeometry>(100);
        }

        
        /// <summary>
        /// Quick check for collision between two rigid bodies, doesn't use more complicated algs.
        /// </summary>
        /// <param name="rb1">Rigid body 1</param>
        /// <param name="rb2">Rigid body 2</param>
        /// <returns>True if rigid body collide or migth collide</returns>
        public bool Check(RigidBody rb1, RigidBody rb2)
        {
            //todo - check bounding boxes first?
            Matrix4 mat1, mat2;
            mat1 = mat2 = Matrix4.Identitity;
            rb1.GetWorldTransformation(ref mat1);
            rb2.GetWorldTransformation(ref mat2);
            return CollisionSolver.QuickTest.Geometry(rb1.PhysicalBody, ref mat1, rb2.PhysicalBody, ref mat2);
        }


        /// <summary>
        /// Quick check for collisions between rigid body and static collisions
        /// </summary>
        /// <param name="rb">Rigid body</param>
        /// <returns>True if rigid body collide or migth collide</returns>
        public bool CheckStatic(RigidBody body)
        {
            Matrix4 mat1, mat2;
            mat1 = mat2 = Matrix4.Identitity;

            body.GetWorldTransformation(ref mat1);
            //using stupid bruteforce for now... OPTIMIZE!!!!
            foreach (StaticCollision sc in _StaticCollisions)
            {
                sc.GetWorldTransformation(ref mat2);
                if (CollisionSolver.QuickTest.Geometry(body.PhysicalBody, ref mat1, sc.StaticGeometry, ref mat2))
                    return true;
            }
            return false;
        }

        /// <summary>
        /// performs quick check for collision for rigid body (used by editor)
        /// </summary>
        /// <param name="body">Rigid body</param>
        /// <returns>True if rigid body collides (simple pass) with any other rigid body or static geometry</returns>
        public bool Check(RigidBody body)
        {
            foreach (RigidBody rb in _RigidBodies)
            {
                if (rb != body)
                {
                    //OPTIMIZE! no need to calculate transformation for body for each call... we can preidentify type of rigid body too
                    if (Check(body, rb))
                        return true;
                }
            }
            return CheckStatic(body);        
        }

        CollisionInfo myinfo = new CollisionInfo();

        protected bool AddContact(RigidBody r1, RigidBody r2, List<PhysicalContact> list, int index)
        {
            //using only 1 contact, might not be enough?
            Matrix4 mat1, mat2;
            mat1 = mat2 = Matrix4.Identitity;
            r1.GetWorldTransformation(ref mat1);
            r2.GetWorldTransformation(ref mat2);


            if (!CollisionSolver.Collision.Geometry(r1.PhysicalBody, ref mat1, r2.PhysicalBody, ref mat2, ref myinfo))
                return false;

            float friction = 1.0f, rest = 1.0f;

            bool moving = false;

            if ((r1.Velocity.LengthSquared > 1) || (r2.Velocity.LengthSquared > 1))
                moving = true;

            try
            {
                if (moving)
                    friction = r1.Material.DynamicFriction * r2.Material.DynamicFriction;
                else
                    friction = r1.Material.StaticFriction * r2.Material.StaticFriction;
                rest = r1.Material.Restitution * r2.Material.Restitution;
            }
            catch
            {
                VisimBox.Console.DebugLine("Warning, rigid body without physical material!");
            }
            

            list[index].SetBodyData(r1, r2, friction, rest);
            list[index].ContactNormal = myinfo.ContactNormal;
            list[index].ContactPoint1 = myinfo.ContactPoint1;
            list[index].ContactPoint2 = myinfo.ContactPoint2;
            list[index].Penetration = myinfo.Penetration;

            //Console.DebugLine("Normal " + myinfo.ContactNormal.ToString());
            //Console.DebugLine("Penetration " + myinfo.Penetration.ToString());

            return true;
        }

        protected int AddContact(RigidBody r1, StaticCollision sc, List<PhysicalContact> list, int index)
        {
            Matrix4 mat1, mat2;
            mat1 = mat2 = Matrix4.Identitity;
            r1.GetWorldTransformation(ref mat1);
            sc.GetWorldTransformation(ref mat2);


            //todo, better dispatcher!!!

            if (!CollisionSolver.Collision.Geometry(r1.PhysicalBody, ref mat1, sc.StaticGeometry, ref mat2, ref myinfo))
                return 0;

            float friction = 1.0f, rest = 1.0f;

            bool moving = false;

            if ((r1.Velocity.LengthSquared > 1))
                moving = true;

            try
            {
                if (moving)
                    friction = r1.Material.DynamicFriction * sc.Material.DynamicFriction;
                else
                    friction = r1.Material.StaticFriction * sc.Material.StaticFriction;
                rest = r1.Material.Restitution * sc.Material.Restitution;
            }
            catch
            {
                VisimBox.Console.DebugLine("Warning, rigid body without physical material!");
            }

            list[index].SetBodyData(r1, null, friction, rest);
            list[index].ContactNormal = myinfo.ContactNormal;
            list[index].ContactPoint1 = myinfo.ContactPoint1;
            list[index].ContactPoint2 = myinfo.ContactPoint2;
            list[index].Penetration = myinfo.Penetration;

            return 1;
        }

        /// <summary>
        /// generates all collision contacts for physical scene
        /// </summary>
        /// <param name="List">List to save contact into</param>
        /// <param name="index">Starting index</param>
        /// <returns>Number of added contacts</returns>
        public int AddContact(List<PhysicalContact> List, int index)
        {
 	        //brutal brute force.... check all rigid bodies (except sleeping) against all other and then against static collisions....

            int rval = 0;

            for (int i = 0; i < _RigidBodies.Count; i++)
            {
                if (index == List.Capacity)
                    return rval;
                RigidBody rb1 = _RigidBodies[i];
                for (int j = i+1; j < _RigidBodies.Count; j++)
                {
                    if (index == List.Capacity)
                        return rval;
                    RigidBody rb2 = _RigidBodies[j];
                    if (!rb1.Sleeping || (!rb2.Sleeping))
                    {
                        if (AddContact(rb1, rb2, List, index))
                        {
                            index++;
                            rval++;
                        }
                    }
                }

                if (!rb1.Sleeping)
                {
                    //now static, again, total stupid bruteforce
                    foreach (StaticCollision sc in _StaticCollisions)
                    {
                        if (index == List.Capacity)
                            return rval;
                        int added = AddContact(rb1, sc, List, index);
                        index += added;
                        rval += added;
                    }
                }
            }

            return rval;
        }

        public void AddStatic(StaticCollision sc)
        {                       
            _StaticCollisions.Add(sc);
            //todo, put in the list
        }

        public void RemoveStatic(StaticCollision sc)
        {
            if (!_StaticCollisions.Remove(sc))
                return;

            //todo, remove from list
        }

        /// <summary>
        /// Adds new static collision to static collisions - this collision will be unmovable / unchangeable
        /// </summary>
        /// <param name="geo">Geometry object</param>
        /// <param name="trans">Transformation (may be null)</param>
        /// <param name="mat">Physical material (may be null - default values used)</param>
        public void AddStatic(IGeometry geo, ITransformation trans, PhysMaterial mat)
        {
            StaticCollision sc = new StaticCollision();
            sc.StaticGeometry = geo;
            sc.Trans = trans;
            sc.Material = mat;
            AddStatic(sc);
            //todo: add to separate list
        }

        /// <summary>
        /// Updates static collision (either transformation or material) - slow in most implemntations, requires locating collision, removing it from
        /// precalculated data and storing it again
        /// </summary>
        /// <param name="sc">registered Static collision</param>
        public void UpdateStatic(StaticCollision sc)
        {            
            //throw new NotImplementedException(); //ignore for now, we're using bruteforce here...
        }

        /// <summary>
        /// Recalculates static collisions from registered statics - in effect forces updatestaticCol for every registered staticcollision body
        /// Force added (with addstaticcol) collisions will be unchanged
        /// </summary>
        public void RecalcStaticCollisions()
        {
            foreach (StaticCollision sc in _StaticCollisions)
            {
                UpdateStatic(sc);//todo - optimize this at least a bit...
            }
        }

        /// <summary>
        /// Adds rigid body to watch
        /// </summary>
        /// <param name="body"></param>
        public void AddRigid(RigidBody body)
        {
            RigidBodies.Add(body);
        }

        /// <summary>
        /// removes rigid body
        /// </summary>
        /// <param name="body"></param>
        /// <returns></returns>
        public bool RemoveRigid(RigidBody body)
        {
            if (!RigidBodies.Contains(body))
                return false;
            RigidBodies.Remove(body);
            return true;
        }

        /// <summary>
        /// Clears collision checker
        /// </summary>
        public void Clear()
        {
            _RigidBodies.Clear();
            _StaticCollisions.Clear();
        }


}
}
