using Intrepid.Utilities;
using UnityEngine;

namespace Intrepid.Core.Entities
{
    public class EntityMotorPhysicsContact : MonoBehaviour
    {
        private const int PASSES = 2;
        private const float DUPLICATED_NORMAL_DOT = 0.98f;
        private const float MIN_WALL_NORMAL_SQR_MAGNITUDE = 0.0001f;

        [GroupConfigurations, SerializeField] private int maxContactNormals = 16;
        [GroupConfigurations, SerializeField] private float minImpactDot = 0.7f;

        private Vector3[] ContactNormals { get; set; }
        private int ContactNormalCount { get; set; }

        private int MaxContactNormals => maxContactNormals;

        private void Awake()
        {
            ContactNormals = new Vector3[MaxContactNormals];
        }

        public void RegisterCollisionContacts(Collision collision, float maxSlopeAngle)
        {
            for (var i = 0; i < collision.contactCount; i++)
            {
                if (!TryGetBlockingWallNormal(
                        collision.GetContact(i).normal,
                        maxSlopeAngle,
                        out var wallNormal))
                {
                    continue;
                }

                AddContactNormal(wallNormal);
            }
        }

        public void ResetContactCounter()
        {
            ContactNormalCount = 0;
        }

        public Vector3 AdjustVelocityByContacts(Vector2 desiredVelocity)
        {
            var horizontalVelocity = new Vector3(
                desiredVelocity.x,
                0f,
                desiredVelocity.y
            );

            return SubtractBlockedVelocity(horizontalVelocity);
        }

        public bool HasImpactAgainstRegisteredContacts(
            Vector3 velocity,
            float minSpeed,
            out Vector3 bounceDirection)
        {
            bounceDirection = Vector3.zero;

            velocity.y = 0f;

            if (velocity.sqrMagnitude < minSpeed * minSpeed)
            {
                return false;
            }

            if (ContactNormalCount <= 0)
            {
                return false;
            }

            var direction = velocity.normalized;

            var bestImpactDot = minImpactDot;
            var bestNormal = Vector3.zero;

            for (var i = 0; i < ContactNormalCount; i++)
            {
                var contactNormal = ContactNormals[i];

                var impactDot = Vector3.Dot(direction, -contactNormal);

                if (impactDot < bestImpactDot)
                {
                    continue;
                }

                bestImpactDot = impactDot;
                bestNormal = contactNormal;
            }

            if (bestNormal == Vector3.zero)
            {
                return false;
            }

            bounceDirection = Vector3.Reflect(direction, bestNormal);
            bounceDirection.y = 0f;

            if (bounceDirection.sqrMagnitude <= 0.0001f)
            {
                bounceDirection = bestNormal;
            }
            else
            {
                bounceDirection.Normalize();
            }

            return true;
        }

        private static bool TryGetBlockingWallNormal(
            Vector3 contactNormal,
            float maxSlopeAngle,
            out Vector3 wallNormal)
        {
            wallNormal = default;

            // Floor or walkable slope.
            if (Vector3.Angle(contactNormal, Vector3.up) <= maxSlopeAngle)
            {
                return false;
            }

            // Ceiling.
            if (contactNormal.y < -0.01f)
            {
                return false;
            }

            // Wall blocking only on XZ.
            contactNormal.y = 0f;

            if (contactNormal.sqrMagnitude <= MIN_WALL_NORMAL_SQR_MAGNITUDE)
            {
                return false;
            }

            wallNormal = contactNormal.normalized;
            return true;
        }

        private void AddContactNormal(Vector3 normal)
        {
            for (var i = 0; i < ContactNormalCount; i++)
            {
                if (Vector3.Dot(ContactNormals[i], normal) > DUPLICATED_NORMAL_DOT)
                {
                    return;
                }
            }

            if (ContactNormalCount >= MaxContactNormals)
            {
                return;
            }

            ContactNormals[ContactNormalCount] = normal;
            ContactNormalCount++;
        }

        private Vector3 SubtractBlockedVelocity(Vector3 velocity)
        {
            for (var pass = 0; pass < PASSES; pass++)
            {
                for (var i = 0; i < ContactNormalCount; i++)
                {
                    var normal = ContactNormals[i];
                    var intoWall = Vector3.Dot(velocity, normal);

                    if (intoWall < 0f)
                    {
                        velocity -= normal * intoWall;
                    }
                }
            }

            return velocity;
        }
    }
}