using Godot;
using System.Collections.Generic;

public partial class XRHand : XRController3D
{
    List<PinJoint3D> joints = new();

    [Export]
    RigidBody3D body;

    List<RigidBody3D> overlaps;

    [Export]
    public float Strength = 1.0f;

    public void OnEnter(Node3D node)
    {
        try
        {
            RigidBody3D body = (RigidBody3D)node;
            overlaps.Add(body);
        }
        catch (System.InvalidCastException) { }
    }

    public void OnLeave(Node3D node)
    {
        try
        {
            RigidBody3D body = (RigidBody3D)node;
            overlaps.Remove(body);
        }
        catch (System.InvalidCastException) { }
    }

    public void OnPress(string name)
    {
        if (name == "select_button")
        {
            foreach (RigidBody3D body in overlaps)
            {
                if (body.Mass > Strength)
                    continue;

                GD.Print("Picked up ", body);

                PinJoint3D joint = new();
                joint.NodeA = body.GetPath();
                joint.NodeB = this.body.GetPath();
                joint.SetParam(PinJoint3D.Param.Bias, 10f);
                joints.Add(joint);

                PinJoint3D joint2 = new();
                joint2.NodeA = this.body.GetPath();
                joint2.NodeB = body.GetPath();
                joint2.SetParam(PinJoint3D.Param.Bias, 10f);
                joints.Add(joint2);

                this.body.AddChild(joint);
                body.AddChild(joint2);
            }
        }
    }

    public void OnRelease(string name)
    {
        if (name == "select_button")
        {
            foreach (var joint in joints)
                joint.QueueFree();
            joints.Clear();
        }
    }

    public void OnFloat(string name, float value)
    {
        // GD.Print("Float input:");
        // GD.Print(name);
        // GD.Print(value);
    }
}