using System.Collections;
using System.Collections.Generic;
using Unity.Mathematics;
using UnityEngine;
using System.Linq;

namespace KindRetargeting
{
    [DefaultExecutionOrder(2)]
    public class ShoulderCorrectionFunction : MonoBehaviour
    {
        private CustomRetargetingScript retargetingScript; // 소스 데이터를 가져올 리타게팅 스크립트

        [Header("설정")]
        [Range(0f, 5f)]
        public float blendStrength = 2f;        // 전체적인 보정 강도
        [Range(0f, 1f)]
        public float maxShoulderBlend = 0.7f;   // 어깨에 최대로 전달될 수 있는 회전 비율
        public bool reverseLeftRotation = false;  // 왼쪽 어깨 회전 방향 반전 설정
        public bool reverseRightRotation = false; // 오른쪽 어깨 회전 방향 반전 설정

        [Header("높이 제한 설정")]
        public float maxHeightDifference = 0.8f; // 최대 높이 차이
        public float minHeightDifference = -0.1f;   // 최소 높이 차이 (이 값 이하에서는 보정하지 않음)

        [Header("보정 커브 설정")]
        public AnimationCurve shoulderCorrectionCurve = AnimationCurve.Linear(0f, 0f, 1f, 1f);

        private float leftBlendWeight = 0f;
        private float rightBlendWeight = 0f;

        private Transform leftShoulder;
        private Transform rightShoulder;
        private Transform leftUpperArm;
        private Transform rightUpperArm;

        private void Start()
        {
            retargetingScript = GetComponent<CustomRetargetingScript>();
            
            leftShoulder = retargetingScript.targetAnimator.GetBoneTransform(HumanBodyBones.LeftShoulder);
            rightShoulder = retargetingScript.targetAnimator.GetBoneTransform(HumanBodyBones.RightShoulder);
            leftUpperArm = retargetingScript.targetAnimator.GetBoneTransform(HumanBodyBones.LeftUpperArm);
            rightUpperArm = retargetingScript.targetAnimator.GetBoneTransform(HumanBodyBones.RightUpperArm);
        }

        private void Update()
        {
            // 왼쪽 어깨 보정
            Vector3 leftElbowPos = retargetingScript.targetAnimator.GetBoneTransform(HumanBodyBones.LeftLowerArm).position;
            float leftHeightDiff = leftElbowPos.y - leftShoulder.position.y;
            float leftRawBlend = Mathf.Clamp01(
                Mathf.InverseLerp(minHeightDifference, maxHeightDifference, leftHeightDiff) * blendStrength
            );
            leftBlendWeight = shoulderCorrectionCurve.Evaluate(leftRawBlend) * maxShoulderBlend;

            // 오른쪽 어깨 보정
            Vector3 rightElbowPos = retargetingScript.targetAnimator.GetBoneTransform(HumanBodyBones.RightLowerArm).position;
            float rightHeightDiff = rightElbowPos.y - rightShoulder.position.y;
            float rightRawBlend = Mathf.Clamp01(
                Mathf.InverseLerp(minHeightDifference, maxHeightDifference, rightHeightDiff) * blendStrength
            );
            rightBlendWeight = shoulderCorrectionCurve.Evaluate(rightRawBlend) * maxShoulderBlend;

            // 어깨와 윗팔 회전 보정 적용
            if (leftBlendWeight > 0.01f)
            {
                Quaternion currentWorldShoulderRot = leftShoulder.rotation;
                Quaternion currentWorldArmRot = leftUpperArm.rotation;
                
                Vector3 shoulderToArm = (leftUpperArm.position - leftShoulder.position).normalized;
                Quaternion targetRotation = Quaternion.FromToRotation(leftShoulder.forward, 
                    reverseLeftRotation ? shoulderToArm : -shoulderToArm);
                Quaternion targetWorldShoulderRot = targetRotation * currentWorldShoulderRot;
                
                leftShoulder.rotation = Quaternion.Lerp(currentWorldShoulderRot, targetWorldShoulderRot, leftBlendWeight);
                leftUpperArm.rotation = currentWorldArmRot;
            }

            if (rightBlendWeight > 0.01f)
            {
                Quaternion currentWorldShoulderRot = rightShoulder.rotation;
                Quaternion currentWorldArmRot = rightUpperArm.rotation;
                
                Vector3 shoulderToArm = (rightUpperArm.position - rightShoulder.position).normalized;
                Quaternion targetRotation = Quaternion.FromToRotation(rightShoulder.forward, 
                    reverseRightRotation ? -shoulderToArm : shoulderToArm);
                Quaternion targetWorldShoulderRot = targetRotation * currentWorldShoulderRot;
                
                rightShoulder.rotation = Quaternion.Lerp(currentWorldShoulderRot, targetWorldShoulderRot, rightBlendWeight);
                rightUpperArm.rotation = currentWorldArmRot;
            }
        }
    }
}