using Godot;

public partial class AnchorMoveMode : Node
{
    private const float GripThreshold = 0.6f;

    private XROrigin3D _origin;
    private XRCamera3D _camera;
    private XRController3D _left;
    private XRController3D _right;

    private bool _wasTwoHand;
    private Vector3 _leftPrev;
    private Vector3 _rightPrev;
    private Vector3 _twoHandMidPrev;
    private float _twoHandYawPrev;

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

    public override void _Process(double delta)
    {
        bool leftGrip = _left.GetFloat("grip") > GripThreshold && !PropGrabber.IsGrabbing(_left);
        bool rightGrip = _right.GetFloat("grip") > GripThreshold && !PropGrabber.IsGrabbing(_right);

        if (leftGrip && rightGrip && !WorldPanel.IsBlocking(_left) && !WorldPanel.IsBlocking(_right))
            TwoHandGrab();
        else
        {
            _wasTwoHand = false;
            if (leftGrip && !WorldPanel.IsBlocking(_left))
                GrabMove(_left, ref _leftPrev);
            else
                _leftPrev = _left.Position;
            if (rightGrip && !WorldPanel.IsBlocking(_right))
                GrabMove(_right, ref _rightPrev);
            else
                _rightPrev = _right.Position;
        }
    }

    private void TwoHandGrab()
    {
        Vector3 mid = Midpoint();
        float yaw = HandsYaw();

        if (!_wasTwoHand)
        {
            _wasTwoHand = true;
            _twoHandMidPrev = mid;
            _twoHandYawPrev = yaw;
            return;
        }

        Vector3 deltaWorld = _origin.GlobalTransform.Basis * (mid - _twoHandMidPrev);
        float yawDelta = _twoHandYawPrev - yaw;
        if (deltaWorld.LengthSquared() > 0.000001f || Mathf.Abs(yawDelta) > 0.000001f)
        {
            Vector3 hxz = new Vector3(_camera.GlobalPosition.X, 0f, _camera.GlobalPosition.Z);
            Transform3D turn = new Transform3D(Basis.Identity, hxz)
                * new Transform3D(Basis.FromEuler(new Vector3(0, yawDelta, 0)), Vector3.Zero)
                * new Transform3D(Basis.Identity, -hxz);
            _origin.GlobalTransform = new Transform3D(Basis.Identity, -deltaWorld) * turn * _origin.GlobalTransform;
        }
        _twoHandMidPrev = mid;
        _twoHandYawPrev = yaw;
    }

    private void GrabMove(XRController3D controller, ref Vector3 prev)
    {
        Vector3 pos = controller.Position;
        Vector3 delta = pos - prev;
        prev = pos;
        if (delta.LengthSquared() == 0f)
            return;
        Vector3 deltaWorld = _origin.GlobalTransform.Basis * delta;
        _origin.GlobalTransform = new Transform3D(Basis.Identity, -deltaWorld) * _origin.GlobalTransform;
    }

    private Vector3 Midpoint() => (_left.Position + _right.Position) * 0.5f;

    private float HandsYaw()
    {
        Vector3 v = _right.Position - _left.Position;
        v.Y = 0f;
        if (v.LengthSquared() < 0.0001f)
            return _twoHandYawPrev;
        return Mathf.Atan2(v.X, v.Z);
    }
}