﻿using System;
using System.IO;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using UnityEngine;

namespace FOHEART_Mocap
{
    public partial class FoheartGloveH1Pose :  MonoBehaviour
    {
        /// <summary>
        /// 不断查找子骨骼
        /// </summary>
        /// <param name="father"></param>
        /// <param name="boneName"></param>
        /// <param name="fullMatch"></param>
        /// <returns></returns>
        static GameObject FindChildGameObject(GameObject father, string boneName, bool fullMatch = false)
        {
            GameObject obj = null;

            if (fullMatch)
            {
                if (father.name.Equals(boneName, StringComparison.CurrentCultureIgnoreCase))
                {
                    obj = father;
                    return father;
                }
            }
            else
            {
                if (father.name.Equals(boneName, StringComparison.CurrentCultureIgnoreCase) &&
                    !father.name.Contains("ik")
                    )
                {
                    obj = father;
                    return father;
                }
                else if (father.name.EndsWith(boneName, StringComparison.CurrentCultureIgnoreCase) && !father.name.Contains("ik"))
                {
                    obj = father;
                    return father;
                }
                else if (father.name.StartsWith(boneName, StringComparison.CurrentCultureIgnoreCase) && !father.name.Contains("ik"))
                {
                    obj = father;
                    return father;
                }
            }

            foreach (Transform child in father.transform)
            {
                obj = FindChildGameObject(child.gameObject, boneName);
                if (obj != null)
                    break;
            }


            return obj;
        }

        static public float[] QToMatrix2(float[] q_wxyz)
        {
            float[] r = new float[9];
            r[0] = 1 - 2 * q_wxyz[2] * q_wxyz[2] - 2 * q_wxyz[3] * q_wxyz[3];
            r[3] = 2 * q_wxyz[1] * q_wxyz[2] + 2 * q_wxyz[0] * q_wxyz[3];
            r[6] = 2 * q_wxyz[1] * q_wxyz[3] - 2 * q_wxyz[0] * q_wxyz[2];
            r[1] = 2 * q_wxyz[1] * q_wxyz[2] - 2 * q_wxyz[0] * q_wxyz[3];
            r[4] = 1 - 2 * q_wxyz[1] * q_wxyz[1] - 2 * q_wxyz[3] * q_wxyz[3];
            r[7] = 2 * q_wxyz[2] * q_wxyz[3] + 2 * q_wxyz[0] * q_wxyz[1];
            r[2] = 2 * q_wxyz[1] * q_wxyz[3] + 2 * q_wxyz[0] * q_wxyz[2];
            r[5] = 2 * q_wxyz[2] * q_wxyz[3] - 2 * q_wxyz[0] * q_wxyz[1];
            r[8] = 1 - 2 * q_wxyz[1] * q_wxyz[1] - 2 * q_wxyz[2] * q_wxyz[2];
            return r;
        }
        /*
测试 ：
输入pos(0,0,0)

pos:点的位移  相当于把点旋转了之后 再加上一个平移。  一般在原点，设置为(0,0,0)
initialPos：旋转之前的点坐标
childPos:旋转之后的点坐标
*/
        static float[] PointRotWithQuat(float[] q_wxyz, float[] pos, float[] intialLoc)
        {
            float[] resultLoc = new float[3];
            /*
            根据父节点的位置和四元数求解子节点的位置
            */

            //四元数转旋转矩阵
            float[] r = new float[9];
            r = QToMatrix2(q_wxyz);
            //子节点的初始位置
            for (int i = 0; i < 3; i++)
            {
                resultLoc[i] = 0;
                for (int j = 0; j < 3; j++)
                {
                    resultLoc[i] += r[i * 3 + j] * intialLoc[j];
                }
                resultLoc[i] += pos[i];
            }

            return resultLoc;
        }

        /*
            tracker相关
         */
        Quaternion getHandTrackerRotBySN(string sn, out int index)
        {
            Quaternion ret = Quaternion.identity;
            index = -1;

            if (trackedObjs == null)
            {
                return ret;
            }
            for (int i = 0; i < 16; i++)
            {
                if (trackedObjs.currentTrackerPosAtt[i].trackerSN == null)
                    continue;

                if (trackedObjs.currentTrackerPosAtt[i].trackerSN == sn)
                {
                    ret = trackedObjs.currentTrackerPosAtt[i].q;
                    index = i;
                    break;
                }
            }


            // Quaternion tempQ = new Quaternion(ret.x, 0.0f, ret.z, ret.w);
            // Quaternion tempQ = new Quaternion(0, ret.y, ret.z, ret.w);
            // Quaternion tempQ = new Quaternion(ret.x, ret.y, 0, ret.w);
            //Quaternion tempQ = new Quaternion(0, ret.y, 0, ret.w);
            // return tempQ;
            return ret;
        }
        Quaternion getHandTrackerRotByIndex(int index)
        {

                     return trackedObjs.currentTrackerPosAtt[index].q; 
        }
        Vector3 getHandTrackerPosByIndex(int index)
        {
            return trackedObjs.currentTrackerPosAtt[index].pos;
        }
        Vector3 getHandTrackerPosBySN(string sn, out int index)
        {
            Vector3 ret = new Vector3(0, 0, 0);
            index = -1;
            if (trackedObjs == null)
            {
                return ret;
            }
            for (int i = 0; i < 16; i++)
            {
                if (trackedObjs.currentTrackerPosAtt[i].trackerSN == null)
                    continue;

                if (trackedObjs.currentTrackerPosAtt[i].trackerSN == sn)
                {
                    ret = trackedObjs.currentTrackerPosAtt[i].pos;
                    index = i;
                    break;
                }
            }

            return ret;
        }


        /*transpose global location from motionvenus coordinate to unity3d
        x in unity equals -x in motionvenus
       y in unity equals z in motionvenus
       z in unity equlas -y in motionvenus
        */
        Vector3 ConvertLocFromMotionVenusToUnity3D(float x, float y, float z)
        {
            Vector3 ret = new Vector3();

            Matrix4x4 motionVenusToUnity3D = new Matrix4x4(
                          new Vector4(-1, 0, 0, 0),
                          new Vector4(0, 0, -1, 0),
                          new Vector4(0, 1, 0, 0),
                          new Vector4(0, 0, 0, 1)
                           );

            Vector4 locMotionVenus = new Vector4(
               x, y, z,
                    0
                    );
            Vector4 transLoc = motionVenusToUnity3D * locMotionVenus;
            ret.Set(transLoc.x, transLoc.y, transLoc.z);

            return ret;
        }

        Quaternion ConvertQFromMotionVenusToUnity3D(Quaternion q)
        {
            Quaternion ret = new Quaternion();

            Matrix4x4 motionVenusToUnity3D = new Matrix4x4(
                      new Vector4(1, 0, 0, 0),
                      new Vector4(0, 0, 1, 0),
                      new Vector4(0, -1, 0, 0),
                      new Vector4(0, 0, 0, 1)
                       );
            Vector4 qFromMv = new Vector4(q.x, q.y, q.z, q.w);
            Vector4 qConv = motionVenusToUnity3D * qFromMv;
            ret.Set(qConv[0], qConv[1], qConv[2], qConv[3]);
            //ret.Set(qConv[0], 0, qConv[2], qConv[3]);
            ret.Normalize();
            return ret;
        }
    }
}
