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

namespace FOHEART_Mocap
{
    class ByteEqualityComparer : EqualityComparer<Byte>
    {
        public override bool Equals(Byte a1, Byte a2)
        {
            return a1 == a2;
        }
        public override int GetHashCode(Byte a)
        {
            return a.GetHashCode();
        }
    }
    public class ActorFrameData
    {
        enum FoheartPluginErr
        {
            PROTOCOL_NOMATCH = -1,
            PACKAGE_LEN_NOMATCH = -2
        };

        public UInt16 motionVenusProtoVer;
      
        //协议版本号
        public string strActorName;
        //角色名称
        public UInt32 suitNumber;
        //套装编号
        public Byte suitType;
        //节点类型
        public UInt32 frameNumber;
        //角色位置
        public Byte boneCount;

        //骨骼数目
        public Byte leftHandNodeCount; //骨骼数目
        public Byte rightHandNodeCount; //骨骼数目
        public Dictionary<Byte, Quaternion> skeleton53RotQuat;
        public Dictionary<Byte, Quaternion> skeleton53RotBiasQuat;

        Dictionary<Byte, Vector3> skeleton53RotEuler;
        public Dictionary<Byte, Vector3> skeleton53RotBiasEuler;

        public Dictionary<Byte, Vector3> skeleton53Location;
        public Dictionary<Byte, Vector3> sensorAccels;
        public Dictionary<Byte, Vector3> sensorMags;
        public Dictionary<Byte, Vector3> sensorGyros;
        public bool isString;
        public bool isBoneRotQuat;
        public bool isBoneRotEuler;
        public bool isBonePositions;
        public bool isSensorAccels;
        public bool isSensorMag;
        public bool isSensorGyro;
        /*tracker的位置*/
        /*胯部的离地高度，指的是人体正直站立时，尾椎骨的离地高度*/
        public float hipHeight = 0.94f;

        public UInt32 bRtSkeletonBiasEuler = 0;

        private const UInt16 EulerScale6 = (1 << 6);
        private const UInt16 EulerScale7 = (1 << 7);

        private const UInt16 QuatScale = (1 << 13);
        private const UInt16 AccelScale = (1 << 10);
        private const UInt32 PositionScale = (1 << 16);

        private const int FrameHeaderSize = 128;

        public _KHHS32PosAttitude_managed khhs32PosAtt;

        public ActorFrameData()
        {
            skeleton53RotQuat = new Dictionary<Byte, Quaternion>(new ByteEqualityComparer());
            skeleton53RotBiasQuat = new Dictionary<Byte, Quaternion>(new ByteEqualityComparer());
            skeleton53RotEuler = new Dictionary<Byte, Vector3>(new ByteEqualityComparer());
            skeleton53RotBiasEuler = new Dictionary<Byte, Vector3>(new ByteEqualityComparer());
            skeleton53Location = new Dictionary<Byte, Vector3>(new ByteEqualityComparer());
            sensorAccels = new Dictionary<Byte, Vector3>(new ByteEqualityComparer());
            sensorMags = new Dictionary<Byte, Vector3>(new ByteEqualityComparer());
            sensorGyros = new Dictionary<Byte, Vector3>(new ByteEqualityComparer());
        }

        public int handleMotionVenusSDK(ref _KHHS32PosAttitude_managed frame)
        {
            int ret = 0;
            khhs32PosAtt = frame;
            
            return ret;
        }
      
        public int deComposeDataAsV2003(IPEndPoint remoteEndpoint, byte[] data, bool dataString, bool containPos, bool containEuler, bool containQuat, bool containSensorAccel, bool containSensorMag, bool containSensorGyro)
        {
            //return 0;

            isString = dataString;
            isBoneRotQuat = containQuat;
            isBoneRotEuler = containEuler;
            isBonePositions = containPos;
            isSensorAccels = containSensorAccel;
            isSensorMag = containSensorMag;
            isSensorGyro = containSensorGyro;

            int index = 0;
            //协议版本号必须与主程序相同
            motionVenusProtoVer = BitConverter.ToUInt16(data, index);
            index += Marshal.SizeOf(motionVenusProtoVer);

            byte nameLength = data[index];
            index += Marshal.SizeOf(nameLength);

            byte[] tempName = new byte[nameLength];
            Array.Copy(data, index, tempName, 0, nameLength);
            strActorName = Encoding.ASCII.GetString(tempName);
            index += (int)nameLength;

            suitNumber = BitConverter.ToUInt32(data, index);
            index += Marshal.SizeOf(suitNumber);

            suitType = data[index];
            index += Marshal.SizeOf(suitType);

            frameNumber = BitConverter.ToUInt32(data, index);
            index += Marshal.SizeOf(frameNumber);

            // Debug.Log(DateTime.Now.Second+":"+DateTime.Now.Millisecond+","+ frameNumber);
            //Debug.Log(Thread.CurrentThread.ManagedThreadId.ToString()+","+frameNumber);
            //Debug.Log( frameNumber);

            boneCount = data[index];
            index += Marshal.SizeOf(boneCount);

            leftHandNodeCount = data[index];
            index += Marshal.SizeOf(leftHandNodeCount);

            rightHandNodeCount = data[index];
            index += Marshal.SizeOf(rightHandNodeCount);

            //0 is binary  1 is string
            byte streamFormat = data[index];
            index += Marshal.SizeOf(streamFormat);

            byte skeletonPosition = data[index];
            index += Marshal.SizeOf(skeletonPosition);

            byte skeletonAngle = data[index];
            index += Marshal.SizeOf(skeletonAngle);

            byte skeletonCoordinate = data[index];
            index += Marshal.SizeOf(skeletonCoordinate);

            byte skeletonRotOrder = data[index];
            index += Marshal.SizeOf(skeletonRotOrder);

            byte sensorAngle = data[index];
            index += Marshal.SizeOf(sensorAngle);

            byte sensorAccel_g = data[index];
            index += Marshal.SizeOf(sensorAccel_g);

            byte sensorLAccel_g = data[index];
            index += Marshal.SizeOf(sensorLAccel_g);

            byte sensorGyro_degreePerSecond = data[index];
            index += Marshal.SizeOf(sensorGyro_degreePerSecond);

            byte sensorMag_mGauss = data[index];
            index += Marshal.SizeOf(sensorMag_mGauss);

#if false
            hipTrackerPosTemp.x = (float)BitConverter.ToSingle(data, index);
            index += Marshal.SizeOf(new Int32());
            hipTrackerPosTemp.y = (float)BitConverter.ToSingle(data, index);
            index += Marshal.SizeOf(new Int32());
            hipTrackerPosTemp.z = (float)BitConverter.ToSingle(data, index);
            index += Marshal.SizeOf(new Int32());
#endif

            hipHeight = (float)BitConverter.ToSingle(data, index);
            index += Marshal.SizeOf(new Int32());
            //Debug.Log("hipHeight:" + hipHeight);


            bRtSkeletonBiasEuler = (UInt32)BitConverter.ToUInt32(data, index);
            index += Marshal.SizeOf(new UInt32());

         

            if (dataString)
            {
                string str = System.Text.Encoding.Default.GetString(data, FrameHeaderSize, data.Length - FrameHeaderSize);

                string[] arrQueryString = str.Trim(' ').Split(' ');

                int floatCount = 0;
                if (containPos)
                {
                    floatCount += boneCount * 3;
                }
                if (containEuler)
                {
                    floatCount += boneCount * 3;
                }
                if (containQuat)
                {
                    floatCount += boneCount * 4;
                }

                //检查数据完整性
                if (floatCount != arrQueryString.Length)
                {

                    Debug.Log("Package length check error!");
                    return PACKAGE_LEN_NOMATCH;//数据不完整
                }
                skeleton53Location.Clear();
                skeleton53RotEuler.Clear();
                skeleton53RotQuat.Clear();

                sensorAccels.Clear();
                sensorMags.Clear();
                sensorGyros.Clear();

                index = 0;
                for (byte i = 0; i < boneCount; i++)
                {
                    if (containPos)
                    {
                        Vector3 posTemp = new Vector3(
                            float.Parse(arrQueryString[index]),
                            float.Parse(arrQueryString[index + 1]),
                            float.Parse(arrQueryString[index + 2]));
                        index += 3;
                        skeleton53Location.Add(i, posTemp);
                    }
                    if (containEuler)
                    {
                        Vector3 eulerTemp = new Vector3(
                            float.Parse(arrQueryString[index]),
                            float.Parse(arrQueryString[index + 1]),
                            float.Parse(arrQueryString[index + 2]));
                        index += 3;
                        skeleton53RotEuler.Add(i, eulerTemp);

                        FOHEARTMath fmath = new FOHEARTMath();
                        float[] quatTemp = fmath.EulerToQuat(
                            eulerTemp.x,
                            eulerTemp.y,
                            eulerTemp.z,
                            FOHEARTMath.ChannelOrder.ZXY);
                    }
                    if (containQuat)
                    {
                        Quaternion eulerTemp = new Quaternion(
                            float.Parse(arrQueryString[index]),
                            float.Parse(arrQueryString[index + 1]),
                            float.Parse(arrQueryString[index + 2]),
                            float.Parse(arrQueryString[index + 3])
                            );
                        index += 4;
                    }
                }
            }
            else
            {
                /*Binary*/
                int checkDataLength = FrameHeaderSize;
                if (containPos)
                {
                    checkDataLength += (boneCount + leftHandNodeCount + rightHandNodeCount) * (3 * Marshal.SizeOf(new Int32()));
                }
                if (containEuler)
                {
                    checkDataLength += (boneCount + leftHandNodeCount + rightHandNodeCount) * (3 * Marshal.SizeOf(new Int16()));
                }
                if (containQuat)
                {
                    checkDataLength += boneCount * (4 * Marshal.SizeOf(new Int16()));
                }
                if (containSensorAccel)
                {
                    checkDataLength += boneCount * (3 * Marshal.SizeOf(new Int16()));
                }
                if (containSensorMag)
                {
                    checkDataLength += boneCount * (3 * Marshal.SizeOf(new Int16()));
                }
                if (containSensorGyro)
                {
                    checkDataLength += boneCount * (3 * Marshal.SizeOf(new Int16()));
                }


                if (bRtSkeletonBiasEuler == 1)
                {
                    checkDataLength += (boneCount + leftHandNodeCount + rightHandNodeCount) * (3 * Marshal.SizeOf(new Int16()));
                }


                if (checkDataLength != data.Length)
                {
                    //检查数据完整性
                    Debug.Log("Package length check error!" + checkDataLength + "," + data.Length);
                    return PACKAGE_LEN_NOMATCH;//数据不完整
                }

                index = FrameHeaderSize;

                skeleton53Location.Clear();
                skeleton53RotEuler.Clear();
                skeleton53RotBiasEuler.Clear();
                skeleton53RotQuat.Clear();
                skeleton53RotBiasQuat.Clear();
                sensorAccels.Clear();
                sensorMags.Clear();
                sensorGyros.Clear();

                int iterTimes = boneCount + leftHandNodeCount + rightHandNodeCount;

                for (byte i = 0; i < iterTimes; i++)
                {
                    if (containPos)
                    {
                        Vector3 posTemp = new Vector3();
                        posTemp.x = (float)BitConverter.ToInt32(data, index) / PositionScale;
                        index += Marshal.SizeOf(new Int32());
                        posTemp.y = (float)BitConverter.ToInt32(data, index) / PositionScale;
                        index += Marshal.SizeOf(new Int32());
                        posTemp.z = (float)BitConverter.ToInt32(data, index) / PositionScale;
                        index += Marshal.SizeOf(new Int32());
                        skeleton53Location.Add(i, posTemp);
                    }
                    if (containEuler)
                    {
                        Vector3 eulerTemp = new Vector3();
                        eulerTemp.x = (float)BitConverter.ToInt16(data, index) / EulerScale7;
                        index += Marshal.SizeOf(new Int16());
                        eulerTemp.y = (float)BitConverter.ToInt16(data, index) / EulerScale7;
                        index += Marshal.SizeOf(new Int16());
                        eulerTemp.z = (float)BitConverter.ToInt16(data, index) / EulerScale7;
                        index += Marshal.SizeOf(new Int16());
                        skeleton53RotEuler.Add(i, eulerTemp);

                        if (13 == i)
                        {
                            //i = i;
                        }
                        if (12 == i)
                        {
                           // i = i;
                        }
                        FOHEARTMath fmath = new FOHEARTMath();
                        float[] quatTemp = fmath.EulerToQuat(
                            eulerTemp.x,
                            eulerTemp.y,
                            eulerTemp.z,
                            FOHEARTMath.ChannelOrder.ZXY);

                        if (12 == i)
                        {

                           // i = i;
                        }
#if false
                        /*判断象限*/
                        if (i == 0)
                        {
                            QuadJudge quadJudge = new QuadJudge();
                            QuadJudge.Quadrant_TypeDef quad;

                            quad = quadJudge.getQuadFromQuat(quatTemp[3], quatTemp[0], quatTemp[1], quatTemp[2]);
                            Debug.Log("q_wxyz:" +
                                quatTemp[3] + "," +
                                 quatTemp[0] + "," +
                                  quatTemp[1] + "," +
                                   quatTemp[2] + "," +
                                   "euler:" +
                                eulerTemp.x + "," +
                                 eulerTemp.y + "," +
                                  eulerTemp.z + "," +
                                "Quad:" + quad);
                        }
#endif

                        /*end*/

                        skeleton53RotQuat.Add((byte)i, new Quaternion(quatTemp[0], quatTemp[1], quatTemp[2], quatTemp[3]));


                    }
                    if (containQuat)
                    {
                        Quaternion eulerTemp = new Quaternion();
                        eulerTemp.x = (float)BitConverter.ToInt16(data, index) / QuatScale;
                        index += Marshal.SizeOf(new Int16());
                        eulerTemp.y = (float)BitConverter.ToInt16(data, index) / QuatScale;
                        index += Marshal.SizeOf(new Int16());
                        eulerTemp.z = (float)BitConverter.ToInt16(data, index) / QuatScale;
                        index += Marshal.SizeOf(new Int16());
                        eulerTemp.w = (float)BitConverter.ToInt16(data, index) / QuatScale;
                        index += Marshal.SizeOf(new Int16());

                        skeleton53RotQuat.Add((byte)i, eulerTemp);
                    }
                    if (containSensorAccel)
                    {
                        Vector3 sensorAccelTemp = new Vector3();
                        sensorAccelTemp.x = (float)BitConverter.ToInt16(data, index) / AccelScale;
                        index += Marshal.SizeOf(new Int16());
                        sensorAccelTemp.y = (float)BitConverter.ToInt16(data, index) / AccelScale;
                        index += Marshal.SizeOf(new Int16());
                        sensorAccelTemp.z = (float)BitConverter.ToInt16(data, index) / AccelScale;
                        index += Marshal.SizeOf(new Int16());
                        sensorAccels.Add(i, sensorAccelTemp);
                    }
                    if (containSensorMag)
                    {
                        Vector3 sensorAccelTemp = new Vector3();
                        sensorAccelTemp.x = (float)BitConverter.ToInt16(data, index) / AccelScale;
                        index += Marshal.SizeOf(new Int16());
                        sensorAccelTemp.y = (float)BitConverter.ToInt16(data, index) / AccelScale;
                        index += Marshal.SizeOf(new Int16());
                        sensorAccelTemp.z = (float)BitConverter.ToInt16(data, index) / AccelScale;
                        index += Marshal.SizeOf(new Int16());
                        sensorMags.Add(i, sensorAccelTemp);
                    }
                    if (containSensorGyro)
                    {
                        Vector3 sensorAccelTemp = new Vector3();
                        sensorAccelTemp.x = (float)BitConverter.ToInt16(data, index) / AccelScale;
                        index += Marshal.SizeOf(new Int16());
                        sensorAccelTemp.y = (float)BitConverter.ToInt16(data, index) / AccelScale;
                        index += Marshal.SizeOf(new Int16());
                        sensorAccelTemp.z = (float)BitConverter.ToInt16(data, index) / AccelScale;
                        index += Marshal.SizeOf(new Int16());
                        sensorGyros.Add(i, sensorAccelTemp);
                    }
                }

                for (byte i = 0; i < boneCount + leftHandNodeCount + rightHandNodeCount; i++)
                {
                    Vector3 eulerTemp = new Vector3();
                    eulerTemp.x = (float)BitConverter.ToInt16(data, index) / EulerScale6;
                    index += Marshal.SizeOf(new Int16());
                    eulerTemp.y = (float)BitConverter.ToInt16(data, index) / EulerScale6;
                    index += Marshal.SizeOf(new Int16());
                    eulerTemp.z = (float)BitConverter.ToInt16(data, index) / EulerScale6;
                    index += Marshal.SizeOf(new Int16());
                    skeleton53RotBiasEuler.Add(i, eulerTemp);
                }
            }
            return 0;//解析正确
        }      

        public int PROTOCOL_NOMATCH { get; set; }

        public int PACKAGE_LEN_NOMATCH { get; set; }
    }
}
