﻿//======= Copyright (c) Valve Corporation, All rights reserved. ===============
//
// Purpose: For controlling in-game objects with tracked devices.
//
//=============================================================================
using System;
using UnityEngine;
using Valve.VR;
using System.Runtime.InteropServices;
using System.Text;

namespace FOHEART_Mocap
{
    /*发送到MotionVenus的数据结构*/
    [StructLayout(LayoutKind.Sequential, Pack = 1)]
    public struct TrackerPosAtt_TypeDef
    {
        public Quaternion q;

        public float quat_x;
        public float quat_y;
        public float quat_z;
        public float quat_w;

        public Vector3 pos;

        public float pos0;
        public float pos1;
        public float pos2;

        public int eTrackingResult;
        public byte bDeviceIsConnected;

        public string trackerSN; //12   tracker的序列号 基站为LHB-DB6059E6 tracker为 LHR-1BEFA64F
    };
    public class SteamVR_TrackedObjectPlus : MonoBehaviour
    {
        void ListDevices()
        {
            //Clear all sn
            for (int i = 0; i < deviceSerialNumbers.Length; i++)
            {
                deviceSerialNumbers[i] = "";
            }
            for (int i = 0; i < SteamVR.connected.Length; ++i)
            {
                if (OpenVR.System == null)
                {
                    continue;
                }

                ETrackedPropertyError error = new ETrackedPropertyError();
                StringBuilder sb = new StringBuilder();

                OpenVR.System.GetStringTrackedDeviceProperty(
                    (uint)i,
                    ETrackedDeviceProperty.Prop_SerialNumber_String,
                    sb,
                    OpenVR.k_unMaxPropertyStringSize,
                    ref error);

                string SerialNumber = sb.ToString();

                OpenVR.System.GetStringTrackedDeviceProperty((uint)i,
                     ETrackedDeviceProperty.Prop_ModelNumber_String,
                     sb,
                     OpenVR.k_unMaxPropertyStringSize,
                     ref error);

                string ModelNumber = sb.ToString();
                if (SerialNumber.Length > 0 || ModelNumber.Length > 0)
                {
                   // Debug.Log("Device " + i.ToString() + " = " + SerialNumber + " | " + ModelNumber);
                }
                if (SerialNumber.Length > 0)
                {
                    deviceSerialNumbers[i] = SerialNumber;
                }
            }

        }

        //OnNewPoses函数进入的次数
        int onNewPosCounter = 0;
        const int maxTracker = 16;
        //所有发现的设备的序列号，包括头盔、基站和tracker
        string[] deviceSerialNumbers = new string[maxTracker];
        public TrackerPosAtt_TypeDef[] currentTrackerPosAtt = new TrackerPosAtt_TypeDef[maxTracker];
        public enum EIndex
        {
            None = -1,
            Hmd = (int)OpenVR.k_unTrackedDeviceIndex_Hmd,
            Device1,
            Device2,
            Device3,
            Device4,
            Device5,
            Device6,
            Device7,
            Device8,
            Device9,
            Device10,
            Device11,
            Device12,
            Device13,
            Device14,
            Device15,
            Device16
        }

        public EIndex index;

        [Tooltip("If not set, relative to parent")]
        public Transform origin;

        public bool isValid { get; private set; }

        private void OnNewPoses(TrackedDevicePose_t[] poses)
        {
            if (index == EIndex.None)
                return;

            var i = (int)index;

            isValid = false;
            if (poses.Length <= i)
                return; 

            // if (!poses[i].bDeviceIsConnected)
            //     return;

            //  if (!poses[i].bPoseIsValid)
            //     return;



#region SEND_TRACKER_POS_ATT_TO_MOTIONVENUS
            //---------
            onNewPosCounter++;
            //最小为1不能等于零
            uint trackerCount = maxTracker;
           
            if (onNewPosCounter % 100 == 0)
            {
                ListDevices();
                //  GC.Collect();
            }
#if false
            /*--start 发现的tracker*/
            GameObject TE_trackerSNObj = GameObject.Find("Canvas/TE_trackerSN");
              var trackerSNComp = TE_trackerSNObj.GetComponent<Text>();
              trackerSNComp.text = "在线的Tracker序列号：\r\n";
            /*--end*/
#endif

            //return;
            for (int trackerId = 0; trackerId < trackerCount; trackerId++)
            {
                var trackerPose = new SteamVR_Utils.RigidTransform(poses[trackerId].mDeviceToAbsoluteTracking);
               // TrackerPosAtt_TypeDef currentTrackerPosAtt;// = new TrackerPosAtt_TypeDef();
                currentTrackerPosAtt[trackerId].quat_x = trackerPose.rot.x;
                currentTrackerPosAtt[trackerId].quat_y = trackerPose.rot.y;
                currentTrackerPosAtt[trackerId].quat_z = trackerPose.rot.z;
                currentTrackerPosAtt[trackerId].quat_w = trackerPose.rot.w;

                currentTrackerPosAtt[trackerId].q = trackerPose.rot;

                currentTrackerPosAtt[trackerId].pos0 = trackerPose.pos.x;
                currentTrackerPosAtt[trackerId].pos1 = trackerPose.pos.y;
                currentTrackerPosAtt[trackerId].pos2 = trackerPose.pos.z;

                currentTrackerPosAtt[trackerId].pos = trackerPose.pos;

                if (trackerId == 1)
                { 
                
                }

                currentTrackerPosAtt[trackerId].eTrackingResult = (int)poses[trackerId].eTrackingResult;
                if (poses[trackerId].bDeviceIsConnected)
                {
                    currentTrackerPosAtt[trackerId].bDeviceIsConnected = 1;

                }
                else
                {
                    currentTrackerPosAtt[trackerId].bDeviceIsConnected = 0;
                }



                if (trackerId == 15)
                {
                    //int tempHalt = 0;
                    //tempHalt = tempHalt;
                }
                if (deviceSerialNumbers[trackerId].Length > 0)
                {
                    currentTrackerPosAtt[trackerId].trackerSN= deviceSerialNumbers[trackerId];

                    if(trackerId!=0)
                    {
                        /*只显示tracker*/
                        if (0 == string.Compare(deviceSerialNumbers[trackerId], 0, "LHR", 0, 3))
                        {
                            Vector3 convertEulerAngle = trackerPose.rot.eulerAngles;
#if false
                            trackerSNComp.text += deviceSerialNumbers[trackerId];
                            switch (poses[trackerId].eTrackingResult)
                            {
                                case ETrackingResult.Uninitialized:
                                    {
                                        trackerSNComp.text += " <color=\"#000000\">状态：已关机</color>";
                                    }
                                    break;
                                case ETrackingResult.Calibrating_OutOfRange:
                                    {
                                        trackerSNComp.text += " <color=\"#FF0000\">状态：定位丢失</color>";
                                    }
                                    break;
                                case ETrackingResult.Running_OK:
                                    {
                                        trackerSNComp.text += " <color=\"#00FF00\">状态：正在定位</color>";
                                    }
                                    break;
                                default:
                                    {
                                        trackerSNComp.text += (int)poses[trackerId].eTrackingResult;
                                    }
                                    break;
                            }
                           
                            
                            trackerSNComp.text += " 位置：";
                            trackerSNComp.text += currentTrackerPosAtt.pos0+",";
                            trackerSNComp.text += currentTrackerPosAtt.pos1 + ",";
                            trackerSNComp.text += currentTrackerPosAtt.pos2 ;
                            trackerSNComp.text += " 角度：";
                            trackerSNComp.text += convertEulerAngle.x + ",";
                            trackerSNComp.text += convertEulerAngle.y + ",";
                            trackerSNComp.text += convertEulerAngle.z;

                            trackerSNComp.text += "\r\n";
#endif
                        }
                       
                     
                    }

                }
                else
                {
                    //currentTrackerPosAtt.trackerSN = new string { ""} ;
                  
                }
            }

/*--start发送包数*/
#if false
            GameObject TE_txFramesObj = GameObject.Find("Canvas/TE_txFrames");
            var txFramesComp = TE_txFramesObj.GetComponent<Text>();
            txFramesComp.text = "发送包数：";
            txFramesComp.text += onNewPosCounter;
#endif
            /*--end*/

#endregion
        }

        SteamVR_Events.Action newPosesAction;

        SteamVR_TrackedObjectPlus()
        {

        }

        private void Awake()
        {
            newPosesAction = SteamVR_Events.NewPosesAction(OnNewPoses);

            //OnEnable();
#if false
            Screen.SetResolution(1024, 768, false);
            //获取按钮游戏对象
            GameObject btnObj = GameObject.Find("Canvas/BTN_startTxLoc");
            //获取按钮脚本组件
            Button btn = (Button)btnObj.GetComponent<Button>();
            //添加点击侦听
            btn.onClick.AddListener(onStartTxLoc);

            btnObj = GameObject.Find("Canvas/BTN_stopTxLoc");
            btn = (Button)btnObj.GetComponent<Button>();
            //添加点击侦听
            btn.onClick.AddListener(onStopTxLoc);
#endif
        }

        void OnEnable()
        {
            var render = SteamVR_Render.instance;
            if (render == null)
            {
                enabled = false;
                return;
            }

            newPosesAction.enabled = true;

            ListDevices();
        }

        void OnDisable()
        {
            newPosesAction.enabled = false;
            isValid = false;
        }

        public void SetDeviceIndex(int index)
        {
            if (System.Enum.IsDefined(typeof(EIndex), index))
                this.index = (EIndex)index;
        }
      
      
        void FixedUpdate()
        {
           // Debug.Log("FixedUpdate");
        }
    }
}