using Godot;

// Regression for the patched godot-box3d addon: ConeTwistJoint3D (swing 0, twist 0) maps to
// a solver-owned Box3D weld joint that locks the two bodies at the relative pose captured at
// creation. Phase 1 asserts a near-rigid weld under load; phase 2 asserts a soft weld
// (bias > 0) stretches under sustained extreme force and recovers without exploding.
public partial class WeldSpike : Node3D
{
    private const float BodyAMass = 1f;
    private const float BodyBMass = 0.05f;
    private const float RigidWeldBias = 0f;
    private const float SoftWeldBias = 0.5f;
    private const float RigidDuration = 3f;
    private const float SoftDuration = 4f;
    private const float LoadDuration = 1.5f;
    private const float PullDuration = 2.5f;
    private const float RigidTorque = 0.5f;
    private const float RigidForce = 0.5f;
    private const float PullForce = 4f;
    private const float MaxSpeed = 10f;
    private const float RigidRotLimitDeg = 2f;
    private const float RigidSepLimit = 0.01f;
    private const float SoftRotLimitDeg = 90f;
    private const float SoftSepLimit = 0.3f;
    private const float SoftSettleRotLimitDeg = 10f;
    private const float SoftSettleSepLimit = 0.05f;

    private enum Phase { RigidWeld, SoftWeld }

    private RigidBody3D _bodyA;
    private RigidBody3D _bodyB;
    private ConeTwistJoint3D _joint;
    private Phase _phase = Phase.RigidWeld;
    private float _elapsed;
    private float _phaseStart;
    private Quaternion _relRotation0;
    private Vector3 _localAnchorA;
    private Vector3 _localAnchorB;
    private float _maxRelRot;
    private float _maxSep;
    private float _maxSpeed;
    private bool _typeOk;
    private bool _rigidRotOk;
    private bool _rigidSepOk;
    private bool _rigidSpeedOk;
    private bool _softStretchOk;
    private bool _softSpeedOk;
    private bool _softSettleOk;
    private int _lastLogStep;

    public override void _Ready()
    {
        _bodyA = new RigidBody3D
        {
            Position = new Vector3(-0.2f, 2f, 0f),
            Mass = BodyAMass,
            GravityScale = 0f,
            ContinuousCd = true,
        };
        _bodyA.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = Vector3.One * 0.4f } });
        AddChild(_bodyA);

        _bodyB = new RigidBody3D
        {
            Position = new Vector3(0.25f, 2f, 0f),
            Mass = BodyBMass,
            GravityScale = 0f,
            ContinuousCd = true,
        };
        _bodyB.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = Vector3.One * 0.1f } });
        AddChild(_bodyB);

        Weld(RigidWeldBias);
        _phaseStart = _elapsed;
        GD.Print($"WELD: rigid weld created at t={_elapsed:F2} s, server joint type = {PhysicsServer3D.JointGetType(_joint.GetRid())} (expect {PhysicsServer3D.JointType.ConeTwist})");
        _typeOk = PhysicsServer3D.JointGetType(_joint.GetRid()) == PhysicsServer3D.JointType.ConeTwist;
        GD.Print("WELD: weld spike scene ready");
    }

    private void Weld(float bias)
    {
        Vector3 pivot = (_bodyA.GlobalPosition + _bodyB.GlobalPosition) * 0.5f;
        _relRotation0 = _bodyA.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
            * _bodyB.GlobalTransform.Basis.GetRotationQuaternion();
        _localAnchorA = _bodyA.GlobalTransform.AffineInverse() * pivot;
        _localAnchorB = _bodyB.GlobalTransform.AffineInverse() * pivot;
        _maxRelRot = 0f;
        _maxSep = 0f;
        _maxSpeed = 0f;
        _joint = new ConeTwistJoint3D
        {
            ExcludeNodesFromCollision = true,
            Position = pivot,
            NodeA = _bodyA.GetPath(),
            NodeB = _bodyB.GetPath(),
            SwingSpan = 0f,
            TwistSpan = 0f,
            Bias = bias,
        };
        AddChild(_joint);
    }

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

        switch (_phase)
        {
            case Phase.RigidWeld:
            {
                if (phaseT < LoadDuration)
                {
                    _bodyA.ApplyTorque(new Vector3(0f, RigidTorque, 0f));
                    _bodyB.ApplyCentralForce(new Vector3(RigidForce, 0f, 0f));
                }
                Track();
                LogBucket(phaseT);
                if (phaseT >= RigidDuration)
                    FinishRigid();
                break;
            }
            case Phase.SoftWeld:
            {
                if (phaseT < PullDuration)
                {
                    _bodyA.ApplyCentralForce(new Vector3(-PullForce, 0f, 0f));
                    _bodyB.ApplyCentralForce(new Vector3(PullForce, 0f, 0f));
                }
                Track();
                LogBucket(phaseT);
                if (phaseT >= SoftDuration)
                    Finish();
                break;
            }
        }
    }

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

    private void Track()
    {
        Quaternion relNow = _bodyA.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
            * _bodyB.GlobalTransform.Basis.GetRotationQuaternion();
        float rotDev = DevAngle(relNow, _relRotation0);
        if (float.IsNaN(rotDev))
        {
            GD.Print("WELD: FAIL — NaN rotation error (explosion guard)");
            GetTree().Quit();
            return;
        }
        _maxRelRot = Mathf.Max(_maxRelRot, rotDev);
        float sep = (_bodyA.GlobalTransform * _localAnchorA).DistanceTo(_bodyB.GlobalTransform * _localAnchorB);
        _maxSep = Mathf.Max(_maxSep, sep);
        float speed = Mathf.Max(_bodyA.LinearVelocity.Length(), _bodyB.LinearVelocity.Length());
        if (float.IsNaN(speed) || speed > MaxSpeed)
        {
            GD.Print($"WELD: FAIL — body speed {speed} m/s exceeds {MaxSpeed} (explosion guard)");
            GetTree().Quit();
            return;
        }
        _maxSpeed = Mathf.Max(_maxSpeed, speed);
    }

    private void LogBucket(float phaseT)
    {
        int step = (int)(phaseT / 0.5f);
        if (step == _lastLogStep)
            return;
        _lastLogStep = step;
        Quaternion relNow = _bodyA.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
            * _bodyB.GlobalTransform.Basis.GetRotationQuaternion();
        float rotDev = DevAngle(relNow, _relRotation0);
        float sep = (_bodyA.GlobalTransform * _localAnchorA).DistanceTo(_bodyB.GlobalTransform * _localAnchorB);
        GD.Print($"WELD: t={_elapsed:F2} phase={_phase} rotDev={Mathf.RadToDeg(rotDev):F2} deg sep={sep:F4} m speedA={_bodyA.LinearVelocity.Length():F2} speedB={_bodyB.LinearVelocity.Length():F2}");
    }

    private void FinishRigid()
    {
        _rigidRotOk = _maxRelRot < Mathf.DegToRad(RigidRotLimitDeg);
        _rigidSepOk = _maxSep < RigidSepLimit;
        _rigidSpeedOk = _maxSpeed < MaxSpeed;
        GD.Print($"WELD: rigid hold — max rot dev {Mathf.RadToDeg(_maxRelRot):F2} deg (limit {RigidRotLimitDeg}) → {(_rigidRotOk ? "PASS" : "FAIL")}");
        GD.Print($"WELD: rigid anchors — max sep {_maxSep:F4} m (limit {RigidSepLimit}) → {(_rigidSepOk ? "PASS" : "FAIL")}");
        GD.Print($"WELD: rigid speeds — max {_maxSpeed:F2} m/s (limit {MaxSpeed}) → {(_rigidSpeedOk ? "PASS" : "FAIL")}");

        // fresh-grab conditions: stop the coasting bodies before re-welding
        _bodyA.LinearVelocity = Vector3.Zero;
        _bodyA.AngularVelocity = Vector3.Zero;
        _bodyB.LinearVelocity = Vector3.Zero;
        _bodyB.AngularVelocity = Vector3.Zero;
        _joint.QueueFree();
        _joint = null;
        _phase = Phase.SoftWeld;
        _phaseStart = _elapsed;
        _lastLogStep = 0;
        Weld(SoftWeldBias);
        GD.Print($"WELD: soft weld re-created at current pose, bias {SoftWeldBias} (weld hertz {SoftWeldBias * 30f:F0}), sustained ±{PullForce} N pull starting");
    }

    private void Finish()
    {
        _softStretchOk = _maxRelRot < Mathf.DegToRad(SoftRotLimitDeg) && _maxSep < SoftSepLimit;
        _softSpeedOk = _maxSpeed < MaxSpeed;
        // settle: assert the stretch has recovered by the end
        Quaternion relNow = _bodyA.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
            * _bodyB.GlobalTransform.Basis.GetRotationQuaternion();
        float settleRot = DevAngle(relNow, _relRotation0);
        float settleSep = (_bodyA.GlobalTransform * _localAnchorA).DistanceTo(_bodyB.GlobalTransform * _localAnchorB);
        _softSettleOk = settleRot < Mathf.DegToRad(SoftSettleRotLimitDeg) && settleSep < SoftSettleSepLimit;
        GD.Print($"WELD: soft excursion bounded — max rot dev {Mathf.RadToDeg(_maxRelRot):F2} deg (limit {SoftRotLimitDeg}), max sep {_maxSep:F4} m (limit {SoftSepLimit}) → {(_softStretchOk ? "PASS" : "FAIL")}");
        GD.Print($"WELD: soft speeds — max {_maxSpeed:F2} m/s (limit {MaxSpeed}) → {(_softSpeedOk ? "PASS" : "FAIL")}");
        GD.Print($"WELD: soft settle — rot dev {Mathf.RadToDeg(settleRot):F2} deg (limit {SoftSettleRotLimitDeg}), sep {settleSep:F4} m (limit {SoftSettleSepLimit}) → {(_softSettleOk ? "PASS" : "FAIL")}");
        bool overall = _typeOk && _rigidRotOk && _rigidSepOk && _rigidSpeedOk && _softStretchOk && _softSpeedOk && _softSettleOk;
        GD.Print($"WELD: overall {(overall ? "PASS — solver-owned rigid weld via ConeTwistJoint3D functional on patched Box3D" : "CHECK — behavior differs, inspect above")}");
        GetTree().Quit();
    }
}