using Godot;
using System.Collections.Generic;

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

    [Export]
    RigidBody3D rb;

    List<RigidBody3D> overlaps = new();

    [Export]
    public float Strength = 1.0f;

    public override void _Ready()
    {
        if (rb == null)
            GD.PrintErr("Missing Rigidbody");
    }

    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)
    {
        GD.Print("Pressed Input ", name);
        if (name == "select_button")
        {
            foreach (RigidBody3D body in overlaps)
            {
                if (body.Mass > Strength)
                    continue;

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

                Generic6DofJoint3D joint = new();
                joint.NodeA = body.GetPath();
                joint.NodeB = rb.GetPath();
                joints.Add(joint);
                rb.AddChild(joint);
                joint.Position = Vector3.Zero;
            }
        }
    }

    public void OnRelease(string name)
    {
        GD.Print("Released Input ", 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);
    }
}