using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using o0.Geometry;
using o0.Num;

namespace o0.IMU
{
    public class MedianFilter<T> where T : IComparable<T>
    {
        public SortedQueue<T> SortedQueue;
        public int MaxCount;
        public double ThresholdRate;
        public MedianFilter(int MaxCount, double ThresholdRate = 2)
        {
            if (ThresholdRate < 1)
                throw new Exception("o0 MedianFilter ThresholdRate Cannot Less Than 1");
            SortedQueue = new SortedQueue<T>();
            this.MaxCount = MaxCount;
            this.ThresholdRate = ThresholdRate;
        }
        public bool Enqueue(T item)
        {
            SortedQueue.Enqueue(item);
            var median = SortedQueue[SortedQueue.Count >> 1];
            if (SortedQueue.Count > MaxCount)
                SortedQueue.Dequeue();
            else
                return false;
            return item.CompareTo(median.Multiply(ThresholdRate)) < 0
                && item.CompareTo(median.Multiply(1 / ThresholdRate)) > 0;
        }
    }
}
