﻿using System;
using System.Threading;
using System.Text;
using UnityEngine;
using System.Runtime.InteropServices;
using System.Collections.Generic;

namespace FOHEART_Mocap
{

    public class FoheartNetFrameManager
    {
        /*---------------------*/
        [DllImport("MotionVenusSDK_x64.dll", EntryPoint = "MotionVenusSDK_ListenUDPPort")]
        public static extern int MotionVenusSDK_ListenUDPPort(int nPort);

        [DllImport("MotionVenusSDK_x64.dll", EntryPoint = "MotionVenusSDK_CloseUDPPort")]
        public static extern int MotionVenusSDK_CloseUDPPort();

        [DllImport("MotionVenusSDK_x64.dll", EntryPoint = "MotionVenusSDK_isGloveNewFramePending")]
        public static extern bool MotionVenusSDK_isGloveNewFramePending(byte[] actorName);

        [DllImport("MotionVenusSDK_x64.dll", EntryPoint = "MotionVenusSDK_resetGloveNewFramePending")]
        public static extern bool MotionVenusSDK_resetGloveNewFramePending(byte[] actorName);

        [DllImport("MotionVenusSDK_x64.dll", EntryPoint = "MotionVenusSDK_GetGloveSkeletonsFrame")]
        public static extern int MotionVenusSDK_GetGloveSkeletonsFrame(byte[] actorName, byte[] pKHHS32PosAttitude);


        [DllImport("MotionVenusSDK_x64.dll", EntryPoint = "MotionVenusSDK_getVersion")]
        public static extern uint MotionVenusSDK_getVersion();
        /*---------------------*/

        bool debugLogEn = false;
        private static FoheartNetFrameManager uniqueInstance;
        private static readonly object locker = new object();
        public static FoheartNetFrameManager GetInstance()
        {
            if (uniqueInstance == null)
            {
                lock (locker)
                {
                    if (uniqueInstance == null)
                    {
                        uniqueInstance = new FoheartNetFrameManager();
                        uniqueInstance.initRec();
                    }
                }
            }
            return uniqueInstance;
        }


        FoheartGloveH1Pose.RealFrameCounter realFrameCounter = new FoheartGloveH1Pose.RealFrameCounter();

        bool motionVenusSDK_ConnectSuccess = false;
        //UDP接收线程
        Thread tRx;
        //UDP广播接收端口
        public int UDP_receivePort;

        /*
         * 自动检测挂载了FoheartModelNormal或者FoheartHandNormal的模型
         */
        public bool autoDetectMocapModels = true;
        [Tooltip("全身动捕脚本绑定的模型列表，脚本会在程序启动时在整个场景中自动查找")]
        public List<FoheartGloveH1Pose> FH_FullBodyPlayerList = new List<FoheartGloveH1Pose>();
        public List<bool> FH_FullBodyPlayerListHandled = new List<bool>();

        private FoheartNetFrameManager()
        {
            UDP_receivePort = 5001;

            if (autoDetectMocapModels)
            {
                FH_FullBodyPlayerList.Clear();
                FH_FullBodyPlayerListHandled.Clear();
            }
        }

        public void addModel(UnityEngine.Object obj)
        {
            FH_FullBodyPlayerList.Add((FoheartGloveH1Pose)obj);
            FH_FullBodyPlayerListHandled.Add(false);
        }
        //初始化
        void Start()
        {


        }

        //UI界面
        void OnGUI()
        {

        }

        //脚本退出时,停止线程
        void OnApplicationQuit()
        {
            /*如果脚本没有打开，则*/
            if (tRx != null)
            {
                tRx.Abort();
            }

            Debug.Log("OnApplicationQuit...");
        }


        public void OnDisable()
        {
            if (tRx != null)
            {
                tRx.Abort();
                Debug.Log("[FoheartNetFrameManager] Close rx thread...");

            }

            if (motionVenusSDK_ConnectSuccess)
            {
                int retMotionVenusSDK_CloseUDPPort = 0;
                retMotionVenusSDK_CloseUDPPort = MotionVenusSDK_CloseUDPPort();
                if (0 == retMotionVenusSDK_CloseUDPPort)
                {
                    Debug.Log("[FoheartNetFrameManager] MotionVenusSDK_CloseUDPPort return " + retMotionVenusSDK_CloseUDPPort);
                }
                else
                {
                    Debug.Log("[FoheartNetFrameManager] MotionVenusSDK_CloseUDPPort return " + retMotionVenusSDK_CloseUDPPort);
                }
            }

        }


        double applicationStartMs = 0;

        //初始化接收器
        private void initRec()
        {

            if (0 == MotionVenusSDK_CloseUDPPort())
            {
                Debug.Log("MotionVenusSDK_CloseUDPPort");
            }

            int motionVenusSDK_ConnectResult = MotionVenusSDK_ListenUDPPort(UDP_receivePort);
            if (0 == motionVenusSDK_ConnectResult)
            {
                motionVenusSDK_ConnectSuccess = true;
                tRx = new Thread(mcReceiveMotionVenusSDKData);
                tRx.Start();
            }
            else
            {

            }
            Debug.Log("MotionVenusSDK_ConnectResult " + motionVenusSDK_ConnectResult);
            if (0 == motionVenusSDK_ConnectResult)
            {
                Debug.Log("All Set");
            }
        }
      

        void mcReceiveMotionVenusSDKData()
        {
            TimeSpan ts = new TimeSpan(DateTime.Now.Ticks);
            double currentTMs = ts.TotalMilliseconds;
            realFrameCounter.currentRxTMs = currentTMs;
            realFrameCounter.lastRxTMs = currentTMs;

            ActorFrameData frameDataTemp = new ActorFrameData();

            while (true)
            {
                /*
                全身动捕的数据分发到各个模型中
                */
                for (int i = 0; i < FH_FullBodyPlayerList.Count; i++)
                {
                    if (FH_FullBodyPlayerList[i] == null)
                    {
                        continue;
                    }
                    _KHHS32PosAttitude_managed k32;
                    byte[] actorNameInUnity = System.Text.Encoding.ASCII.GetBytes(FH_FullBodyPlayerList[i].ActorName);
                    if (MotionVenusSDK_isGloveNewFramePending(actorNameInUnity))
                    {
                        byte[] khs53Bytes = new byte[Marshal.SizeOf(typeof(_KHHS32PosAttitude_managed))];
                        MotionVenusSDK_GetGloveSkeletonsFrame(actorNameInUnity, khs53Bytes);
                        k32 = (_KHHS32PosAttitude_managed)bytesToStruct(khs53Bytes, typeof(_KHHS32PosAttitude_managed));
                    }
                    else
                    {
                        continue;
                    }

                    ASCIIEncoding encoding = new ASCIIEncoding();
                    int actualNameLen = 0;
                    for (int j = 0; j < k32.hdr.AvatarNameLength; j++)
                    {
                        if (k32.hdr.AvatarName[j] != 0x00)
                        {
                            actualNameLen++;
                        }
                    }
                    string strActorName = encoding.GetString(k32.hdr.AvatarName, 0, actualNameLen);

                    if (FH_FullBodyPlayerList[i].autoDetectActorName)
                    {
                        FH_FullBodyPlayerList[i].actorFrameData.handleMotionVenusSDK(ref k32);
                        FH_FullBodyPlayerList[i].frameProperties.dataPending = true;
                    }
                    else
                    {
                        if (string.Equals(FH_FullBodyPlayerList[i].ActorName, strActorName))
                        {
                            CalculateRealFrameRate();
                            // Debug.Log("frameDataTemp:" + frameDataTemp.local_KHS53PosAttitude_managed.hdr.frameNumber + "a:" + a);

                            FH_FullBodyPlayerList[i].actorFrameData.handleMotionVenusSDK(ref k32);
                            FH_FullBodyPlayerList[i].frameProperties.dataPending = true;
                            FH_FullBodyPlayerListHandled[i] = true;
                            //Debug.Log(FH_FullBodyPlayerList.Count + " " + FH_FullBodyPlayerList[i].ActorName + " [" + getCurrentMs() + "]Emit:" + k53.hdr.frameNumber);
                        }
                        else
                        {
                            // Debug.Log("frameDataTemp:" + frameDataTemp.local_KHS53PosAttitude_managed.hdr.frameNumber + "b:" + b+"pending:"+ model.dataPending);
                        }
                    }
                }
                for (int i = 0; i < FH_FullBodyPlayerListHandled.Count; i++)
                {
                    if (FH_FullBodyPlayerListHandled[i])
                    {
                        byte[] actorNameInUnity = System.Text.Encoding.ASCII.GetBytes(FH_FullBodyPlayerList[i].ActorName);
                        MotionVenusSDK_resetGloveNewFramePending(actorNameInUnity);
                    }
                }
                Thread.Sleep(2);
            }

        }

        public void MakeAllActorOnFloor()
        {
#if false
            foreach (FoheartModelNormal player in PlayerList)
            {
                player.keepActorOnFloor.calculateBias();
            }
#endif
        }

        public void SetAdditionRot(float x, float y, float z)
        {
#if false
            foreach (FoheartModelNormal player in PlayerList)
            {
                player.rotationAdjust.bodyX = (int)x;
                player.rotationAdjust.bodyY = (int)y;
                player.rotationAdjust.bodyZ = (int)z;
            }
#endif
        }

#region bytesToStruct
        /// <summary>
        /// Byte array to struct or classs.
        /// </summary>
        /// <param name=”bytes”>Byte array</param>
        /// <param name=”type”>Struct type or class type.
        /// Egg:class Human{...};
        /// Human human=new Human();
        /// Type type=human.GetType();</param>
        /// <returns>Destination struct or class.</returns>
        public static object bytesToStruct(byte[] bytes, Type type)
        {

            int size = Marshal.SizeOf(type);//Get size of the struct or class.          
            if (bytes.Length < size)
            {
                return null;
            }
            IntPtr structPtr = Marshal.AllocHGlobal(size);//Allocate memory space of the struct or class. 
            Marshal.Copy(bytes, 0, structPtr, size);//Copy byte array to the memory space.
            object obj = Marshal.PtrToStructure(structPtr, type);//Convert memory space to destination struct or class.         
            Marshal.FreeHGlobal(structPtr);//Release memory space.    
            return obj;
        }
#endregion

        void CalculateRealFrameRate()
        {
            realFrameCounter.currentRxCounter++;
            TimeSpan ts = new TimeSpan(DateTime.Now.Ticks);
            realFrameCounter.currentRxTMs = ts.TotalMilliseconds;
            if ((realFrameCounter.currentRxTMs - realFrameCounter.lastRxTMs) > 1000.0)
            {
                int bias = realFrameCounter.currentRxCounter - realFrameCounter.lastRxCounter;
                if (debugLogEn)
                    Debug.Log("net bias:" + bias);
                realFrameCounter.lastRxTMs = realFrameCounter.currentRxTMs;
                realFrameCounter.lastRxCounter = realFrameCounter.currentRxCounter;
            }
        }

        double getCurrentMs()
        {
            TimeSpan ts = new TimeSpan(DateTime.Now.Ticks);
            double ret = ts.TotalMilliseconds;
            ret -= applicationStartMs;
            return ret;
        }
    }

}