using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Assertions;

namespace GuidewireSim
{
    public class CollisionSolvingStep : MonoBehaviour
    {
        MathHelper mathHelper; //!< The component MathHelper that provides math related helper functions.
        CollisionHandler collisionHandler; //!< The component CollisionHandler that tracks all collisions.

        Vector3 deltaPosition = new Vector3(); //!< The correction of @p spherePositionPrediction in method SolveCollisionConstraint().
        Vector3 initialPositionPrediction = new Vector3();

        float sphereRadius = 5f; //!< The radius of a sphere of the guidewire.
        [SerializeField] float collisionMargin = 0.1f; /**< A margin by which a colliding element of the guidewire is set away from the object
                                                        *   colliding with in the direction of the normal.
                                                        *   @note Without this margin, the colliding element of the guidewire (e.g. a sphere) is
                                                        *   corrected such that its surface exactly touches the object colliding with, which
                                                        *   results in the guidewire still penetrating the object.
                                                        */
        [Range(0.00001f, 1f)] float collisionStiffness = 0.001f; //!< The collision constraint stiffness parameter.

        private void Awake()
        {
            mathHelper = GetComponent<MathHelper>();
            Assert.IsNotNull(mathHelper);

            collisionHandler = GetComponent<CollisionHandler>();
            Assert.IsNotNull(collisionHandler);
        }

        public void SolveCollisionConstraints(Vector3[] spherePositionPredictions, int solverStep, int constraintSolverSteps)
        {
            // Debug.Log("Collision Count " + collisionHandler.registeredCollisions.Count);
            for (int collisionIndex = 0; collisionIndex < collisionHandler.registeredCollisions.Count; collisionIndex++)
            {
                CollisionPair collisionPair = collisionHandler.registeredCollisions[collisionIndex];
                int sphereID = collisionPair.sphereID;
                Vector3 spherePositionPrediction = spherePositionPredictions[sphereID];
                

                // Debug.Log("Contact Count" + collisionPair.vesselCollider.contactCount);
                // Vector3 computedNormal = collisionPair.vesselCollider.GetContact(0).normal;
                // Vector3 lineCenterPointToCenterPoint = spherePositionPrediction - collisionPair.vesselCollider.transform.position;
                
                // Vector3 deviation =  computedNormal - lineCenterPointToCenterPoint.normalized;
                // Debug.Log("deviation" + deviation.ToString("e3"));


                SolveCollisionConstraint(spherePositionPrediction, collisionPair, solverStep, out deltaPosition);
                CorrectCollisionPredictions(sphereID, spherePositionPredictions, solverStep, constraintSolverSteps);
            }

            // Pauses the simulation after the first collision has been resolved.
            // if (collisionHandler.registeredCollisions.Count >= 1)
            // {
            //     Time.timeScale = 0f;
            // }
        }

        // @attention Current calculation of the normal only works for spheres.
        protected virtual void SolveCollisionConstraint(Vector3 spherePositionPrediction, CollisionPair collisionPair, int solverStep,
                                                        out Vector3 deltaPosition)
        {
            // int sphereID = collisionPair.sphereID;

            ContactPoint contactPoint = collisionPair.vesselCollider.GetContact(0);
            Vector3 otherPosition = contactPoint.otherCollider.transform.position;

            // solution one
            // there is a difference between closest point to transform.position and closest point to position prediction.
            // closestSurfacePoint = collisionPair.vesselCollider.collider.ClosestPoint(spherePositionPrediction);
            // normalVector = contactPoint.normal;


            // solution two
            float otherRadius = 15f;
            Vector3 closestSurfacePoint = otherPosition + otherRadius * (spherePositionPrediction - otherPosition).normalized;
            Vector3 normalVector = (spherePositionPrediction - otherPosition).normalized;


            //solution three
            // closestSurfacePoint = contactPoint.point;
            // normalVector = (closestSurfacePoint - otherPosition).normalized;


            if (solverStep == 0)
            {
                DrawCollisionInformation(spherePositionPrediction, contactPoint, otherPosition, closestSurfacePoint, normalVector);
            }

            deltaPosition = CalculateDeltaPosition(spherePositionPrediction, closestSurfacePoint, normalVector);



            // Debug.Log("solveCollConstraint");

            // Vector3 pq = new Vector3();
            // float[,] nnT = new float[3,3];
            // float[,] nnTInverse = new float[3,3];
            // Vector3 nTnnTInverse = new Vector3();
            // float nTnnTInversepq = new float();
            // Vector3 nTnnTInversepqn = new Vector3();
            // Vector3 prnqc = new Vector3();


            // Vector3 bloodVesselCenterPoint = collisionPair.vesselCollider.transform.position;
            // Vector3 closestSurfacePoint2 = otherObject.point;

            // Debug.Log("closestSurfacePoint " + closestSurfacePoint.ToString("e3"));
            // Debug.Log("closestSurfacePoint2 " + closestSurfacePoint2.ToString("e3"));


            // Debug.Log("spherePosition" + collisionPair.sphere.position);
            // Debug.Log("spherePositionPrediction" + spherePositionPrediction);
            // Debug.Log("closestSurfacePoint" + closestSurfacePoint);
            // Debug.Log("bloodVesselCenterPoint" + bloodVesselCenterPoint);


            // if (insideCollision)
            // {
            //     normalVector = (bloodVesselCenterPoint - closestSurfacePoint).normalized;
            // }
            // else
            // {
            //     normalVector = (closestSurfacePoint - bloodVesselCenterPoint).normalized;
            // }


            // Debug.Log("normalVector" + normalVector.ToString("e2"));
            // pq = spherePositionPrediction - closestSurfacePoint;
            // Debug.Log("pq" + pq);

            // nnT = mathHelper.VectorColumnVectorMultiplication(normalVector, normalVector);
            // for (int i = 0; i < 3; i++)
            // {
            //     for (int j = 0; j < 3; j++)
            //     {
            //         Debug.Log("nnT [" + i + "," + j + "] " + nnT[i,j]);
            //     }
            // }
            // nnTInverse = mathHelper.MatrixInverse(nnT);
            // for (int i = 0; i < 3; i++)
            // {
            //     for (int j = 0; j < 3; j++)
            //     {
            //         Debug.Log("nnTInverse [" + i + "," + j + "] " + nnTInverse[i,j]);
            //     }
            // }
            // nTnnTInverse = mathHelper.ColumnVectorMatrixMultiplication(normalVector, nnTInverse);
            // Debug.Log("nTnnTInverse" + nTnnTInverse);
            // nTnnTInversepq = Vector3.Dot(nTnnTInverse, pq);
            // nTnnTInversepqn = nTnnTInversepq * normalVector;

            // deltaPosition = - nTnnTInversepqn;
            // Debug.Log("deltaPosition: " + deltaPosition);

            // deltaPosition = spherePositionPrediction - closestSurfacePoint;
            // Debug.Log("p" + spherePositionPrediction);
            // Debug.Log("q" + closestSurfacePoint);

            // Debug.Log("prnq" + prnq);


            // Vector3 absNormalVector = new Vector3();
            // for (int vectorIndex = 0; vectorIndex < 3; vectorIndex++)
            // {
            //     absNormalVector[vectorIndex] = Mathf.Abs(normalVector[vectorIndex]);
            // }
            // float nTn = Vector3.Dot(absNormalVector, absNormalVector);
            // float pqn = Vector3.Dot(pq, normalVector);
            // Debug.Log("ntn" + nTn);
            // Debug.Log("pqn" + pqn);
            // float factor = - pqn / nTn;
            // Debug.Log("factor" + factor);

            // prnqc = prnq - collisionMargin * normalVector;

            // deltaPosition = factor * normalVector;
        }

        protected void DrawCollisionInformation(Vector3 spherePositionPrediction, ContactPoint contactPoint, Vector3 otherPosition,
                                              Vector3 closestSurfacePoint, Vector3 normalVector)
        {
            Debug.DrawLine(closestSurfacePoint, closestSurfacePoint + 10f * normalVector, Color.blue, 2f);
            // Debug.DrawLine(otherPosition, spherePositionPrediction, Color.yellow, 2f);

            DebugExtension.DrawPoint(spherePositionPrediction, Color.white);

            DebugExtension.DrawPoint(contactPoint.point, Color.black);
            DebugExtension.DrawPoint(closestSurfacePoint, Color.yellow);
        }

        protected Vector3 CalculateDeltaPosition(Vector3 spherePositionPrediction, Vector3 closestSurfacePoint, Vector3 normalVector)
        {
            // return - (spherePositionPrediction - sphereRadius * normalVector - closestSurfacePoint);
            return - (spherePositionPrediction - sphereRadius * normalVector - closestSurfacePoint - collisionMargin * normalVector);
        }

        private void CorrectCollisionPredictions(int sphereIndex, Vector3[] spherePositionPredictions, int solverStep, int constraintSolverSteps)
        {
            Assert.IsTrue(sphereIndex >= 0);

            if (solverStep == 0)
            {
                initialPositionPrediction = spherePositionPredictions[sphereIndex];
            }

            // spherePositionPredictions[sphereIndex] += Mathf.Pow(1 - (1 - collisionStiffness), 1 / 1000f) * deltaPosition;
            spherePositionPredictions[sphereIndex] += collisionStiffness * deltaPosition;
            // spherePositionPredictions[sphereIndex] += deltaPosition;

            if (solverStep == constraintSolverSteps - 1)
            {
                Debug.DrawLine(initialPositionPrediction, spherePositionPredictions[sphereIndex], Color.red, 2f);
                DebugExtension.DrawPoint(spherePositionPredictions[sphereIndex], Color.red);
            }
        }
    }
}