using Unity.Burst;
using Unity.Collections;
using Unity.Jobs;
using UnityEngine;

namespace LibMMD.Motion
{
    public struct MmdPrePhysicsIkInput
    {
        public Vector3 RootPosition, MidPosition, TipPosition;
        public Quaternion RootRotation, MidRotation, TipRotation;
        public Vector3 TargetPosition, HintPosition;
        public Quaternion TargetRotation;
        public float Weight, PositionWeight, RotationWeight, HintWeight;
        public bool HasHint;
    }

    public struct MmdPrePhysicsIkOutput
    {
        public Quaternion RootRotation, MidRotation, TipRotation;
    }

    [BurstCompile(FloatPrecision.Standard, FloatMode.Fast)]
    public struct MmdPrePhysicsIkJob : IJobParallelFor
    {
        [ReadOnly] public NativeArray<MmdPrePhysicsIkInput> Inputs;
        [WriteOnly] public NativeArray<MmdPrePhysicsIkOutput> Outputs;

        public void Execute(int index)
        {
            var input = Inputs[index];
            var weight = Mathf.Clamp01(input.Weight);
            var target = Vector3.Lerp(input.TipPosition, input.TargetPosition,
                Mathf.Clamp01(input.PositionWeight) * weight);
            var upper = input.MidPosition - input.RootPosition;
            var lower = input.TipPosition - input.MidPosition;
            var toTarget = target - input.RootPosition;
            var upperLength = upper.magnitude;
            var lowerLength = lower.magnitude;
            var distance = toTarget.magnitude;

            var output = new MmdPrePhysicsIkOutput
            {
                RootRotation = input.RootRotation,
                MidRotation = input.MidRotation,
                TipRotation = Quaternion.Slerp(input.TipRotation, input.TargetRotation,
                    Mathf.Clamp01(input.RotationWeight) * weight)
            };
            if (upperLength < 0.00001f || lowerLength < 0.00001f || distance < 0.00001f)
            {
                Outputs[index] = output;
                return;
            }

            var direction = toTarget / distance;
            var bendReference = input.MidPosition - input.RootPosition;
            if (input.HasHint && input.HintWeight > 0f)
            {
                bendReference = Vector3.Lerp(bendReference,
                    input.HintPosition - input.RootPosition,
                    Mathf.Clamp01(input.HintWeight) * weight);
            }
            var bendDirection = bendReference - direction * Vector3.Dot(bendReference, direction);
            if (bendDirection.sqrMagnitude < 0.00000001f)
                bendDirection = Vector3.Cross(direction, Vector3.up);
            if (bendDirection.sqrMagnitude < 0.00000001f)
                bendDirection = Vector3.Cross(direction, Vector3.right);
            bendDirection.Normalize();

            var clampedDistance = Mathf.Clamp(distance,
                Mathf.Abs(upperLength - lowerLength) + 0.00001f,
                upperLength + lowerLength - 0.00001f);
            var along = (upperLength * upperLength - lowerLength * lowerLength +
                         clampedDistance * clampedDistance) / (2f * clampedDistance);
            var height = Mathf.Sqrt(Mathf.Max(0f, upperLength * upperLength - along * along));
            var solvedMid = input.RootPosition + direction * along + bendDirection * height;
            var solvedUpper = solvedMid - input.RootPosition;
            var rootDelta = Quaternion.FromToRotation(upper, solvedUpper);
            output.RootRotation = rootDelta * input.RootRotation;

            var rotatedLower = rootDelta * lower;
            var solvedLower = target - solvedMid;
            var midDelta = Quaternion.FromToRotation(rotatedLower, solvedLower);
            output.MidRotation = midDelta * rootDelta * input.MidRotation;
            Outputs[index] = output;
        }
    }
}