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

namespace VisimBox.Physics
{
    /// <summary>
    /// Contact resolver
    /// </summary>
    public class ContactResolver
    {
        /// <summary>
        /// Quick set of both epsilons to value
        /// </summary>
        public float Epsilon
        {
            set
            {
                VelocityEpsilon = PositionEpsilon = value;
            }
        }

        /// <summary>
        /// Velocity epsilon (smallest value)
        /// </summary>
        public float VelocityEpsilon;
        /// <summary>
        /// Position epsilon (smallest value)
        /// </summary>
        public float PositionEpsilon;

        /// <summary>
        /// Number of iterations used
        /// </summary>
        public int Iterations { 
            get
            {
                return VelocityIterations / 2;
            }
            set
            {
                VelocityIterations = value / 2;
                PositionInterations = value / 2;
            }
        }

        public int VelocityIterations;
        public int PositionInterations;
        public int UsedVelIterations;
        public int UsedPosIterations;


        /// <summary>
        /// Creates new contact resolver
        /// </summary>
        /// <param name="iterations">Iterations limit</param>
        /// <param name="velEps">Epsilon for velocities</param>
        /// <param name="posEps">Epsilon for positions</param>
        public ContactResolver(int iterations, float velEps = 0.001f, float posEps = 0.001f)
        {
            Iterations = iterations;
            VelocityEpsilon = velEps;
            PositionEpsilon = posEps;
        }

        /// <summary>
        /// Resolves list of contacts from first to total
        /// </summary>
        /// <param name="contacts">List of contacts</param>
        /// <param name="total">Total number of valid contacts stored from the start</param>
        /// <param name="time">elapsed time</param>
        public void ResolveContacts(List<PhysicalContact> contacts, int total, float time)
        {
            if (total == 0)
                return;

            //prepare contacts for processing

            for (int i = 0; i < total; i++)
            {
                contacts[i].CalcInternal(time);
            }

            //adjust positions
            ResolvePositions(contacts, total, time);

            //adjust velocities
            ResolveVelocities(contacts, total, time);
        }

        /// <summary>
        /// iteratively resolves velocities of contacts
        /// </summary>
        /// <param name="contacts">List of contacts</param>
        /// <param name="total">Total number of valid contacts stored from the start</param>
        /// <param name="time">elapsed time</param>
        public void ResolveVelocities(List<PhysicalContact> contacts, int total, float time)
        {
            Vector3 vel1, vel2, ang1, ang2;
            vel1 = vel2 = ang1 = ang2 = new Vector3();

            UsedVelIterations = 0;

            while (UsedVelIterations < VelocityIterations)
            {
                //search for the object with biggest velocity change

                int index = -1;
                float max = VelocityEpsilon;

                for (int i = 0; i < total; i++)
                {
                    if (contacts[i].DesiredDeltaV > max)
                    {
                        max = contacts[i].DesiredDeltaV;
                        index = i;
                    }
                }

                if (index == -1)
                    //done, let's move out
                    break;

                //wake contacts

                PhysicalContact pc = contacts[index];

                pc.SetAwake();

                pc.ApplyVelocityChange(ref vel1, ref vel2, ref ang1, ref ang2);

                //check all contacts for cases when body collides with more objects
                for (int i = 0; i < total; i++)
                {
                    PhysicalContact phys = contacts[i];

                    for (int b = 0; b < 2; b++)
                    {
                        if (phys.Bodies[b] != null)
                        {
                            for (int d = 0; d < 2; d++)
                            {
                                if (phys.Bodies[b] == pc.Bodies[d])
                                {
                                    Vector3 deltav;
                                    if (d == 0)
                                    {
                                        deltav = vel1 + ang1.Cross(ref phys.RelativeContactPos[b]);                                        
                                    }
                                    else
                                    {
                                        deltav = vel2 + ang2.Cross(ref phys.RelativeContactPos[b]);                                        
                                    }
                                    phys.ContactToWorld.TransformTranspose(ref deltav);

                                    deltav.Mul(((b != 0) ? -1 : 1));

                                    phys.ContactVelocity += deltav;
                                }
                            }
                        }
                    }
                }
                UsedVelIterations++;
            }
        }

        /// <summary>
        /// Resolves positions of contacts
        /// </summary>
        /// <param name="contacts">List of contacts</param>
        /// <param name="total">Total number of valid contacts stored from the start</param>
        /// <param name="time">elapsed time</param>
        public void ResolvePositions(List<PhysicalContact> contacts, int total, float time)
        {
            Vector3 linear1, linear2, angular1, angular2;
            linear1 =  linear2 = angular1 = angular2 = new Vector3();

            UsedPosIterations = 0;

            while (UsedPosIterations < PositionInterations)
            {
                //search for the object with biggest penetration

                int index = -1;
                float max = PositionEpsilon;

                for (int i = 0; i < total; i++)
                {
                    if (contacts[i].Penetration > max)
                    {
                        max = contacts[i].Penetration;
                        index = i;
                    }
                }

                if (index == -1)
                    //done, let's move out
                    break;

                PhysicalContact pc = contacts[index];

                pc.SetAwake();

                pc.ApplyPositionChange(ref linear1, ref linear2, ref angular1, ref angular2);

                //check all contacts for cases when body collides with more objects
                for (int i = 0; i < total; i++)
                {
                    PhysicalContact phys = contacts[i];

                    for (int b = 0; b < 2; b++)
                    {
                        if (phys.Bodies[b] != null)
                        {
                            for (int d = 0; d < 2; d++)
                            {
                                if (phys.Bodies[b] == pc.Bodies[d])
                                {
                                    Vector3 deltaposition;
                                    Vector3 angchange;
                                    if (d == 0)
                                    {
                                        deltaposition = linear1;
                                        angchange = angular1;
                                    }
                                    else
                                    {
                                        deltaposition = linear2;
                                        angchange = angular2;
                                    }
                                    deltaposition+=angchange.Cross(ref phys.RelativeContactPos[d]);

                                    phys.Penetration += deltaposition.Dot(ref phys.ContactNormal) * ((b == 1) ? 1 : -1);
                                }
                            }
                        }
                    }
                }

                UsedPosIterations++;
            }
        }
    }
}
