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;

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

    // What the ragdoll sees. REAL-INPUT mode (default; `-- ideal` disables it) reproduces the headset:
    // tracking poses update at the 90 Hz display rate (sample-and-hold between 144 Hz physics ticks)
    // with a little jitter — the smooth per-tick input of the ideal mode hid the 2.1 instabilities.
    private Transform3D Camera => _origin * Tracked(0, _headLocal);
    private Transform3D Left => _origin * Tracked(1, _leftLocal);
    private Transform3D Right => _origin * Tracked(2, _rightLocal);

    private bool _realInput = true, _noiseOn = true, _holdOn = true;
    private string[] _only;   // `-- only=jump,fall`: run just the phases whose names contain one of these
    private const float TrackingHz = 90f;
    private readonly Transform3D[] _held = new Transform3D[3];
    private readonly long[] _heldFrame = { -1, -1, -1 };
    private readonly RandomNumberGenerator _noise = new() { Seed = 3 };
    private float _clock;

    private Transform3D Tracked(int i, Transform3D truth)
    {
        if (!_realInput)
            return truth;
        long frame = _holdOn ? (long)(_clock * TrackingHz) : (long)(_clock * 100000f);
        if (frame != _heldFrame[i])
        {
            _heldFrame[i] = frame;
            float p = _noiseOn ? 0.0005f : 0f, r = _noiseOn ? Mathf.DegToRad(0.1f) : 0f;   // pessimistic for inside-out tracking
            Vector3 dp = new(_noise.Randfn(0f, p), _noise.Randfn(0f, p), _noise.Randfn(0f, p));
            Basis dr = Basis.FromEuler(new Vector3(_noise.Randfn(0f, r), _noise.Randfn(0f, r), _noise.Randfn(0f, r)));
            _held[i] = new Transform3D(dr * truth.Basis, truth.Origin + dp);
        }
        return _held[i];
    }

    public override void _Ready()
    {
        _profile = BodyProfile.Estimate(EyeHeight);
        foreach (string arg in OS.GetCmdlineUserArgs())
        {
            if (arg == "ideal")
                _realInput = false;
            if (arg == "nonoise")
                _noiseOn = false;
            if (arg == "nohold")
                _holdOn = false;
            if (arg.StartsWith("only="))
                _only = arg[5..].Split(',');
        }
        GD.Print($"RAGDOLL: input mode {(_realInput ? "REAL (90 Hz tracking, jitter)" : "ideal")} at {Engine.PhysicsTicksPerSecond} Hz physics");
        StaticBody3D floor = MakeStatic(new Vector3(40f, 1f, 40f), new Vector3(0f, -0.5f, 0f), 29);
        floor.AddToGroup(PropGrabber.NoGrabGroup);   // like main.tscn's floor
        AddChild(floor);
        _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 (crouch, hands on the edge, stand and press)", () => SetupObstacle(0.9f), _ => TickHumanMantle(both: true), 7f, () => FinishMantle(true)));
        _phases.Add(("one hand can't mantle", () => SetupObstacle(0.9f), _ => TickHumanMantle(both: false), 7f, () => 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(("holds a crouch (eyes 1.0 m) without sliding or spinning", Rest, t => TickCrouchHold(1.0f), 5f, FinishCrouchHold));
        _phases.Add(("holds a deep crouch (eyes 0.7 m) without sliding or spinning", Rest, t => TickCrouchHold(0.7f), 5f, FinishCrouchHold));
        _phases.Add(("holds poses (stand, crouch, deep crouch, lean, reach, kneel) without view/body drift", Rest, TickPoseHold, 18f, FinishPoseHold));
        _phases.Add(("hanging from a bar keeps the view on the body", () => SetupBarHang(), TickHang, 4f, FinishHang));
        _phases.Add(("climbs a wall hand over hand (grab, pull, regrab higher)", SetupClimbWall, TickClimbWall, 8f, FinishClimbWall));
        _phases.Add(("walks off a 1 m ledge and lands: a dip the view follows, no bounce", () => SetupLedgeTop(1f), TickDrop, 4f, () => FinishDrop(1f)));
        _phases.Add(("big fall (5 m): legs fold fully, then it recovers", () => SetupLedgeTop(5f), TickDrop, 5f, () => FinishDrop(5f)));
        _phases.Add(("standing jump", Rest, t => TickJump(0f), 4f, () => FinishJump(crouched: false)));
        _phases.Add(("jump from a crouch goes higher", Rest, t => TickJump(0.45f), 4.5f, () => FinishJump(crouched: true)));
        _phases.Add(("looks down past vertical while crouched (body doesn't flip around)", Rest, TickLookDown, 5f, FinishLookDown));
        _phases.Add(("stick crouch lowers the body and the view, and stands back up", Rest, TickStickCrouch, 6f, FinishStickCrouch));
        _phases.Add(("standing on a seesaw tips it (body weight reaches movable ground)", SetupSeesaw, _ => { }, 3f, FinishSeesaw));
        _phases.Add(("playground: swing board hangs, seesaw on its pivot, big plank grabbable", SetupPlayground, _ => { }, 2f, FinishPlayground));
        _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)));
        if (_only != null)
            _phases.RemoveAll(ph => ph.Name != "hinge sign" && !Array.Exists(_only, o => ph.Name.Contains(o)));
        Next();
    }

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

    public override void _PhysicsProcess(double delta)
    {
        if (_phase >= _phases.Count)
            return;
        float dt = (float)delta;
        _t += dt;
        _clock += 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;
        if (_driver != null)
            _driver.StickCrouch = 0f;
        _phases[_phase].Setup();
        _snapsAtStart = _ragdoll.Snaps;
    }

    // Exactly PlayerRig's tick order.
    private void Drive(float dt)
    {
        _driver ??= new RagdollDriver { Solver = _solver, Ragdoll = _ragdoll };
        Vector3 stick = _ragdoll.Built && !_needBuild ? PlayerRig.HoldBack(_stickVelocity, _ragdoll.HeldBack) : _stickVelocity;
        _origin.Origin += stick * dt;
        _driver.Tick(_profile, ref _origin, Tracked(0, _headLocal), Tracked(1, _leftLocal), Tracked(2, _rightLocal), _stickVelocity, dt);
        Measure();
    }

    private RagdollDriver _driver;
    private bool _needBuild
    {
        get => _driver == null || _driver.RebuildRequested;
        set
        {
            _driver ??= new RagdollDriver { Solver = _solver, Ragdoll = _ragdoll };
            _driver.RebuildRequested = value;
            if (value)
            {
                // New phase: resample tracking now (a held sample from the last phase would build
                // the body where the previous phase left the head).
                for (int i = 0; i < 3; i++)
                    _heldFrame[i] = -1;
                _driver.Head.Reset();
                _driver.Left.Reset();
                _driver.Right.Reset();
            }
        }
    }

    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()
    {
        float core = Mathf.Max(_ragdoll.Pelvis.LinearVelocity.Length(), Mathf.Max(_ragdoll.Chest.LinearVelocity.Length(), _ragdoll.Head.LinearVelocity.Length()));
        GD.Print($"RAGDOLL:   body core speed at rest {core:F3} m/s (limit 0.03)");
        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 && core < 0.03f && _maxSpeed < 0.15f && _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);
            _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);
            _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.
    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);  // standing tall also lifts the hips; one hand never gets you up
    }

    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));
        bool handsAtBar = _ragdoll.Built && !_needBuild
            && _ragdoll.Hand(1).GlobalPosition.DistanceTo(_bar.GlobalPosition + new Vector3(0.22f, 0f, 0f)) < 0.1f
            && (!both || _ragdoll.Hand(0).GlobalPosition.DistanceTo(_bar.GlobalPosition + new Vector3(-0.22f, 0f, 0f)) < 0.1f);
        if (!_barGrabbed && t > 0.3f && handsAtBar)
        {
            // The game's own world-grab detection must find the bar under the palm.
            if (both)
            {
                for (int side = 0; side < 2; side++)
                {
                    RigidBody3D hand = _ragdoll.Hand(side);
                    Vector3 palm = PropGrabber.PalmPoint(hand, side);
                    StaticBody3D found = PropGrabber.WorldUnderPalm(palm, hand.GetWorld3D()) ?? PropGrabber.WorldUnderPalm(hand.GlobalPosition, hand.GetWorld3D());
                    _grabDetect &= found == _bar;
                    GD.Print($"RAGDOLL:     world grab detection side {side}: palm {palm}, bar {_bar.GlobalPosition}, found {(found == null ? "nothing" : found.Name)}");
                }
            }
            _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 _grabDetect = true;

    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;
            GD.Print($"RAGDOLL:   world grab detection {(_grabDetect ? "found the bar" : "FAILED")}");
            return _finite && NewSnaps == 0 && _maxPelvisRise > 0.25f && _grabDetect;
        }
        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 platform whose edge is 0.5 m ahead.
    private void SetupLedgeTop(float height)
    {
        Rest();
        _origin = new Transform3D(Basis.Identity, new Vector3(0f, height, 0f));
        _stage.AddChild(MakeStatic(new Vector3(3f, height, 3f), new Vector3(0f, height * 0.5f, 1f), 29));
        _dropEyeErr = _dropEyeErrLate = _dropRebound = 0f;
        _dropMinPelvis = float.MaxValue;
        _dropStand = 0f;
    }

    private float _dropEyeErr, _dropEyeErrLate, _dropMinPelvis = float.MaxValue, _dropStand, _dropRebound;

    private void TickDrop(float dt)
    {
        _stickVelocity = _t > 0.5f && _t < 2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero;
        if (!_ragdoll.Built || _needBuild)
            return;
        float pelvis = _ragdoll.Pelvis.GlobalPosition.Y;
        if (_t < 0.5f)
            _dropStand = pelvis - _ragdoll.GroundY;   // standing hips above the feet
        else
        {
            _dropEyeErr = Mathf.Max(_dropEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
            if (_ragdoll.Grounded && _ragdoll.GroundY < 0.5f)
            {
                if (pelvis < _dropMinPelvis)
                {
                    _dropMinPelvis = pelvis;
                    _dropRebound = pelvis;
                }
                _dropRebound = Mathf.Max(_dropRebound, pelvis);
            }
            if (_dropMinPelvis < float.MaxValue && _ragdoll.Pelvis.LinearVelocity.Y > -0.5f)
                _dropEyeErrLate = Mathf.Max(_dropEyeErrLate, _ragdoll.Eye.DistanceTo(Camera.Origin));
        }
    }

    private bool FinishDrop(float height)
    {
        float dip = _dropStand - _dropMinPelvis, overshoot = _dropRebound - _dropStand;
        // The dip grows with the impact; a big fall folds the legs (hips near the ground). Rebound
        // past the stance would be a bounce. The view rides the body throughout.
        bool dipOk = height < 2f ? dip > 0.12f && dip < 0.5f : dip > 0.45f;
        GD.Print($"RAGDOLL:   landing from {height} m: dip {dip:F3} m (stance {_dropStand:F3}), rebound above the stance {overshoot:F3} (limit 0.05), eye vs camera max {_dropEyeErr:F3} / after the impact {_dropEyeErrLate:F3} (limit 0.1)");
        bool landed = dipOk && overshoot < 0.05f && _dropEyeErrLate < 0.1f;
        if (!landed)
            return false;
        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
            && Mathf.Abs(_ragdoll.Pelvis.GlobalPosition.Y - _ragdoll.GroundY - _dropStand) < 0.05f;
    }

    // ---------------------------------------------------------------- jump

    private float _jumpStand, _jumpApex, _jumpEyeErr, _standJumpApex;
    private bool _jumpPressed;

    // Optionally squat (eyes `crouch` lower) first, then press jump at 2 s; the player keeps the pose.
    private void TickJump(float crouch)
    {
        float k = Mathf.SmoothStep(0f, 1f, Mathf.Clamp((_t - 0.8f) / 0.8f, 0f, 1f)) * (crouch > 0f ? 1f : 0f);
        _headLocal = new Transform3D(new Basis(Vector3.Right, -0.2f * k), new Vector3(0f, EyeHeight - crouch * k, -0.05f * k));
        _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f - crouch * k, -0.3f));
        _rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 1.0f - crouch * k, -0.3f));
        _measureFrom = float.MaxValue;
        if (!_ragdoll.Built || _needBuild)
            return;
        float pelvis = _ragdoll.Pelvis.GlobalPosition.Y;
        if (_t < 0.7f)
        {
            _jumpStand = pelvis;
            _jumpApex = 0f;
            _jumpEyeErr = 0f;
            _jumpPressed = false;
        }
        if (_t >= 2f && !_jumpPressed)
        {
            _jumpPressed = true;
            _ragdoll.Jump();
        }
        if (_jumpPressed)
        {
            _jumpApex = Mathf.Max(_jumpApex, pelvis - _jumpStand);
            _jumpEyeErr = Mathf.Max(_jumpEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
        }
    }

    private bool FinishJump(bool crouched)
    {
        GD.Print($"RAGDOLL:   {(crouched ? "crouched" : "standing")} jump: hips rose {_jumpApex:F3} m above the standing stance, eye vs camera max {_jumpEyeErr:F3}, grounded {_ragdoll.Grounded}, jumping {_ragdoll.Jumping}, snaps {NewSnaps}");
        bool ok = _finite && NewSnaps == 0 && _ragdoll.Grounded && !_ragdoll.Jumping && _jumpEyeErr < 0.12f && _origin.Origin.Length() < 0.15f;
        if (!crouched)
        {
            _standJumpApex = _jumpApex;
            return ok && _jumpApex > 0.3f && _jumpApex < 0.65f;
        }
        return ok && _jumpApex > _standJumpApex + 0.1f;
    }

    // ---------------------------------------------------------------- looking down past vertical

    private float _lookYaw0, _lookYawMax, _lookChestMax, _lookPitchMax;

    private static float YawOfBody(RigidBody3D b)
    {
        Vector3 f = BodyMath.HeadFacing(b.GlobalTransform.Basis.Orthonormalized());
        return f == Vector3.Zero ? 0f : BodyMath.YawOf(f);
    }

    // Crouch (eyes 1.0 m), then look down to 125° below the horizon (chin on the chest, looking
    // behind the feet) and back, hands down near the floor — interacting with stuff on the ground.
    private void TickLookDown(float dt)
    {
        float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 1f, 1f));
        float pitch = Mathf.DegToRad(125f) * Mathf.Sin(Mathf.Pi * Mathf.Clamp((_t - 1.5f) / 3f, 0f, 1f));
        _lookPitchMax = Mathf.Max(_lookPitchMax, pitch);
        Basis head = new(Vector3.Right, -pitch);
        Vector3 pivot = new(0f, Mathf.Lerp(EyeHeight, 1.0f, k) + TrueBodySolver.EyeToNeckPivot.Y, -0.1f * k + TrueBodySolver.EyeToNeckPivot.Z);
        _headLocal = new Transform3D(head, pivot - head * TrueBodySolver.EyeToNeckPivot);
        _leftLocal = new Transform3D(Aim(Vector3.Down + Vector3.Forward), new Vector3(-0.2f, Mathf.Lerp(1.0f, 0.3f, k), -0.4f));
        _rightLocal = new Transform3D(Aim(Vector3.Down + Vector3.Forward), new Vector3(0.2f, Mathf.Lerp(1.0f, 0.3f, k), -0.4f));
        _measureFrom = float.MaxValue;
        if (!_ragdoll.Built || _needBuild)
            return;
        if (_t < 1.4f)
        {
            _lookYaw0 = YawOfBody(_ragdoll.Pelvis);
            _lookYawMax = _lookChestMax = _lookPitchMax = 0f;
            return;
        }
        _lookYawMax = Mathf.Max(_lookYawMax, Mathf.Abs(Mathf.AngleDifference(_lookYaw0, YawOfBody(_ragdoll.Pelvis))));
        _lookChestMax = Mathf.Max(_lookChestMax, Mathf.Abs(Mathf.AngleDifference(_lookYaw0, YawOfBody(_ragdoll.Chest))));
    }

    private bool FinishLookDown()
    {
        GD.Print($"RAGDOLL:   look down to {Mathf.RadToDeg(_lookPitchMax):F0}°: pelvis yaw change max {Mathf.RadToDeg(_lookYawMax):F1}°, chest {Mathf.RadToDeg(_lookChestMax):F1}° (limit 15), eye error {_ragdoll.Eye.DistanceTo(Camera.Origin):F3}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && _lookYawMax < Mathf.DegToRad(15f) && _lookChestMax < Mathf.DegToRad(15f)
            && _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.08f;
    }

    // ---------------------------------------------------------------- stick crouch

    private float _stickEyeLow, _stickOriginLow, _stickEyeErr;

    private void TickStickCrouch(float dt)
    {
        _measureFrom = float.MaxValue;
        if (_driver == null)
            return;
        // Hold the stick down for 1 s (1.2 m/s → capped at 0.6 m here), hold, then back up.
        float c = _t < 0.5f ? 0f : _t < 3.5f ? Mathf.Min(0.6f, (_t - 0.5f) * 1.2f) : Mathf.Max(0f, 0.6f - (_t - 3.5f) * 1.2f);
        _driver.StickCrouch = c;
        if (_t > 2.5f && _t < 3.4f && _ragdoll.Built)
        {
            _stickEyeLow = _ragdoll.Eye.Y;
            _stickOriginLow = _origin.Origin.Y;
            _stickEyeErr = Mathf.Max(_stickEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
        }
    }

    private bool FinishStickCrouch()
    {
        GD.Print($"RAGDOLL:   stick crouch 0.6 m: eye y {_stickEyeLow:F3} (standing {EyeHeight}), origin y {_stickOriginLow:F3}, eye vs camera {_stickEyeErr:F3}; back up: eye y {_ragdoll.Eye.Y:F3}, origin y {_origin.Origin.Y:F3}, snaps {NewSnaps}");
        bool ok = _finite && NewSnaps == 0 && Mathf.Abs(_stickEyeLow - (EyeHeight - 0.6f)) < 0.1f && Mathf.Abs(_stickOriginLow + 0.6f) < 0.08f
            && _stickEyeErr < 0.06f && Mathf.Abs(_ragdoll.Eye.Y - EyeHeight) < 0.06f && Mathf.Abs(_origin.Origin.Y) < 0.06f;
        _stickEyeErr = 0f;
        return ok;
    }

    private Vector3 _crouchPelvis;
    private float _crouchYaw, _crouchDrift, _crouchSpin;

    // Squat down over 1.5 s (looking a bit down, hands in front of the knees) and hold.
    private void TickCrouchHold(float eye)
    {
        float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 1.5f, 1f));
        float y = Mathf.Lerp(EyeHeight, eye, k);
        _headLocal = new Transform3D(new Basis(Vector3.Right, -0.3f * k), new Vector3(0f, y, -0.1f * k));
        _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, Mathf.Lerp(1.0f, y - 0.6f, k), -0.35f));
        _rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, Mathf.Lerp(1.0f, y - 0.6f, k), -0.35f));
        _measureFrom = float.MaxValue;
        if (!_ragdoll.Built || _needBuild)
            return;
        Vector3 f = -_ragdoll.Pelvis.GlobalTransform.Basis.Z;
        float yaw = Mathf.Atan2(-f.X, -f.Z);
        if (_t < 2f)
        {
            _crouchPelvis = _ragdoll.Pelvis.GlobalPosition;
            _crouchYaw = yaw;
            _crouchDrift = _crouchSpin = 0f;
            return;
        }
        Vector3 d = _ragdoll.Pelvis.GlobalPosition - _crouchPelvis;
        d.Y = 0f;
        _crouchDrift = Mathf.Max(_crouchDrift, d.Length());
        _crouchSpin = Mathf.Max(_crouchSpin, Mathf.Abs(Mathf.AngleDifference(_crouchYaw, yaw)));
    }

    private bool FinishCrouchHold()
    {
        GD.Print($"RAGDOLL:   crouch hold: pelvis drift {_crouchDrift:F3} m, yaw change {Mathf.RadToDeg(_crouchSpin):F1}°, eye error {_ragdoll.Eye.DistanceTo(Camera.Origin):F3}, origin {_origin.Origin}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && _crouchDrift < 0.05f && _crouchSpin < Mathf.DegToRad(5f) && _origin.Origin.Length() < 0.05f
            && _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.05f;
    }

    private float _poseDrift, _poseEyeErr;

    // Six held poses, 3 s each (1 s blend + 2 s hold): the view rides the body, so any pose the body
    // can't match would slowly walk the play space away — that must not happen.
    private void TickPoseHold(float dt)
    {
        _measureFrom = float.MaxValue;
        (float eye, float pitch, float fwd, Vector3 l, Vector3 r)[] poses =
        {
            (EyeHeight, 0f, 0f, new(-0.2f, 1.0f, -0.3f), new(0.2f, 1.0f, -0.3f)),
            (1.0f, -0.3f, -0.1f, new(-0.2f, 0.4f, -0.35f), new(0.2f, 0.4f, -0.35f)),
            (0.7f, -0.3f, -0.1f, new(-0.2f, 0.2f, -0.4f), new(0.2f, 0.2f, -0.4f)),
            (1.35f, -0.6f, -0.35f, new(-0.2f, 0.9f, -0.6f), new(0.2f, 0.9f, -0.6f)),
            (EyeHeight, 0f, 0f, new(-0.2f, 1.45f, -0.9f), new(0.25f, 1.6f, -0.85f)),
            (0.9f, 0f, 0.05f, new(-0.25f, 0.6f, -0.2f), new(0.25f, 0.6f, -0.2f)),
        };
        int i = Mathf.Min((int)(_t / 3f), poses.Length - 1);
        int prev = Mathf.Max(i - 1, 0);
        float k = Mathf.SmoothStep(0f, 1f, Mathf.Clamp((_t - i * 3f) / 1f, 0f, 1f));
        var a = poses[prev];
        var b = poses[i];
        _headLocal = new Transform3D(new Basis(Vector3.Right, Mathf.Lerp(a.pitch, b.pitch, k)), new Vector3(0f, Mathf.Lerp(a.eye, b.eye, k), Mathf.Lerp(a.fwd, b.fwd, k)));
        _leftLocal = new Transform3D(Basis.Identity, a.l.Lerp(b.l, k));
        _rightLocal = new Transform3D(Basis.Identity, a.r.Lerp(b.r, k));
        if (_ragdoll.Built && !_needBuild && _t - i * 3f > 2.5f)
            _poseEyeErr = Mathf.Max(_poseEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
        _poseDrift = Mathf.Max(_poseDrift, new Vector2(_origin.Origin.X, _origin.Origin.Z).Length());
    }

    private bool FinishPoseHold()
    {
        GD.Print($"RAGDOLL:   pose hold: max play-space drift {_poseDrift:F3} m (limit 0.05), eye vs camera at rest max {_poseEyeErr:F3} m (limit 0.08), snaps {NewSnaps}");
        bool ok = _finite && NewSnaps == 0 && _poseDrift < 0.05f && _poseEyeErr < 0.08f;
        _poseDrift = _poseEyeErr = 0f;
        return ok;
    }

    private float _hangEyeErr;

    // Grab the bar and just hang (controllers stay up): the view must stay on the character's eye.
    private void TickHang(float dt)
    {
        TickPullUp(both: true);
        float y = BarY - 0.03f;
        Basis grip = Aim(Vector3.Up + Vector3.Forward * 0.2f);
        _rightLocal = new Transform3D(grip, new Vector3(0.22f, y, BarHangZ + 0.03f));
        _leftLocal = new Transform3D(grip, new Vector3(-0.22f, y, BarHangZ + 0.03f));
        _headLocal = new Transform3D(Basis.Identity, new Vector3(0f, EyeHeight + 0.15f * Mathf.Sin(_t * 2f), 0.05f * Mathf.Sin(_t * 1.3f)));
        if (_barGrabbed && _t > 1.5f)
            _hangEyeErr = Mathf.Max(_hangEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
    }

    private bool FinishHang()
    {
        GD.Print($"RAGDOLL:   hang: eye vs camera max {_hangEyeErr:F3} m (limit 0.08), climbing {_ragdoll.Climbing}, snaps {NewSnaps}");
        _ragdoll.SetHeld(0, null);
        _ragdoll.SetHeld(1, null);
        bool ok = _finite && NewSnaps == 0 && _hangEyeErr < 0.08f;
        _hangEyeErr = 0f;
        return ok;
    }

    // Walk to the waist-high ledge, crouch a little with the hands on the edge close to the body,
    // then stand up while pressing the hands down, lean in, walk on.
    private void TickHumanMantle(bool both)
    {
        float t = _t;
        _stickVelocity = t < 1f || (t > 4.5f && t < 5.5f) ? new Vector3(0f, 0f, -0.8f) : Vector3.Zero;   // stop before the ledge's far end
        float crouch = Mathf.SmoothStep(0f, 1f, Mathf.Clamp((t - 1f) / 0.8f, 0f, 1f));
        float rise = Mathf.SmoothStep(0f, 1f, Mathf.Clamp((t - 2.2f) / 1.6f, 0f, 1f));
        float eye = EyeHeight - 0.25f * crouch + 0.35f * rise;
        float lean = 0.25f * rise;
        _headLocal = new Transform3D(new Basis(Vector3.Right, -0.3f * crouch), new Vector3(0f, eye, -lean));
        float handZ = ObstacleFront - 0.08f - _origin.Origin.Z;
        float handY = t < 1f ? 1.0f : Mathf.Lerp(0.95f, 0.55f, rise) - 0.05f * crouch;
        Basis down = Aim(Vector3.Down + Vector3.Forward * 0.3f);
        _leftLocal = both ? new Transform3D(down, new Vector3(-0.2f, handY, handZ)) : new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
        _rightLocal = new Transform3D(down, new Vector3(0.2f, handY, handZ));
        if (_ragdoll.Built && !_needBuild)
            _maxPelvisRise = Mathf.Max(_maxPelvisRise, _ragdoll.Pelvis.GlobalPosition.Y - 0.99f);
        if (t > 4.5f)
        {
            _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));
            _headLocal = new Transform3D(Basis.Identity, new Vector3(0f, EyeHeight, 0f));
        }
    }

    private const float ClimbFace = -0.4f;
    private StaticBody3D _wall;
    private readonly ConeTwistJoint3D[] _climbWeld = new ConeTwistJoint3D[2];
    private readonly Vector3[] _gripWorld = new Vector3[2];
    private float _climbStartPelvis, _climbEyeErr;

    private void SetupClimbWall()
    {
        Rest();
        _wall = MakeStatic(new Vector3(3f, 4f, 0.5f), new Vector3(0f, 2f, ClimbFace - 0.25f), 29);
        _stage.AddChild(_wall);
        _climbWeld[0] = _climbWeld[1] = null;
        _gripWorld[0] = new Vector3(-0.2f, 1.75f, ClimbFace + 0.05f);
        _gripWorld[1] = new Vector3(0.2f, 1.75f, ClimbFace + 0.05f);
        _climbEyeErr = 0f;
    }

    private void Grip(int side, bool on)
    {
        if (on && _climbWeld[side] == null)
        {
            RigidBody3D hand = _ragdoll.Hand(side);
            _climbWeld[side] = new ConeTwistJoint3D { Position = hand.GlobalPosition, NodeA = hand.GetPath(), NodeB = _wall.GetPath(), SwingSpan = 0f, TwistSpan = 0f, Bias = 0f };
            _stage.AddChild(_climbWeld[side]);
            _ragdoll.SetHeld(side, _wall);
        }
        else if (!on && _climbWeld[side] != null)
        {
            _climbWeld[side].QueueFree();
            _climbWeld[side] = null;
            _ragdoll.SetHeld(side, null);
        }
    }

    // Hold targets live in the world (the holds); while gripping, the player pulls the controllers
    // down in the room. Free hands reach for the next hold 45 cm higher.
    private readonly Vector3[] _gripLocal = new Vector3[2];
    private readonly float[] _pullAtGrip = new float[2];
    private int _climbStep;          // 0 reach+grip both, 1 pull, 2 right moves up, 3 left moves up, 4 pull, 5 done
    private float _climbStepT, _pulled;

    // Like a climber: grip two holds, pull (hands go down ~45 cm relative to the BODY/room — the
    // play space rides up with the body), move one hand to a higher hold and only then the other,
    // pull again.
    private void TickClimbWall(float dt)
    {
        float t = _t;
        _measureFrom = float.MaxValue;
        if (!_ragdoll.Built || _needBuild)
            return;
        if (t < dt * 1.5f)
        {
            _climbStep = 0;
            _climbStepT = 0f;
            _pulled = 0f;
        }
        _climbStepT += dt;
        bool pulling = _climbStep == 1 || _climbStep == 4;
        if (pulling)
            _pulled = Mathf.MoveToward(_pulled, _climbStep == 1 ? 0.45f : 0.9f, 0.4f * dt);
        Basis aim = Aim(Vector3.Up + Vector3.Forward * 0.15f);
        for (int side = 0; side < 2; side++)
        {
            Vector3 local;
            if (_climbWeld[side] != null)
                local = _gripLocal[side] - Vector3.Up * (_pulled - _pullAtGrip[side]);
            else
            {
                Vector3 current = side == 0 ? _leftLocal.Origin : _rightLocal.Origin;
                local = current.MoveToward(_gripWorld[side] - _origin.Origin, 1.2f * dt);
                if (_ragdoll.Hand(side).GlobalPosition.DistanceTo(_gripWorld[side]) < 0.08f && (_climbStep == 0 || _climbStep == 2 + (1 - side)))
                {
                    Grip(side, true);
                    _gripLocal[side] = local;
                    _pullAtGrip[side] = _pulled;
                }
            }
            if (side == 0)
                _leftLocal = new Transform3D(aim, local);
            else
                _rightLocal = new Transform3D(aim, local);
        }
        switch (_climbStep)
        {
            case 0 when _climbWeld[0] != null && _climbWeld[1] != null:
                _climbStartPelvis = _ragdoll.Pelvis.GlobalPosition.Y;
                Next(1);
                break;
            case 1 when _pulled >= 0.45f:
                MoveUp(1);
                Next(2);
                break;
            case 2 when _climbWeld[1] != null && _climbStepT > 0.2f:
                MoveUp(0);
                Next(3);
                break;
            case 3 when _climbWeld[0] != null && _climbStepT > 0.2f:
                Next(4);
                break;
            case 4 when _pulled >= 0.9f:
                Next(5);
                break;
        }
        if (t > 1f)
            _climbEyeErr = Mathf.Max(_climbEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));

        void Next(int step)
        {
            _climbStep = step;
            _climbStepT = 0f;
        }

        void MoveUp(int side)
        {
            Grip(side, false);
            // Next hold: 30 cm above the hand that keeps holding.
            _gripWorld[side] = _ragdoll.Hand(1 - side).GlobalPosition + Vector3.Up * 0.3f + Vector3.Right * (side == 0 ? -0.4f : 0.4f);
            _gripWorld[side].Z = ClimbFace + 0.05f;
        }
    }

    private bool FinishClimbWall()
    {
        float rise = _ragdoll.Pelvis.GlobalPosition.Y - _climbStartPelvis;
        GD.Print($"RAGDOLL:   wall climb: reached step {_climbStep}/5, pelvis rose {rise:F3} m (> 0.35), holding {_climbWeld[0] != null}/{_climbWeld[1] != null}, eye vs camera max {_climbEyeErr:F3} (< {PlayerRagdoll.MaxViewLead + 0.03f:F2}), snaps {NewSnaps}");
        Grip(0, false);
        Grip(1, false);
        // MaxViewLead 0.08 is enforced before each step; this is measured after it (+ one tick of motion).
        // Two 45 cm pulls minus the sag while hanging one-handed during the two hand moves.
        return _finite && NewSnaps == 0 && _climbStep == 5 && rise > 0.35f && _climbEyeErr < PlayerRagdoll.MaxViewLead + 0.03f;
    }

    private RigidBody3D _seesaw;

    // A 3 m plank along Z hinged 0.45 m up at z = −1; the player stands on its near half.
    private void SetupSeesaw()
    {
        Rest();
        StaticBody3D pivot = MakeStatic(new Vector3(0.6f, 0.45f, 0.3f), new Vector3(0f, 0.225f, -1f), 29);
        _stage.AddChild(pivot);
        _seesaw = MakeBox(new Vector3(0.6f, 0.06f, 3f), 12f, new Vector3(0f, 0.48f, -1f));
        HingeJoint3D hinge = new() { Transform = new Transform3D(new Basis(Vector3.Back, Vector3.Up, Vector3.Left), new Vector3(0f, 0.48f, -1f)), NodeA = pivot.GetPath(), NodeB = _seesaw.GetPath() };
        _stage.AddChild(hinge);
        _origin = new Transform3D(Basis.Identity, new Vector3(0f, 0.51f, -0.2f));   // standing on the plank, 0.8 m from the pivot
    }

    private bool FinishSeesaw()
    {
        float tilt = Mathf.RadToDeg(_seesaw.GlobalTransform.Basis.Z.AngleTo(Vector3.Back));
        GD.Print($"RAGDOLL:   seesaw tilted {tilt:F1}° under the player (> 8), player feet y {_ragdoll.GroundY:F2}, snaps {NewSnaps}");
        return _finite && NewSnaps == 0 && tilt > 8f;
    }

    private Playground _playground;

    private void SetupPlayground()
    {
        Rest();
        _playground = new Playground { Position = new Vector3(0f, 0f, 40f) };   // far from the player
        _stage.AddChild(_playground);
    }

    private bool FinishPlayground()
    {
        RigidBody3D board = _playground.GetNode<RigidBody3D>("SwingBoard"), seesaw = _playground.GetNode<RigidBody3D>("Seesaw"), plank = _playground.GetNode<RigidBody3D>("Plank0");
        Vector3 plankEnd = plank.GlobalTransform * new Vector3(0.9f, 0.05f, 0f);
        PhysicsBody3D grabbed = PropGrabber.BodyUnderPalm(plankEnd, plank.GetWorld3D());
        GD.Print($"RAGDOLL:   playground: swing board y {board.GlobalPosition.Y:F2} (hangs ~1.75), seesaw y {seesaw.GlobalPosition.Y:F2} (on pivot ~0.48), plank end grab finds {(grabbed == null ? "nothing" : grabbed.Name)}");
        return board.GlobalPosition.Y > 1.5f && seesaw.GlobalPosition.Y > 0.35f && grabbed is RigidBody3D && grabbed.Name.ToString().StartsWith("Plank");  // planks are stacked
    }

    // 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;
    }
}