using System.Collections.Generic;
using Godot;

// Headless regression for the Stage-1 playtest collapse (2026-10-01), now with the full Stage-2
// body (chest + PhysicsArms + hands). Two families:
// - Stacking: seated chest overlapping the static table, 50 kg anvil rigid-welded to two
//   chase hands, the logged grab sequence (L, R two-handed, L release, L re-grab), the real
//   PropShop with the chest plowing through the props. Did NOT reproduce the NaN — kept as
//   the guard for the solver-stacking hypothesis.
// - Yaw drift: head still while the torso yaw target drifts slowly (0.2 deg/s) across the
//   body's yaw — the actual root cause. The old servo's error quaternion hit W = 1.0000001
//   there, GetAngle() = 2*acos(W) went NaN, and AngularVelocity += NaN killed the chest.
// Run: godot-mono --headless --fixed-fps 90 --path . scenes/collapse_spike.tscn
public partial class CollapseSpike : Node3D
{
    private const float PalmGrabOffset = 0.045f;
    private const float RunawaySpeed = 50f;
    private const int DriftYawCount = 40;
    private static readonly Vector3 HandSize = new(0.035f, 0.09f, 0.12f);
    private static readonly Vector3 AnvilSize = new(0.28f, 0.16f, 0.13f);

    private struct Scenario
    {
        public string Name;
        public Vector3 Head;
        public float Duration;
        public bool Anvil;
        public bool Table;
        public bool Props;
        public bool Sweep;
        public bool Still;
        public float Yaw;
        public float YawDrift;
    }

    private readonly List<Scenario> _scenarios = new();
    private int _index = -1;
    private float _t;
    private int _step;
    private Node3D _world;
    private Node3D _head;
    private PhysicsTorso _torso;
    private PhysicsArms _arms;
    private readonly RigidBody3D[] _hands = new RigidBody3D[2];
    private readonly Node3D[] _targets = new Node3D[2];
    private readonly ConeTwistJoint3D[] _welds = new ConeTwistJoint3D[2];
    private readonly Vector3[] _targetBase = new Vector3[2];
    private RigidBody3D _anvil;
    private bool _failed;
    private float _maxTorsoSpeed;
    private float _maxTorsoSpin;
    private float _maxPropSpeed;
    private float _maxYawErrDeg;
    private readonly HashSet<string> _done = new();
    private readonly List<string> _results = new();
    private int _failCount;
    private readonly RandomNumberGenerator _rng = new() { Seed = 1302 };

    public override void _Ready()
    {
        // Legacy chase bodies (PhysicsHands/Torso/Arms) were tuned at 90 Hz; the project runs 144 Hz
        // since body step 2, so this regression pins their design rate.
        Engine.PhysicsTicksPerSecond = 90;
        // A real profile is measured standing once (T-pose) and does not shrink when seated:
        // every scenario, seated or not, uses the same standing 1.65 m-eye profile.
        BodyProfile.Current = BodyProfile.Estimate(1.65f);
        // Seated head 1.01 m (the playtest calibration); chest center z = head z + 0.12.
        // Table front edge is z = -0.2, chest half-depth 0.065: overlap starts at head z < -0.255.
        Add("standing, no table overlap, anvil", new(0.2f, 1.65f, 0.3f), 6f, anvil: true, table: true);
        Add("seated, chest clear of table, anvil", new(0.2f, 1.01f, 0.05f), 6f, anvil: true, table: true);
        Add("seated, chest 5 cm into table, no props", new(0.2f, 1.01f, -0.31f), 6f, table: true);
        Add("seated, chest 15 cm into table, no props", new(0.2f, 1.01f, -0.41f), 6f, table: true);
        Add("seated, chest 5 cm into table, anvil", new(0.2f, 1.01f, -0.31f), 6f, anvil: true, table: true);
        Add("seated, chest 15 cm into table, anvil", new(0.2f, 1.01f, -0.41f), 6f, anvil: true, table: true);
        Add("standing, anvil pressed into chest, no table", new(0.2f, 1.65f, 0.3f), 6f, anvil: true);
        Add("seated, real PropShop, chest sweeps through props, anvil", new(0.2f, 1.01f, -0.31f), 6f, anvil: true, props: true, sweep: true);
        Add("seated, real PropShop, chest sweeps through props, no grabs", new(0.2f, 1.01f, -0.31f), 6f, props: true, sweep: true);
        for (int i = 0; i < DriftYawCount; i++)
        {
            float yaw = -3f + i * 0.1537f;
            _scenarios.Add(new Scenario
            {
                Name = $"seated still, yaw {yaw:F2} drifting 0.2 deg/s, anvil held",
                Head = new(0.2f, 1.01f, 0.05f), Duration = 2.5f, Anvil = true, Table = true,
                Still = true, Yaw = yaw, YawDrift = 0.0035f,
            });
        }
        NextScenario();
    }

    private void Add(string name, Vector3 head, float duration, bool anvil = false, bool table = false, bool props = false, bool sweep = false)
    {
        _scenarios.Add(new Scenario { Name = name, Head = head, Duration = duration, Anvil = anvil, Table = table || props, Props = props, Sweep = sweep });
    }

    private void NextScenario()
    {
        if (_world != null)
        {
            _world.QueueFree();
            _world = null;
        }
        _index++;
        if (_index >= _scenarios.Count)
        {
            foreach (string line in _results)
                GD.Print(line);
            GD.Print($"COLLAPSE: {_scenarios.Count - _failCount}/{_scenarios.Count} scenarios clean");
            GD.Print($"COLLAPSE: overall {(_failCount == 0 ? "PASS — no NaN/runaway in any scenario" : "FAIL — see above")}");
            GetTree().Quit();
            return;
        }
        _t = 0f;
        _step = 0;
        _failed = false;
        _maxTorsoSpeed = 0f;
        _maxTorsoSpin = 0f;
        _maxPropSpeed = 0f;
        _maxYawErrDeg = 0f;
        _done.Clear();
        _welds[0] = _welds[1] = null;
        Build(_scenarios[_index]);
    }

    private void Build(Scenario s)
    {
        _world = new Node3D();
        AddChild(_world);

        StaticBody3D floor = new() { CollisionMask = 0b11101 };
        floor.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(10f, 0.1f, 10f) }, Position = new Vector3(0f, -0.05f, 0f) });
        _world.AddChild(floor);

        if (s.Props)
        {
            _world.AddChild(new PropShop());
        }
        else if (s.Table)
        {
            StaticBody3D table = new() { CollisionMask = 0b11101 };
            table.AddChild(new CollisionShape3D { Position = new Vector3(0f, 0.725f, -0.6f), Shape = new BoxShape3D { Size = new Vector3(1.4f, 0.05f, 0.8f) } });
            foreach (Vector3 o in new Vector3[] { new(-0.62f, 0, 0.32f), new(0.62f, 0, 0.32f), new(-0.62f, 0, -0.32f), new(0.62f, 0, -0.32f) })
                table.AddChild(new CollisionShape3D { Position = new Vector3(o.X, 0.3625f, -0.6f + o.Z), Shape = new BoxShape3D { Size = new Vector3(0.06f, 0.725f, 0.06f) } });
            _world.AddChild(table);
        }

        _head = new Node3D { Position = s.Head };
        _world.AddChild(_head);
        _torso = new PhysicsTorso { HeadSource = _head };
        _world.AddChild(_torso);
        Vector3 chest = s.Head + new Vector3(0f, -0.42f, 0.12f);
        _torso.Body.GlobalPosition = chest;

        Vector3 anvilPos = s.Table ? new Vector3(s.Head.X, 0.83f, Mathf.Min(chest.Z - 0.2f, -0.3f)) : chest + new Vector3(0f, 0f, -0.2f);
        _anvil = null;
        if (s.Props)
        {
            foreach (Node n in GetTree().GetNodesInGroup("prop"))
                if (n is RigidBody3D rb && rb.Name.ToString().StartsWith("Anvil"))
                    _anvil = rb;
            anvilPos = _anvil.GlobalPosition;
            if (!s.Anvil)
                _anvil = null;
        }
        else if (s.Anvil)
        {
            _anvil = new RigidBody3D
            {
                Name = "Anvil",
                Mass = 50f,
                Position = anvilPos,
                CollisionMask = 0b11101,
                GravityScale = s.Table ? 1f : 0f,
                PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0f, Friction = 0.95f },
            };
            _anvil.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = AnvilSize } });
            _world.AddChild(_anvil);
        }

        for (int h = 0; h < 2; h++)
        {
            float side = h == 0 ? -1f : 1f;
            // Palm offset: left +X, right -X — the hand sits just outside the anvil's X face.
            Vector3 handPos = anvilPos + new Vector3(side * (AnvilSize.X * 0.5f + HandSize.X * 0.5f + 0.005f), 0f, 0f);
            _hands[h] = new RigidBody3D
            {
                Mass = 0.4f,
                GravityScale = 0f,
                ContinuousCd = true,
                Position = handPos,
                CollisionLayer = 0b1000,
                CollisionMask = 0b1001,
            };
            _hands[h].AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = HandSize } });
            _world.AddChild(_hands[h]);
            _targets[h] = new Node3D { Position = handPos };
            _world.AddChild(_targets[h]);
            _targetBase[h] = handPos;
        }

        _arms = new PhysicsArms { HeadSource = _head, Torso = _torso, HandLeft = _hands[0], HandRight = _hands[1] };
        _world.AddChild(_arms);
    }

    public override void _PhysicsProcess(double delta)
    {
        if (_world == null)
            return;
        float dt = (float)delta;
        _t += dt;
        _step++;
        Scenario s = _scenarios[_index];

        // Controllers: settle, grab L, grab R, haul the anvil toward the chest and down into the
        // table edge with a wobble, release L, re-grab L (the logged sequence), keep hauling.
        Vector3 haul = Vector3.Zero;
        if (_t > 1.0f && !s.Still)
        {
            float k = Mathf.Min((_t - 1.0f) / 1.0f, 1f);
            haul = new Vector3(0f, -0.05f, 0.35f) * k
                 + new Vector3(0.04f * Mathf.Sin(9f * _t), 0.03f * Mathf.Sin(7f * _t), 0.05f * Mathf.Sin(5f * _t));
        }
        for (int h = 0; h < 2; h++)
            _targets[h].GlobalPosition = _targetBase[h] + haul;

        if (_anvil != null)
        {
            Once("grabL", _t > 0.4f, () => Grab(0));
            Once("grabR", _t > 0.7f, () => Grab(1));
            if (!s.Still)
            {
                Once("releaseL", _t > 2.6f, () => Release(0));
                Once("regrabL", _t > 3.2f, () => Grab(0));
            }
        }

        for (int h = 0; h < 2; h++)
            PhysicsHands.Drive(_hands[h], _targets[h].GlobalTransform, dt);

        Vector3 sway = s.Still ? Vector3.Zero : s.Sweep
            ? new Vector3(-0.2f + 0.55f * Mathf.Sin(0.9f * _t), 0f, -0.12f + 0.14f * Mathf.Sin(1.7f * _t))
            : new Vector3(0.03f * Mathf.Sin(2f * _t), 0f, 0.04f * Mathf.Sin(1.3f * _t));
        // Still: yaw parked, then drifting slowly across the body's yaw from t = 1 s.
        // Moving: arbitrary base yaw plus sub-millidegree tracker noise.
        float yaw = s.Still ? s.Yaw + s.YawDrift * Mathf.Max(_t - 2f, -1f) : 0.37f + 1e-5f * _rng.Randfn();
        _head.GlobalTransform = new Transform3D(new Basis(new Quaternion(Vector3.Up, yaw)), s.Head + sway);

        if (!_failed)
            Monitor(s);

        if (_t >= s.Duration)
        {
            bool ok = !_failed;
            if (!ok)
                _failCount++;
            if (!ok || !s.Still)
                _results.Add($"COLLAPSE: [{(ok ? "PASS" : "FAIL")}] {s.Name}: torso |v| max {_maxTorsoSpeed:F2} m/s, |w| max {_maxTorsoSpin:F2} rad/s, props |v| max {_maxPropSpeed:F2} m/s, torso err {_torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition):F3} m");
            else
                _results.Add($"COLLAPSE: [PASS] {s.Name}: yaw error max after settle {_maxYawErrDeg:F3} deg");
            NextScenario();
        }
    }

    private void Monitor(Scenario s)
    {
        RigidBody3D torso = _torso.Body;
        Check("torso", torso);
        Check("handL", _hands[0]);
        Check("handR", _hands[1]);
        if (_anvil != null)
            Check("anvil", _anvil);
        for (int side = 0; side < 2; side++)
        {
            if (_arms.UpperArm(side) == null)
                continue;
            Check($"upper arm {side}", _arms.UpperArm(side));
            Check($"forearm {side}", _arms.Forearm(side));
            if (_arms.Forearm(side).LinearVelocity.Length() > RunawaySpeed || _arms.UpperArm(side).LinearVelocity.Length() > RunawaySpeed)
                Fail($"arm {side} runaway");
        }
        foreach (Node n in GetTree().GetNodesInGroup("prop"))
        {
            if (n is not RigidBody3D prop || prop.IsQueuedForDeletion())
                continue;
            Check($"prop '{prop.Name}'", prop);
            _maxPropSpeed = Mathf.Max(_maxPropSpeed, prop.LinearVelocity.Length());
            if (prop.LinearVelocity.Length() > RunawaySpeed)
                Fail($"prop '{prop.Name}' runaway {prop.LinearVelocity.Length():F1} m/s");
        }
        if (_failed)
            return;
        _maxTorsoSpeed = Mathf.Max(_maxTorsoSpeed, torso.LinearVelocity.Length());
        _maxTorsoSpin = Mathf.Max(_maxTorsoSpin, torso.AngularVelocity.Length());
        if (torso.LinearVelocity.Length() > RunawaySpeed || (_anvil != null && _anvil.LinearVelocity.Length() > RunawaySpeed))
            Fail($"runaway speed (torso {torso.LinearVelocity.Length():F1}, anvil {_anvil?.LinearVelocity.Length() ?? 0f:F1})");
        if (s.Still && _t > 1.5f)
        {
            Vector3 bodyFwd = -torso.GlobalTransform.Basis.Z;
            Vector3 headFwd = -_head.GlobalTransform.Basis.Z;
            float err = Mathf.RadToDeg(Mathf.Atan2(bodyFwd.Cross(headFwd).Y, bodyFwd.Dot(headFwd)));
            _maxYawErrDeg = Mathf.Max(_maxYawErrDeg, Mathf.Abs(err));
        }
    }

    private void Check(string name, RigidBody3D body)
    {
        if (!body.GlobalTransform.IsFinite() || !body.LinearVelocity.IsFinite() || !body.AngularVelocity.IsFinite())
            Fail($"{name} non-finite: pos {body.GlobalPosition} v {body.LinearVelocity} w {body.AngularVelocity}");
    }

    private void Fail(string why)
    {
        if (_failed)
            return;
        _failed = true;
        GD.Print($"COLLAPSE: FIRST ANOMALY in '{_scenarios[_index].Name}' t={_t:F3}s step {_step}: {why}");
    }

    private void Once(string key, bool cond, System.Action action)
    {
        if (cond && _done.Add(key))
            action();
    }

    private void Grab(int h)
    {
        RigidBody3D hand = _hands[h];
        Vector3 pivot = hand.GlobalPosition + hand.GlobalTransform.Basis * new Vector3(h == 0 ? PalmGrabOffset : -PalmGrabOffset, 0f, 0f);
        ConeTwistJoint3D weld = new()
        {
            ExcludeNodesFromCollision = true,
            Position = pivot,
            NodeA = hand.GetPath(),
            NodeB = _anvil.GetPath(),
            SwingSpan = 0f,
            TwistSpan = 0f,
            Bias = 0f,
        };
        _world.AddChild(weld);
        _welds[h] = weld;
    }

    private void Release(int h)
    {
        _welds[h]?.QueueFree();
        _welds[h] = null;
    }
}