using Godot;

public partial class GrabSpike : Node3D
{
    private const float HandMass = 0.4f;
    private const float HandMaxForce = 90f;
    private const float HandMaxSpeed = 2.5f;
    private const float ReachSpeed = 1f;
    private const float HandMaxSpin = 120f;
    private const float HandRadius = 0.05f;
    private const float MoveDistance = 2f;
    private const float MoveDuration = 2f;
    private const float DragRampTime = 1f;
    private const float LiftHeight = 0.3f;
    private const float MaxSpeed = 20f;
    private const float ReachTimeout = 3f;
    private const float RigidWeldBias = 0f;
    private const float RigidRotLimitDeg = 2f;
    private const float RigidSepLimit = 0.01f;
    private const float SpinRotLimitDeg = 3.5f;
    private const float TakeoverRotLimitDeg = 5f;
    private const float SpinLimit = 40f;
    private const float PullDistance = 0.3f;
    private const float SpinAngleDeg = 360f;
    private const float SpinDuration = 2.5f;
    private const float SpinHoldDuration = 0.5f;
    private const float TugDuration = 1.5f;
    private const float SettleDuration = 0.5f;
    private const float FollowDuration = 1f;
    private const float TugPull = 0.5f;
    private const float FollowSway = 0.03f;
    private const float TwoHandDuration = TugDuration + FollowDuration;
    private const float TakeoverDuration = 1.5f;

    private enum Phase { Reach, Drag, Spin, Hand2Reach, TwoHanded, TwoSettle, TwoFollow, Takeover }

    private RigidBody3D _prop;
    private RigidBody3D _hand;
    private RigidBody3D _hand2;
    private Node3D _target;
    private Node3D _target2;
    private ConeTwistJoint3D _joint;
    private ConeTwistJoint3D _joint2;
    private Vector3 _grabPoint;
    private Vector3 _grab2Point;
    private Vector3 _propAnchor;
    private Vector3 _handAnchor;
    private Vector3 _propAnchor2;
    private Vector3 _handAnchor2;
    private Quaternion _relRotation1;
    private Quaternion _relRotation2;
    private Quaternion _spinFrom;
    private Quaternion _spinTo;
    private Quaternion _cmd1 = Quaternion.Identity;
    private Vector3 _takeoverBase;
    private Phase _phase = Phase.Reach;
    private float _phaseStart;
    private float _elapsed;
    private bool _dragRotOk;
    private bool _dragSepOk;
    private bool _dragSpeedOk;
    private bool _spinRotOk;
    private bool _spinSepOk;
    private bool _spinSpeedOk;
    private bool _twoHandedOk;
    private bool _takeoverRotOk;
    private bool _takeoverSepOk;
    private bool _takeoverSpeedOk;
    private float _maxDragRot;
    private float _maxDragSep;
    private float _maxDragPropSpeed;
    private float _maxDragHandSpeed;
    private float _maxSpinRot;
    private float _maxSpinSep;
    private float _maxSpinPropSpeed;
    private float _maxSpinHandSpeed;
    private float _maxTwoAnchor1;
    private float _maxTugAnchor2;
    private float _maxFollowAnchor2;
    private float _maxTwoPropSpeed;
    private float _maxTwoHandSpeed;
    private float _maxTwoHand2Speed;
    private float _maxTwoSpin;
    private float _maxTakeoverRot;
    private float _maxTakeoverSep;
    private float _maxTakeoverPropSpeed;
    private float _maxTakeoverHandSpeed;
    private int _lastLogStep;

    public override void _Ready()
    {
        // Legacy chase bodies (PhysicsHands/Torso/Arms) were tuned at 90 Hz; the project runs 144 Hz
        // since body step 2, so this regression pins their design rate.
        Engine.PhysicsTicksPerSecond = 90;
        StaticBody3D floor = new() { Position = new Vector3(0, -0.5f, 0) };
        floor.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(20, 1, 20) } });
        AddChild(floor);

        _prop = new RigidBody3D { Position = new Vector3(0, 0.15f, 0), Rotation = new Vector3(0, Mathf.Pi / 2f, 0), Mass = 1f };
        _prop.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = Vector3.One * 0.3f } });
        AddChild(_prop);

        _hand = new RigidBody3D
        {
            Position = new Vector3(0, 0.9f, 0),
            Mass = HandMass,
            GravityScale = 0f,
            ContinuousCd = true,
            ContactMonitor = true,
            MaxContactsReported = 4,
        };
        _hand.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = HandRadius } });
        _hand.BodyEntered += OnHand1Touched;
        AddChild(_hand);

        _hand2 = new RigidBody3D
        {
            Position = new Vector3(0, 0.15f, -1f),
            Mass = HandMass,
            GravityScale = 0f,
            ContinuousCd = true,
            ContactMonitor = true,
            MaxContactsReported = 4,
        };
        _hand2.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = HandRadius } });
        _hand2.BodyEntered += OnHand2Touched;
        AddChild(_hand2);

        _target = new Node3D { Position = _prop.Position };
        AddChild(_target);
        _target2 = new Node3D { Position = _hand2.Position };
        AddChild(_target2);
        GD.Print("GRAB: grab spike scene ready");
    }

    private void OnHand1Touched(Node body)
    {
        if (body != _prop || _phase != Phase.Reach)
            return;
        _grabPoint = _hand.GlobalPosition;
        _relRotation1 = _hand.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
            * _prop.GlobalTransform.Basis.GetRotationQuaternion();
        _propAnchor = _prop.GlobalTransform.AffineInverse() * _grabPoint;
        _handAnchor = _hand.GlobalTransform.AffineInverse() * _grabPoint;
        _joint = MakeWeld(_hand, _prop, _grabPoint, RigidWeldBias);
        _maxDragRot = 0f;
        _maxDragSep = 0f;
        _maxDragPropSpeed = 0f;
        _maxDragHandSpeed = 0f;
        _phase = Phase.Drag;
        _phaseStart = _elapsed;
        _lastLogStep = 0;
        GD.Print($"GRAB: rigid weld grab at t={_elapsed:F2} s, pivot {_grabPoint}, server joint type = {PhysicsServer3D.JointGetType(_joint.GetRid())} (expect {PhysicsServer3D.JointType.ConeTwist})");
    }

    private void OnHand2Touched(Node body)
    {
        if (body != _prop || _phase != Phase.Hand2Reach)
            return;
        _grab2Point = _hand2.GlobalPosition;
        _relRotation2 = _hand2.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
            * _prop.GlobalTransform.Basis.GetRotationQuaternion();
        _propAnchor2 = _prop.GlobalTransform.AffineInverse() * _grab2Point;
        _handAnchor2 = _hand2.GlobalTransform.AffineInverse() * _grab2Point;
        _joint2 = MakeWeld(_hand2, _prop, _grab2Point, RigidWeldBias);
        _maxTwoAnchor1 = 0f;
        _maxTugAnchor2 = 0f;
        _maxFollowAnchor2 = 0f;
        _maxTwoPropSpeed = 0f;
        _maxTwoHandSpeed = 0f;
        _maxTwoHand2Speed = 0f;
        _maxTwoSpin = 0f;
        _phase = Phase.TwoHanded;
        _phaseStart = _elapsed;
        _lastLogStep = 0;
        GD.Print($"GRAB: second hand rigid weld at t={_elapsed:F2} s, pivot {_grab2Point}");
    }

    public override void _PhysicsProcess(double delta)
    {
        float dt = (float)delta;
        _elapsed += dt;
        float phaseT = _elapsed - _phaseStart;

        switch (_phase)
        {
            case Phase.Reach:
                _target.GlobalPosition = _prop.GlobalPosition;
                _target2.GlobalPosition = _hand2.GlobalPosition;
                if (phaseT > ReachTimeout)
                    Fail("hand never touched the prop");
                break;
            case Phase.Drag:
            {
                _target.GlobalPosition = _grabPoint + new Vector3(DragOffset(phaseT), LiftHeight * Mathf.Min(phaseT / DragRampTime, 1f), 0f);
                _maxDragPropSpeed = Mathf.Max(_maxDragPropSpeed, _prop.LinearVelocity.Length());
                _maxDragHandSpeed = Mathf.Max(_maxDragHandSpeed, _hand.LinearVelocity.Length());
                TrackRigidWeld(ref _maxDragRot, ref _maxDragSep);
                int step = (int)(phaseT / 0.5f);
                if (step > _lastLogStep && phaseT < MoveDuration)
                {
                    _lastLogStep = step;
                    GD.Print($"GRAB: drag t={phaseT:F2} rotDev={Mathf.RadToDeg(RotDev1()):F2} deg sep={Sep1():F4} m propV={_prop.LinearVelocity.Length():F2}");
                }
                if (phaseT >= MoveDuration)
                    FinishDrag();
                break;
            }
            case Phase.Spin:
            {
                _cmd1 = _spinFrom * new Quaternion(Vector3.Up,
                    Mathf.DegToRad(SpinAngleDeg) * Mathf.SmoothStep(0f, 1f, Mathf.Min(phaseT / SpinDuration, 1f)));
                _target.GlobalPosition = _grabPoint + new Vector3(MoveDistance, LiftHeight, 0f);
                _maxSpinPropSpeed = Mathf.Max(_maxSpinPropSpeed, _prop.LinearVelocity.Length());
                _maxSpinHandSpeed = Mathf.Max(_maxSpinHandSpeed, _hand.LinearVelocity.Length());
                TrackRigidWeld(ref _maxSpinRot, ref _maxSpinSep);
                int step = (int)(phaseT / 0.5f);
                if (step > _lastLogStep)
                {
                    _lastLogStep = step;
                    GD.Print($"GRAB: fast spin t={phaseT:F2} rotDev={Mathf.RadToDeg(RotDev1()):F2} deg sep={Sep1():F4} m handW={_hand.AngularVelocity.Length():F1} rad/s");
                }
                if (phaseT >= SpinDuration + SpinHoldDuration)
                    FinishSpin();
                break;
            }
            case Phase.Hand2Reach:
                _target.GlobalPosition = _grabPoint + new Vector3(MoveDistance, LiftHeight, 0f);
                _target2.GlobalPosition = _prop.GlobalPosition;
                if (phaseT > ReachTimeout)
                    Fail("second hand never touched the prop");
                break;
            case Phase.TwoHanded:
            {
                float pull = TugPull * Mathf.SmoothStep(0f, 1f, Mathf.Min(phaseT / 1f, 1f));
                float zSway = 0.35f * Mathf.Sin(2f * phaseT) * (1f - Mathf.SmoothStep(0f, 1f, Mathf.Min(phaseT / TugDuration, 1f)));
                _target.GlobalPosition = _grabPoint + new Vector3(MoveDistance + pull, LiftHeight, zSway);
                _target2.GlobalPosition = _grab2Point + new Vector3(-pull, 0f, -zSway);
                _maxTwoAnchor1 = Mathf.Max(_maxTwoAnchor1, Sep1());
                _maxTugAnchor2 = Mathf.Max(_maxTugAnchor2, Sep2());
                _maxTwoPropSpeed = Mathf.Max(_maxTwoPropSpeed, _prop.LinearVelocity.Length());
                _maxTwoHandSpeed = Mathf.Max(_maxTwoHandSpeed, _hand.LinearVelocity.Length());
                _maxTwoHand2Speed = Mathf.Max(_maxTwoHand2Speed, _hand2.LinearVelocity.Length());
                _maxTwoSpin = Mathf.Max(_maxTwoSpin, _prop.AngularVelocity.Length());
                int step = (int)(phaseT / 0.5f);
                if (step > _lastLogStep)
                {
                    _lastLogStep = step;
                    GD.Print($"GRAB: two-handed tug t={phaseT:F2} anchor1={Sep1():F3} anchor2={Sep2():F3} spin={_prop.AngularVelocity.Length():F1} propV={_prop.LinearVelocity.Length():F2}");
                }
                if (phaseT >= TugDuration)
                {
                    _phase = Phase.TwoSettle;
                    _phaseStart = _elapsed;
                    _lastLogStep = 0;
                    GD.Print("GRAB: tug done, settling before the follow assert");
                }
                break;
            }
            case Phase.TwoSettle:
            {
                _target.GlobalPosition = _grabPoint + new Vector3(MoveDistance + TugPull, LiftHeight, 0f);
                _target2.GlobalPosition = _prop.GlobalTransform * _propAnchor2;
                if (phaseT >= SettleDuration)
                {
                    _phase = Phase.TwoFollow;
                    _phaseStart = _elapsed;
                    _lastLogStep = 0;
                    GD.Print($"GRAB: settled at anchor2={Sep2():F3} m, second hand now follows with a small out-of-phase sway");
                }
                break;
            }
            case Phase.TwoFollow:
            {
                _target.GlobalPosition = _grabPoint + new Vector3(MoveDistance + TugPull, LiftHeight, 0f);
                _target2.GlobalPosition = _prop.GlobalTransform * _propAnchor2 + new Vector3(0f, 0f, -FollowSway * Mathf.Sin(2f * Mathf.Pi * 1.5f * phaseT));
                _maxTwoAnchor1 = Mathf.Max(_maxTwoAnchor1, Sep1());
                _maxFollowAnchor2 = Mathf.Max(_maxFollowAnchor2, Sep2());
                _maxTwoPropSpeed = Mathf.Max(_maxTwoPropSpeed, _prop.LinearVelocity.Length());
                _maxTwoHandSpeed = Mathf.Max(_maxTwoHandSpeed, _hand.LinearVelocity.Length());
                _maxTwoHand2Speed = Mathf.Max(_maxTwoHand2Speed, _hand2.LinearVelocity.Length());
                _maxTwoSpin = Mathf.Max(_maxTwoSpin, _prop.AngularVelocity.Length());
                int step = (int)(phaseT / 0.5f);
                if (step > _lastLogStep)
                {
                    _lastLogStep = step;
                    GD.Print($"GRAB: two-handed follow t={phaseT:F2} anchor1={Sep1():F3} anchor2={Sep2():F3} propV={_prop.LinearVelocity.Length():F2}");
                }
                if (phaseT >= FollowDuration)
                    FinishTwoHanded();
                break;
            }
            case Phase.Takeover:
            {
                _target.GlobalPosition = _grabPoint + new Vector3(MoveDistance, LiftHeight, 0f);
                float move = PullDistance * Mathf.SmoothStep(0f, 1f, Mathf.Min(phaseT / TakeoverDuration, 1f));
                _target2.GlobalPosition = _takeoverBase + new Vector3(-move, 0f, 0f);
                _maxTakeoverPropSpeed = Mathf.Max(_maxTakeoverPropSpeed, _prop.LinearVelocity.Length());
                _maxTakeoverHandSpeed = Mathf.Max(_maxTakeoverHandSpeed, _hand2.LinearVelocity.Length());
                TrackTakeoverWeld();
                int step = (int)(phaseT / 0.5f);
                if (step > _lastLogStep)
                {
                    _lastLogStep = step;
                    GD.Print($"GRAB: takeover t={phaseT:F2} rotDev={Mathf.RadToDeg(RotDev2()):F2} deg sep={Sep2():F4} m propV={_prop.LinearVelocity.Length():F2}");
                }
                if (phaseT >= TakeoverDuration)
                    Finish();
                break;
            }
        }

        Chase(_hand, _target, dt, _phase == Phase.Reach || _phase == Phase.Hand2Reach ? ReachSpeed : HandMaxSpeed);
        Chase(_hand2, _target2, dt, _phase == Phase.Reach || _phase == Phase.Hand2Reach ? ReachSpeed : HandMaxSpeed);
        SteerHand(_hand, _cmd1, dt);
        SteerHand(_hand2, Quaternion.Identity, dt);
    }

    private ConeTwistJoint3D MakeWeld(RigidBody3D hand, RigidBody3D prop, Vector3 pivot, float bias)
    {
        ConeTwistJoint3D weld = new()
        {
            ExcludeNodesFromCollision = true,
            Position = pivot,
            NodeA = hand.GetPath(),
            NodeB = prop.GetPath(),
            SwingSpan = 0f,
            TwistSpan = 0f,
            Bias = bias,
        };
        AddChild(weld);
        return weld;
    }

    private void TrackRigidWeld(ref float maxRot, ref float maxSep)
    {
        maxRot = Mathf.Max(maxRot, RotDev1());
        maxSep = Mathf.Max(maxSep, Sep1());
    }

    private void TrackTakeoverWeld()
    {
        Quaternion relNow = _hand2.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
            * _prop.GlobalTransform.Basis.GetRotationQuaternion();
        _maxTakeoverRot = Mathf.Max(_maxTakeoverRot, DevAngle(relNow, _relRotation2));
        _maxTakeoverSep = Mathf.Max(_maxTakeoverSep, Sep2());
    }

    private float RotDev1() => DevAngle(
        _hand.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
            * _prop.GlobalTransform.Basis.GetRotationQuaternion(),
        _relRotation1);

    private float RotDev2() => DevAngle(
        _hand2.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
            * _prop.GlobalTransform.Basis.GetRotationQuaternion(),
        _relRotation2);

    private float Sep1() =>
        (_prop.GlobalTransform * _propAnchor).DistanceTo(_hand.GlobalTransform * _handAnchor);

    private float Sep2() =>
        (_prop.GlobalTransform * _propAnchor2).DistanceTo(_hand2.GlobalTransform * _handAnchor2);

    private static float DevAngle(Quaternion relNow, Quaternion rel0)
    {
        Quaternion dev = relNow * rel0.Inverse();
        if (dev.W < 0f)
            dev = new Quaternion(-dev.X, -dev.Y, -dev.Z, -dev.W);
        // GetAngle() NaNs when w exceeds 1 by a float epsilon; clamp like WeldSpike does
        if (float.IsNaN(dev.W))
            return float.NaN;
        return 2f * Mathf.Acos(Mathf.Clamp(Mathf.Abs(dev.W), 0f, 1f));
    }

    private static float DragOffset(float t)
    {
        float cruiseSpeed = MoveDistance / (MoveDuration - DragRampTime);
        if (t <= DragRampTime)
            return 0.5f * cruiseSpeed / DragRampTime * t * t;
        if (t <= MoveDuration - DragRampTime)
            return 0.5f * cruiseSpeed * DragRampTime + cruiseSpeed * (t - DragRampTime);
        float remaining = MoveDuration - t;
        return MoveDistance - 0.5f * cruiseSpeed / DragRampTime * remaining * remaining;
    }

    private static void SteerHand(RigidBody3D hand, Quaternion cmd, float dt)
    {
        Quaternion q = cmd * hand.GlobalTransform.Basis.GetRotationQuaternion().Inverse();
        Vector3 spin = q.GetAxis() * (q.GetAngle() / dt);
        if (spin.Length() > HandMaxSpin)
            spin = spin.Normalized() * HandMaxSpin;
        hand.AngularVelocity = spin;
    }

    private static void Chase(RigidBody3D body, Node3D target, float dt, float maxSpeed)
    {
        Vector3 to = target.GlobalPosition - body.GlobalPosition;
        Vector3 desired = to / dt;
        if (desired.Length() > maxSpeed)
            desired = desired.Normalized() * maxSpeed;
        Vector3 dv = desired - body.LinearVelocity;
        Vector3 force = dv / dt * HandMass;
        if (force.Length() > HandMaxForce)
            force = force.Normalized() * HandMaxForce;
        body.LinearVelocity += force / HandMass * dt;
    }

    private void FinishDrag()
    {
        _dragRotOk = _maxDragRot < Mathf.DegToRad(RigidRotLimitDeg) && !float.IsNaN(_maxDragRot);
        _dragSepOk = _maxDragSep < RigidSepLimit;
        _dragSpeedOk = _maxDragPropSpeed < MaxSpeed && _maxDragHandSpeed < MaxSpeed;
        GD.Print($"GRAB: drag rigid — rot dev max {Mathf.RadToDeg(_maxDragRot):F2} deg (limit {RigidRotLimitDeg}), sep max {_maxDragSep:F4} m (limit {RigidSepLimit}) → {(_dragRotOk && _dragSepOk ? "PASS" : "FAIL")}");
        GD.Print($"GRAB: drag speeds — prop max {_maxDragPropSpeed:F2} m/s, hand max {_maxDragHandSpeed:F2} m/s (limit {MaxSpeed}) → {(_dragSpeedOk ? "PASS" : "FAIL")}");
        _spinFrom = _hand.GlobalTransform.Basis.GetRotationQuaternion();
        _spinTo = _spinFrom * new Quaternion(Vector3.Up, Mathf.DegToRad(SpinAngleDeg));
        _maxSpinRot = 0f;
        _maxSpinSep = 0f;
        _maxSpinPropSpeed = 0f;
        _maxSpinHandSpeed = 0f;
        _phase = Phase.Spin;
        _phaseStart = _elapsed;
        _lastLogStep = 0;
        GD.Print($"GRAB: fast spin starting — {SpinAngleDeg:F0} deg in {SpinDuration:0.#} s around world up");    }

    private void FinishSpin()
    {
        _spinRotOk = _maxSpinRot < Mathf.DegToRad(SpinRotLimitDeg) && !float.IsNaN(_maxSpinRot);
        _spinSepOk = _maxSpinSep < RigidSepLimit;
        _spinSpeedOk = _maxSpinPropSpeed < MaxSpeed && _maxSpinHandSpeed < MaxSpeed;
        GD.Print($"GRAB: fast spin rigid — rot dev max {Mathf.RadToDeg(_maxSpinRot):F2} deg (limit {SpinRotLimitDeg}), sep max {_maxSpinSep:F4} m (limit {RigidSepLimit}) → {(_spinRotOk && _spinSepOk ? "PASS" : "FAIL")}");
        GD.Print($"GRAB: fast spin speeds — prop max {_maxSpinPropSpeed:F2} m/s, hand max {_maxSpinHandSpeed:F2} m/s (limit {MaxSpeed}) → {(_spinSpeedOk ? "PASS" : "FAIL")}");
        _hand.AngularVelocity = Vector3.Zero;
        _hand2.GlobalTransform = new Transform3D(Basis.Identity, _prop.GlobalPosition + new Vector3(0.45f, 0f, 0f));
        _hand2.LinearVelocity = Vector3.Zero;
        _hand2.AngularVelocity = Vector3.Zero;
        _phase = Phase.Hand2Reach;
        _phaseStart = _elapsed;
        _lastLogStep = 0;
    }

    private void FinishTwoHanded()
    {
        bool primaryRigidOk = _maxTwoAnchor1 < RigidSepLimit;
        bool tugRigidOk = _maxTugAnchor2 < RigidSepLimit;
        bool followOk = _maxFollowAnchor2 < RigidSepLimit;
        bool spinOk = _maxTwoSpin < SpinLimit;
        bool speedOk = _maxTwoPropSpeed < MaxSpeed && _maxTwoHandSpeed < MaxSpeed && _maxTwoHand2Speed < MaxSpeed;
        GD.Print($"GRAB: two-handed primary rigid — anchor1 max {_maxTwoAnchor1:F4} m (limit {RigidSepLimit}) → {(primaryRigidOk ? "PASS" : "FAIL")}");
        GD.Print($"GRAB: two-handed tug rigid — anchor2 max {_maxTugAnchor2:F4} m (limit {RigidSepLimit}) → {(tugRigidOk ? "PASS" : "FAIL")}");
        GD.Print($"GRAB: two-handed follow — anchor2 max {_maxFollowAnchor2:F4} m (limit {RigidSepLimit}) → {(followOk ? "PASS" : "FAIL")}");
        GD.Print($"GRAB: two-handed spin — prop max {_maxTwoSpin:F1} rad/s (limit {SpinLimit}) → {(spinOk ? "PASS" : "FAIL")}");
        GD.Print($"GRAB: two-handed speed — prop max {_maxTwoPropSpeed:F2} m/s, hands max {Mathf.Max(_maxTwoHandSpeed, _maxTwoHand2Speed):F2} m/s (limit {MaxSpeed}) → {(speedOk ? "PASS" : "FAIL")}");
        _twoHandedOk = primaryRigidOk && tugRigidOk && followOk && spinOk && speedOk;

        _joint.QueueFree();
        _joint = null;
        _takeoverBase = _target2.GlobalPosition;
        _maxTakeoverRot = 0f;
        _maxTakeoverSep = 0f;
        _maxTakeoverPropSpeed = 0f;
        _maxTakeoverHandSpeed = 0f;
        _phase = Phase.Takeover;
        _phaseStart = _elapsed;
        _lastLogStep = 0;
        GD.Print($"GRAB: primary released, surviving rigid weld untouched (pivot {_grab2Point})");
    }

    private void Finish()
    {
        _takeoverRotOk = _maxTakeoverRot < Mathf.DegToRad(TakeoverRotLimitDeg) && !float.IsNaN(_maxTakeoverRot);
        _takeoverSepOk = _maxTakeoverSep < RigidSepLimit;
        _takeoverSpeedOk = _maxTakeoverPropSpeed < MaxSpeed && _maxTakeoverHandSpeed < MaxSpeed;
        GD.Print($"GRAB: takeover rigid — rot dev max {Mathf.RadToDeg(_maxTakeoverRot):F2} deg (limit {TakeoverRotLimitDeg}), sep max {_maxTakeoverSep:F4} m (limit {RigidSepLimit}) → {(_takeoverRotOk && _takeoverSepOk ? "PASS" : "FAIL")}");
        GD.Print($"GRAB: takeover speeds — prop max {_maxTakeoverPropSpeed:F2} m/s, hand2 max {_maxTakeoverHandSpeed:F2} m/s (limit {MaxSpeed}) → {(_takeoverSpeedOk ? "PASS" : "FAIL")}");
        bool overall = _dragRotOk && _dragSepOk && _dragSpeedOk &&
                       _spinRotOk && _spinSepOk && _spinSpeedOk &&
                       _twoHandedOk &&
                       _takeoverRotOk && _takeoverSepOk && _takeoverSpeedOk;
        GD.Print($"GRAB: overall {(overall ? "PASS — solver-owned rigid weld grab (v5.3) functional on Box3D" : "CHECK — behavior differs, inspect above")}");
        GetTree().Quit();
    }

    private void Fail(string reason)
    {
        GD.Print($"GRAB: overall CHECK — {reason}");
        GetTree().Quit();
    }
}