using System.Collections.Generic;
using UnityEngine;
using System;

namespace Models
{
    /// <summary>
    /// 捕捉骨骼的动作转换
    /// </summary>
    public struct PoseTransform
    {
        /// <summary>
        /// MediaPipe中动捕节点标识
        /// </summary>
        public readonly PoseLandmarkType MediaPipeName;

        /// <summary>
        /// Unity中绑定骨骼名称
        /// </summary>
        public readonly HumanBodyBones UnityName;

        /// <summary>
        /// 取平均的长度
        /// </summary>
        public static int AverageLength;

        /// <summary>
        /// 取平均之后的结果
        /// </summary>
        public Vector3 AveragePos;

        /// <summary>
        /// 节点的父节点列表
        /// </summary>
        public readonly List<PoseTransform> Parent;

        /// <summary>
        /// 骨骼节点的初始角度
        /// </summary>
        public Quaternion PreviousQuaternion;

        /// <summary>
        /// 骨骼节点的当前角度
        /// </summary>
        public Quaternion CurrentQuaternion;

        private int _count;
        private readonly Queue<PoseLandmark> _landmarkQueue;

        public PoseTransform(
            PoseLandmarkType type,
            int averageLength = 5
        )
        {
            MediaPipeName = type;
            UnityName = GetRelatedBone(type);
            AveragePos = new Vector3();
            Parent = new List<PoseTransform>();
            PreviousQuaternion = new Quaternion();
            CurrentQuaternion = new Quaternion();
            AverageLength = averageLength;
            
            _count = 0;
            _landmarkQueue = new Queue<PoseLandmark>();
        }

        public static void UpdatePosition(ref PoseTransform pose, PoseLandmark landmark)
        {
            if (pose._count < AverageLength)
            {
                pose._landmarkQueue.Enqueue(landmark);
                pose._count++;
            }
            else
            {
                pose._landmarkQueue.Dequeue();
                pose._landmarkQueue.Enqueue(landmark);

                var sum = new Vector3();

                foreach (var poseLandmark in pose._landmarkQueue)
                {
                    sum += new Vector3(poseLandmark.X, poseLandmark.Y, poseLandmark.Z);
                }

                pose.AveragePos = sum / AverageLength;
            }
        }

        public static void CalculateRotation(PoseTransform poseTransform)
        {
            
            //var temp = _posLandmarks.Find(t=>t.mediaPipeName == landmark.Type.ToString());
             var sum = new Vector3(0,0,0);
             
             if (landmark.Visibility > 0.6)
             {
                 var temp = new Vector4(landmark.X, landmark.Y, landmark.Z, 1);
                 
                 
                 MediaPipePos = new Vector3(-temp.x, -temp.y, -temp.z);
                 
             }

             if (UnityName != HumanBodyBones.LastBone)
             {
                 Debug.Log(UnityName+".MediaPipePos:"+MediaPipePos);
             }
            
            foreach (var parents in Parent)
            {
               
                    sum += parents.MediaPipePos;
                    //Debug.Log("parents:"+parents.UnityName+".MediaPipePos:"+parents.MediaPipePos);
                    
            }
            //Debug.Log("sum:"+sum);
            var currentPos = sum / Parent.Count;
            
            CurrentQuaternion = Quaternion.LookRotation(MediaPipePos - currentPos);
            
        }

        /// <summary>
        /// 获得同相关捕捉点关联的骨骼
        /// </summary>
        /// <param name="type">捕捉点的种类</param>
        /// <returns>关联骨骼的种类</returns>
        private static HumanBodyBones GetRelatedBone(PoseLandmarkType type)
        {
            switch (type)
            {
                case PoseLandmarkType.LeftHip:
                    return HumanBodyBones.LeftUpperLeg;
                case PoseLandmarkType.RightHip:
                    return HumanBodyBones.RightUpperLeg;
                case PoseLandmarkType.LeftShoulder:
                    return HumanBodyBones.LeftUpperArm;
                case PoseLandmarkType.RightShoulder:
                    return HumanBodyBones.RightUpperArm;
                case PoseLandmarkType.Nose:
                    return HumanBodyBones.Head;
                case PoseLandmarkType.LeftElbow:
                    return HumanBodyBones.LeftLowerArm;
                case PoseLandmarkType.RightElbow:
                    return HumanBodyBones.RightLowerArm;
                case PoseLandmarkType.LeftWrist:
                    return HumanBodyBones.LeftHand;
                case PoseLandmarkType.RightWrist:
                    return HumanBodyBones.RightHand;
                case PoseLandmarkType.LeftKnee:
                    return HumanBodyBones.LeftLowerLeg;
                case PoseLandmarkType.RightKnee:
                    return HumanBodyBones.RightLowerLeg;
                case PoseLandmarkType.LeftAnkle:
                    return HumanBodyBones.LeftFoot;
                case PoseLandmarkType.RightAnkle:
                    return HumanBodyBones.RightFoot;
                case PoseLandmarkType.LeftFootIndex:
                    return HumanBodyBones.LeftToes;
                case PoseLandmarkType.RightFootIndex:
                    return HumanBodyBones.RightToes;
                default:
                    return HumanBodyBones.LastBone;
            }
        }
        
        
    }
}