using SystemUtilities;
using UnityEngine;

namespace Player
{
    public class LegIK : MonoBehaviour
    {
        // ----- Serialized Fields
        [Header("References")]
        [SerializeField] private Transform target;
        [SerializeField] private Transform body;
        [SerializeField] private Transform mid;
        [SerializeField] private Transform foot;
        [SerializeField] private Transform toe;
        [SerializeField] private Transform end;
        
        [Header("Settings")]
        [SerializeField] private Vector3 legOffsetRot = new Vector3(0f, 90f, 90f);
        [SerializeField] private Vector3 footNormalOffsetRot = new Vector3(0f, 0f, 90f);
        
        [Space(8f)]
        [SerializeField] private Vector3 footMinRots;
        [SerializeField] private Vector3 footMaxRots;
        [SerializeField] private Vector3 toeMinRots;
        [SerializeField] private Vector3 toeMaxRots;
        [SerializeField] private float testAngle1;
        
        // ----- Fields
        private float rootLength;
        private float midLength;
        private float footLength;
        private float toeLength;
        
        private float footOffsetAngle;
        private float toeOffsetAngle;
        
        private float defFootFloorDist;
        private float defToeFloorDist;
        private float defEndFloorDist;
        
        // ----- Unity Events
        void Awake()
        {
            Initialize();
        }
        
        void LateUpdate()
        {
            SolveIK();
        }
        
        // ----- Private Methods
        private void Initialize()
        {
            rootLength = Vector3.Distance(transform.position, mid.position);
            midLength = Vector3.Distance(mid.position, foot.position);
            footLength = Vector3.Distance(foot.position, toe.position);
            toeLength = Vector3.Distance(toe.position, end.position);
            
            footOffsetAngle = Vector3.Angle(mid.forward, foot.forward);
            toeOffsetAngle = Vector3.Angle(foot.forward, toe.forward);
            
            defFootFloorDist = Mathf.Abs(body.position.y - foot.position.y);
            defToeFloorDist = Mathf.Abs(body.position.y - toe.position.y);
            defEndFloorDist = Mathf.Abs(body.position.y - end.position.y);
            
            target.position = foot.position;
        }
        
        private void SolveIK()
        {
            Vector3 targetPos = target.position;
            float a = rootLength;
            float b = midLength;
            float c = Vector3.Distance(transform.position, targetPos);
            
            float maxLength = rootLength + midLength;
            if (maxLength <= c)
            {
                Vector3 dir = (targetPos - transform.position).normalized;
                targetPos = transform.position + dir * maxLength;
                c = maxLength;
            }
            
            float sp = 0.5f * (a + b + c);
            float area = Mathf.Sqrt(sp * (sp - a) * (sp - b) * (sp - c));
            float height = 2f * area / c;
            float length = Mathf.Sqrt(a * a - height * height);
            
            Vector3 rootToTargetDir = (targetPos - transform.position).normalized;
            Vector3 rootToTargetDirUp = Vector3.Cross(rootToTargetDir, body.right).normalized;
            Vector3 midPos = transform.position + rootToTargetDir * length + rootToTargetDirUp * height;
            
            Vector3 rootToMidDir = (midPos - transform.position).normalized;
            Quaternion rootRot = Quaternion.LookRotation(rootToMidDir, -body.right) * Quaternion.Euler(legOffsetRot);
            
            Vector3 midToTargetDir = (targetPos - midPos).normalized;
            Quaternion midRot = Quaternion.LookRotation(midToTargetDir, -body.right) * Quaternion.Euler(legOffsetRot);
            
            transform.rotation = rootRot;
            mid.rotation = midRot;
            //foot.rotation = Quaternion.AngleAxis(footOffsetAngle, mid.right) * mid.rotation;
            toe.rotation = Quaternion.AngleAxis(toeOffsetAngle, foot.right) * foot.rotation;
            
            (float footFloorDist, Vector3 footFloorNormal) = RaycastFloor(foot.position);
            float footAngle = GetTriangleAngle(footLength, footFloorDist - defToeFloorDist);
            Vector3 footAsMidRot = Quaternion.AngleAxis(90f - footOffsetAngle - (footAngle - footOffsetAngle), -mid.right) * footFloorNormal;
            Vector3 footRot = (Quaternion.LookRotation(footAsMidRot, mid.right) * Quaternion.Euler(footNormalOffsetRot)).eulerAngles;
            
            footRot.x = Mathf.Clamp(footRot.x.Convert360AngleTo180(), mid.eulerAngles.x + footMinRots.x, mid.eulerAngles.x + footMaxRots.x);
            footRot.y = Mathf.Clamp(footRot.y.Convert360AngleTo180(), mid.eulerAngles.y + footMinRots.y, mid.eulerAngles.y + footMaxRots.y);
            footRot.z = Mathf.Clamp(footRot.z.Convert360AngleTo180(), mid.eulerAngles.z + footMinRots.z, mid.eulerAngles.z + footMaxRots.z);
            
            /*float angleX = Vector3.SignedAngle(Vector3.ProjectOnPlane(footFloorNormal, mid.right), mid.right, mid.right);
            float angleY = Vector3.SignedAngle(Vector3.ProjectOnPlane(footFloorNormal, mid.up), mid.up, mid.up);
            float angleZ = Vector3.SignedAngle(Vector3.ProjectOnPlane(footFloorNormal, mid.forward), mid.forward, mid.forward);
            Debug.Log($"angleX = {angleX}, angleY = {angleY}, angleZ = {angleZ}");
            
            footRot = Quaternion.AngleAxis(Mathf.Clamp(angleX, footMinRots.x, footMaxRots.x), mid.right) * footRot;
            footRot = Quaternion.AngleAxis(Mathf.Clamp(angleY, footMinRots.y, footMaxRots.y), mid.up) * footRot;
            footRot = Quaternion.AngleAxis(Mathf.Clamp(angleZ, footMinRots.z, footMaxRots.z), mid.forward) * footRot;*/
            
            foot.rotation = Quaternion.Euler(footRot);
            
            /*(float toeFloorDist, Vector3 toeFloorNormal) = RaycastFloor(end.position);
            float toeAngle = GetTriangleAngle(toeLength, toeFloorDist - defEndFloorDist);
            Quaternion toeAsFootRot = Quaternion.AngleAxis(toeOffsetAngle, toe.right) * foot.rotation;
            toe.rotation = ClampRotBasedOnFloorNormal(toeAsFootRot, toeFloorNormal, footFloorNormal, toeMinRots, toeMaxRots);*/
        }
        
        private (float, Vector3) RaycastFloor(Vector3 pos)
        {
            const float dist = 1f;
            bool hits = Physics.Raycast(pos + body.up * (0.5f * dist), -body.up, out RaycastHit hit, dist);
            Debug.DrawRay(hit.point, hit.normal * 0.5f, Color.yellow);
            return (hits ? Vector3.Distance(pos, hit.point) : 0f, hit.normal);
        }
        
        private float GetTriangleAngle(float hypotenuse, float vertical)
        {
            if (hypotenuse <= vertical) return 0f;
            float horizontal = Mathf.Sqrt(hypotenuse * hypotenuse - vertical * vertical);
            return Mathf.Asin(horizontal / hypotenuse) * (180f / Mathf.PI);
        }
        
        private Quaternion ClampRotBasedOnFloorNormal(Quaternion rot, Vector3 normal, Vector3 upVector, Vector3 minRots, Vector3 maxRots)
        {
            float angleNormalX = -Vector3.SignedAngle(Vector3.ProjectOnPlane(normal, Vector3.right), upVector, Vector3.right);
            float angleNormalY = -Vector3.SignedAngle(Vector3.ProjectOnPlane(normal, Vector3.up), upVector, Vector3.up);
            float angleNormalZ = -Vector3.SignedAngle(Vector3.ProjectOnPlane(normal, Vector3.forward), upVector, Vector3.forward);
            
            Quaternion clampedRot = rot;
            clampedRot = Quaternion.AngleAxis(Mathf.Clamp(angleNormalX, minRots.x, maxRots.x), Vector3.right) * clampedRot;
            clampedRot = Quaternion.AngleAxis(Mathf.Clamp(angleNormalY, minRots.y, maxRots.y), Vector3.up) * clampedRot;
            clampedRot = Quaternion.AngleAxis(Mathf.Clamp(angleNormalZ, minRots.z, maxRots.z), Vector3.forward) * clampedRot;
            
            return clampedRot;
        }
    }
}