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

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<PoseLandmarkType> 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<PoseLandmarkType>();
            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();

                var parentsum = new Vector3(0f,0f,0f);

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

                pose.AveragePos = sum / AverageLength;
                
                

                
                //pose.CurrentQuaternion = Quaternion.LookRotation(pose.AveragePos - parentsum).normalized;
               
            }
        }

        public static void CalculateRotation(ref PoseTransform poseTransform, PoseLandmark landmark)
        {
            
        }

        /// <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;
            }
        }
        
        
    }
}