using UnityEngine;

namespace KindRetargeting
{
    /// <summary>
    /// 커스텀 Two-Bone IK 솔버.
    /// 소스 무릎 위치를 비율 스케일하여 타겟 무릎을 직접 배치합니다.
    /// cosine law 없이 동작하므로 180° 특이점이 없고 역관절도 자연스럽게 지원합니다.
    /// </summary>
    [System.Serializable]
    public class TwoBoneIKSolver
    {
        [System.Serializable]
        public class LimbIK
        {
            public bool enabled = true;
            public Transform target;
            public Transform bendGoal;

            [Range(0f, 1f)] public float positionWeight = 0f;
            [Range(0f, 1f)] public float rotationWeight = 0f;
            [Range(0f, 1f)] public float bendGoalWeight = 0f;

            [HideInInspector] public Transform upper;
            [HideInInspector] public Transform lower;
            [HideInInspector] public Transform end;
            [HideInInspector] public float upperLength;
            [HideInInspector] public float lowerLength;

            [HideInInspector] public Vector3 localBendNormal;

            // 소스 본 참조 (소스 무릎 위치 기반 IK용)
            [HideInInspector] public Transform sourceUpper;
            [HideInInspector] public Transform sourceLower;
            [HideInInspector] public Transform sourceEnd;
        }

        [HideInInspector] public Animator animator;

        [Header("팔")]
        public LimbIK leftArm = new LimbIK();
        public LimbIK rightArm = new LimbIK();

        [Header("다리")]
        public LimbIK leftLeg = new LimbIK();
        public LimbIK rightLeg = new LimbIK();

        private bool isInitialized;

        public void Initialize(Animator targetAnimator)
        {
            animator = targetAnimator;
            if (animator == null || !animator.isHuman) return;

            CacheLimb(leftArm, HumanBodyBones.LeftUpperArm, HumanBodyBones.LeftLowerArm, HumanBodyBones.LeftHand);
            CacheLimb(rightArm, HumanBodyBones.RightUpperArm, HumanBodyBones.RightLowerArm, HumanBodyBones.RightHand);
            CacheLimb(leftLeg, HumanBodyBones.LeftUpperLeg, HumanBodyBones.LeftLowerLeg, HumanBodyBones.LeftFoot);
            CacheLimb(rightLeg, HumanBodyBones.RightUpperLeg, HumanBodyBones.RightLowerLeg, HumanBodyBones.RightFoot);

            isInitialized = true;
        }

        private void CacheLimb(LimbIK limb, HumanBodyBones upperBone, HumanBodyBones lowerBone, HumanBodyBones endBone)
        {
            limb.upper = animator.GetBoneTransform(upperBone);
            limb.lower = animator.GetBoneTransform(lowerBone);
            limb.end = animator.GetBoneTransform(endBone);

            if (limb.upper == null || limb.lower == null || limb.end == null) return;

            limb.upperLength = Vector3.Distance(limb.upper.position, limb.lower.position);
            limb.lowerLength = Vector3.Distance(limb.lower.position, limb.end.position);

            Vector3 ab = limb.lower.position - limb.upper.position;
            Vector3 bc = limb.end.position - limb.lower.position;
            Vector3 bendNormal = Vector3.Cross(ab, bc);

            if (bendNormal.sqrMagnitude < 0.0001f)
            {
                bendNormal = Vector3.Cross(ab.normalized, Vector3.up);
                if (bendNormal.sqrMagnitude < 0.0001f)
                    bendNormal = Vector3.Cross(ab.normalized, Vector3.forward);
            }

            bendNormal.Normalize();
            limb.localBendNormal = Quaternion.Inverse(limb.upper.rotation) * bendNormal;
        }

        public void OnUpdate()
        {
            if (!isInitialized) return;

            SolveLimb(leftArm);
            SolveLimb(rightArm);
            SolveLimb(leftLeg);
            SolveLimb(rightLeg);
        }

        private void SolveLimb(LimbIK limb)
        {
            if (!limb.enabled || limb.target == null || limb.positionWeight < 0.001f) return;
            if (limb.upper == null || limb.lower == null || limb.end == null) return;

            float upperLen = Vector3.Distance(limb.upper.position, limb.lower.position);
            float lowerLen = Vector3.Distance(limb.lower.position, limb.end.position);

            Vector3 targetPos = Vector3.Lerp(limb.end.position, limb.target.position, limb.positionWeight);

            // --- 무릎 위치 결정 ---
            Vector3 kneePos;

            if (limb.sourceUpper != null && limb.sourceLower != null && limb.sourceEnd != null)
            {
                // 소스 무릎 위치 기반: 소스의 무릎 위치를 타겟 비율로 스케일
                kneePos = ComputeKneePosFromSource(limb, upperLen, lowerLen, targetPos);
            }
            else
            {
                // 소스 참조 없음: 기존 bendGoal 기반 fallback (팔 등)
                kneePos = ComputeKneePosFromBendGoal(limb, upperLen, lowerLen, targetPos);
            }

            // --- 본 회전 적용 ---
            Vector3 currentUpperDir = (limb.lower.position - limb.upper.position).normalized;
            Vector3 desiredUpperDir = (kneePos - limb.upper.position).normalized;
            limb.upper.rotation = Quaternion.FromToRotation(currentUpperDir, desiredUpperDir) * limb.upper.rotation;

            Vector3 currentLowerDir = (limb.end.position - limb.lower.position).normalized;
            Vector3 desiredLowerDir = (targetPos - limb.lower.position).normalized;
            limb.lower.rotation = Quaternion.FromToRotation(currentLowerDir, desiredLowerDir) * limb.lower.rotation;

            if (limb.rotationWeight > 0.001f)
            {
                limb.end.rotation = Quaternion.Slerp(
                    limb.end.rotation,
                    limb.target.rotation,
                    limb.rotationWeight
                );
            }
        }

        /// <summary>
        /// 소스 무릎 위치를 hip→foot 직선 기준으로 분해(투영+수직)한 뒤
        /// 타겟 비율로 스케일하여 타겟 무릎 위치를 결정합니다.
        /// 소스가 역관절이면 수직 성분이 반대쪽 → 타겟도 자연스럽게 역관절.
        /// </summary>
        private Vector3 ComputeKneePosFromSource(LimbIK limb, float upperLen, float lowerLen, Vector3 targetPos)
        {
            // 소스 체인 길이
            float sourceUpperLen = Vector3.Distance(limb.sourceUpper.position, limb.sourceLower.position);
            float sourceLowerLen = Vector3.Distance(limb.sourceLower.position, limb.sourceEnd.position);
            float sourceChain = sourceUpperLen + sourceLowerLen;
            float targetChain = upperLen + lowerLen;

            if (sourceChain < 0.001f || targetChain < 0.001f)
                return limb.lower.position;

            // 소스 hip → foot 방향
            Vector3 sourceHipToFoot = limb.sourceEnd.position - limb.sourceUpper.position;
            float sourceHipToFootMag = sourceHipToFoot.magnitude;
            if (sourceHipToFootMag < 0.001f)
                return limb.lower.position;
            Vector3 sourceHipToFootDir = sourceHipToFoot / sourceHipToFootMag;

            // 소스 hip → knee 벡터를 투영(직선 성분)과 수직(오프셋 성분)으로 분해
            Vector3 sourceHipToKnee = limb.sourceLower.position - limb.sourceUpper.position;
            float projection = Vector3.Dot(sourceHipToKnee, sourceHipToFootDir);
            Vector3 rejection = sourceHipToKnee - projection * sourceHipToFootDir;

            // 소스 체인 길이 대비 비율로 정규화 → 타겟 체인 길이로 스케일
            float scale = targetChain / sourceChain;
            float scaledProjection = projection * scale;
            Vector3 scaledRejection = rejection * scale;

            // 타겟 hip → foot 방향
            Vector3 targetHipToFoot = targetPos - limb.upper.position;
            float targetHipToFootMag = targetHipToFoot.magnitude;
            if (targetHipToFootMag < 0.001f)
                return limb.lower.position;
            Vector3 targetHipToFootDir = targetHipToFoot / targetHipToFootMag;

            // 타겟 무릎 위치: 타겟 hip에서 투영 + 수직 성분 적용
            Vector3 kneePos = limb.upper.position
                            + targetHipToFootDir * scaledProjection
                            + scaledRejection;

            return kneePos;
        }

        /// <summary>
        /// bendGoal 기반 fallback (소스 참조가 없는 사지용).
        /// 기존 cosine law + bendNormal 방식.
        /// </summary>
        private Vector3 ComputeKneePosFromBendGoal(LimbIK limb, float upperLen, float lowerLen, Vector3 targetPos)
        {
            float chainLength = upperLen + lowerLen;
            Vector3 toTarget = targetPos - limb.upper.position;
            float targetDist = toTarget.magnitude;
            if (targetDist < 0.001f) return limb.lower.position;
            Vector3 toTargetDir = toTarget / targetDist;

            Vector3 bendNormal = limb.upper.rotation * limb.localBendNormal;

            if (limb.bendGoal != null && limb.bendGoalWeight > 0.001f)
            {
                Vector3 goalNormal = Vector3.Cross(
                    limb.bendGoal.position - limb.upper.position,
                    targetPos - limb.upper.position
                );
                if (goalNormal.sqrMagnitude > 0.01f)
                {
                    bendNormal = Vector3.Lerp(bendNormal, goalNormal.normalized, limb.bendGoalWeight);
                }
            }
            bendNormal.Normalize();

            float clampedDist = Mathf.Clamp(targetDist, Mathf.Abs(upperLen - lowerLen) + 0.001f, chainLength - 0.001f);
            float cosUpper = (clampedDist * clampedDist + upperLen * upperLen - lowerLen * lowerLen)
                           / (2f * clampedDist * upperLen);
            cosUpper = Mathf.Clamp(cosUpper, -1f, 1f);
            float upperAngleDeg = Mathf.Acos(cosUpper) * Mathf.Rad2Deg;

            Vector3 kneeDir = Quaternion.AngleAxis(-upperAngleDeg, bendNormal) * toTargetDir;
            return limb.upper.position + kneeDir * upperLen;
        }

        public float CalculateAutoFloorHeight(float comfortRatio = 0.98f)
        {
            if (animator == null || leftLeg.upper == null || leftLeg.lower == null || leftLeg.end == null) return 0f;

            float upperLen = Vector3.Distance(leftLeg.upper.position, leftLeg.lower.position);
            float lowerLen = Vector3.Distance(leftLeg.lower.position, leftLeg.end.position);
            float totalLegLength = upperLen + lowerLen;
            float comfortHeight = totalLegLength * comfortRatio;

            Transform hips = animator.GetBoneTransform(HumanBodyBones.Hips);
            Transform foot = leftLeg.end;
            if (hips == null || foot == null) return 0f;

            float currentHipToFoot = hips.position.y - foot.position.y;
            float heightDiff = currentHipToFoot - comfortHeight;

            return -heightDiff;
        }
    }
}