using Godot;

public partial class PropGrabber : Node
{
    private const float GripThreshold = 0.6f;
    private const float PalmGrabOffset = 0.045f;
    private const float GrabRadius = 0.16f;
    private const float CenterGrabRadius = 0.12f;
    private const float RigidWeldBias = 0f;
    private const float BreakDistance = 0.25f;
    private const float BreakTime = 0.25f;
    private const float BreakDistanceBoth = 0.4f;
    private const float GripRipDistance = 0.8f;

    private static PropGrabber _instance;

    private XRController3D _left;
    private XRController3D _right;

    private readonly RigidBody3D[] _held = new RigidBody3D[2];
    private readonly ConeTwistJoint3D[] _joints = new ConeTwistJoint3D[2];
    private readonly Vector3[] _localAnchor = new Vector3[2];
    private readonly uint[] _grabSeq = new uint[2];
    private readonly float[] _breakTimer = new float[2];
    private readonly bool[] _gripPrev = new bool[2];
    private uint _grabCount;

    public override void _Ready()
    {
        _instance = this;
        _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
        _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
    }

    public static bool IsGrabbing(XRController3D controller) =>
        _instance != null &&
        ((_instance._left == controller && _instance._held[0] != null) ||
         (_instance._right == controller && _instance._held[1] != null));

    public override void _Process(double delta)
    {
        UpdateHand(_left, 0);
        UpdateHand(_right, 1);
    }

    public override void _PhysicsProcess(double delta)
    {
        float dt = (float)delta;
        HoldUpdate(_left, 0, dt);
        HoldUpdate(_right, 1, dt);
    }

    private void UpdateHand(XRController3D controller, int hand)
    {
        bool grip = controller.GetFloat("grip") > GripThreshold;
        int other = 1 - hand;

        if (grip && !_gripPrev[hand] && _held[hand] == null)
        {
            RigidBody3D prop = FindProp(controller, hand);
            RigidBody3D handBody = PhysicsHands.GetBody(controller);
            if (prop != null && handBody != null)
            {
                _held[hand] = prop;
                _grabSeq[hand] = _grabCount++;
                Vector3 pivot = PalmPoint(handBody, hand);
                _joints[hand] = MakeWeld(handBody, prop, pivot, RigidWeldBias);
                _localAnchor[hand] = prop.GlobalTransform.AffineInverse() * pivot;
                GD.Print($"PropGrabber: grabbed '{prop.Name}' ({prop.Mass:0.###} kg, {(hand == 0 ? "left" : "right")}{(_held[other] == prop ? ", two-handed" : "")})");
            }
        }

        if (!grip && _held[hand] != null)
        {
            RigidBody3D prop = _held[hand];
            _held[hand] = null;
            DestroyJoint(hand);
            if (IsInstanceValid(prop))
                GD.Print($"PropGrabber: released '{prop.Name}' at {prop.LinearVelocity.Length():F2} m/s");
        }
        _gripPrev[hand] = grip;
    }

    private void HoldUpdate(XRController3D controller, int hand, float delta)
    {
        RigidBody3D body = _held[hand];
        if (body == null)
            return;
        if (!IsInstanceValid(body))
        {
            _held[hand] = null;
            DestroyJoint(hand);
            return;
        }

        RigidBody3D handBody = PhysicsHands.GetBody(controller);
        ConeTwistJoint3D joint = _joints[hand];
        if (handBody == null || !IsInstanceValid(handBody) || joint == null || !IsInstanceValid(joint))
        {
            _held[hand] = null;
            DestroyJoint(hand);
            return;
        }

        if (handBody.GlobalPosition.DistanceTo(controller.GlobalPosition) > PhysicsHands.SnapDistance)
        {
            BreakGrip(hand, body, "grip dragged off");
            return;
        }

        float separation = (body.GlobalTransform * _localAnchor[hand]).DistanceTo(PalmPoint(handBody, hand));

        int other = 1 - hand;
        bool twoHanded = _held[other] == body && IsInstanceValid(_held[other]) &&
                         _joints[other] != null && IsInstanceValid(_joints[other]);
        if (twoHanded)
        {
            RigidBody3D otherHandBody = PhysicsHands.GetBody(other == 0 ? _left : _right);
            float otherSeparation = otherHandBody == null
                ? 0f
                : (body.GlobalTransform * _localAnchor[other]).DistanceTo(PalmPoint(otherHandBody, other));
            if (separation > BreakDistanceBoth && otherSeparation > BreakDistanceBoth)
            {
                BreakGrip(hand, body, $"both hands stretched ({separation:0.###} m)");
                BreakGrip(other, body, "both hands stretched");
                return;
            }
        }

        if (separation > GripRipDistance)
        {
            BreakGrip(hand, body, $"ripped ({separation:0.###} m)");
            return;
        }

        if (separation > BreakDistance)
            _breakTimer[hand] += delta;
        else
            _breakTimer[hand] = 0f;
        if (_breakTimer[hand] > BreakTime)
        {
            BreakGrip(hand, body, $"stretched ({separation:0.###} m for {BreakTime:0.#} s)");
            return;
        }
    }

    private int PrimaryHand(RigidBody3D body)
    {
        int primary = -1;
        for (int hand = 0; hand < 2; hand++)
        {
            if (_held[hand] == body && (primary == -1 || _grabSeq[hand] < _grabSeq[primary]))
                primary = hand;
        }
        return primary;
    }

    private ConeTwistJoint3D MakeWeld(RigidBody3D handBody, RigidBody3D prop, Vector3 pivot, float bias)
    {
        ConeTwistJoint3D weld = new()
        {
            ExcludeNodesFromCollision = true,
            Position = pivot,
            NodeA = handBody.GetPath(),
            NodeB = prop.GetPath(),
            SwingSpan = 0f,
            TwistSpan = 0f,
            Bias = bias,
        };
        AddChild(weld);
        return weld;
    }

    private void BreakGrip(int hand, RigidBody3D body, string reason)
    {
        if (_held[hand] != body)
            return;
        _held[hand] = null;
        DestroyJoint(hand);
        GD.Print($"PropGrabber: grip broke on '{body.Name}' ({reason})");
    }

    private void DestroyJoint(int hand)
    {
        if (_joints[hand] != null && IsInstanceValid(_joints[hand]))
            _joints[hand].QueueFree();
        _joints[hand] = null;
        _breakTimer[hand] = 0f;
    }

    private static Vector3 PalmPoint(RigidBody3D handBody, int hand) =>
        handBody.GlobalPosition + handBody.GlobalTransform.Basis * new Vector3(PalmOffset(hand), 0f, 0f);

    private static float PalmOffset(int hand) => hand == 0 ? PalmGrabOffset : -PalmGrabOffset;

    private RigidBody3D FindProp(XRController3D controller, int hand)
    {
        RigidBody3D handBody = PhysicsHands.GetBody(controller);
        if (handBody == null)
            return null;
        Vector3 palm = PalmPoint(handBody, hand);
        RigidBody3D nearest = NearestProp(palm, GrabRadius);
        if (nearest != null)
            return nearest;
        return NearestProp(handBody.GlobalPosition, CenterGrabRadius);
    }

    private RigidBody3D NearestProp(Vector3 point, float radius)
    {
        RigidBody3D nearest = null;
        float nearestDist = radius;
        foreach (Node node in GetTree().GetNodesInGroup("prop"))
        {
            if (node is not RigidBody3D prop)
                continue;
            float dist = prop.GlobalPosition.DistanceTo(point);
            if (dist < nearestDist)
            {
                nearestDist = dist;
                nearest = prop;
            }
        }
        return nearest;
    }
}