using Syntriax.Engine.Core;
using Syntriax.Engine.Physics2D.Abstract;

namespace Syntriax.Engine.Physics2D;

public class CollisionResolver2D : ICollisionResolver2D
{
    public void Resolve(CollisionDetectionInformation collisionInformation)
    {
        Vector2D displacementVector = collisionInformation.Normal * collisionInformation.Penetration;

        ICollider2D left = collisionInformation.Left;
        ICollider2D right = collisionInformation.Right;

        bool isLeftStatic = left.RigidBody2D?.IsStatic ?? true;
        bool isRightStatic = right.RigidBody2D?.IsStatic ?? true;

        if (isLeftStatic && isRightStatic)
            return;

        if (isLeftStatic)
            right.Transform.Position += displacementVector;
        else if (isRightStatic)
            left.Transform.Position -= displacementVector;
        else
        {
            float leftMass = left.RigidBody2D?.Mass ?? float.Epsilon;
            float rightMass = right.RigidBody2D?.Mass ?? float.Epsilon;
            float sumMass = leftMass + rightMass;

            float leftMomentumPercentage = leftMass / sumMass;
            float rightMomentumPercentage = rightMass / sumMass;

            right.Transform.Position += leftMomentumPercentage * displacementVector;
            left.Transform.Position -= rightMomentumPercentage * displacementVector;
        }

        left.Recalculate();
        right.Recalculate();

        left.OnCollisionResolved?.Invoke(collisionInformation.Left, collisionInformation);
        right.OnCollisionResolved?.Invoke(right, collisionInformation);
    }
}