using UnityEngine;

namespace KindRetargeting
{
    /// <summary>
    /// HIK 스타일 2-Pass 접지 시스템.
    ///
    /// Pass 1 (Update, Order 5 → IK 전):
    ///   IK 타겟 위치를 수정하여 Toes가 바닥을 뚫지 않도록 보정.
    ///   Toe Pivot 감지: 발끝이 바닥에 있고 발목이 올라가면
    ///   발목 타겟을 역산하여 Toes가 groundHeight에 고정.
    ///
    /// Pass 2 (LateUpdate → IK 후):
    ///   IK 결과의 잔차를 Foot 회전으로 미세 보정.
    ///   위치 변경 없음 — 본 길이 보존.
    ///
    /// 힙 높이 보정은 CRS의 floorHeight가 담당합니다 (이중 보정 방지).
    /// </summary>
    [DefaultExecutionOrder(5)]
    public class FootGroundingController : MonoBehaviour
    {
        [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;

        // Toes의 Foot 로컬 오프셋 (T-pose에서 캐싱)
        private Vector3 leftLocalToesOffset;
        private Vector3 rightLocalToesOffset;

        // flat 상태에서 발목 최소 높이 (Foot.y - Toes.y)
        private float leftFootHeight;
        private float rightFootHeight;

        // Toes 본 존재 여부
        private bool leftHasToes;
        private bool rightHasToes;

        // 스무딩용: 이전 프레임 보정량
        private float leftPrevAdj;
        private float rightPrevAdj;

        private bool isInitialized;

        private void Start()
        {
            ikSolver = GetComponent<TwoBoneIKSolver>();
            animator = GetComponent<Animator>();

            if (animator == null || !animator.isHuman || ikSolver == null) return;
            if (leftFoot == null && rightFoot == 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;

            // Toes 존재 여부 + 캐싱
            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; // Toes 없을 때 기본 발목 높이
            }

            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>
        private void Update()
        {
            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));
        }

        /// <summary>
        /// 발 IK 타겟을 접지 모드에 따라 보정합니다.
        /// Toes가 없는 아바타는 발목 Y 클램프만 수행합니다.
        /// </summary>
        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;

            // AIRBORNE 체크
            if (ankleY - groundHeight > activationHeight) return 0f;

            float weight = groundingWeight * ikWeight;

            // === Toes 없는 아바타: 단순 Y 클램프 ===
            if (!hasToes)
            {
                float minAnkleY = groundHeight + footHeight;
                if (ankleY < minAnkleY)
                {
                    float adj = (minAnkleY - ankleY) * weight;
                    anklePos.y += adj;
                    ankleTarget.position = anklePos;
                    return adj;
                }
                return 0f;
            }

            // === Toes 있는 아바타: 예측 기반 보정 ===
            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)
                {
                    // PLANTED: 발 전체가 바닥 근처
                    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
                {
                    // TOE_PIVOT: 발끝 고정, 발목 올라감
                    if (toesError > 0f)
                    {
                        adjustment = toesError * weight;
                        anklePos.y += adjustment;
                    }
                }

                ankleTarget.position = anklePos;
            }
            else
            {
                // Toes 충분히 위 → 발목만 바닥 아래 방지
                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>
        private void LateUpdate()
        {
            if (!isInitialized || groundingWeight < 0.001f) return;

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

        /// <summary>
        /// IK 후 실제 Toes 위치를 확인하고, Foot 본을 pitch 회전하여 잔차 보정.
        /// 바닥 아래로 뚫린 경우만 보정합니다.
        /// </summary>
        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);
        }
    }
}