using UnityEngine;
using System.Collections.Generic;

public class OptitrackRawDataReceiver : MonoBehaviour
{
    [Header("OptiTrack 설정")]
    public OptitrackStreamingClient StreamingClient;
    public string SkeletonAssetName = "Skeleton1";

    private OptitrackSkeletonDefinition m_skeletonDef;
    private Dictionary<int, OptitrackPose> m_lastBonePoses = new Dictionary<int, OptitrackPose>();

    // FillBoneSnapshot 패턴: 락 안에서 데이터를 복사하여 torn read 방지
    private Dictionary<int, Vector3> m_snapshotPositions = new Dictionary<int, Vector3>();
    private Dictionary<int, Quaternion> m_snapshotOrientations = new Dictionary<int, Quaternion>();

    void Start()
    {
        if (StreamingClient == null)
        {
            StreamingClient = FindFirstObjectByType<OptitrackStreamingClient>();
            if (StreamingClient == null)
            {
                Debug.LogError("OptiTrack Streaming Client를 찾을 수 없습니다.");
                return;
            }
        }

        // 스켈레톤 등록
        StreamingClient.RegisterSkeleton(this, SkeletonAssetName);
    }

    void Update()
    {
        if (StreamingClient == null) return;

        // 스켈레톤 정의 가져오기
        if (m_skeletonDef == null)
        {
            m_skeletonDef = StreamingClient.GetSkeletonDefinitionByName(SkeletonAssetName);
            if (m_skeletonDef == null) return;
        }

        // FillBoneSnapshot으로 락 보호 하에 스냅샷 복사 (torn read 방지)
        OptitrackHiResTimer.Timestamp ts;
        if (!StreamingClient.FillBoneSnapshot(m_skeletonDef.Id, m_snapshotPositions, m_snapshotOrientations, out ts))
            return;

        // 스냅샷에서 OptitrackPose로 변환하여 저장
        foreach (var bone in m_skeletonDef.Bones)
        {
            if (m_snapshotPositions.TryGetValue(bone.Id, out Vector3 pos) &&
                m_snapshotOrientations.TryGetValue(bone.Id, out Quaternion ori))
            {
                if (!m_lastBonePoses.TryGetValue(bone.Id, out OptitrackPose existingPose))
                {
                    existingPose = new OptitrackPose();
                    m_lastBonePoses[bone.Id] = existingPose;
                }
                existingPose.Position = pos;
                existingPose.Orientation = ori;
            }
        }
    }

    // 외부에서 원본 데이터 접근을 위한 메서드
    public Dictionary<int, OptitrackPose> GetRawBonePoses()
    {
        return m_lastBonePoses;
    }

    // 특정 본의 원본 데이터 가져오기
    public OptitrackPose GetRawBonePose(int boneId)
    {
        if (m_lastBonePoses.TryGetValue(boneId, out OptitrackPose pose))
        {
            return pose;
        }
        return null;
    }
}