using UnityEngine;
using Unity.Mathematics; 

namespace RebootKit.FPPKit {
    public class FPPLocomotion : MonoBehaviour {
        [SerializeField] CharacterController m_CharacterController;

        public float maxStrafeSpeed = 2.0f;
        public float maxMovementSpeedBackward = 2.0f;
        public float maxMovementSpeed = 4.0f;
        public float maxSprintSpeed = 15.0f;
        public float jumpHeight = 1.0f;
        public float gravity = 10f;
        public float maxFallSpeed = 20f;
        public float damping = 20.0f;
        
        [Range(0.0f, 1.0f)]
        public float airControlRatio = 0.5f;
        
        float3 m_CurrentVelocity; 
        bool m_IsFalling;

        bool m_IsSprinting;
        bool m_JumpRequested;

        float3 m_PendingInputValue; 

        public bool IsGrounded => m_CharacterController.isGrounded;
        public bool IsMovingBackwards { get; private set; }
        public bool IsSprinting => m_IsSprinting;
        public bool IsStrafing { get; private set; }

        public float3 Velocity => m_CurrentVelocity;
        public float3 LocalVelocity => m_CharacterController.transform.InverseTransformDirection(m_CurrentVelocity);

        void Update() {
            ConsumePendingInput();
            UpdateVerticalVelocity();

            m_CharacterController.Move(m_CurrentVelocity * Time.deltaTime);

            ApplyFriction();
            DetectFall();
        }

        void DetectFall() {
            if (m_IsFalling && m_CharacterController.isGrounded) {
                m_IsFalling = false;
            } else if (!m_CharacterController.isGrounded) {
                m_IsFalling = true;
            }
        }

        void ConsumePendingInput() {
            if (!IsGrounded) {
                m_PendingInputValue *= airControlRatio;
            }

            m_PendingInputValue.y = 0.0f;
            
            float3 localInputValue = m_CharacterController.transform.InverseTransformDirection(m_PendingInputValue);

            float pendingInputMagnitude = math.length(m_PendingInputValue); 
            float3 direction = float3.zero; 

            if (pendingInputMagnitude > 0.0f) {
                direction = math.normalize(m_PendingInputValue); 
            }

            IsStrafing = false;
            IsMovingBackwards = false;

            float movementSpeed;
            if (math.dot(localInputValue, Vector3.forward) <= 0.0f) {
                movementSpeed = maxMovementSpeedBackward;
                IsMovingBackwards = true;
            } else if (m_IsSprinting) {
                movementSpeed = maxSprintSpeed;
            } else if (math.abs(localInputValue.x) > 0.0f && math.abs(localInputValue.z) < 0.1f) {
                movementSpeed = maxStrafeSpeed;
            } else {
                movementSpeed = maxMovementSpeed;
            }
            
            float3 movementVelocity = m_CurrentVelocity; 
            movementVelocity.y = 0.0f;
            movementVelocity += direction * (movementSpeed * pendingInputMagnitude);
            movementVelocity.y = 0.0f;

            float movementVelocityMagnitude = math.length(movementVelocity); 
            float3 movementVelocityDirection = movementVelocityMagnitude > 0.0f ? math.normalize(movementVelocity) : float3.zero; 

            if (movementVelocityMagnitude > movementSpeed) {
                movementVelocityMagnitude = movementSpeed;
            }

            movementVelocity = movementVelocityDirection * movementVelocityMagnitude;

            m_CurrentVelocity.x = movementVelocity.x;
            m_CurrentVelocity.z = movementVelocity.z;

            m_PendingInputValue = float3.zero; 
        }

        void UpdateVerticalVelocity() {
            if (m_CharacterController.isGrounded) {
                if (m_JumpRequested) {
                    m_CurrentVelocity.y = math.sqrt(2.0f * gravity * jumpHeight); 
                    m_JumpRequested = false;
                } else {
                    m_CurrentVelocity.y = -1f;
                }
            } else {
                m_CurrentVelocity.y -= gravity * Time.deltaTime;
                m_CurrentVelocity.y = math.max(m_CurrentVelocity.y, -maxFallSpeed);
            }
        }

        void ApplyFriction() {
            if (!IsGrounded) {
                return;
            }

            float3 movementVelocity = m_CurrentVelocity;  
            movementVelocity.y = 0.0f;

            movementVelocity = math.lerp(movementVelocity, float3.zero, damping * Time.deltaTime);

            m_CurrentVelocity.x = movementVelocity.x;
            m_CurrentVelocity.z = movementVelocity.z;
        }

        public void AddVelocity(float3 velocity) { 
            m_CurrentVelocity += velocity;
        }

        public void AddMovementInput(float3 input, float scale) { 
            m_PendingInputValue += input * scale;
        }

        public void Jump() {
            if (!m_CharacterController.isGrounded) {
                return;
            }

            m_JumpRequested = true;
        }

        public void StartSprint() {
            m_IsSprinting = true;
        }

        public void StopSprint() {
            m_IsSprinting = false;
        }
    }
}