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

namespace Tracking
{

    public class HMDTracker : CalibratableTracker
    {
        public int framesToConsider = 10;
        public float SteerRotation => framesToConsider > 1
            ? CalculateSteerRotationMultiFrame()
            : CalculateSteerRotationSingleFrame();

        public float LeanRotation => framesToConsider > 1
            ? CalculateLeanRotationMultiFrame()
            : CalculateLeanRotationSingleFrame();

        protected override string KeyPrefix => "fw";
        private Queue<float> previousValues;
        private Queue<float> previousLeanValues;

        private void OnEnable()
        {
            if (framesToConsider > 1) previousValues = new Queue<float>(framesToConsider);
            if (framesToConsider > 1) previousLeanValues = new Queue<float>(framesToConsider);
        }

        /*private void OnGUI()
        {
            GUI.TextArea(new Rect(0, 400, 200, 90),
                $"LeanRotation: {LeanRotation}\nRelativeRotation: {RelativeRotation}\nRelativePosition: {RelativePosition}");
        }*/

        private float CalculateSteerRotationSingleFrame()
        {
            return RelativeRotation.y;
        }

        private float CalculateSteerRotationMultiFrame()
        {
            var angleThisFrame = CalculateSteerRotationSingleFrame();
            if (previousValues.Count > framesToConsider) previousValues.Dequeue();

            previousValues.Enqueue(angleThisFrame);

            //easiest way -> avg; TODO maybe in future, remove outliers
            return previousValues.Average();
        }

        private float CalculateLeanRotationSingleFrame()
        {
            return RelativeRotation.z;
        }

        private float CalculateLeanRotationMultiFrame()
        {
            var angleThisFrame = CalculateLeanRotationSingleFrame();
            if (previousLeanValues.Count > framesToConsider) previousLeanValues.Dequeue();

            previousLeanValues.Enqueue(angleThisFrame);

            //easiest way -> avg; TODO maybe in future, remove outliers
            return previousLeanValues.Average();
        }
    }
}