using Models;
using UnityEngine;
using System.Collections.Concurrent;

namespace Utils.PoseTransformHandlers
{
    public class AverageHandler : IPoseTransformHandler
    {
        public static int AverageLength = 3;
        // 线程安全的队列集合
        // 据文档描述甚至没有使用锁
        private readonly ConcurrentQueue<PoseLandmark> _queue = new ConcurrentQueue<PoseLandmark>();
        
        public void ReceivePoseLandmark(PoseLandmark landmark)
        {
            if (_queue.Count < AverageLength)
            {
                _queue.Enqueue(landmark);
            }
            else
            {
                if (_queue.TryDequeue(out _))
                {
                    _queue.Enqueue(landmark);
                }
            }
        }

        public Vector3 GetResultPosition()
        {
            var sum = new Vector3();

            foreach (var item in _queue)
            {
                sum += new Vector3(item.X, item.Y, item.Z);
            }

            return sum / AverageLength;
        }
    }
}