using Engine.Core;

namespace Engine.Physics2D;

public class CollisionResolver2D : ICollisionResolver2D
{
    public void Resolve(CollisionDetectionInformation collisionInformation)
    {
        ICollider2D left = collisionInformation.Detector;
        ICollider2D right = collisionInformation.Detected;

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

        if (isLeftStatic && isRightStatic)
            return;

        Displace(collisionInformation, left, right, isLeftStatic, isRightStatic);
        Bounce(collisionInformation, left, right, isLeftStatic, isRightStatic);

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

        left.Resolve(collisionInformation);
        right.Resolve(collisionInformation);
    }

    private static void Displace(CollisionDetectionInformation collisionInformation, ICollider2D left, ICollider2D right, bool isLeftStatic, bool isRightStatic)
    {
        Vector2D displacementVector = collisionInformation.Normal * collisionInformation.Penetration;

        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;
        }
    }

    private static void Bounce(CollisionDetectionInformation collisionInformation, ICollider2D left, ICollider2D right, bool isLeftStatic, bool isRightStatic)
    {
        Vector2D leftVelocity = left.RigidBody2D?.Velocity ?? Vector2D.Zero;
        Vector2D rightVelocity = right.RigidBody2D?.Velocity ?? Vector2D.Zero;

        Vector2D relativeVelocity = leftVelocity - rightVelocity;
        float velocityAlongNormal = Vector2D.Dot(relativeVelocity, collisionInformation.Normal);

        if (velocityAlongNormal > 0)
        {
            collisionInformation = collisionInformation.Reverse();
            velocityAlongNormal = -velocityAlongNormal;
        }

        float e = (left.RigidBody2D?.Material.Restitution ?? 0f).Add(right.RigidBody2D?.Material.Restitution ?? 0f).Divide(2f);

        float leftMassEffective = isLeftStatic ? float.PositiveInfinity : left.RigidBody2D?.Mass ?? float.Epsilon;
        float rightMassEffective = isRightStatic ? float.PositiveInfinity : right.RigidBody2D?.Mass ?? float.Epsilon;
        float impulse = -(1f + e) * velocityAlongNormal / ((1f / leftMassEffective) + (1f / rightMassEffective));

        if (!isLeftStatic)
            left.RigidBody2D?.Velocity += impulse / leftMassEffective * collisionInformation.Normal;

        if (!isRightStatic)
            right.RigidBody2D?.Velocity -= impulse / rightMassEffective * collisionInformation.Normal;
    }
}