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

// Headless regression for body step 2 — the muscle-driven ragdoll player (PlayerRagdoll) driven
// exactly like PlayerRig drives it, from a simulated play space (origin + head/controller poses).
// godot-mono --headless --fixed-fps 90 --path . scenes/ragdoll_spike.tscn
// Must print "RAGDOLL: overall PASS".
public partial class RagdollSpike : Node3D
{
    private const float EyeHeight = 1.65f;

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

    // Simulated play space.
    private Transform3D _origin = Transform3D.Identity;
    private Transform3D _headLocal, _leftLocal, _rightLocal;
    private Vector3 _stickVelocity;
    private Vector3 _prevL, _prevR;
    private bool _needBuild;

    // Measurements (reset per phase).
    private float _maxEyeErr, _maxHandErr, _endHandErr, _maxSpeed, _minCamZ, _minEyeZ, _maxElbowErr;
    private bool _finite;
    private int _snapsAtStart;
    private float _measureFrom;

    private Transform3D Camera => _origin * _headLocal;
    private Transform3D Left => _origin * _leftLocal;
    private Transform3D Right => _origin * _rightLocal;

    public override void _Ready()
    {
        _profile = BodyProfile.Estimate(EyeHeight);
        AddChild(MakeStatic(new Vector3(40f, 1f, 40f), new Vector3(0f, -0.5f, 0f), 29));
        _ragdoll = new PlayerRagdoll { Name = "Ragdoll" };
        AddChild(_ragdoll);

        _phases.Add(("hinge sign", SetupHinge, TickHinge, 0.3f, FinishHinge));
        _phases.Add(("stands still, head on the camera, hands on the controllers", Rest, _ => { }, 4f, FinishStand));
        _phases.Add(("reaches forward, out and overhead", Rest, TickReach, 4.5f, FinishReach));
        _phases.Add(("fast arm swings stay attached", Rest, TickSwing, 3f, FinishSwing));
        _phases.Add(("stick-walking into a wall is stopped (view pushed back)", SetupWall, _ => _stickVelocity = new Vector3(0f, 0f, -2f), 3f, FinishWall));
        _phases.Add(("room-scale walking into a wall is stopped", SetupWall, TickRoomWalk, 3f, FinishWall));
        _phases.Add(("standing walk into a low bar is blocked", SetupBar, _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 3f, FinishBarBlocked));
        _phases.Add(("crouched walk passes under the low bar", SetupBar, TickCrouchWalk, 4f, FinishBarPassed));
        _phases.Add(("leans over a table without being pushed back", SetupTable, TickLean, 3f, FinishLean));
        _phases.Add(("pushes a 10 kg crate", SetupCrate, TickPush, 3f, FinishCrate));
        _phases.Add(("12 kg held close to the body", () => SetupWeight(close: true), _ => { }, 3f, () => FinishWeight(close: true)));
        _phases.Add(("12 kg held at arm's length sags more", () => SetupWeight(close: false), _ => { }, 3f, () => FinishWeight(close: false)));
        _phases.Add(("teleport + snap turn carry the body", Rest, TickTeleport, 2f, FinishTeleport));
        _phases.Add(("random motion soak", Rest, TickRandom, 8f, FinishSoak));
        _phases.Add(("a knee-high box blocks walking (no stepping onto it)", () => SetupObstacle(0.6f), _ => _stickVelocity = new Vector3(0f, 0f, -1.5f), 3f, FinishBlockedLow));
        _phases.Add(("a table can't be walked over", SetupTableObstacle, _ => _stickVelocity = new Vector3(0f, 0f, -1.5f), 3f, FinishBlockedLow));
        _phases.Add(("walks up a 0.25 m step", () => SetupObstacle(0.25f, 8f), _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 3f, FinishStep));
        _phases.Add(("two-handed mantle onto a 0.9 m ledge", () => SetupObstacle(0.9f), t => TickMantle(both: true), 6f, () => FinishMantle(true)));
        _phases.Add(("one hand can't mantle", () => SetupObstacle(0.9f), t => TickMantle(both: false), 6f, () => FinishMantle(false)));
        _phases.Add(("two-handed pull-up on a grabbed bar", () => SetupBarHang(), t => TickPullUp(both: true), 3.5f, () => FinishPullUp(true)));
        _phases.Add(("one-handed pull-up is weaker", () => SetupBarHang(), t => TickPullUp(both: false), 3.5f, () => FinishPullUp(false)));
        _phases.Add(("crawls through a 0.6 m tunnel", SetupTunnel, TickCrawl, 7f, FinishCrawl));
        _phases.Add(("walks off a 1 m ledge and lands", SetupLedgeTop, _ => _stickVelocity = _t > 0.5f && _t < 2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero, 4f, FinishDrop));
        _phases.Add(("walks up an 18° ramp", () => SetupRamp(18f), _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 4f, () => FinishRamp(true)));
        _phases.Add(("can't walk up a 40° ramp", () => SetupRamp(40f), _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 4f, () => FinishRamp(false)));
        Next();
    }

    // ---------------------------------------------------------------- harness

    public override void _PhysicsProcess(double delta)
    {
        if (_phase >= _phases.Count)
            return;
        float dt = (float)delta;
        _t += dt;
        _phases[_phase].Tick(dt);
        if (_phase > 0)
            Drive(dt);
        if (_t >= _phases[_phase].Duration)
        {
            bool ok = _phases[_phase].Finish();
            _allOk &= ok;
            GD.Print($"RAGDOLL: {_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($"RAGDOLL: overall {(_allOk ? "PASS — muscle ragdoll player functional" : "CHECK — inspect failures above")}");
            GetTree().Quit();
            return;
        }
        _maxEyeErr = _maxHandErr = _endHandErr = _maxSpeed = _maxElbowErr = 0f;
        _minCamZ = _minEyeZ = float.MaxValue;
        _finite = true;
        _measureFrom = 1f;
        _stickVelocity = Vector3.Zero;
        _phases[_phase].Setup();
        _snapsAtStart = _ragdoll.Snaps;
    }

    // Exactly PlayerRig's tick order.
    private void Drive(float dt)
    {
        _origin.Origin += _stickVelocity * dt;
        if (_ragdoll.Built && !_needBuild)
            _origin.Origin += _ragdoll.OriginCorrection(Camera.Origin, _origin.Origin.Y, dt);
        _solver.Solve(_profile, Camera, Left, Right, _origin.Origin.Y, dt);
        if (_needBuild)
        {
            _ragdoll.Build(_profile, _solver, Left, Right);
            _needBuild = false;
            _prevL = Left.Origin;
            _prevR = Right.Origin;
        }
        Vector3 lv = (Left.Origin - _prevL) / dt, rv = (Right.Origin - _prevR) / dt;
        _prevL = Left.Origin;
        _prevR = Right.Origin;
        _ragdoll.Step(_solver, Left, Right, lv, rv, _origin.Origin.Y, dt);
        Measure();
    }

    private void Measure()
    {
        foreach (RigidBody3D b in _ragdoll.Bodies)
            _finite &= BodyMath.IsFinite(b);
        _minCamZ = Mathf.Min(_minCamZ, Camera.Origin.Z);
        _minEyeZ = Mathf.Min(_minEyeZ, _ragdoll.Eye.Z);
        if (_t < _measureFrom)
            return;
        _maxEyeErr = Mathf.Max(_maxEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
        float handErr = Mathf.Max(_ragdoll.Hand(0).GlobalPosition.DistanceTo(Left.Origin), _ragdoll.Hand(1).GlobalPosition.DistanceTo(Right.Origin));
        _maxHandErr = Mathf.Max(_maxHandErr, handErr);
        _endHandErr = handErr;
        float speed = 0f;
        foreach (RigidBody3D b in _ragdoll.Bodies)
            speed = Mathf.Max(speed, b.LinearVelocity.Length());
        _maxSpeed = speed;
        for (int side = 0; side < 2; side++)
        {
            Vector3 up = _solver.Elbow[side] - _solver.Shoulder[side], fore = _solver.Wrist[side] - _solver.Elbow[side];
            float target = up.AngleTo(fore);
            _maxElbowErr = Mathf.Max(_maxElbowErr, Mathf.Abs(target - _ragdoll.ElbowFlex(side)));
        }
    }


    private int NewSnaps => _ragdoll.Snaps - _snapsAtStart;

    private static StaticBody3D MakeStatic(Vector3 size, Vector3 pos, uint mask = 0)
    {
        StaticBody3D body = new() { Position = pos, CollisionMask = mask };
        body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
        return body;
    }

    private RigidBody3D MakeBox(Vector3 size, float mass, Vector3 pos)
    {
        RigidBody3D body = new() { Mass = mass, Position = pos, CollisionMask = 29 };
        body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
        _stage.AddChild(body);
        return body;
    }

    // Standing, looking ahead, hands in front at waist height (elbows ~90°), controllers pointing forward.
    private void Rest()
    {
        _origin = Transform3D.Identity;
        _headLocal = new Transform3D(Basis.Identity, new Vector3(0f, EyeHeight, 0f));
        _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
        _rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 1.0f, -0.3f));
        _solver.Reset();
        _needBuild = true;
    }

    // ---------------------------------------------------------------- hinge sign

    private RigidBody3D _hingeBody;
    private HingeJoint3D _hinge;
    private Basis _hingeStart;

    private void SetupHinge()
    {
        StaticBody3D anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0f, 5f, 0f));
        _stage.AddChild(anchor);
        _hingeBody = new RigidBody3D { Position = new Vector3(0f, 5f, 0f), GravityScale = 0f };
        _hingeBody.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.4f, 0.1f, 0.1f) } });
        _stage.AddChild(_hingeBody);
        _hinge = new HingeJoint3D { Position = new Vector3(0f, 5f, 0f), NodeA = anchor.GetPath(), NodeB = _hingeBody.GetPath() };
        _hinge.SetFlag(HingeJoint3D.Flag.EnableMotor, true);
        _hinge.SetParam(HingeJoint3D.Param.MotorTargetVelocity, 1f);
        _hinge.SetParam(HingeJoint3D.Param.MotorMaxImpulse, 50f);
        _stage.AddChild(_hinge);
        _hingeStart = _hingeBody.GlobalTransform.Basis;
    }

    private void TickHinge(float dt) { }

    private bool FinishHinge()
    {
        // Joint +Z = world +Z here. Positive motor velocity rotated the body about ±Z?
        Vector3 x = _hingeBody.GlobalTransform.Basis.X;
        float about = _hingeStart.X.Cross(x).Z;
        PlayerRagdoll.HingeSign = about >= 0f ? 1f : -1f;
        GD.Print($"RAGDOLL:   +motor velocity turns the child about {(about >= 0 ? "+" : "-")}Z → HingeSign {PlayerRagdoll.HingeSign}");
        return Mathf.Abs(about) > 0.05f;
    }

    // ---------------------------------------------------------------- phases

    private bool FinishStand()
    {
        GD.Print($"RAGDOLL:   eye error max {_maxEyeErr:F3} m, hand error max {_maxHandErr:F3} m, elbow error max {Mathf.RadToDeg(_maxElbowErr):F1}°, end speed {_maxSpeed:F3} m/s, grounded {_ragdoll.Grounded}, snaps {NewSnaps}, pelvis y {_ragdoll.Pelvis.GlobalPosition.Y:F3}");
        return _finite && NewSnaps == 0 && _ragdoll.Grounded && _maxEyeErr < 0.03f && _maxHandErr < 0.03f && _maxSpeed < 0.05f && _maxElbowErr < Mathf.DegToRad(8f);
    }

    private readonly List<string> _reachLog = new();
    private bool _reachOk = true;

    private void TickReach(float dt)
    {
        _measureFrom = float.MaxValue;
        float reach = _profile.ArmLength;
        float sh = EyeHeight - _profile.ShoulderDrop;
        (Vector3 l, Vector3 r)[] poses =
        {
            (new Vector3(-0.2f, sh, -reach - 0.05f), new Vector3(0.2f, sh, -reach - 0.05f)),                                  // forward, straight
            (new Vector3(-_profile.ShoulderHalfWidth - reach, sh, 0f), new Vector3(_profile.ShoulderHalfWidth + reach, sh, 0f)), // T
            (new Vector3(-0.2f, sh + reach, -0.05f), new Vector3(0.2f, sh + reach, -0.05f)),                                     // overhead
        };
        int i = Mathf.Min((int)(_t / 1.5f), poses.Length - 1);
        float local = _t - i * 1.5f;
        // Controllers point along the arm, as a real hand does at full reach.
        Vector3 shL = new(-_profile.ShoulderHalfWidth, sh, 0.06f), shR = new(_profile.ShoulderHalfWidth, sh, 0.06f);
        _leftLocal = new Transform3D(Aim(poses[i].l - shL), poses[i].l);
        _rightLocal = new Transform3D(Aim(poses[i].r - shR), poses[i].r);
        if (local > 1.4f && local - dt <= 1.4f)
        {
            // Unreachable by up to the girdle travel: the hand stops at the arm's end, not inside the body.
            float errL = _ragdoll.Hand(0).GlobalPosition.DistanceTo(Left.Origin), errR = _ragdoll.Hand(1).GlobalPosition.DistanceTo(Right.Origin);
            float e = Mathf.Max(errL, errR);
            _reachLog.Add($"pose {i}: hand error {e:F3} m");
            _reachOk &= e < 0.06f;
        }
    }

    // Controller basis whose aim (−Z) points along `dir`, thumb up where possible.
    private static Basis Aim(Vector3 dir)
    {
        Vector3 z = -dir.Normalized();
        Vector3 up = Mathf.Abs(z.Y) > 0.9f ? Vector3.Back : Vector3.Up;
        Vector3 x = up.Cross(z).Normalized();
        return new Basis(x, z.Cross(x), z);
    }

    private bool FinishReach()
    {
        GD.Print($"RAGDOLL:   {string.Join(", ", _reachLog)}; snaps {NewSnaps}");
        bool ok = _finite && NewSnaps == 0 && _reachOk && _reachLog.Count == 3;
        _reachLog.Clear();
        _reachOk = true;
        return ok;
    }

    private void TickSwing(float dt)
    {
        float a = Mathf.Sin(_t * Mathf.Tau * 1.5f);
        _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.25f, 1.15f + 0.25f * a, -0.35f - 0.15f * a));
        _rightLocal = new Transform3D(new Basis(Vector3.Right, 0.8f * a), new Vector3(0.25f + 0.2f * a, 1.15f - 0.25f * a, -0.35f));
    }

    private bool FinishSwing()
    {
        GD.Print($"RAGDOLL:   hand lag max {_maxHandErr:F3} m (limit 0.15), eye error max {_maxEyeErr:F3}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && _maxHandErr < 0.15f && _maxEyeErr < 0.08f;
    }

    private const float WallFace = -1.0f;

    private void SetupWall()
    {
        Rest();
        _stage.AddChild(MakeStatic(new Vector3(4f, 3f, 0.4f), new Vector3(0f, 1.5f, WallFace - 0.2f), 29));
    }

    private void TickRoomWalk(float dt)
    {
        Vector3 step = new(0f, 0f, -1f * dt);
        _headLocal.Origin += step;
        _leftLocal.Origin += step;
        _rightLocal.Origin += step;
    }

    private bool FinishWall()
    {
        GD.Print($"RAGDOLL:   wall face z {WallFace}: camera min z {_minCamZ:F3}, eye min z {_minEyeZ:F3}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && _minCamZ > WallFace - 0.02f && _minEyeZ > WallFace - 0.02f && _minCamZ < WallFace + 0.5f;
    }

    private const float BarZ = -0.8f;

    private void SetupBar()
    {
        Rest();
        // A beam across the path whose underside is at 1.2 m.
        _stage.AddChild(MakeStatic(new Vector3(4f, 0.1f, 0.1f), new Vector3(0f, 1.25f, BarZ), 29));
    }

    private bool FinishBarBlocked()
    {
        // The chest hits the beam; the head may lean over it, but the view can't run off ahead of the body.
        GD.Print($"RAGDOLL:   bar at z {BarZ}: eye min z {_minEyeZ:F3}, camera min z {_minCamZ:F3}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && _minEyeZ > BarZ - 0.25f && _minCamZ > _minEyeZ - PlayerRagdoll.FreeSlack - 0.02f;
    }

    private void TickCrouchWalk(float dt)
    {
        // Crouch over the first second (eyes to 1.0 m, looking slightly down), then walk forward.
        float k = Mathf.Min(_t / 1f, 1f);
        _headLocal = new Transform3D(new Basis(Vector3.Right, -0.3f * k), new Vector3(0f, Mathf.Lerp(EyeHeight, 1.0f, k), 0f));
        _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, Mathf.Lerp(1.0f, 0.65f, k), -0.3f));
        _rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, Mathf.Lerp(1.0f, 0.65f, k), -0.3f));
        _stickVelocity = _t > 1.2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero;
    }

    private bool FinishBarPassed()
    {
        GD.Print($"RAGDOLL:   crouched: camera min z {_minCamZ:F3} (bar at {BarZ}), eye {_ragdoll.Eye.Y:F2} m, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && _minCamZ < BarZ - 0.6f;
    }

    private void SetupTable()
    {
        Rest();
        _stage.AddChild(MakeStatic(new Vector3(1.4f, 0.05f, 0.8f), new Vector3(0f, 0.725f, -0.85f), 29));
    }

    private void TickLean(float dt)
    {
        float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 1.5f, 1f));
        _headLocal = new Transform3D(new Basis(Vector3.Right, -0.6f * k), new Vector3(0f, Mathf.Lerp(EyeHeight, 1.3f, k), Mathf.Lerp(0f, -0.4f, k)));
        _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 0.85f, Mathf.Lerp(-0.3f, -0.7f, k)));
        _rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 0.85f, Mathf.Lerp(-0.3f, -0.7f, k)));
        _measureFrom = 2f;
    }

    private bool FinishLean()
    {
        GD.Print($"RAGDOLL:   eye error after the lean max {_maxEyeErr:F3} m (limit 0.08), hand error {_endHandErr:F3}, origin drift {_origin.Origin.Length():F3}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && _maxEyeErr < 0.08f && _origin.Origin.Length() < 0.05f;
    }

    private RigidBody3D _crate;
    private Vector3 _crateStart;

    private void SetupCrate()
    {
        Rest();
        _crate = MakeBox(new Vector3(0.4f, 1.4f, 0.4f), 10f, new Vector3(0f, 0.7f, -0.55f));  // within reach at chest height
        _crateStart = _crate.Position;
    }

    private void TickPush(float dt)
    {
        float k = Mathf.Clamp((_t - 0.5f) / 2f, 0f, 1f);
        _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.15f, 1.25f, Mathf.Lerp(-0.2f, -0.6f, k)));
        _rightLocal = new Transform3D(Basis.Identity, new Vector3(0.15f, 1.25f, Mathf.Lerp(-0.2f, -0.6f, k)));
    }

    private bool FinishCrate()
    {
        float moved = _crateStart.Z - _crate.GlobalPosition.Z;
        GD.Print($"RAGDOLL:   crate pushed {moved:F3} m (front face now z {_crate.GlobalPosition.Z + 0.2f:F3}), hands z {_ragdoll.Hand(0).GlobalPosition.Z:F3}/{_ragdoll.Hand(1).GlobalPosition.Z:F3} y {_ragdoll.Hand(0).GlobalPosition.Y:F3}, elbows {Mathf.RadToDeg(_ragdoll.ElbowFlex(0)):F0}°, pelvis z {_ragdoll.Pelvis.GlobalPosition.Z:F3}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && moved > 0.15f;
    }

    private RigidBody3D _weight;
    private float _closeSag = -1f;

    private void SetupWeight(bool close)
    {
        Rest();
        float sh = EyeHeight - _profile.ShoulderDrop;
        _rightLocal = close
            ? new Transform3D(Basis.Identity, new Vector3(0.18f, sh - 0.1f, -0.25f))
            : new Transform3D(Basis.Identity, new Vector3(_profile.ShoulderHalfWidth, sh, -_profile.ArmLength * 0.95f));
        _measureFrom = 2f;
        // Build now so the weight can be welded to the placed hand.
        Drive(1f / 90f);
        RigidBody3D hand = _ragdoll.Hand(1);
        Vector3 at = hand.GlobalTransform * new Vector3(0f, 0f, -0.12f);  // just beyond the fingers
        _weight = MakeBox(new Vector3(0.12f, 0.12f, 0.12f), 12f, at);
        _weight.GlobalPosition = at;
        _ragdoll.SetHeld(1, _weight);
        ConeTwistJoint3D weld = new() { Position = hand.GlobalPosition, NodeA = hand.GetPath(), NodeB = _weight.GetPath(), SwingSpan = 0f, TwistSpan = 0f, Bias = 0f };
        _stage.AddChild(weld);
    }

    private bool FinishWeight(bool close)
    {
        float sag = Right.Origin.Y - _ragdoll.Hand(1).GlobalPosition.Y;
        GD.Print($"RAGDOLL:   right hand sag {sag:F3} m under 12 kg ({(close ? "close" : "far")}), snaps {NewSnaps}");
        if (close)
        {
            _closeSag = sag;
            return _finite && NewSnaps == 0 && sag < 0.08f;
        }
        return _finite && NewSnaps == 0 && sag > _closeSag + 0.03f;
    }

    private bool _teleported, _turned;
    private float _teleportEyeErr;

    private void TickTeleport(float dt)
    {
        _measureFrom = 1.2f;
        if (!_teleported && _t > 0.5f)
        {
            Transform3D before = _origin;
            _origin = new Transform3D(new Basis(Vector3.Up, Mathf.Pi / 2f), new Vector3(5f, 0f, 3f));
            Transform3D delta = _origin * before.AffineInverse();
            _ragdoll.TeleportBy(delta);
            _solver.Shift(delta);
            _prevL = delta * _prevL;
            _prevR = delta * _prevR;
            _teleported = true;
        }
        if (!_turned && _t > 0.9f)
        {
            Vector3 pivot = Camera.Origin;
            Transform3D rot = new Transform3D(Basis.Identity, pivot) * new Transform3D(new Basis(Vector3.Up, Mathf.DegToRad(30f)), Vector3.Zero) * new Transform3D(Basis.Identity, -pivot);
            _origin = rot * _origin;
            _ragdoll.TeleportBy(rot);
            _solver.Shift(rot);
            _prevL = rot * _prevL;
            _prevR = rot * _prevR;
            _turned = true;
        }
    }

    private bool FinishTeleport()
    {
        GD.Print($"RAGDOLL:   after teleport + snap turn: eye error max {_maxEyeErr:F3} m, hand error max {_maxHandErr:F3}, snaps {NewSnaps}");
        _teleported = _turned = false;
        return _finite && NewSnaps == 0 && _maxEyeErr < 0.05f && _maxHandErr < 0.06f;
    }

    // ---------------------------------------------------------------- obstacles, mantling, climbing, crawling

    private const float ObstacleFront = -0.55f;

    // A box from ObstacleFront 2 m deep (wide), `height` tall.
    private void SetupObstacle(float height, float depth = 2f)
    {
        Rest();
        _stage.AddChild(MakeStatic(new Vector3(3f, height, depth), new Vector3(0f, height * 0.5f, ObstacleFront - depth * 0.5f), 29));
    }

    private void SetupTableObstacle()
    {
        Rest();
        _stage.AddChild(MakeStatic(new Vector3(1.4f, 0.05f, 0.8f), new Vector3(0f, 0.725f, ObstacleFront - 0.4f), 29));
        foreach (float x in new[] { -0.65f, 0.65f })
            foreach (float z in new[] { ObstacleFront - 0.05f, ObstacleFront - 0.75f })
                _stage.AddChild(MakeStatic(new Vector3(0.06f, 0.7f, 0.06f), new Vector3(x, 0.35f, z), 29));
    }

    private bool FinishBlockedLow()
    {
        GD.Print($"RAGDOLL:   obstacle front z {ObstacleFront}: pelvis z {_ragdoll.Pelvis.GlobalPosition.Z:F3}, feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, camera min z {_minCamZ:F3}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && _ragdoll.GroundY < 0.1f && _origin.Origin.Y < 0.1f && _ragdoll.Pelvis.GlobalPosition.Z > ObstacleFront - 0.3f;
    }

    private bool FinishStep()
    {
        GD.Print($"RAGDOLL:   step: feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, pelvis z {_ragdoll.Pelvis.GlobalPosition.Z:F3}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && Mathf.Abs(_ragdoll.GroundY - 0.25f) < 0.03f && _origin.Origin.Y > 0.2f && _ragdoll.Pelvis.GlobalPosition.Z < ObstacleFront - 0.3f;
    }

    private float _maxPelvisRise, _lastRise;

    // Put both hands on the ledge, press down (controllers sink as the real arms straighten), then
    // walk forward onto it.
    // Walk up to the ledge (the legs collider stops you), hands on top just ahead, press down.
    private void TickMantle(bool both)
    {
        float t = _t;
        _stickVelocity = t < 1f ? new Vector3(0f, 0f, -0.8f) : t > 3.5f ? new Vector3(0f, 0f, -0.8f) : Vector3.Zero;
        float down = Mathf.Clamp((t - 1.6f) / 1.5f, 0f, 1f);
        float y = t < 1.6f ? Mathf.Lerp(1.0f, 0.95f, Mathf.Clamp(t - 1f, 0f, 0.6f) / 0.6f) : Mathf.Lerp(0.95f, 0.35f, down);
        // Hands just past the ledge edge (aimed in the room, like a real player looking at it).
        float z = t < 1f ? -0.4f : ObstacleFront - 0.1f - _origin.Origin.Z;
        Vector3 l = new(-0.22f, y, z), r = new(0.22f, y, z);
        _rightLocal = new Transform3D(Aim(Vector3.Down + Vector3.Forward * 0.3f), r);
        _leftLocal = both ? new Transform3D(Aim(Vector3.Down + Vector3.Forward * 0.3f), l) : new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
        if (t > 3.6f)
        {
            // Up and walking on: hands come back to the body like a real player's.
            _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
            _rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 1.0f, -0.3f));
        }
        if (_ragdoll.Built && !_needBuild)
            _maxPelvisRise = Mathf.Max(_maxPelvisRise, _ragdoll.Pelvis.GlobalPosition.Y - 0.99f);
    }

    private bool FinishMantle(bool both)
    {
        GD.Print($"RAGDOLL:   mantle ({(both ? "two hands" : "one hand")}): pelvis rose up to {_maxPelvisRise:F3} m, feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, snaps {NewSnaps}");
        bool onTop = _ragdoll.GroundY > 0.85f && _origin.Origin.Y > 0.8f;
        _lastRise = _maxPelvisRise;
        _maxPelvisRise = 0f;
        return _finite && NewSnaps == 0 && (both ? onTop : !onTop && _lastRise < 0.25f);
    }

    private StaticBody3D _bar;
    private const float BarY = 1.95f, BarHangZ = -0.15f;
    private float _barPelvisStart;
    private bool _barGrabbed;

    private void SetupBarHang()
    {
        Rest();
        _bar = new StaticBody3D { Position = new Vector3(0f, BarY, BarHangZ), CollisionMask = 29 };
        _bar.AddChild(new CollisionShape3D { Shape = new CylinderShape3D { Radius = 0.025f, Height = 2f }, RotationDegrees = new Vector3(0f, 0f, 90f) });
        _stage.AddChild(_bar);
        _barGrabbed = false;
        _maxPelvisRise = 0f;
    }

    // Reach up to the bar, grip it (the PropGrabber weld), then pull the controllers down.
    private void TickPullUp(bool both)
    {
        float t = _t;
        float pull = Mathf.Clamp((t - 0.8f) / 1.5f, 0f, 1f);
        float y = Mathf.Lerp(BarY - 0.03f, BarY - 0.75f, pull);
        Basis grip = Aim(Vector3.Up + Vector3.Forward * 0.2f);
        _rightLocal = new Transform3D(grip, new Vector3(0.22f, y, BarHangZ + 0.03f));
        _leftLocal = both ? new Transform3D(grip, new Vector3(-0.22f, y, BarHangZ + 0.03f)) : new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
        if (!_barGrabbed && t > 0.6f && _ragdoll.Built)
        {
            _barGrabbed = true;
            _barPelvisStart = _ragdoll.Pelvis.GlobalPosition.Y;
            for (int side = both ? 0 : 1; side < 2; side++)
            {
                RigidBody3D hand = _ragdoll.Hand(side);
                ConeTwistJoint3D weld = new() { Position = hand.GlobalPosition, NodeA = hand.GetPath(), NodeB = _bar.GetPath(), SwingSpan = 0f, TwistSpan = 0f, Bias = 0f };
                _stage.AddChild(weld);
                _ragdoll.SetHeld(side, _bar);
            }
        }
        if (_barGrabbed)
            _maxPelvisRise = Mathf.Max(_maxPelvisRise, _ragdoll.Pelvis.GlobalPosition.Y - _barPelvisStart);
    }

    private float _twoHandRise;

    private bool FinishPullUp(bool both)
    {
        GD.Print($"RAGDOLL:   pull-up ({(both ? "two hands" : "one hand")}): pelvis rose {_maxPelvisRise:F3} m, braced {_ragdoll.Braced}, snaps {NewSnaps}");
        _ragdoll.SetHeld(0, null);
        _ragdoll.SetHeld(1, null);
        if (both)
        {
            _twoHandRise = _maxPelvisRise;
            return _finite && NewSnaps == 0 && _maxPelvisRise > 0.25f;
        }
        return _finite && NewSnaps == 0 && _maxPelvisRise < _twoHandRise * 0.6f;
    }

    private const float TunnelStart = -1.0f, TunnelEnd = -3.5f;
    private float _minCamY;

    private void SetupTunnel()
    {
        Rest();
        float len = TunnelStart - TunnelEnd;
        float mid = (TunnelStart + TunnelEnd) * 0.5f;
        _stage.AddChild(MakeStatic(new Vector3(1.4f, 0.1f, len), new Vector3(0f, 0.65f, mid), 29));
        _stage.AddChild(MakeStatic(new Vector3(0.1f, 0.6f, len), new Vector3(-0.65f, 0.3f, mid), 29));
        _stage.AddChild(MakeStatic(new Vector3(0.1f, 0.6f, len), new Vector3(0.65f, 0.3f, mid), 29));
        _minCamY = float.MaxValue;
    }

    // Get down on hands and knees (eyes ~0.4 m, looking ahead), then stick-crawl forward.
    private void TickCrawl(float dt)
    {
        float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 2f, 1f));
        _headLocal = new Transform3D(new Basis(Vector3.Right, -0.35f * k), new Vector3(0f, Mathf.Lerp(EyeHeight, 0.42f, k), Mathf.Lerp(0f, -0.25f, k)));
        _leftLocal = new Transform3D(Aim(Vector3.Forward + Vector3.Down), new Vector3(-0.2f, Mathf.Lerp(1.0f, 0.12f, k), Mathf.Lerp(-0.3f, -0.45f, k)));
        _rightLocal = new Transform3D(Aim(Vector3.Forward + Vector3.Down), new Vector3(0.2f, Mathf.Lerp(1.0f, 0.12f, k), Mathf.Lerp(-0.3f, -0.45f, k)));
        _stickVelocity = _t > 2.3f ? new Vector3(0f, 0f, -0.8f) : Vector3.Zero;
        _measureFrom = float.MaxValue;
        if (_t > 2.3f)
            _minCamY = Mathf.Min(_minCamY, Camera.Origin.Y);
    }

    private bool FinishCrawl()
    {
        GD.Print($"RAGDOLL:   crawl: camera min z {_minCamZ:F3} (tunnel {TunnelStart}..{TunnelEnd}), eye y {_ragdoll.Eye.Y:F2}, pelvis y {_ragdoll.Pelvis.GlobalPosition.Y:F2}, origin y {_origin.Origin.Y:F3}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && _minCamZ < TunnelStart - 1.5f && Mathf.Abs(_origin.Origin.Y) < 0.1f;
    }


    // Start standing on a 1 m platform whose edge is 0.5 m ahead.
    private void SetupLedgeTop()
    {
        Rest();
        _origin = new Transform3D(Basis.Identity, new Vector3(0f, 1f, 0f));
        _stage.AddChild(MakeStatic(new Vector3(3f, 1f, 3f), new Vector3(0f, 0.5f, 1f), 29));
    }

    private bool FinishDrop()
    {
        GD.Print($"RAGDOLL:   drop: feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, pelvis {_ragdoll.Pelvis.GlobalPosition}, eye error {_ragdoll.Eye.DistanceTo(Camera.Origin):F3}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && Mathf.Abs(_ragdoll.GroundY) < 0.05f && Mathf.Abs(_origin.Origin.Y) < 0.1f
            && _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.1f && _ragdoll.Pelvis.GlobalPosition.Z > -3f;
    }

    // A slope rising away (−Z) from ObstacleFront up to 1 m, then a platform.
    private void SetupRamp(float degrees)
    {
        Rest();
        float a = Mathf.DegToRad(degrees);
        float len = 1f / Mathf.Sin(a);
        float run = Mathf.Cos(a) * len;
        _stage.AddChild(new StaticBody3D { CollisionMask = 29, Transform = new Transform3D(new Basis(Vector3.Right, a), new Vector3(0f, 0.5f - 0.05f / Mathf.Cos(a), ObstacleFront - run * 0.5f)) });
        _stage.GetChild<StaticBody3D>(_stage.GetChildCount() - 1).AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(3f, 0.1f, len) } });
        _stage.AddChild(MakeStatic(new Vector3(3f, 1f, 3f), new Vector3(0f, 0.5f, ObstacleFront - run - 1.5f), 29));
    }

    private bool FinishRamp(bool walkable)
    {
        GD.Print($"RAGDOLL:   ramp: feet y {_ragdoll.GroundY:F3}, origin y {_origin.Origin.Y:F3}, pelvis {_ragdoll.Pelvis.GlobalPosition}, snaps {NewSnaps}");
        bool up = _ragdoll.GroundY > 0.6f;
        // Stopped at the slope's foot: on the floor, view level, not flung away.
        bool stopped = Mathf.Abs(_origin.Origin.Y) < 0.1f && _ragdoll.Pelvis.GlobalPosition.Length() < 2.5f;
        return _finite && NewSnaps == 0 && (walkable ? up : !up && stopped);
    }

    private readonly RandomNumberGenerator _rng = new() { Seed = 11 };
    private Vector3 _ra, _rb, _rc;

    private void TickRandom(float dt)
    {
        if (_t < dt * 1.5f)
        {
            _ra = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
            _rb = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
            _rc = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
        }
        Vector3 W(Vector3 ph, float f) => new(Mathf.Sin(_t * f + ph.X), Mathf.Sin(_t * f * 1.3f + ph.Y), Mathf.Sin(_t * f * 0.7f + ph.Z));
        Vector3 h = W(_ra, 1.1f);
        _headLocal = new Transform3D(Basis.FromEuler(new Vector3(0.4f * h.X, 0.9f * h.Y, 0.2f * h.Z)), new Vector3(0.1f * h.Z, EyeHeight - 0.15f + 0.1f * h.X, 0.1f * h.Y));
        float sh = EyeHeight - _profile.ShoulderDrop;
        Vector3 l = W(_rb, 2.3f), r = W(_rc, 2.0f);
        _leftLocal = new Transform3D(Basis.FromEuler(new Vector3(1.2f * l.Y, 0.8f * l.Z, 1.0f * l.X)), new Vector3(-0.25f + 0.15f * l.X, sh - 0.25f + 0.2f * l.Y, -0.3f + 0.15f * l.Z));
        _rightLocal = new Transform3D(Basis.FromEuler(new Vector3(1.2f * r.Y, 0.8f * r.Z, 1.0f * r.X)), new Vector3(0.25f + 0.15f * r.X, sh - 0.25f + 0.2f * r.Y, -0.3f + 0.15f * r.Z));
    }

    private bool FinishSoak()
    {
        GD.Print($"RAGDOLL:   random motion: eye error max {_maxEyeErr:F3}, hand error max {_maxHandErr:F3}, elbow error max {Mathf.RadToDeg(_maxElbowErr):F1}°, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && _maxEyeErr < 0.08f && _maxHandErr < 0.12f;
    }
}