#if AR_FOUNDATION_5 || AR_FOUNDATION_6
using Unity.Collections;
using Unity.XR.PXR;
using UnityEngine;
using UnityEngine.XR.ARSubsystems;
#if PICO_OPENXR_SDK
using Unity.XR.OpenXR.Features.PICOSupport;
#endif
public class PXR_HumanBodySubsystem : XRHumanBodySubsystem
{
    internal const string k_SubsystemId = "PXR_HumanBodySubsystem";

    class HumanBodyProvider : Provider
    {
        bool isBodyTrackingSupported = false;
        bool init = false;

        private BodyTrackingGetDataInfo bdi = new BodyTrackingGetDataInfo();
        private BodyTrackingData bd = new BodyTrackingData();
        BodyTrackingStatus bs = new BodyTrackingStatus();
        bool istracking = false;
        public override void Start()
        {
            PLog.i(k_SubsystemId, "Start");
            init = true;
        }

        public override void Stop()
        {
            PLog.i(k_SubsystemId, "Stop");
#if PICO_OPENXR_SDK
            int ret = BodyTrackingFeature.StopBodyTracking();
#else
        int ret = PXR_MotionTracking.StopBodyTracking();
#endif
        }

        public override void Destroy()
        {
            PLog.i(k_SubsystemId, "Destroy");
        }

        public override void GetSkeleton(TrackableId trackableId, Allocator allocator, ref NativeArray<XRHumanBodyJoint> skeleton)
        {
            PLog.d(k_SubsystemId, $"GetSkeleton isBodyTrackingSupported={isBodyTrackingSupported}");
#if UNITY_ANDROID
            if (isBodyTrackingSupported)
            {
                // Get the position and orientation data of each body node.
#if PICO_OPENXR_SDK
                BodyTrackingFeature.GetBodyTrackingState(ref istracking, ref bs);
#else
                PXR_MotionTracking.GetBodyTrackingState(ref istracking, ref bs);
#endif
                // Debug.Log($"GetBodyTrackingState stateCode  = {bs.stateCode}  message  = {bs.message} ");
                // If not calibrated, invoked system motion tracker app for calibration.


                // If not calibrated, invoked system motion tracker app for calibration.
                if (bs.stateCode!=BodyTrackingStatusCode.BT_VALID)
                {
                    return;
                }

                int ret = -1;

#if PICO_OPENXR_SDK
                ret = BodyTrackingFeature.GetBodyTrackingData(ref bdi, ref bd);
#else
                ret = PXR_MotionTracking.GetBodyTrackingData(ref bdi, ref bd);
#endif

                // if the return is successful
                if (ret == 0)
                {
                    skeleton = new NativeArray<XRHumanBodyJoint>((int)BodyTrackerRole.ROLE_NUM, allocator);
                    for (int i = 0; i < (int)BodyTrackerRole.ROLE_NUM; i++)
                    {
                        BodyTrackerTransPose localPose = bd.roleDatas[i].localPose;
                        Vector3 pos = new Vector3((float)bd.roleDatas[i].localPose.PosX, (float)bd.roleDatas[i].localPose.PosY,
                            (float)bd.roleDatas[i].localPose.PosZ);
                        Quaternion qu = new Quaternion((float)bd.roleDatas[i].localPose.RotQx, (float)bd.roleDatas[i].localPose.RotQy,
                            (float)bd.roleDatas[i].localPose.RotQz, (float)bd.roleDatas[i].localPose.RotQw);

                        if (i == 0)
                        {
                            qu *= Quaternion.Euler(new Vector3(0, 180, 0));
                        }
                        else if (i == 1)
                        {
                            qu *= Quaternion.Euler(new Vector3(0, 0, -95));
                        }
                        else if (i == 2)
                        {
                            qu *= Quaternion.Euler(new Vector3(0, 0, 95));
                        }
                        else if (i == 4)
                        {
                            qu *= Quaternion.Euler(new Vector3(0, 0, -90));
                        }
                        else if (i == 3 || i == 5 || i == 12)
                        {
                            qu *= Quaternion.Euler(new Vector3(0, 0, 90));
                        }
                        else if (i == 7)
                        {
                            qu *= Quaternion.Euler(new Vector3(180, -90, 0));
                        }
                        else if (i == 6 || i == 9 || i == 15)
                        {
                            qu *= Quaternion.Euler(new Vector3(0, 90, 90));
                        }
                        else if (i == 8 || i == 10 || i == 11)
                        {
                            qu *= Quaternion.Euler(new Vector3(0, 90, 0));
                        }
                        else if (i == 13 || i == 14 || i == 16 || i == 17 || i == 18 || i == 19 || i == 20)
                        {
                            qu *= Quaternion.Euler(new Vector3(0, 0, 180));
                        }
                        else if (i == 21)
                        {
                            qu *= Quaternion.Euler(new Vector3(180, 0, 180));
                        }

                        Pose pose = new Pose(pos, qu);
                        XRHumanBodyJoint mXRHumanBodyJoint = new XRHumanBodyJoint(i, 0, Vector3.one, pose, Vector3.one, pose, true);
                        skeleton[i] = mXRHumanBodyJoint;
                    }
                }
            }
#endif
        }

        public override TrackableChanges<XRHumanBody> GetChanges(XRHumanBody defaultHumanBody, Allocator allocator)
        {
            PLog.d(k_SubsystemId, $"GetChanges init={init}, bodyTracking={PXR_ProjectSetting.GetProjectConfig().bodyTracking} ");
            if (init)
            {
                if (PXR_ProjectSetting.GetProjectConfig().bodyTracking)
                {
                    PXR_Plugin.MotionTracking.UPxr_WantBodyTrackingService();

                    // Query whether the current device supports human body tracking.
#if PICO_OPENXR_SDK
                    isBodyTrackingSupported = BodyTrackingFeature.IsBodyTrackingSupported();
#else
                    PXR_MotionTracking.GetBodyTrackingSupported(ref isBodyTrackingSupported);
#endif
                    if (isBodyTrackingSupported)
                    {
                        BodyTrackingBoneLength bones = new BodyTrackingBoneLength();
                        
#if PICO_OPENXR_SDK
                        // Start BodyTracking
                        BodyTrackingFeature.StartBodyTracking(BodyJointSet.BODY_JOINT_SET_BODY_FULL_START, bones);
                        // Has Pico motion tracker completed calibration (0: not completed; 1: completed)?
                        BodyTrackingFeature.GetBodyTrackingState(ref istracking, ref bs);
#else 
                        // Start BodyTracking
                        PXR_MotionTracking.StartBodyTracking(BodyJointSet.BODY_JOINT_SET_BODY_FULL_START, bones);
                        // Has Pico motion tracker completed calibration (0: not completed; 1: completed)?
                        PXR_MotionTracking.GetBodyTrackingState(ref istracking, ref bs);
#endif
                        // Debug.Log($"GetBodyTrackingState stateCode  = {bs.stateCode}  message  = {bs.message} ");
                        // If not calibrated, invoked system motion tracker app for calibration.
                        
                        if (bs.stateCode!=BodyTrackingStatusCode.BT_VALID)
                        {
                            if (bs.message==BodyTrackingMessage.BT_MESSAGE_TRACKER_NOT_CALIBRATED||bs.message==BodyTrackingMessage.BT_MESSAGE_UNKNOWN)
                            {
#if PICO_OPENXR_SDK
                                BodyTrackingFeature.StartMotionTrackerCalibApp();
#else
                                PXR_MotionTracking.StartMotionTrackerCalibApp();
#endif
                            }
                        }
                        // If not calibrated, invoked system motion tracker app for calibration.
                       
                    }
                }
                init = false;
                return new TrackableChanges<XRHumanBody>(1, 0, 0, allocator);
            }
            else
            {
                return new TrackableChanges<XRHumanBody>(0, 1, 0, allocator);
            }
        }
    }

    [RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.SubsystemRegistration)]
    static void RegisterDescriptor()
    {
        PLog.i(k_SubsystemId, "RegisterDescriptor");
#if AR_FOUNDATION_5
        var mXRHumanBodySubsystemCinfo = new XRHumanBodySubsystemCinfo
#endif

#if AR_FOUNDATION_6
        var mXRHumanBodySubsystemCinfo = new XRHumanBodySubsystemDescriptor.Cinfo
#endif
        {
            id = k_SubsystemId,
            providerType = typeof(HumanBodyProvider),
            subsystemTypeOverride = typeof(PXR_HumanBodySubsystem),
            supportsHumanBody2D = false,
            supportsHumanBody3D = true,
            supportsHumanBody3DScaleEstimation = true,
        };

#if AR_FOUNDATION_6
        XRHumanBodySubsystemDescriptor.Register(mXRHumanBodySubsystemCinfo);
#endif

#if AR_FOUNDATION_5
        if (!Register(mXRHumanBodySubsystemCinfo))
        {
            PLog.e(k_SubsystemId, $"Failed to register the {k_SubsystemId} subsystem.");
        }
        else
        {
            PLog.i(k_SubsystemId, $"success to register the {k_SubsystemId} subsystem.");
        }
#endif

    }

}
#endif