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

namespace VisimBox.Physics
{
    /// <summary>
    /// One physical scene
    /// </summary>
    public class PhysicalScene : IDisposable
    {
        /// <summary>
        /// Maximal number of contacts permited per frame
        /// </summary>
        public int MaximumContacts { 
            get
            {
                return ContactList.Count;
            }
            set
            {
                int prev = ContactList.Capacity;
                ContactList.Capacity = value;
                if (prev < value)
                {
                    for (int i = 0; i < (value - prev); i++)
                    {
                        ContactList.Add(new PhysicalContact());
                    }
                }
            }
        }

        protected Vector3 _Gravity = new Vector3(0, 0, -9.81f);

        /// <summary>
        /// Global gravity in the scene which is applied to any registered body. 
        /// Changing gravity will attempt to change internal fixed acceleration of any registered rigid body.
        /// </summary>
        public Vector3 Gravity
        {
            get
            {
                return _Gravity;
            }
            set
            {
                if (value != _Gravity)
                {
                    SetGravity(value);
                }
            }
        }

        public bool CalculateIterations;



        protected List<PhysicalContact> ContactList;

        protected List<IContactGenerator> ContactGenerators;

        protected ContactResolver Resolver;

        protected CollisionChecker ColChecker;


        protected List<RigidBody> RigidBodies { get { return ColChecker.RigidBodies; } }

        protected List<StaticCollision> StaticCollisions { get { return ColChecker.StaticCollisions; } }

        
        public PhysicalScene(int bodies = 20, int maxcontactgen = 100, int maxcontacts = 200, int iterations = 4)
        {
            ContactGenerators = new List<IContactGenerator>(maxcontactgen);
            
            ContactList = new List<PhysicalContact>(maxcontacts);
            for (int i = 0; i < maxcontacts; i++)
                ContactList.Add(new PhysicalContact());
            
            int staticcols = 500;
            ColChecker = new CollisionChecker(bodies, staticcols);
            //RigidBodies = new List<RigidBody>(bodies);
            //StaticCollisions = new List<StaticCollision>(500);

            Resolver = new ContactResolver(iterations);
        }
        
        /// <summary>
        /// Registers new rigid body to physical scene
        /// </summary>
        /// <param name="body">Body to register</param>
        public void RegisterBody(RigidBody body)
        {
            ColChecker.AddRigid(body);
            body.PhysWorld = this;
            if (body.HasStandardGravity &&
                (_Gravity.LengthSquared != 0))
            {                
                body.Acceleration = body.Acceleration+_Gravity;
            }
        }

        /// <summary>
        /// Removes rigid body from register
        /// </summary>
        /// <param name="body"></param>
        public void UnregisterBody(RigidBody body)
        {
            if (!ColChecker.RemoveRigid(body))
                return;
            body.PhysWorld = null;
            RemoveG(body);
        }

        /// <summary>
        /// Registers new static collision object in collision system - might be slow depending on implementation and type of static collision
        /// Note, this should be used only for editors etc - for normal use you should use prebuilt saved static collision
        /// </summary>
        /// <param name="sc">Static collision to register</param>
        public void RegisterStaticCol(StaticCollision sc)
        {
            ColChecker.AddStatic(sc);            
            sc.PhysWorld = this;
        }

        /// <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 UpdateStaticCol(StaticCollision sc)
        {
            ColChecker.UpdateStatic(sc);
        }


        public void UnRegisterStaticCol(StaticCollision sc)
        {
            ColChecker.RemoveStatic(sc);
        }

        /// <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()
        {
            ColChecker.RecalcStaticCollisions();
        }

        /// <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 AddStaticCol(IGeometry geo, ITransformation trans, PhysMaterial mat)
        {
            ColChecker.AddStatic(geo, trans, mat);
        }       

        /// <summary>
        /// gravity was changed
        /// </summary>
        protected void SetGravity(Vector3 newgravity)
        {
            Vector3 change = newgravity - _Gravity;
            foreach (RigidBody rb in RigidBodies)
            {
                if (rb.HasStandardGravity)
                {
                    rb.Acceleration = rb.Acceleration + change;
                }
            }

            _Gravity = newgravity;
        }

        /// <summary>
        /// Registers new contact generator (each registered generator will be polled each frame to provide any contacts)
        /// </summary>
        /// <param name="gen">Contact generator</param>
        public void RegisterContactGenerator(IContactGenerator gen)
        {
            ContactGenerators.Add(gen);
        }

        /// <summary>
        /// Removes contact generator from registered
        /// </summary>
        /// <param name="gen">Contac generator</param>
        public void UnRegisterContactGenerator(IContactGenerator gen)
        {
            if (ContactGenerators.Contains(gen))
                ContactGenerators.Remove(gen);

        }

        /// <summary>
        /// Start physical frame
        /// </summary>
        public void StartFrame()
        {
            foreach (RigidBody rbody in RigidBodies)
            {
                rbody.ClearAccumulator();
                rbody.UpdateValues();
            }
        }

        /// <summary>
        /// Perform timetick
        /// </summary>
        /// <param name="time"></param>
        public void TimeTick(float time)
        {
            //apply forces generators

            foreach (RigidBody rbody in RigidBodies)
            {
                rbody.PhysicalTimeTick(time);
                //rbody.Intergrator(time);
            }

            int usedcontacts = GenerateContacts();

            if (CalculateIterations)
                Resolver.Iterations = usedcontacts * 4;
            Resolver.ResolveContacts(ContactList, usedcontacts, time);
        }

        /// <summary>
        /// Generates contacts - checks collision system and all directly registered contact generators
        /// </summary>
        /// <returns>Number of used contacts</returns>
        public int GenerateContacts()
        {           
            //start with collision checker
            int index = ColChecker.AddContact(ContactList, 0);

            //check registered contact generators
            foreach (IContactGenerator rb in ContactGenerators)
            {
                int rval = rb.AddContact(ContactList, index);
                index += rval;
            }
            return index;
        }

        /// <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 CheckRigidBodyCollision(RigidBody body)
        {
            return ColChecker.Check(body);
        }


        /// <summary>
        /// Removes default gravity from rigid body (used on cleaning);
        /// </summary>
        /// <param name="rb"></param>
        protected void RemoveG(RigidBody rb)
        {
            if (rb.HasStandardGravity)
            {
                rb.Acceleration = rb.Acceleration - _Gravity;
            }
        }

        /// <summary>
        /// Cleans physical scene, unregisters rigidbodies, static cols, cleans lists...
        /// </summary>
        public void Clear()
        {
            foreach (RigidBody rb in RigidBodies)
            {
                rb.PhysWorld = null;
                RemoveG(rb);
            }

            foreach (StaticCollision sc in StaticCollisions)
            {
                sc.PhysWorld = null;
            }

            ColChecker.Clear();

            ContactList.Clear();
            ContactGenerators.Clear();
            // clear Resolver
        }

        #region IDisposable Members

        public void Dispose()
        {
            Clear();
        }

        #endregion
    }
}
