using System.Collections.Generic;
using Godot;

public partial class BodySpike : Node3D
{
    private enum Phase { Settle, Pairs, Track, Yaw, Impact, Snap, Upright }

    private const float PhaseSettle = 0.8f;
    private const float PhasePairs1 = 1.3f;
    private const float PhasePairs2 = 1.7f;
    private const float PhaseTrack = 3.5f;
    private const float PhaseYaw = 2f;
    private const float PhaseImpact = 2f;
    private const float PhaseSnap = 0.3f;
    private const float PhaseUpright = 2f;

    private const float TrackPosLimit = 0.06f;
    private const float TrackSpeedLimit = 8f;
    private const float YawPeakLimitDeg = 15f;
    private const float YawSettleLimitDeg = 4f;
    private const float ImpactDipLimit = 0.2f;
    private const float RecoverLimit = 0.02f;
    private const float ImpactorMinDistLimit = 0.1f;
    private const float ImpactorSpeedAfterHitLimit = 1.5f;
    private const float SnapLimit = 0.05f;
    private const float UprightLimitDeg = 3f;
    private const float YawHoldLimitDeg = 5f;

    private PhysicsTorso _torso;
    private Node3D _head;
    private RigidBody3D _ball;
    private RigidBody3D _handProbe;
    private RigidBody3D _prop;
    private RigidBody3D _impactor;

    private Phase _phase = Phase.Settle;
    private float _phaseStart;
    private float _elapsed;
    private readonly List<string> _resultNames = new();
    private readonly List<bool> _resultOk = new();

    private Vector3 _chestRef;
    private bool _chestStillDuringHandDrop;
    private float _chestDropDip;
    private float _maxTrackErr;
    private float _maxTrackSpeed;
    private float _maxYawErrAfterRamp;
    private float _yawErrEnd;
    private float _maxImpactDip;
    private float _minImpactorDist = float.MaxValue;
    private float _impactorTouchTime = -1f;
    private float _impactorSpeedAfterHit = -1f;

    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;
        BuildWorld();
        _head = new Node3D { Position = new Vector3(0f, 1.6f, 0f) };
        AddChild(_head);

        _torso = new PhysicsTorso { HeadSource = _head };
        AddChild(_torso);
        GD.Print("BODY: body spike scene ready");
    }

    private void BuildWorld()
    {
        StaticBody3D ground = new() { CollisionLayer = 1, CollisionMask = 0 };
        ground.AddChild(new CollisionShape3D
        {
            Shape = new BoxShape3D { Size = new Vector3(20f, 0.1f, 20f) },
            Position = new Vector3(0f, -0.05f, 0f),
        });
        AddChild(ground);

        _ball = new RigidBody3D
        {
            Mass = 1f,
            Position = new Vector3(-2f, 0.4f, 0f),
            CollisionLayer = 1,
            CollisionMask = 1,
            PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.2f, Friction = 0.8f },
        };
        _ball.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.05f } });
        AddChild(_ball);

        _handProbe = new RigidBody3D
        {
            Mass = 0.4f,
            Position = new Vector3(0f, 1.7f, 0.12f),
            CollisionLayer = 0b1000,
            CollisionMask = 0b1001,
            PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0f, Friction = 0.8f },
        };
        _handProbe.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.035f, 0.09f, 0.12f) } });
        AddChild(_handProbe);

        _prop = new RigidBody3D
        {
            Mass = 2f,
            Position = new Vector3(0f, 1.9f, 0.12f),
            CollisionLayer = 1,
            CollisionMask = 0b1101,
            PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.3f, Friction = 0.8f },
        };
        _prop.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.06f } });
        AddChild(_prop);
        _prop.Freeze = true;
        _prop.Visible = false;

        _impactor = new RigidBody3D
        {
            Mass = 5f,
            GravityScale = 0f,
            CollisionLayer = 1,
            CollisionMask = 0b1101,
            PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.2f, Friction = 0.6f },
        };
        _impactor.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.06f } });
        AddChild(_impactor);
        _impactor.Freeze = true;
    }

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

        Vector3 p = _torso.Body.GlobalPosition;
        if (float.IsNaN(p.X) || float.IsNaN(p.Y) || float.IsNaN(p.Z))
        {
            Record("no NaN in body state", false);
            Finish();
            return;
        }

        switch (_phase)
        {
            case Phase.Settle:
                if (phaseT >= PhaseSettle)
                {
                    _chestRef = _torso.Body.GlobalPosition;
                    StartPhase(Phase.Pairs);
                    GD.Print("BODY: pairs phase — one-way floor, hand-through-chest exclusion, prop contact");
                }
                break;

            case Phase.Pairs:
            {
                if (phaseT >= PhasePairs1 && _prop.Freeze)
                {
                    _chestStillDuringHandDrop = _torso.Body.GlobalPosition.DistanceTo(_chestRef) < 0.02f;
                    _chestRef = _torso.Body.GlobalPosition;
                    _chestDropDip = 0f;
                    _prop.Freeze = false;
                    _prop.Visible = true;
                    GD.Print("BODY: dropping 2 kg ball onto chest top");
                }
                _chestDropDip = Mathf.Max(_chestDropDip, _torso.Body.GlobalPosition.DistanceTo(_chestRef));
                if (phaseT >= PhasePairs1 + PhasePairs2)
                    FinishPairs();
                break;
            }

            case Phase.Track:
            {
                float t = phaseT;
                Vector3 headPos = new(0.6f * Mathf.Sin(1.1f * t), 1.6f, 0.8f * Mathf.Sin(0.7f * t));
                Basis headBasis = new(new Quaternion(Vector3.Up, 0.6f * Mathf.Sin(0.5f * t)));
                _head.GlobalTransform = new Transform3D(headBasis, headPos);
                if (phaseT > 0.5f)
                {
                    _maxTrackErr = Mathf.Max(_maxTrackErr, _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition));
                    _maxTrackSpeed = Mathf.Max(_maxTrackSpeed, _torso.Body.LinearVelocity.Length());
                }
                if (phaseT >= PhaseTrack)
                {
                    Record($"track error max {_maxTrackErr:F3} m (limit {TrackPosLimit})", _maxTrackErr < TrackPosLimit);
                    Record($"track speed max {_maxTrackSpeed:F2} m/s (limit {TrackSpeedLimit})", _maxTrackSpeed < TrackSpeedLimit);
                    StartPhase(Phase.Yaw);
                    GD.Print("BODY: yaw phase — head turns 90 deg over 1 s");
                }
                break;
            }

            case Phase.Yaw:
            {
                float yaw = Mathf.Pi / 2f * Mathf.Min(phaseT / 1f, 1f);
                _head.GlobalTransform = new Transform3D(new Basis(new Quaternion(Vector3.Up, yaw)), _head.GlobalPosition);
                float err = Mathf.Abs(YawErrorDeg(_torso.Body.GlobalTransform.Basis, _head.GlobalTransform.Basis));
                if (phaseT > 0.4f)
                    _maxYawErrAfterRamp = Mathf.Max(_maxYawErrAfterRamp, err);
                if (phaseT > PhaseYaw - 0.5f)
                    _yawErrEnd = Mathf.Max(_yawErrEnd, err);
                if (phaseT >= PhaseYaw)
                {
                    Record($"yaw error peak {_maxYawErrAfterRamp:F1} deg (limit {YawPeakLimitDeg})", _maxYawErrAfterRamp < YawPeakLimitDeg);
                    Record($"yaw settled {_yawErrEnd:F1} deg (limit {YawSettleLimitDeg})", _yawErrEnd < YawSettleLimitDeg);
                    StartPhase(Phase.Impact);
                    GD.Print("BODY: impact phase — 5 kg ball at 3 m/s into the chest");
                }
                break;
            }

            case Phase.Impact:
            {
                _maxImpactDip = Mathf.Max(_maxImpactDip, _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition));
                _minImpactorDist = Mathf.Min(_minImpactorDist, _impactor.GlobalPosition.DistanceTo(_torso.Body.GlobalPosition));
                if (_impactorTouchTime >= 0f && _impactorSpeedAfterHit < 0f && phaseT > _impactorTouchTime + 0.2f)
                    _impactorSpeedAfterHit = _impactor.LinearVelocity.Length();
                if (phaseT >= PhaseImpact)
                    FinishImpact();
                break;
            }

            case Phase.Snap:
            {
                if (phaseT >= PhaseSnap)
                {
                    float err = _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition);
                    Record($"snap re-locate (err {err:F3} m, limit {SnapLimit})", err < SnapLimit);
                    StartPhase(Phase.Upright);
                    _torso.Body.AngularVelocity += _torso.Body.GlobalTransform.Basis.X * 6f;
                    GD.Print("BODY: upright phase — pitch impulse, body must self-right");
                }
                break;
            }

            case Phase.Upright:
            {
                if (phaseT >= PhaseUpright)
                    FinishUpright();
                break;
            }
        }

        if (_phase == Phase.Impact && _impactorTouchTime < 0f
            && _impactor.GlobalPosition.DistanceTo(_torso.Body.GlobalPosition) < 0.25f)
            _impactorTouchTime = _elapsed - _phaseStart;
    }

    private void StartPhase(Phase phase)
    {
        _phase = phase;
        _phaseStart = _elapsed;
        if (phase == Phase.Impact)
        {
            _maxImpactDip = 0f;
            _minImpactorDist = float.MaxValue;
            _impactorTouchTime = -1f;
            _impactorSpeedAfterHit = -1f;
            _impactor.Freeze = false;
            Vector3 chest = _torso.Body.GlobalPosition;
            Vector3 fwd = -_torso.Body.GlobalTransform.Basis.Z;
            _impactor.GlobalPosition = chest + fwd * 0.5f + Vector3.Up * 0.05f;
            _impactor.LinearVelocity = -fwd * 3f;
            _impactor.Visible = true;
            GD.Print($"BODY: impactor armed freeze={_impactor.Freeze} pos={_impactor.GlobalPosition} v={_impactor.LinearVelocity}");
        }
    }

    private void FinishPairs()
    {
        float chestY = _torso.Body.GlobalPosition.Y;
        bool handBelow = _handProbe.GlobalPosition.Y < chestY - 0.4f;
        Record($"hand-config body falls through own chest (hand y {_handProbe.GlobalPosition.Y:F2}, chest y {chestY:F2})", handBelow);
        Record("chest undisturbed while hand-config body falls through", _chestStillDuringHandDrop);
        bool propContact = _chestDropDip > 0.0001f;
        bool propBounded = _chestDropDip < 0.1f;
        Record($"dropped ball transfers contact into chest (dip {_chestDropDip:F4} m)", propContact && propBounded);
        _prop.LinearVelocity = Vector3.Zero;
        _prop.GlobalPosition = new Vector3(-2f, 0.3f, 1f);
        StartPhase(Phase.Track);
        GD.Print("BODY: track phase — head walks a smooth path");
    }

    private void FinishImpact()
    {
        Record($"impact chest dip max {_maxImpactDip:F3} m (limit {ImpactDipLimit})", _maxImpactDip < ImpactDipLimit);
        float recovered = _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition);
        Record($"impact recovery {recovered:F3} m (limit {RecoverLimit})", recovered < RecoverLimit);
        Record($"impactor never passed through (min dist {_minImpactorDist:F3} m, limit {ImpactorMinDistLimit})", _minImpactorDist > ImpactorMinDistLimit);
        Record($"impactor slowed after hit (|v| {_impactorSpeedAfterHit:F2} m/s, limit {ImpactorSpeedAfterHitLimit})",
            _impactorSpeedAfterHit >= 0f && _impactorSpeedAfterHit < ImpactorSpeedAfterHitLimit);
        StartPhase(Phase.Snap);
        _head.GlobalPosition += new Vector3(2.5f, 0f, 0f);
        GD.Print("BODY: snap phase — head teleported 2.5 m");
    }

    private void FinishUpright()
    {
        float up = _torso.Body.GlobalTransform.Basis.Y.Dot(Vector3.Up);
        float uprightDeg = Mathf.RadToDeg(Mathf.Acos(Mathf.Clamp(up, -1f, 1f)));
        float yawErr = Mathf.Abs(YawErrorDeg(_torso.Body.GlobalTransform.Basis, _head.GlobalTransform.Basis));
        Record($"self-right to upright {uprightDeg:F1} deg (limit {UprightLimitDeg})", uprightDeg < UprightLimitDeg);
        Record($"yaw held through tumble {yawErr:F1} deg (limit {YawHoldLimitDeg})", yawErr < YawHoldLimitDeg);
        Finish();
    }

    private static float YawErrorDeg(Basis bodyBasis, Basis headBasis)
    {
        Vector3 bodyFwd = -bodyBasis.Z;
        Vector3 headFwd = -headBasis.Z;
        bodyFwd.Y = 0f;
        headFwd.Y = 0f;
        if (bodyFwd.LengthSquared() < 0.0001f || headFwd.LengthSquared() < 0.0001f)
            return 0f;
        bodyFwd = bodyFwd.Normalized();
        headFwd = headFwd.Normalized();
        return Mathf.RadToDeg(Mathf.Atan2(bodyFwd.Cross(headFwd).Dot(Vector3.Up), bodyFwd.Dot(headFwd)));
    }

    private void Record(string name, bool ok)
    {
        _resultNames.Add(name);
        _resultOk.Add(ok);
        GD.Print($"BODY: {name} → {(ok ? "PASS" : "FAIL")}");
    }

    private void Finish()
    {
        bool overall = true;
        foreach (bool ok in _resultOk)
            overall &= ok;
        GD.Print($"BODY: overall {(overall ? "PASS — physical torso functional on Box3D" : "CHECK — inspect failures above")}");
        GetTree().Quit();
    }
}