using System.Collections.Generic;
using UnityEngine;
using UnityEngine.XR.Hands;

public class XRHandJointLogger : MonoBehaviour
{
    [SerializeField] private Transform xrOrigin;
    [SerializeField, Range(1f, 60f)] private float handSampleRateHz = 15f;

    private XRHandSubsystem handSubsystem;
    private double nextSampleTime;
    private readonly List<XRHandSubsystem> handSubsystems = new();

    private void Update()
    {
        if (XRSessionLogger.Instance == null)
            return;

        if (handSubsystem == null || !handSubsystem.running)
            FindRunningHandSubsystem();

        if (handSubsystem == null || !handSubsystem.running)
            return;

        double now = Time.realtimeSinceStartupAsDouble;
        if (now < nextSampleTime)
            return;

        nextSampleTime = now + (1.0 / handSampleRateHz);
        LogHand("left", handSubsystem.leftHand);
        LogHand("right", handSubsystem.rightHand);
    }

    private void FindRunningHandSubsystem()
    {
        handSubsystems.Clear();
        SubsystemManager.GetSubsystems(handSubsystems);
        handSubsystem = null;
        foreach (var subsystem in handSubsystems)
        {
            if (subsystem != null && subsystem.running)
            {
                handSubsystem = subsystem;
                break;
            }
        }
    }

    private void LogHand(string label, XRHand hand)
    {
        bool handTracked = hand.isTracked;

        for (int i = XRHandJointID.BeginMarker.ToIndex();
             i < XRHandJointID.EndMarker.ToIndex(); i++)
        {
            XRHandJointID jointId = XRHandJointIDUtility.FromIndex(i);
            XRHandJoint joint = hand.GetJoint(jointId);
            bool valid = joint.TryGetPose(out Pose jointPose);

            Vector3 worldPosition = Vector3.zero;
            Quaternion worldRotation = Quaternion.identity;

            if (valid)
            {
                if (xrOrigin != null)
                {
                    worldPosition = xrOrigin.TransformPoint(jointPose.position);
                    worldRotation = xrOrigin.rotation * jointPose.rotation;
                }
                else
                {
                    worldPosition = jointPose.position;
                    worldRotation = jointPose.rotation;
                }
            }

            XRSessionLogger.Instance.LogHandJoint(label, jointId.ToString(), handTracked,
                valid, worldPosition, worldRotation);
        }
    }
}