using StereoKit;

namespace dofdemo;


public static class Data
{

}

public class Grab
{
    public Pose pose;
    private Vec3 pos_offset;
    private Quat ori_offset;
    public Hand? held_by;
    private bool snap_to_hand = false;

    public Grab(float x, float y, float z, bool snap_to_hand = false)
    {
        pose = new Pose(x, y, z);
        pos_offset = Vec3.Zero;
        ori_offset = Quat.Identity;
        held_by = null;
        this.snap_to_hand = snap_to_hand;
    }

    public bool OnGrab(ref Hand hand, ref Grab grab_ref)
    {
        if (!Held) // Allow only one hand at a time to grab
        {
            if (Vec3.Distance(hand.palm.position, pose.position) < 0.14f)
            {
                held_by = hand;
                grab_ref = this;
                pos_offset = hand.palm.orientation.Inverse * (hand.palm.position - pose.position);
                ori_offset = hand.palm.orientation.Inverse * pose.orientation;
                return true;
            }
        }
        return false;
    }

    public bool Held => held_by != null;

    public void OnRelease()
    {
        held_by = null;
    }

    public void Frame()
    {
        if (Held)
        {
            if (snap_to_hand)
            {
                pose.orientation = held_by.palm.orientation;
                pose.position = held_by.palm.position;
            }
            else
            {
                pose.orientation = held_by.palm.orientation * ori_offset;
                pose.position = held_by.palm.position - held_by.palm.orientation * pos_offset;
            }
        }
    }
}