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

namespace GuidewireSim
{
    public class CollisionSolvingStep_UnityVariant : CollisionSolvingStep
    {
        protected override void SolveCollisionConstraint(Vector3 spherePositionPrediction, CollisionPair collisionPair, int solverStep,
                                                         out Vector3 deltaPosition)
        {
            ContactPoint contactPoint = collisionPair.vesselCollider.GetContact(0);
            Vector3 otherPosition = contactPoint.otherCollider.transform.position;

            Vector3 closestSurfacePoint = contactPoint.point;
            // Vector3 normalVector = (closestSurfacePoint - otherPosition).normalized;
            Vector3 normalVector = contactPoint.normal;
            // Debug.Log(normalVector.ToString("e2"));
            // Debug.Log(closestSurfacePoint.ToString("e5"));
            // Debug.Log((40-Mathf.Sqrt(50)  - closestSurfacePoint[2]).ToString("e5"));

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

            deltaPosition = CalculateDeltaPosition(spherePositionPrediction, closestSurfacePoint, normalVector);

            // Vector3 correction = spherePositionPrediction + 0.001f * deltaPosition;
            // float difference = Vector3.Distance(spherePositionPrediction, correction);
            // Debug.Log(difference.ToString("e3"));
        }
    }
}