chameleon/Assets/Unused/FingerIK.cs

using System.Collections.Generic;
using UnityEngine;

/// <summary>
/// Single-finger procedural pose + IK solver for a LOCAL +Y bone chain. Flexion (curl) rotates around
/// each joint's local hinge axis (default local -X = palm-ward on the WhiteHand rig); spread/abduction
/// is around local Z on the base joint only. Any chain length (thumb and long fingers share the code).
///
/// Two drive modes, blended per frame:
/// 1. Procedural curl: <see cref="curl"/> (0..1) lerps every joint from rest toward its per-joint
///    <see cref="maxCurl"/> - written first each frame, and the SEED pose for the IK pass.
/// 2. IK reach: with a <see cref="target"/> and <see cref="ikWeight"/> &gt; 0, CCD bends the chain from
///    that seed, hinge/limit-constrained mid-sweep; the final pose is Slerp(seed, solved, ikWeight),
///    so curl and IK never fight and nothing accumulates frame-to-frame.
///
/// All curl math is REST-RELATIVE (the captured rest localRotation is the 0-degree datum), preserving
/// the rig's modeled fan/opposition offsets. Added at runtime by <see cref="HandIK"/>, or wired by hand
/// via <see cref="Setup"/>; set <see cref="SolveInLateUpdate"/> only when using this component alone.
/// </summary>
[ExecuteAlways]
[DisallowMultipleComponent]
public class FingerIK : MonoBehaviour
{
    [Header("Chain")]
    [Tooltip("Rotating joints, root -> tip (EXCLUDES the *_end leaf). Auto-filled by HandIK.Setup.")]
    [SerializeField] private List<Transform> joints = new List<Transform>();

    [Tooltip("Fingertip transform (the *_end leaf). Has no joint of its own; CCD reaches it to the target.")]
    [SerializeField] private Transform tip;

    [Tooltip("Optional world-space goal. When set (and ikWeight > 0) CCD reaches the tip to it; otherwise curl drives the pose. HandIK's matching Finger Target slot overrides this when assigned; an empty HandIK slot leaves a target set here intact.")]
    [SerializeField] private Transform target;

    [Header("Solver")]
    [Tooltip("CCD sweeps per Solve. 8-12 is plenty for short finger chains.")]
    [Range(1, 16)] [SerializeField] private int iterations = 10;

    [Tooltip("Stop early once the tip is within this distance (world units) of the target.")]
    [SerializeField] private float tolerance = 0.001f;

    [Tooltip("0 = pure curl pose (no IK), 1 = full IK reach. Blends the solved pose against the curl seed.")]
    [Range(0f, 1f)] [SerializeField] private float ikWeight = 1f;

    [Header("Curl")]
    [Tooltip("0 = open (rest), 1 = fully curled (each joint at its maxCurl). Drives the pose and seeds the IK.")]
    [Range(0f, 1f)] [SerializeField] private float curl;

    [Tooltip("Curl bias per joint depth so distal joints curl more than the base for a natural fist (1 = uniform).")]
    [SerializeField] private AnimationCurve curlProfile = AnimationCurve.Linear(0f, 1f, 1f, 1f);

    [Tooltip("Spread/abduction around local Z (degrees), applied to the base joint only. For the thumb this is opposition.")]
    [SerializeField] private float spread;

    [Tooltip("Spread clamp (degrees), [min, max], applied to the base joint's spread/opposition DOF.")]
    [SerializeField] private Vector2 spreadLimits = new Vector2(-12f, 12f);

    [Header("Per-joint limits (degrees, rest-relative; + = curl toward palm)")]
    [Tooltip("Local hinge (flexion) axis per joint. Default local -X (-1,0,0) = palm-ward flex on this rig. joints[i] uses hingeAxes[i]; short arrays reuse the last entry.")]
    [SerializeField] private Vector3[] hingeAxes;

    [Tooltip("Min curl angle per joint. 0 = open hand. Negative allows hyperextension.")]
    [SerializeField] private float[] minCurl;

    [Tooltip("Max curl angle per joint (closed fist).")]
    [SerializeField] private float[] maxCurl;

    [Header("Execution")]
    [Tooltip("If true, this component solves itself in LateUpdate. Leave OFF when a HandIK manager drives the solve (avoids double-solving).")]
    [SerializeField] private bool solveInLateUpdate;

    // Rest localRotation per joint, captured before the first Solve mutates the bones.
    // Serialized so an edit-mode capture survives domain reload; do NOT re-capture in a
    // curled pose or the rest datum is poisoned.
    [SerializeField, HideInInspector] private Quaternion[] restRotations;

    // Per-frame scratch: the curl seed and the solved local rotations, so the final blend
    // uses seed-vs-solved cleanly instead of blending in place.
    private Quaternion[] _seedLocal;
    private Quaternion[] _solvedLocal;

    private static readonly Vector3 SpreadAxis = Vector3.forward; // local Z

    public Transform Tip => tip;
    public Transform Target => target;
    public float Curl => curl;
    public float IkWeight => ikWeight;
    public float Spread => spread;

    public bool SolveInLateUpdate
    {
        get => solveInLateUpdate;
        set => solveInLateUpdate = value;
    }

    private void Awake()
    {
        if (HasChain() && (restRotations == null || restRotations.Length != joints.Count))
            CaptureRest();
        EnsureScratch();
    }

    private void LateUpdate()
    {
        if (Application.isPlaying && solveInLateUpdate) Solve();
    }

#if UNITY_EDITOR
    // Standalone edit-mode drive: when this finger has no HandIK owner (solveInLateUpdate ON),
    // tick the solve from the editor loop so curl/target edits show live without entering Play.
    // Managed fingers (HandIK sets solveInLateUpdate OFF) make this a no-op -> no double-solve.
    private void OnEnable()
    {
        if (Application.isPlaying) return;
        UnityEditor.EditorApplication.update -= EditorTick;
        UnityEditor.EditorApplication.update += EditorTick;
    }

    private void OnDisable() => UnityEditor.EditorApplication.update -= EditorTick;

    private void EditorTick()
    {
        if (Application.isPlaying) return;
        if (!this) { UnityEditor.EditorApplication.update -= EditorTick; return; }
        if (solveInLateUpdate) Solve();
    }
#endif

    /// <summary>
    /// Wires the chain (rotating joints root..tip, plus the *_end tip leaf), sizes the
    /// per-joint limit arrays to sensible defaults, and captures the rest pose. Called by
    /// HandIK or by hand. Capturing rest here (right after the joints are assigned, before
    /// any Solve) keeps the rest datum at the modeled bind pose.
    /// </summary>
    public void Setup(IList<Transform> chainJoints, Transform tipLeaf)
    {
        joints = chainJoints != null ? new List<Transform>(chainJoints) : new List<Transform>();
        tip = tipLeaf;
        EnsureLimitArrays();
        // Capture rest ONLY when missing/stale (chain length changed). Re-capturing during a
        // re-discovery of an already-posed hand would record a curled pose as the 0-degree
        // datum and permanently poison every curl. Use ResetToRest first, then re-capture.
        if (restRotations == null || restRotations.Length != joints.Count) CaptureRest();
        EnsureScratch();
    }

    /// <summary>Overrides the per-joint hinge limits (degrees, rest-relative). Lengths are clamped/extended to the joint count.</summary>
    public void SetLimits(float[] mins, float[] maxes)
    {
        EnsureLimitArrays();
        if (mins != null)
            for (int i = 0; i < minCurl.Length; i++) minCurl[i] = mins[Mathf.Min(i, mins.Length - 1)];
        if (maxes != null)
            for (int i = 0; i < maxCurl.Length; i++) maxCurl[i] = maxes[Mathf.Min(i, maxes.Length - 1)];
    }

    /// <summary>
    /// Overrides the per-joint local hinge (flexion) axis. Sized to the joint count; a short
    /// array reuses its last entry. Lets HandIK push the rig's curl direction centrally so a
    /// re-discovery corrects any stale serialized axis on existing FingerIK components.
    /// </summary>
    public void SetHingeAxes(Vector3[] axes)
    {
        EnsureLimitArrays();
        if (axes == null || axes.Length == 0) return;
        for (int i = 0; i < hingeAxes.Length; i++)
        {
            Vector3 a = axes[Mathf.Min(i, axes.Length - 1)];
            hingeAxes[i] = a == Vector3.zero ? Vector3.left : a.normalized;
        }
    }

    /// <summary>Sets the base-joint spread/opposition clamp (degrees).</summary>
    public void SetSpreadLimits(float min, float max) => spreadLimits = new Vector2(min, max);

    /// <summary>Captures each joint's current localRotation as the rest reference (0-degree curl datum).</summary>
    public void CaptureRest()
    {
        if (!HasChain()) return;
        restRotations = new Quaternion[joints.Count];
        for (int i = 0; i < joints.Count; i++)
            restRotations[i] = joints[i] ? joints[i].localRotation : Quaternion.identity;
    }

    public void SetTarget(Transform t) => target = t;
    public void SetCurl(float value) => curl = Mathf.Clamp01(value);
    public void SetIkWeight(float value) => ikWeight = Mathf.Clamp01(value);
    public void SetIterations(int value) => iterations = Mathf.Clamp(value, 1, 16);
    public void SetSpread(float degrees) => spread = Mathf.Clamp(degrees, spreadLimits.x, spreadLimits.y);

    /// <summary>Restores every joint to its captured rest localRotation.</summary>
    public void ResetToRest()
    {
        if (!HasChain() || restRotations == null) return;
        for (int i = 0; i < joints.Count && i < restRotations.Length; i++)
            if (joints[i]) joints[i].localRotation = restRotations[i];
    }

    /// <summary>
    /// One solve step. (1) Write the procedural curl seed to every joint. (2) If no target
    /// or ikWeight &lt;= 0, that seed IS the pose (cheap, deterministic). (3) Otherwise run
    /// CCD from the seed, constraining each joint to its hinge + limits mid-sweep, then
    /// blend each joint Slerp(seed, solved, ikWeight). Spread is applied last on the base.
    /// </summary>
    public void Solve()
    {
        if (!HasChain() || restRotations == null || restRotations.Length != joints.Count) return;
        EnsureScratch();

        WriteCurlSeed();

        bool runIk = target && tip && ikWeight > 0f;
        if (runIk)
        {
            SolveCcd();
            SolveBaseLateral(); // target-driven sideways DOF on the base (folds in manual spread)
            for (int i = 0; i < joints.Count; i++)
            {
                if (!joints[i]) continue;
                joints[i].localRotation = ikWeight >= 1f
                    ? _solvedLocal[i]
                    : Quaternion.Slerp(_seedLocal[i], _solvedLocal[i], ikWeight);
            }
        }
        else
        {
            ApplySpread(); // manual spread only when IK isn't driving the base laterally
        }
    }

    private bool HasChain() => joints != null && joints.Count > 0;

    private void EnsureScratch()
    {
        int n = joints != null ? joints.Count : 0;
        if (_seedLocal == null || _seedLocal.Length != n) _seedLocal = new Quaternion[n];
        if (_solvedLocal == null || _solvedLocal.Length != n) _solvedLocal = new Quaternion[n];
    }

    private Vector3 HingeAxis(int i)
    {
        if (hingeAxes == null || hingeAxes.Length == 0) return Vector3.left;
        Vector3 a = hingeAxes[Mathf.Min(i, hingeAxes.Length - 1)];
        return a == Vector3.zero ? Vector3.left : a.normalized;
    }

    private float MinCurl(int i) =>
        (minCurl != null && minCurl.Length > 0) ? minCurl[Mathf.Min(i, minCurl.Length - 1)] : 0f;

    private float MaxCurl(int i) =>
        (maxCurl != null && maxCurl.Length > 0) ? maxCurl[Mathf.Min(i, maxCurl.Length - 1)] : 90f;

    private void EnsureLimitArrays()
    {
        int n = joints != null ? joints.Count : 0;
        if (hingeAxes == null || hingeAxes.Length != n)
        {
            hingeAxes = new Vector3[n];
            for (int i = 0; i < n; i++) hingeAxes[i] = Vector3.left;
        }
        if (minCurl == null || minCurl.Length != n) minCurl = new float[n]; // 0 = open
        if (maxCurl == null || maxCurl.Length != n)
        {
            maxCurl = new float[n];
            for (int i = 0; i < n; i++) maxCurl[i] = 90f;
        }
    }

    // Procedural curl pose: each joint = rest * AngleAxis(Lerp(0, maxCurl, curl * profile), hinge).
    // Stored into _seedLocal AND written to the transform so CCD bends from this pose.
    private void WriteCurlSeed()
    {
        float span = Mathf.Max(1, joints.Count - 1);
        for (int i = 0; i < joints.Count; i++)
        {
            float depth = joints.Count > 1 ? i / span : 0f;
            float bias = curlProfile != null ? curlProfile.Evaluate(depth) : 1f;
            float theta = Mathf.Clamp(Mathf.Lerp(0f, MaxCurl(i), curl * bias), MinCurl(i), MaxCurl(i));
            Quaternion seed = restRotations[i] * Quaternion.AngleAxis(theta, HingeAxis(i));
            _seedLocal[i] = seed;
            if (joints[i]) joints[i].localRotation = seed;
        }
    }

    // Hinge-constrained CCD: sweep tip -> base. Each joint rotates ONLY about its world hinge
    // axis by the in-plane angle that swings the tip toward the target, then is clamped to its
    // limits. Rotating along the real DOF (instead of a free minimal-arc that the hinge clamp
    // then discards) keeps CCD in the rig's curl direction, so it actually reaches curl-side
    // targets within [min, max]. Re-read the tip after every write (the subtree moved). Snapshot
    // the result into _solvedLocal so the final blend is seed-vs-solved.
    private void SolveCcd()
    {
        float sqrTol = tolerance * tolerance;

        for (int it = 0; it < iterations; it++)
        {
            for (int i = joints.Count - 1; i >= 0; i--)
            {
                Transform j = joints[i];
                if (!j) continue;

                Vector3 jointPos = j.position;
                Vector3 toTip = tip.position - jointPos;
                Vector3 toTarget = target.position - jointPos;
                if (toTip.sqrMagnitude < 1e-8f || toTarget.sqrMagnitude < 1e-8f) continue;

                // World hinge axis (invariant under the joint's own twist about local X).
                Vector3 axisW = (j.rotation * HingeAxis(i)).normalized;
                if (!TryInPlaneAngle(toTip, toTarget, axisW, out float hingeAng)) continue;

                j.rotation = Quaternion.AngleAxis(hingeAng, axisW) * j.rotation;

                ConstrainToHinge(i);
            }

            if ((tip.position - target.position).sqrMagnitude <= sqrTol) break;
        }

        for (int i = 0; i < joints.Count; i++)
            _solvedLocal[i] = joints[i] ? joints[i].localRotation : restRotations[i];
    }

    // Rest-relative swing-twist hinge constraint: isolate the twist about the hinge axis
    // from (Inverse(rest) * local), drop the swing (spread/Y drift the world CCD added),
    // extract the signed hinge angle, clamp to [min, max], and rebuild rest * AngleAxis.
    // One assignment enforces BOTH the hinge plane and the limits with no accumulated drift.
    private void ConstrainToHinge(int i)
    {
        Transform j = joints[i];
        if (!j) return;

        Vector3 axis = HingeAxis(i);
        Quaternion rel = Quaternion.Inverse(restRotations[i]) * j.localRotation;

        // Twist component of rel about axis (swing-twist for a principal axis).
        Vector3 v = new Vector3(rel.x, rel.y, rel.z);
        Vector3 p = Vector3.Project(v, axis);
        float mag = Mathf.Sqrt(p.x * p.x + p.y * p.y + p.z * p.z + rel.w * rel.w);
        Quaternion twist = mag < 1e-8f
            ? Quaternion.identity // 180-degree swing: undefined twist
            : new Quaternion(p.x / mag, p.y / mag, p.z / mag, rel.w / mag);

        // Signed hinge angle: ToAngleAxis gives [0,180] with a possibly-flipped axis;
        // recover the sign from the dot with the hinge axis.
        twist.ToAngleAxis(out float ang, out Vector3 ax);
        float signed = ang * Mathf.Sign(Vector3.Dot(ax, axis));
        signed = Mathf.Clamp(signed, MinCurl(i), MaxCurl(i));

        j.localRotation = restRotations[i] * Quaternion.AngleAxis(signed, axis);
    }

    // Target-driven lateral (abduction) on the base joint about local Z, run once after the
    // flexion CCD. Z tip-motion is orthogonal to the X flexion plane on this rig, so a small
    // sideways swing toward the target composes cleanly on top of the curl/flexion pose. The
    // manual spread is folded in as a bias and the total is clamped to spreadLimits, then written
    // into the base's solved rotation so the ikWeight blend covers lateral too.
    private void SolveBaseLateral()
    {
        Transform b = joints[0];
        if (!b || !tip || !target) return;

        b.localRotation = _solvedLocal[0]; // pose the base at its flexion-solved rotation to read world Z
        Vector3 axisW = (b.rotation * SpreadAxis).normalized;
        Vector3 toTip = tip.position - b.position;
        Vector3 toTarget = target.position - b.position;

        TryInPlaneAngle(toTip, toTarget, axisW, out float latAng); // degenerate -> 0 (spread bias still applies)

        latAng = Mathf.Clamp(latAng + spread, spreadLimits.x, spreadLimits.y);
        _solvedLocal[0] = _solvedLocal[0] * Quaternion.AngleAxis(latAng, SpreadAxis);
    }

    // Signed angle from toTip to toTarget projected onto the plane orthogonal to axis; false when
    // either projection is degenerate (angle is then 0 and must not be applied as a solve step).
    private static bool TryInPlaneAngle(Vector3 toTip, Vector3 toTarget, Vector3 axis, out float angle)
    {
        angle = 0f;
        Vector3 tipPlane = Vector3.ProjectOnPlane(toTip, axis);
        Vector3 tgtPlane = Vector3.ProjectOnPlane(toTarget, axis);
        if (tipPlane.sqrMagnitude < 1e-10f || tgtPlane.sqrMagnitude < 1e-10f) return false;

        angle = Vector3.SignedAngle(tipPlane, tgtPlane, axis);
        return true;
    }

    // Base-joint spread/opposition about local Z, composed AFTER curl so the two DOFs don't
    // fight. Applied on the final base rotation when IK isn't driving the base laterally.
    private void ApplySpread()
    {
        if (joints.Count == 0 || !joints[0]) return;
        float s = Mathf.Clamp(spread, spreadLimits.x, spreadLimits.y);
        if (Mathf.Approximately(s, 0f)) return;
        joints[0].localRotation = joints[0].localRotation * Quaternion.AngleAxis(s, SpreadAxis);
    }

    private void OnDrawGizmosSelected()
    {
        if (!HasChain()) return;

        Gizmos.color = Color.cyan;
        for (int i = 0; i < joints.Count - 1; i++)
            if (joints[i] && joints[i + 1]) Gizmos.DrawLine(joints[i].position, joints[i + 1].position);

        Transform last = joints[joints.Count - 1];
        if (last && tip) Gizmos.DrawLine(last.position, tip.position);
        if (tip) Gizmos.DrawWireSphere(tip.position, 0.03f);

        if (target && tip)
        {
            Gizmos.color = Color.yellow;
            Gizmos.DrawLine(tip.position, target.position);
            Gizmos.DrawWireSphere(target.position, 0.05f);
        }
    }
}