using UnityEngine;

namespace KindRetargeting
{
    /// <summary>
    /// HIK 스타일 2-Pass 접지 시스템.
    ///
    /// Pass 1 (OnUpdate, Order 5 → IK 전):
    ///   IK 타겟 위치를 수정하여 Toes가 바닥을 뚫지 않도록 보정.
    ///
    /// Pass 2 (OnLateUpdate → IK 후):
    ///   IK 결과의 잔차를 Foot 회전으로 미세 보정.
    /// </summary>
    [System.Serializable]
    public class FootGroundingController
    {
        [Header("접지 설정")]
        [Tooltip("바닥 Y 좌표 (월드 공간)")]
        public float groundHeight = 0f;

        [Tooltip("접지 보정 강도")]
        [Range(0f, 1f)]
        public float groundingWeight = 1f;

        [Tooltip("이 높이 이상이면 AIRBORNE (보정 안 함)")]
        public float activationHeight = 0.5f;

        [Tooltip("Toes가 이 범위 안이면 접지 중으로 판정")]
        public float plantThreshold = 0.02f;

        [Header("스무딩")]
        [Tooltip("보정량 변화 속도 (높을수록 빠른 반응)")]
        [Range(1f, 30f)]
        public float smoothSpeed = 10f;

        private TwoBoneIKSolver ikSolver;
        private Animator animator;

        private Transform leftFoot;
        private Transform rightFoot;
        private Transform leftToes;
        private Transform rightToes;

        private Vector3 leftLocalToesOffset;
        private Vector3 rightLocalToesOffset;

        private float leftFootHeight;
        private float rightFootHeight;

        private bool leftHasToes;
        private bool rightHasToes;

        private float leftPrevAdj;
        private float rightPrevAdj;

        private bool isInitialized;

        public void Initialize(TwoBoneIKSolver ikSolver, Animator animator)
        {
            this.ikSolver = ikSolver;
            this.animator = animator;

            if (animator == null || !animator.isHuman || ikSolver == null) return;

            leftFoot = animator.GetBoneTransform(HumanBodyBones.LeftFoot);
            rightFoot = animator.GetBoneTransform(HumanBodyBones.RightFoot);
            leftToes = animator.GetBoneTransform(HumanBodyBones.LeftToes);
            rightToes = animator.GetBoneTransform(HumanBodyBones.RightToes);

            if (leftFoot == null || rightFoot == null) return;

            leftHasToes = leftToes != null;
            rightHasToes = rightToes != null;

            if (leftHasToes)
            {
                leftLocalToesOffset = leftFoot.InverseTransformPoint(leftToes.position);
                leftFootHeight = Mathf.Abs(leftFoot.position.y - leftToes.position.y);
            }
            else
            {
                leftFootHeight = 0.05f;
            }

            if (rightHasToes)
            {
                rightLocalToesOffset = rightFoot.InverseTransformPoint(rightToes.position);
                rightFootHeight = Mathf.Abs(rightFoot.position.y - rightToes.position.y);
            }
            else
            {
                rightFootHeight = 0.05f;
            }

            isInitialized = true;
        }

        /// <summary>
        /// Pass 1: Pre-IK — IK 타겟 위치를 수정합니다.
        /// </summary>
        public void OnUpdate()
        {
            if (!isInitialized || groundingWeight < 0.001f) return;

            float leftAdj = AdjustFootTarget(
                ikSolver.leftLeg, leftLocalToesOffset, leftFootHeight,
                leftHasToes, ikSolver.leftLeg.positionWeight);
            float rightAdj = AdjustFootTarget(
                ikSolver.rightLeg, rightLocalToesOffset, rightFootHeight,
                rightHasToes, ikSolver.rightLeg.positionWeight);

            float dt = Time.deltaTime * smoothSpeed;
            leftPrevAdj = Mathf.Lerp(leftPrevAdj, leftAdj, Mathf.Clamp01(dt));
            rightPrevAdj = Mathf.Lerp(rightPrevAdj, rightAdj, Mathf.Clamp01(dt));
        }

        private float AdjustFootTarget(TwoBoneIKSolver.LimbIK limb, Vector3 localToesOffset,
            float footHeight, bool hasToes, float ikWeight)
        {
            if (limb.target == null || ikWeight < 0.01f) return 0f;

            Transform ankleTarget = limb.target;
            Vector3 anklePos = ankleTarget.position;
            float ankleY = anklePos.y;

            if (ankleY - groundHeight > activationHeight) return 0f;

            float weight = groundingWeight * ikWeight;

            if (!hasToes)
            {
                float minAnkleY = groundHeight + footHeight;
                if (ankleY < minAnkleY)
                {
                    float adj = (minAnkleY - ankleY) * weight;
                    anklePos.y += adj;
                    ankleTarget.position = anklePos;
                    return adj;
                }
                return 0f;
            }

            Vector3 predictedToesWorld = anklePos + ankleTarget.rotation * localToesOffset;
            float predictedToesY = predictedToesWorld.y;

            float adjustment = 0f;

            if (predictedToesY < groundHeight + plantThreshold)
            {
                float toesError = groundHeight - predictedToesY;

                if (ankleY < groundHeight + footHeight + plantThreshold)
                {
                    float minAnkleY = groundHeight + footHeight;
                    if (ankleY < minAnkleY)
                    {
                        adjustment = (minAnkleY - ankleY) * weight;
                        anklePos.y += adjustment;
                    }

                    if (toesError > 0f)
                    {
                        float extra = toesError * weight;
                        anklePos.y += extra;
                        adjustment += extra;
                    }
                }
                else
                {
                    if (toesError > 0f)
                    {
                        adjustment = toesError * weight;
                        anklePos.y += adjustment;
                    }
                }

                ankleTarget.position = anklePos;
            }
            else
            {
                float minAnkleY = groundHeight + footHeight;
                if (ankleY < minAnkleY)
                {
                    adjustment = (minAnkleY - ankleY) * weight;
                    anklePos.y += adjustment;
                    ankleTarget.position = anklePos;
                }
            }

            return adjustment;
        }

        /// <summary>
        /// Pass 2: Post-IK — 잔차를 Foot 회전으로 미세 보정합니다.
        /// </summary>
        public void OnLateUpdate()
        {
            if (!isInitialized || groundingWeight < 0.001f) return;

            if (leftHasToes)
                AlignFootToGround(leftFoot, leftToes, ikSolver.leftLeg.positionWeight);
            if (rightHasToes)
                AlignFootToGround(rightFoot, rightToes, ikSolver.rightLeg.positionWeight);
        }

        private void AlignFootToGround(Transform foot, Transform toes, float ikWeight)
        {
            if (foot == null || toes == null) return;
            if (ikWeight < 0.01f) return;

            if (foot.position.y - groundHeight > activationHeight) return;

            float weight = groundingWeight * ikWeight;

            float actualToesY = toes.position.y;
            float error = actualToesY - groundHeight;

            if (Mathf.Abs(error) < 0.001f) return;

            if (error > plantThreshold) return;

            Vector3 footToToes = toes.position - foot.position;
            float horizontalDist = new Vector2(footToToes.x, footToToes.z).magnitude;

            if (horizontalDist < 0.001f) return;

            float pitchAngle = Mathf.Atan2(error, horizontalDist) * Mathf.Rad2Deg;

            Vector3 footForwardFlat = new Vector3(footToToes.x, 0f, footToToes.z).normalized;
            Vector3 pitchAxis = Vector3.Cross(Vector3.up, footForwardFlat);
            if (pitchAxis.sqrMagnitude < 0.001f) return;
            pitchAxis.Normalize();

            Quaternion correction = Quaternion.AngleAxis(-pitchAngle, pitchAxis);
            foot.rotation = Quaternion.Slerp(foot.rotation, correction * foot.rotation, weight);
        }
    }
}