using System;
using System.Collections.Generic;
using Godot;

// Headless regression for the forked addon's Generic6DOFJoint3D → Box3D mapping
// (ball = b3SphericalJoint with cone/twist limits + torque-capped motor; drive = b3MotorJoint;
// all-locked = weld; drive + angular limits = b3MotorJoint with the fork's swing-ellipse/twist limits,
// the shoulder). Run first whenever the addon binary changes.
// godot-mono --headless --fixed-fps 90 --path . scenes/sixdof_spike.tscn
public partial class SixDofSpike : Node3D
{
    private const float SwingLimitDeg = 30f;
    private const float TwistLimitDeg = 10f;
    private const float LimitToleranceDeg = 3f;
    private const float PivotLimit = 0.01f;
    private const float LeverMass = 1f;
    private const float LeverLength = 0.6f;
    private const float StrongMuscle = 10f;
    private const float WeakMuscle = 1.5f;
    private const float MuscleKp = 20f;
    private const float MuscleMaxSpin = 8f;
    // Limited drive (the shoulder): asymmetric swing bounds about joint Y / Z, twist about X.
    private static readonly float LimLowerY = Mathf.DegToRad(-20f), LimUpperY = Mathf.DegToRad(60f);
    private static readonly float LimLowerZ = Mathf.DegToRad(-40f), LimUpperZ = Mathf.DegToRad(100f);
    private static readonly float LimTwist = Mathf.DegToRad(45f);
    private const float SocketKp = 30f, SocketForce = 2000f;

    private readonly List<(string Name, Action<float> Tick, float Duration, Func<bool> Finish)> _phases = new();
    private int _phase = -1;
    private float _t;
    private bool _allOk = true;
    private Node3D _stage;

    public override void _Ready()
    {
        _phases.Add(("joint type", _ => { }, 0.1f, PhaseType));
        _phases.Add(("ball holds pivot and swings freely", BallTick, 1.5f, BallFinish));
        _phases.Add(("cone + twist limits hold under torque", LimitTick, 1.5f, LimitFinish));
        _phases.Add(("strong muscle holds a 2.9 N·m lever horizontal", t => MuscleTick(t, false), 1.5f, () => MuscleFinish(true)));
        _phases.Add(("weak muscle cannot hold it", t => MuscleTick(t, false), 1.5f, () => MuscleFinish(false)));
        _phases.Add(("muscle tracks a moving target", t => MuscleTick(t, true), 1.8f, MuscleTrackFinish));
        _phases.Add(("drive follows a moving target", DriveTick, 1.5f, DriveFollowFinish));
        _phases.Add(("hand pushing into a wall pushes the anchor body away (reaction)", DriveBlockedTick, 1.5f, DriveBlockedFinish));
        _phases.Add(("all-locked config is rigid", WeldTick, 1.5f, WeldFinish));
        _phases.Add(("live flag toggling stays finite", ToggleTick, 1f, ToggleFinish));
        _phases.Add(("limited drive holds the asymmetric swing ellipse + twist under hard torque", LimitedDriveTorqueTick, 4f, LimitedDriveTorqueFinish));
        _phases.Add(("limited drive: muscle reaches targets inside the range, stops at the bound outside it", LimitedDriveMuscleTick, 6f, LimitedDriveMuscleFinish));
        _phases.Add(("limited drive: the linear socket drive still moves the joint (shrug)", LimitedDriveSocketTick, 1.5f, LimitedDriveSocketFinish));
        Next();
    }

    public override void _PhysicsProcess(double delta)
    {
        if (_phase >= _phases.Count)
            return;
        float dt = (float)delta;
        _t += dt;
        _phases[_phase].Tick(dt);
        if (_t >= _phases[_phase].Duration)
        {
            bool ok = _phases[_phase].Finish();
            _allOk &= ok;
            GD.Print($"6DOF: {_phases[_phase].Name} → {(ok ? "PASS" : "FAIL")}");
            Next();
        }
    }

    private void Next()
    {
        _stage?.QueueFree();
        _stage = new Node3D();
        AddChild(_stage);
        _t = 0f;
        _phase++;
        if (_phase >= _phases.Count)
        {
            GD.Print($"6DOF: overall {(_allOk ? "PASS — Generic6DOFJoint3D functional on patched Box3D" : "CHECK — inspect failures above")}");
            GetTree().Quit();
            return;
        }
        Setup(_phase);
    }

    // ---- scene pieces ----
    private StaticBody3D _anchor;
    private RigidBody3D _bodyA;
    private RigidBody3D _box;
    private Generic6DofJoint3D _joint;
    private Basis _frameLocalA;
    private Basis _frameLocalB;
    private Vector3 _pivotLocalB;
    private Vector3 _pivotWorld;
    private float _maxPivotSep;
    private float _maxSwing;
    private float _maxTwist;
    private float _swingRange;
    private float _startAngle;
    private float _maxErrDeg;
    private float _endErrDeg;
    private Vector3 _bodyAStart;
    private bool _finite = true;
    private StaticBody3D _wall;

    private RigidBody3D MakeBox(Vector3 size, float mass, Vector3 pos, float gravity = 1f)
    {
        var body = new RigidBody3D { Mass = mass, GravityScale = gravity, Position = pos };
        body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
        _stage.AddChild(body);
        return body;
    }

    private StaticBody3D MakeStatic(Vector3 size, Vector3 pos)
    {
        var body = new StaticBody3D { Position = pos };
        body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
        _stage.AddChild(body);
        return body;
    }

    // Configure BEFORE AddChild (the node pushes everything to the server on enter-tree).
    private Generic6DofJoint3D MakeJoint(PhysicsBody3D a, PhysicsBody3D b, Vector3 at, Action<Generic6DofJoint3D> configure)
    {
        var joint = new Generic6DofJoint3D { Position = at, NodeA = a.GetPath(), NodeB = b.GetPath(), ExcludeNodesFromCollision = true };
        configure(joint);
        _stage.AddChild(joint);
        _frameLocalA = a.GlobalTransform.Basis.Inverse() * joint.GlobalTransform.Basis;
        _frameLocalB = b.GlobalTransform.Basis.Inverse() * joint.GlobalTransform.Basis;
        _pivotLocalB = b.GlobalTransform.AffineInverse() * at;
        _pivotWorld = at;
        return joint;
    }

    private static void ForAxes(Action<Action<Generic6DofJoint3D.Flag, bool>, Action<Generic6DofJoint3D.Param, float>> body, Generic6DofJoint3D j)
    {
        body((f, v) => { j.SetFlagX(f, v); j.SetFlagY(f, v); j.SetFlagZ(f, v); },
             (p, v) => { j.SetParamX(p, v); j.SetParamY(p, v); j.SetParamZ(p, v); });
    }

    private void Setup(int phase)
    {
        _maxPivotSep = _maxSwing = _maxTwist = _swingRange = _maxErrDeg = _endErrDeg = 0f;
        _finite = true;
        switch (phase)
        {
            case 0:
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j));
                break;
            case 1:
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j));
                _box.ApplyCentralImpulse(new Vector3(2.5f, 0, 0));
                _startAngle = SwingAngle();
                break;
            case 2:
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                // Twist axis = joint X; the box hangs along -Y, so the joint basis is rotated to point X down.
                _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j =>
                {
                    j.Basis = new Basis(Vector3.Forward, Mathf.Pi / 2f);
                    j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, Mathf.DegToRad(-TwistLimitDeg));
                    j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, Mathf.DegToRad(TwistLimitDeg));
                    foreach (var set in new Action<Generic6DofJoint3D.Param, float>[] { j.SetParamY, j.SetParamZ })
                    {
                        set(Generic6DofJoint3D.Param.AngularLowerLimit, Mathf.DegToRad(-SwingLimitDeg));
                        set(Generic6DofJoint3D.Param.AngularUpperLimit, Mathf.DegToRad(SwingLimitDeg));
                    }
                });
                break;
            case 3:
            case 4:
            case 5:
            {
                // Lever sticking out horizontally along +X from a static shoulder.
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(LeverLength, 0.06f, 0.06f), LeverMass, new Vector3(LeverLength * 0.5f, 3, 0));
                float cap = phase == 4 ? WeakMuscle : StrongMuscle;
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) =>
                {
                    f(Generic6DofJoint3D.Flag.EnableAngularLimit, false);
                    f(Generic6DofJoint3D.Flag.EnableMotor, true);
                    p(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
                }, j));
                break;
            }
            case 6:
            case 7:
            {
                _bodyA = MakeBox(new Vector3(0.3f, 0.5f, 0.2f), 10f, new Vector3(0, 1.5f, 0), gravity: 0f);
                _box = MakeBox(new Vector3(0.04f, 0.09f, 0.12f), 0.4f, new Vector3(0.4f, 1.5f, 0), gravity: 0f);
                _joint = MakeJoint(_bodyA, _box, new Vector3(0.4f, 1.5f, 0), j => ForAxes((f, p) =>
                {
                    f(Generic6DofJoint3D.Flag.EnableLinearLimit, false);
                    f(Generic6DofJoint3D.Flag.EnableAngularLimit, false);
                    f(Generic6DofJoint3D.Flag.EnableLinearSpring, true);
                    f(Generic6DofJoint3D.Flag.EnableAngularSpring, true);
                    p(Generic6DofJoint3D.Param.LinearSpringStiffness, 10f);
                    p(Generic6DofJoint3D.Param.LinearSpringDamping, 1f);
                    p(Generic6DofJoint3D.Param.LinearMotorForceLimit, 90f);
                    p(Generic6DofJoint3D.Param.AngularSpringStiffness, 10f);
                    p(Generic6DofJoint3D.Param.AngularSpringDamping, 1f);
                    p(Generic6DofJoint3D.Param.AngularMotorForceLimit, 5f);
                }, j));
                _bodyAStart = _bodyA.GlobalPosition;
                if (phase == 7)
                    _wall = MakeStatic(new Vector3(0.1f, 2f, 2f), new Vector3(0.6f, 1.5f, 0));
                break;
            }
            case 8:
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), _ => { });
                break;
            case 9:
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j));
                break;
            case 10:
            case 11:
            case 12:
                // An upper arm (2 kg, 0.3 m) hanging from a static chest through a shoulder-style DRIVE:
                // linear velocity motor = socket, angular velocity motor = muscle, angular limits on.
                // Joint X points down the arm (twist axis), joint Y = world +X, joint Z = world +Z.
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(0.08f, 0.3f, 0.08f), 2f, new Vector3(0, 2.85f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j =>
                {
                    j.Basis = new Basis(Vector3.Forward, Mathf.Pi / 2f);
                    ForAxes((f, p) =>
                    {
                        f(Generic6DofJoint3D.Flag.EnableLinearLimit, false);
                        f(Generic6DofJoint3D.Flag.EnableAngularLimit, true);
                        f(Generic6DofJoint3D.Flag.EnableLinearMotor, true);
                        f(Generic6DofJoint3D.Flag.EnableMotor, true);
                        p(Generic6DofJoint3D.Param.LinearMotorForceLimit, SocketForce);
                        p(Generic6DofJoint3D.Param.AngularMotorForceLimit, phase == 11 ? 30f : 0.5f);
                    }, j);
                    j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, -LimTwist);
                    j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, LimTwist);
                    j.SetParamY(Generic6DofJoint3D.Param.AngularLowerLimit, LimLowerY);
                    j.SetParamY(Generic6DofJoint3D.Param.AngularUpperLimit, LimUpperY);
                    j.SetParamZ(Generic6DofJoint3D.Param.AngularLowerLimit, LimLowerZ);
                    j.SetParamZ(Generic6DofJoint3D.Param.AngularUpperLimit, LimUpperZ);
                });
                _overshootDeg = -999f;
                _maxTwist = 0f;
                _reach = Vector4.Zero;
                _muscleStage = -1;
                _stageErr.Clear();
                break;
        }
    }

    // ---- measurements ----
    private Quaternion FrameA() => (_anchor != null ? _anchor.GlobalTransform.Basis : _bodyA.GlobalTransform.Basis).GetRotationQuaternion() * _frameLocalA.GetRotationQuaternion();
    private Quaternion FrameB() => _box.GlobalTransform.Basis.GetRotationQuaternion() * _frameLocalB.GetRotationQuaternion();
    private Quaternion Rel() => FrameA().Inverse() * FrameB();

    private float SwingAngle()
    {
        Vector3 xa = FrameA() * Vector3.Right;
        Vector3 xb = FrameB() * Vector3.Right;
        return Mathf.RadToDeg(xa.AngleTo(xb));
    }

    private float TwistAngle()
    {
        Quaternion r = Rel();
        return Mathf.RadToDeg(Mathf.Abs(2f * Mathf.Atan2(r.X, r.W)));
    }

    private float PivotSep() => (_box.GlobalTransform * _pivotLocalB).DistanceTo(_pivotWorld);

    private void TrackFinite() => _finite &= BodyMath.IsFinite(_box) && (_bodyA == null || !IsInstanceValid(_bodyA) || BodyMath.IsFinite(_bodyA));

    // ---- phases ----
    private bool PhaseType()
    {
        PhysicsServer3D.JointType type = PhysicsServer3D.JointGetType(_joint.GetRid());
        GD.Print($"6DOF:   server joint type {type}");
        return type == PhysicsServer3D.JointType.Type6Dof;
    }

    private void BallTick(float dt)
    {
        TrackFinite();
        _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep());
        _swingRange = Mathf.Max(_swingRange, Mathf.Abs(SwingAngle() - _startAngle));
    }

    private bool BallFinish()
    {
        GD.Print($"6DOF:   pivot separation max {_maxPivotSep:F4} m (limit {PivotLimit}), swing range {_swingRange:F1} deg (> 20)");
        return _finite && _maxPivotSep < PivotLimit && _swingRange > 20f;
    }

    private void LimitTick(float dt)
    {
        TrackFinite();
        // Strong torques about all axes: swing pushes against the cone, spin against the twist range.
        _box.ApplyTorque(new Vector3(6f, 3f, 6f) * Mathf.Sign(Mathf.Sin(_t * 4f)));
        _maxSwing = Mathf.Max(_maxSwing, SwingAngle());
        _maxTwist = Mathf.Max(_maxTwist, TwistAngle());
        _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep());
    }

    private bool LimitFinish()
    {
        GD.Print($"6DOF:   swing max {_maxSwing:F1} deg (cone {SwingLimitDeg}), twist max {_maxTwist:F1} deg (limit {TwistLimitDeg}), pivot {_maxPivotSep:F4} m");
        return _finite && _maxSwing < SwingLimitDeg + LimitToleranceDeg && _maxTwist < TwistLimitDeg + LimitToleranceDeg
            && _maxSwing > SwingLimitDeg * 0.6f && _maxPivotSep < PivotLimit;
    }

    private Quaternion _muscleTarget = Quaternion.Identity;

    // Muscle = PD on the relative rotation, written as the motor target velocity (joint-frame-A
    // axes); the motor's torque cap is the strength. BodyMath.RotationError gives the error in
    // frame-A coordinates because both quaternions are expressed relative to frame A.
    private void MuscleTick(float dt, bool moving)
    {
        TrackFinite();
        _muscleTarget = moving
            ? new Quaternion(Vector3.Back, Mathf.DegToRad(60f) * Mathf.Min(_t / 1f, 1f))
            : Quaternion.Identity;
        Vector3 err = BodyMath.RotationError(_muscleTarget, Rel());
        Vector3 w = (err * MuscleKp).LimitLength(MuscleMaxSpin);
        _joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X);
        _joint.SetParamY(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Y);
        _joint.SetParamZ(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Z);
        float errDeg = Mathf.RadToDeg(err.Length());
        if (_t > 0.5f)
            _maxErrDeg = Mathf.Max(_maxErrDeg, errDeg);
        _endErrDeg = errDeg;
    }

    private bool MuscleFinish(bool expectHold)
    {
        GD.Print($"6DOF:   lever error after settle max {_maxErrDeg:F1} deg, end {_endErrDeg:F1} deg (gravity torque {LeverMass * 9.81f * LeverLength * 0.5f:F2} N·m)");
        return _finite && (expectHold ? _maxErrDeg < 3f : _maxErrDeg > 20f);
    }

    private bool MuscleTrackFinish()
    {
        GD.Print($"6DOF:   tracking a 60 deg raise: end error {_endErrDeg:F1} deg (limit 3)");
        return _finite && _endErrDeg < 3f;
    }

    private Vector3 DriveTarget => new Vector3(0.4f + 0.5f * Mathf.Min(_t / 1f, 1f), 0, 0);

    private void DriveTick(float dt)
    {
        TrackFinite();
        // Equilibrium points are offsets of the target from the creation pose, in joint frame A.
        Vector3 offset = DriveTarget - new Vector3(0.4f, 0, 0);
        _joint.SetParamX(Generic6DofJoint3D.Param.LinearSpringEquilibriumPoint, offset.X);
        _joint.SetParamY(Generic6DofJoint3D.Param.LinearSpringEquilibriumPoint, offset.Y);
        _joint.SetParamZ(Generic6DofJoint3D.Param.LinearSpringEquilibriumPoint, offset.Z);
    }

    private bool DriveFollowFinish()
    {
        Vector3 handRel = _bodyA.GlobalTransform.AffineInverse() * _box.GlobalPosition;
        float err = handRel.DistanceTo(DriveTarget);
        GD.Print($"6DOF:   hand at {handRel} vs target {DriveTarget}: error {err:F3} m (limit 0.03)");
        return _finite && err < 0.03f;
    }

    private float _maxHandX;

    private void DriveBlockedTick(float dt)
    {
        DriveTick(dt);
        _maxHandX = Mathf.Max(_maxHandX, _box.GlobalPosition.X);
    }

    private bool DriveBlockedFinish()
    {
        float moved = _bodyA.GlobalPosition.X - _bodyAStart.X;
        // Hand half-depth 0.02 → its centre can't pass x = 0.53 without penetrating the wall face.
        GD.Print($"6DOF:   hand x max {_maxHandX:F3} (wall face 0.55, hand half-width 0.02), anchor body moved {moved:F3} m (< -0.02 = pushed away)");
        return _finite && _maxHandX < 0.545f && moved < -0.02f;
    }

    private void WeldTick(float dt)
    {
        TrackFinite();
        _box.ApplyTorque(new Vector3(0.5f, 0.5f, 0.5f));
        _box.ApplyCentralForce(new Vector3(5f, 0, 0));
        _maxSwing = Mathf.Max(_maxSwing, Mathf.RadToDeg(Rel().GetAngle()));
        _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep());
    }

    private bool WeldFinish()
    {
        GD.Print($"6DOF:   rotation drift max {_maxSwing:F2} deg (limit 2), pivot {_maxPivotSep:F4} m");
        return _finite && _maxSwing < 2f && _maxPivotSep < PivotLimit;
    }

    // ---- limited drive (shoulder) ----
    private float _overshootDeg;
    private Vector4 _reach;      // max +Y, max -Y, max +Z, max -Z swing components reached (deg)
    private int _muscleStage;
    private readonly List<string> _stageErr = new();
    private bool _stagesOk = true;

    // Swing rotation vector (joint Y/Z components, rad) taking frame A's X onto frame B's X, and the
    // remaining twist about X (rad).
    private void SwingTwist(out Vector2 swing, out float twist)
    {
        Quaternion rel = Rel();
        Vector3 b = rel * Vector3.Right;
        float angle = Vector3.Right.AngleTo(b);
        Vector3 axis = Vector3.Right.Cross(b);
        swing = axis.LengthSquared() > 1e-12f ? new Vector2(axis.Y, axis.Z).Normalized() * angle : Vector2.Zero;
        Quaternion swingQ = axis.LengthSquared() > 1e-12f ? new Quaternion(axis.Normalized(), angle) : Quaternion.Identity;
        Quaternion t = swingQ.Inverse() * rel;
        twist = 2f * Mathf.Atan2(t.X, t.W);
        if (twist > Mathf.Pi) twist -= Mathf.Tau;
        if (twist < -Mathf.Pi) twist += Mathf.Tau;
    }

    // Radius of the quadrant ellipse in the swing direction (same formula as the fork).
    private static float SwingLimit(Vector2 dir)
    {
        float a = dir.X >= 0f ? LimUpperY : -LimLowerY, b = dir.Y >= 0f ? LimUpperZ : -LimLowerZ;
        float u = dir.X / a, v = dir.Y / b;
        return 1f / Mathf.Sqrt(u * u + v * v);
    }

    // Socket: linear velocity motor toward the pivot (+ a moving offset), like PlayerRagdoll.ShoulderDrive.
    private void SocketDrive(Vector3 offset)
    {
        Vector3 err = _pivotWorld + offset - _box.GlobalTransform * _pivotLocalB;
        Vector3 v = FrameA().Inverse() * (err * SocketKp);
        _joint.SetParamX(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.X);
        _joint.SetParamY(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Y);
        _joint.SetParamZ(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Z);
    }

    private void Muscle(Quaternion target)
    {
        Vector3 err = BodyMath.RotationError(target, Rel());
        Vector3 w = (err * MuscleKp).LimitLength(MuscleMaxSpin);
        _joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X);
        _joint.SetParamY(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Y);
        _joint.SetParamZ(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Z);
    }

    private void TrackLimits()
    {
        TrackFinite();
        SwingTwist(out Vector2 s, out float tw);
        if (s.Length() > Mathf.DegToRad(2f))
            _overshootDeg = Mathf.Max(_overshootDeg, Mathf.RadToDeg(s.Length() - SwingLimit(s.Normalized())));
        _maxTwist = Mathf.Max(_maxTwist, Mathf.RadToDeg(Mathf.Abs(tw)));
        _reach = new Vector4(Mathf.Max(_reach.X, Mathf.RadToDeg(s.X)), Mathf.Max(_reach.Y, -Mathf.RadToDeg(s.X)),
            Mathf.Max(_reach.Z, Mathf.RadToDeg(s.Y)), Mathf.Max(_reach.W, -Mathf.RadToDeg(s.Y)));
        _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep());
    }

    private void LimitedDriveTorqueTick(float dt)
    {
        SocketDrive(Vector3.Zero);
        Muscle(Rel());  // weak (0.5 N·m) muscle holding nothing in particular
        // A hard torque sweeping all swing directions (joint Y/Z) plus an alternating twist torque.
        float phi = _t * 3f;
        Vector3 local = new Vector3(6f * Mathf.Sign(Mathf.Sin(_t * 5f)), 25f * Mathf.Cos(phi), 25f * Mathf.Sin(phi));
        _box.ApplyTorque(FrameA() * local);
        TrackLimits();
    }

    private bool LimitedDriveTorqueFinish()
    {
        GD.Print($"6DOF:   25 N·m sweep: swing overshoot max {_overshootDeg:F1} deg (limit {LimitToleranceDeg}); reached +Y {_reach.X:F0}/60, -Y {_reach.Y:F0}/20, +Z {_reach.Z:F0}/100, -Z {_reach.W:F0}/40 deg; twist max {_maxTwist:F1} deg (limit 45); socket {_maxPivotSep:F4} m");
        bool reached = _reach.X > 50f && _reach.Y > 14f && _reach.Z > 85f && _reach.W > 32f;
        return _finite && _overshootDeg < LimitToleranceDeg && reached && _maxTwist < Mathf.RadToDeg(LimTwist) + LimitToleranceDeg && _maxPivotSep < PivotLimit;
    }

    // Targets (swing Y, swing Z, twist, deg): inside, inside, inside, then one beyond the +Y bound.
    private static readonly Vector3[] MuscleTargets = { new(40f, 0f, 0f), new(0f, 80f, 0f), new(-15f, -30f, 30f), new(85f, 0f, 0f) };

    private static Quaternion SwingTwistQuat(Vector3 deg)
    {
        Vector3 rv = new(0f, Mathf.DegToRad(deg.X), Mathf.DegToRad(deg.Y));
        Quaternion swing = rv.LengthSquared() > 1e-12f ? new Quaternion(rv.Normalized(), rv.Length()) : Quaternion.Identity;
        return swing * new Quaternion(Vector3.Right, Mathf.DegToRad(deg.Z));
    }

    private void LimitedDriveMuscleTick(float dt)
    {
        SocketDrive(Vector3.Zero);
        int stage = Mathf.Min((int)(_t / 1.5f), MuscleTargets.Length - 1);
        Quaternion target = SwingTwistQuat(MuscleTargets[stage]);
        Muscle(target);
        TrackLimits();
        if (_t >= (stage + 1) * 1.5f - dt * 1.5f && _muscleStage < stage)
        {
            _muscleStage = stage;
            SwingTwist(out Vector2 s, out float tw);
            Vector3 got = new(Mathf.RadToDeg(s.X), Mathf.RadToDeg(s.Y), Mathf.RadToDeg(tw));
            if (stage < 3)
            {
                float err = Mathf.RadToDeg(BodyMath.RotationError(target, Rel()).Length());
                _stageErr.Add($"target {MuscleTargets[stage]} → {got.X:F0},{got.Y:F0},{got.Z:F0} err {err:F1}");
                _stagesOk &= err < 3f;
            }
            else
            {
                _stageErr.Add($"target {MuscleTargets[stage]} (beyond +Y 60) → swing Y {got.X:F1}");
                _stagesOk &= got.X < 60f + LimitToleranceDeg && got.X > 55f;
            }
        }
    }

    private bool LimitedDriveMuscleFinish()
    {
        GD.Print($"6DOF:   30 N·m muscle: {string.Join("; ", _stageErr)}; overshoot max {_overshootDeg:F1} deg; socket {_maxPivotSep:F4} m");
        return _finite && _stagesOk && _stageErr.Count == MuscleTargets.Length && _overshootDeg < LimitToleranceDeg && _maxPivotSep < PivotLimit;
    }

    private Vector3 ShrugOffset => new Vector3(0f, 0.05f, -0.03f) * Mathf.Min(_t / 0.5f, 1f);

    private void LimitedDriveSocketTick(float dt)
    {
        SocketDrive(ShrugOffset);
        Muscle(Quaternion.Identity);
        TrackFinite();
        SwingTwist(out Vector2 s, out _);
        _maxSwing = Mathf.Max(_maxSwing, Mathf.RadToDeg(s.Length()));
    }

    private bool LimitedDriveSocketFinish()
    {
        float err = (_box.GlobalTransform * _pivotLocalB).DistanceTo(_pivotWorld + ShrugOffset);
        GD.Print($"6DOF:   socket moved {ShrugOffset.Length():F3} m: tracking error {err:F4} m (limit 0.005), arm swing max {_maxSwing:F1} deg");
        return _finite && err < 0.005f && _maxSwing < 5f;
    }

    private int _toggles;

    private void ToggleTick(float dt)
    {
        TrackFinite();
        _toggles++;
        bool on = _toggles % 2 == 0;
        _joint.SetFlagY(Generic6DofJoint3D.Flag.EnableMotor, on);
        _joint.SetFlagX(Generic6DofJoint3D.Flag.EnableAngularLimit, !on);
        _box.ApplyTorque(new Vector3(1f, 0, 1f));
    }

    private bool ToggleFinish()
    {
        GD.Print($"6DOF:   {_toggles} live flag toggles, finite {_finite}, pivot {PivotSep():F4} m");
        return _finite && PivotSep() < 0.05f;
    }
}