AC_KCC_CharacterControllerFixUpdate

using UnityEngine;
using KinematicCharacterController;

namespace AC.Downloads.KCCIntegration
{
    public enum OrientationMethod
    {
        TowardsCamera,
        TowardsMovement,
        TankControls,
    }

    public struct PlayerCharacterInputs
    {
        public float MoveAxisForward;
        public float MoveAxisRight;
        public bool RunHeld;
    }

    public struct AICharacterInputs
    {
        public Vector3 MoveVector;
        public Vector3 LookVector;
        public bool RunHeld;
    }

    public class AC_KCC_CharacterController : MonoBehaviour, ICharacterController
    {
        [SerializeField] private KinematicCharacterMotor motor;
        public KinematicCharacterMotor Motor => motor;

        [Header("Stable Movement")]
        [SerializeField] private float WalkSpeed = 4f;
        [SerializeField] private float RunSpeed = 8f;
        [SerializeField] private float tankTurnSpeed = 3f;
        [SerializeField] private float StableMovementSharpness = 15f;
        [SerializeField] private float OrientationSharpness = 10f;
        [SerializeField] private OrientationMethod OrientationMethod = OrientationMethod.TowardsCamera;

        [Header("Air Movement")]
        [SerializeField] private float maxStepHeight = 2f;
        [SerializeField] private float MaxAirMoveSpeed = 5f;
        [SerializeField] private float AirAccelerationSpeed = 15f;
        [SerializeField] private float Drag = 0.1f;

        [Header("Misc")]
        [SerializeField] private float BonusOrientationSharpness = 10f;
        [SerializeField] private Vector3 Gravity = new Vector3(0, -30f, 0);

        private Vector3 _moveInputVector;
        private Vector3 _lookInputVector;
        private Vector3 _internalVelocityAdd = Vector3.zero;

        // Variables for movement locking logic
        private Vector3 _lockedMoveDirection = Vector3.zero;
        private bool _isMovementHeld = false;
        private Vector2 _previousRawInput = Vector2.zero;
        private Quaternion _previousCameraRotation;

        private bool isRunning;
        private bool isAIControlled;

        private void Awake()
        {
            Motor.CharacterController = this;
            _previousCameraRotation = KickStarter.CameraMainTransform.rotation;
        }

        public void SetInputs(ref PlayerCharacterInputs inputs)
        {
            // Get current raw input from the player (analog or digital values)
            Vector2 currentRawInput = new Vector2(inputs.MoveAxisRight, inputs.MoveAxisForward);
            bool isMoving = currentRawInput.sqrMagnitude > 0f;

            // Get the current camera rotation and compute a planar rotation
            Quaternion cameraRotation = KickStarter.CameraMainTransform.rotation;
            Vector3 cameraPlanarDirection = Vector3.ProjectOnPlane(cameraRotation * Vector3.forward, Motor.CharacterUp).normalized;
            if (cameraPlanarDirection.sqrMagnitude == 0f)
            {
                cameraPlanarDirection = Vector3.ProjectOnPlane(cameraRotation * Vector3.up, Motor.CharacterUp).normalized;
            }
            Quaternion cameraPlanarRotation = Quaternion.LookRotation(cameraPlanarDirection, Motor.CharacterUp);

            // Compute the new raw move direction in world space based on current camera orientation
            Vector3 newRawInput = cameraPlanarRotation * Vector3.ClampMagnitude(new Vector3(inputs.MoveAxisRight, 0f, inputs.MoveAxisForward), 1f);

            // If the player is moving, update the locked direction if input has changed
            if (isMoving)
            {
                if (!_isMovementHeld)
                {
                    _lockedMoveDirection = newRawInput;
                    _isMovementHeld = true;
                }
                else
                {
                    // If input changes beyond a small threshold, update the locked direction immediately
                    if ((currentRawInput - _previousRawInput).sqrMagnitude > 0.001f)
                    {
                        _lockedMoveDirection = newRawInput;
                    }
                }
            }
            else
            {
                _isMovementHeld = false;
                _lockedMoveDirection = Vector3.zero;
            }

            // Use the locked movement direction if movement is held, otherwise use the new input
            _moveInputVector = _isMovementHeld ? _lockedMoveDirection : newRawInput;

            // Determine look direction based on the chosen orientation method
            switch (OrientationMethod)
            {
                case OrientationMethod.TowardsCamera:
                    _lookInputVector = cameraPlanarDirection;
                    break;
                case OrientationMethod.TowardsMovement:
                    _lookInputVector = _moveInputVector.normalized;
                    break;
                default:
                    break;
            }

            isRunning = inputs.RunHeld;
            isAIControlled = false;

            // Store current raw input and camera rotation for next frame comparisons.
            _previousRawInput = currentRawInput;
            _previousCameraRotation = cameraRotation;
        }

        public void SetInputs(ref AICharacterInputs inputs)
        {
            _moveInputVector = inputs.MoveVector;
            _lookInputVector = inputs.LookVector;
            isRunning = inputs.RunHeld;
            isAIControlled = true;
        }

        public void BeforeCharacterUpdate(float deltaTime) { }

        public void UpdateRotation(ref Quaternion currentRotation, float deltaTime)
        {
            if (_lookInputVector.sqrMagnitude > 0f && OrientationSharpness > 0f)
            {
                // Compute the desired target rotation
                Quaternion targetRotation = Quaternion.LookRotation(_lookInputVector, Motor.CharacterUp);
                // Rotate towards target using the shortest path.
                // The multiplier (90f) can be adjusted to control turning speed.
                float maxDegreesDelta = OrientationSharpness * deltaTime * 90f;
                currentRotation = Quaternion.RotateTowards(currentRotation, targetRotation, maxDegreesDelta);
            }

            Vector3 currentUp = currentRotation * Vector3.up;
            Vector3 smoothedGravityDir = Vector3.Slerp(currentUp, Vector3.up, 1 - Mathf.Exp(-BonusOrientationSharpness * deltaTime));
            currentRotation = Quaternion.FromToRotation(currentUp, smoothedGravityDir) * currentRotation;
        }

        public void UpdateVelocity(ref Vector3 currentVelocity, float deltaTime)
        {
            // Ground movement
            if (Motor.GroundingStatus.IsStableOnGround)
            {
                float currentVelocityMagnitude = currentVelocity.magnitude;
                Vector3 effectiveGroundNormal = Motor.GroundingStatus.GroundNormal;

                currentVelocity = Motor.GetDirectionTangentToSurface(currentVelocity, effectiveGroundNormal) * currentVelocityMagnitude;
                Vector3 inputRight = Vector3.Cross(_moveInputVector, Motor.CharacterUp);
                Vector3 reorientedInput = Vector3.Cross(effectiveGroundNormal, inputRight).normalized * _moveInputVector.magnitude;

                float speed = isRunning ? RunSpeed : WalkSpeed;
                Vector3 targetMovementVelocity = reorientedInput * speed;

                currentVelocity = Vector3.Lerp(currentVelocity, targetMovementVelocity, 1f - Mathf.Exp(-StableMovementSharpness * deltaTime));
            }
            // Air movement
            else
            {
                if (_moveInputVector.sqrMagnitude > 0f)
                {
                    Vector3 addedVelocity = _moveInputVector * AirAccelerationSpeed * deltaTime;
                    Vector3 currentVelocityOnInputsPlane = Vector3.ProjectOnPlane(currentVelocity, Motor.CharacterUp);

                    if (currentVelocityOnInputsPlane.magnitude < MaxAirMoveSpeed)
                    {
                        Vector3 newTotal = Vector3.ClampMagnitude(currentVelocityOnInputsPlane + addedVelocity, MaxAirMoveSpeed);
                        addedVelocity = newTotal - currentVelocityOnInputsPlane;
                    }
                    else
                    {
                        if (Vector3.Dot(currentVelocityOnInputsPlane, addedVelocity) > 0f)
                        {
                            addedVelocity = Vector3.ProjectOnPlane(addedVelocity, currentVelocityOnInputsPlane.normalized);
                        }
                    }

                    if (Motor.GroundingStatus.FoundAnyGround)
                    {
                        if (Vector3.Dot(currentVelocity + addedVelocity, addedVelocity) > 0f)
                        {
                            Vector3 perpenticularObstructionNormal = Vector3.Cross(Vector3.Cross(Motor.CharacterUp, Motor.GroundingStatus.GroundNormal), Motor.CharacterUp).normalized;
                            addedVelocity = Vector3.ProjectOnPlane(addedVelocity, perpenticularObstructionNormal);
                        }
                    }

                    currentVelocity += addedVelocity;
                }

                currentVelocity += Gravity * deltaTime;
                currentVelocity *= (1f / (1f + (Drag * deltaTime)));
            }

            if (_internalVelocityAdd.sqrMagnitude > 0f)
            {
                currentVelocity += _internalVelocityAdd;
                _internalVelocityAdd = Vector3.zero;
            }

            Vector3 nextPosition = Motor.TransientPosition + (currentVelocity * Time.deltaTime);
            bool nextPositionStable = Physics.Raycast(nextPosition + Vector3.up, Vector3.down, 1f + maxStepHeight);
            if (!nextPositionStable)
            {
                if (Physics.Raycast(nextPosition + Vector3.down * maxStepHeight, -currentVelocity, out RaycastHit hitInfo, 1f))
                {
                    currentVelocity = Vector3.Reflect(currentVelocity, -hitInfo.normal);
                }
                else
                {
                    currentVelocity = new Vector3(0f, currentVelocity.y, 0f);
                }
            }
        }

        public void AfterCharacterUpdate(float deltaTime) { }

        public void PostGroundingUpdate(float deltaTime)
        {
            if (Motor.GroundingStatus.IsStableOnGround && !Motor.LastGroundingStatus.IsStableOnGround)
            {
                OnLanded();
            }
            else if (!Motor.GroundingStatus.IsStableOnGround && Motor.LastGroundingStatus.IsStableOnGround)
            {
                OnLeaveStableGround();
            }
        }

        public bool IsColliderValidForCollisions(Collider coll) => true;
        public void OnGroundHit(Collider hitCollider, Vector3 hitNormal, Vector3 hitPoint, ref HitStabilityReport hitStabilityReport) { }
        public void OnMovementHit(Collider hitCollider, Vector3 hitNormal, Vector3 hitPoint, ref HitStabilityReport hitStabilityReport) { }
        public void AddVelocity(Vector3 velocity) => _internalVelocityAdd += velocity;
        public void ProcessHitStabilityReport(Collider hitCollider, Vector3 hitNormal, Vector3 hitPoint, Vector3 atCharacterPosition, Quaternion atCharacterRotation, ref HitStabilityReport hitStabilityReport) { }
        protected void OnLanded() { }
        protected void OnLeaveStableGround() { }
        public void OnDiscreteCollisionDetected(Collider hitCollider) { }
    }
}