using Godot;

// Analytic 2-bone arm solve shared by PhysicsArms and TrueBody (one instance per arm — it keeps
// the elbow's smoothed pole angle). Soft-IK clamp, then the elbow is picked on its circle as an
// ANGLE around the arm axis relative to a torso-frame down/back/out reference: mostly what the
// hand's forward axis suggests (the forearm continues behind the wrist), clamped to ±110° and
// rate-limited so it can never swing through the inward/up side (the 180° flip), and capped
// at max(shoulder, wrist) + 5 cm height (no chicken wings).
public sealed class ArmSolver
{
    public const float SoftIKFactor = 0.98f;
    public const float PoleHandWeight = 0.85f;
    public const float PoleSmoothRate = 12f;
    public const float PoleMaxAngle = 1.92f;  // 110 deg outward from down/back/out
    public const float PoleMaxInward = 0.6f;  // 35 deg inward: a controller turned outward must not fold the elbows toward each other (owner)
    public const float PoleMaxSpeed = 6f;     // rad/s
    public const float ElbowRiseAllowance = 0.05f;

    private float _poleAngle;
    private Vector3 _lastRef;

    public Vector3 Elbow { get; private set; }
    public Vector3 Wrist { get; private set; }
    // Unit vector from the arm axis toward the elbow (defined even for a straight arm); the
    // elbow hinge axis is Pole × (shoulder→wrist).
    public Vector3 Pole { get; private set; } = Vector3.Back;

    // side = the arm's outward direction (−torso X for the left arm, +X for the right).
    // handBasis = the controller's basis (null → torso-frame reference only).
    public void Solve(Vector3 shoulder, Vector3 rawTarget, Basis torsoBasis, Vector3 side, Basis? handBasis,
        float upperLen, float foreLen, float delta)
    {
        float reach = upperLen + foreLen;
        Vector3 shoulderToTarget = rawTarget - shoulder;
        if (shoulderToTarget.LengthSquared() < 0.0001f)
            shoulderToTarget = torsoBasis * new Vector3(0f, -0.25f, -0.3f);

        float rawDist = shoulderToTarget.Length();
        // Below |upper − fore| the triangle can't close (law of cosines invalid) — with unequal
        // anatomical segments a hand near the shoulder would otherwise throw the elbow away.
        float d = Mathf.Clamp(rawDist, Mathf.Abs(upperLen - foreLen) + 0.01f, reach * SoftIKFactor);
        Vector3 dir = shoulderToTarget / rawDist;
        Vector3 wrist = shoulder + dir * d;

        // Elbow circle: center on the arm axis, radius from the law of cosines.
        float along = (upperLen * upperLen - foreLen * foreLen + d * d) / (2f * d);
        float radius = Mathf.Sqrt(Mathf.Max(upperLen * upperLen - along * along, 0f));
        Vector3 center = shoulder + dir * along;

        Vector3 reference = Vector3.Down * 2f + torsoBasis * Vector3.Back * 0.8f + side * 0.3f;
        reference -= dir * reference.Dot(dir);
        if (reference.LengthSquared() > 0.06f)
            _lastRef = reference.Normalized();
        else
        {
            // Arm pointing along the reference: keep the previous one, re-projected.
            reference = _lastRef - dir * _lastRef.Dot(dir);
            reference = reference.LengthSquared() > 1e-8f ? reference.Normalized() : dir.Cross(torsoBasis.X).Normalized();
        }
        Vector3 refN = reference.Normalized();
        Vector3 refB = dir.Cross(refN);

        float targetAngle = 0f;
        if (handBasis is Basis hb)
        {
            Vector3 handForward = (hb * Vector3.Forward).Normalized();
            Vector3 handElbow = wrist - handForward * foreLen - center;
            handElbow -= dir * handElbow.Dot(dir);
            float confidence = Mathf.Clamp(handElbow.Length() / (0.5f * radius + 1e-4f), 0f, 1f);
            if (handElbow.LengthSquared() > 1e-8f)
            {
                // Read the suggestion as the representation NEAREST the current elbow angle:
                // pointing the controller down puts it at the ±180° seam, where jitter would
                // otherwise flip the clamped elbow to the other side.
                float raw = Mathf.Atan2(handElbow.Dot(refB), handElbow.Dot(refN));
                raw = _poleAngle + Mathf.Wrap(raw - _poleAngle, -Mathf.Pi, Mathf.Pi);
                targetAngle = raw * PoleHandWeight * confidence;
            }
        }
        // Asymmetric: the elbow may swing far out, but only a little in toward the body's midline.
        float outward = refB.Dot(side);   // > 0: a positive angle swings the elbow outward
        float inwardLimit = Mathf.Lerp(PoleMaxAngle, PoleMaxInward, Mathf.Clamp(Mathf.Abs(outward) / 0.5f, 0f, 1f));
        targetAngle = outward >= 0f ? Mathf.Clamp(targetAngle, -inwardLimit, PoleMaxAngle) : Mathf.Clamp(targetAngle, -PoleMaxAngle, inwardLimit);
        float step = (targetAngle - _poleAngle) * (1f - Mathf.Exp(-delta * PoleSmoothRate));
        _poleAngle += Mathf.Clamp(step, -PoleMaxSpeed * delta, PoleMaxSpeed * delta);
        Vector3 pole = refN.Rotated(dir, _poleAngle);

        Pole = CapElbowHeight(pole, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance);
        Elbow = center + Pole * radius;
        Wrist = Elbow + (wrist - Elbow).Normalized() * foreLen;
    }

    // A real wrist flexes, so following the hand axis literally can throw the elbow above
    // the shoulder ("chicken wing"). Keep the elbow no higher than yMax by sliding it down
    // its circle, preserving its sideways heading.
    private static Vector3 CapElbowHeight(Vector3 pole, Vector3 dir, Vector3 center, float radius, float yMax)
    {
        if (center.Y + pole.Y * radius <= yMax)
            return pole;
        Vector3 vertical = Vector3.Up - dir * dir.Y;
        if (vertical.LengthSquared() < 1e-6f)
            return pole;  // arm vertical: the circle is horizontal, height can't change
        vertical = vertical.Normalized();
        Vector3 lateral = dir.Cross(vertical);
        float cosMax = Mathf.Clamp((yMax - center.Y) / (radius * vertical.Y), -1f, 1f);
        float sin = Mathf.Sqrt(1f - cosMax * cosMax);
        float sideSign = pole.Dot(lateral) >= 0f ? 1f : -1f;
        return (vertical * cosMax + lateral * (sin * sideSign)).Normalized();
    }
}