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 ByteToShort
    {
        public static short TwoByteToFloat(byte high, byte low)
        {
            if (BitConverter.IsLittleEndian)
                return BitConverter.ToInt16(new byte[] { low, high }, 0);
            else
                return BitConverter.ToInt16(new byte[] { high, low }, 0);
        }

        protected Func<short, double> CorrectRate;//给出数据是正确的可能性 0-1
        public int MinCount;//最少多少个采样，允许固定顺序//0则说明已经固定了顺序//如果序列化，请务必判断该值为0，如果不为0则给整个类赋null
        protected int Count;
        protected double CorrectRate0;
        protected double CorrectRate1;
        public bool Reverse;
        public short Short;
        public ByteToShort(Func<short, double> CorrectRate, int MinCount = 10, bool Reverse = false)
        {
            this.CorrectRate = CorrectRate;
            this.MinCount = MinCount;
            this.Count = 0;
            this.Reverse = Reverse;
        }
        public bool Update(byte b0, byte b1)
        {
            if (MinCount == 0)//已经固定
            {
                Short = Reverse ? TwoByteToFloat(b1, b0) : TwoByteToFloat(b0, b1);
                return true;
            }
            var s0 = TwoByteToFloat(b0, b1);
            var s1 = TwoByteToFloat(b1, b0);
            //Short = p0 < p1 ? s1 : s0;
            CorrectRate0 += CorrectRate(s0);
            CorrectRate1 += CorrectRate(s1);
            ++Count;
            if (Count >= MinCount)
            {
                if (CorrectRate0 / Count < 0.5 && CorrectRate1 / Count < 0.5)
                    throw new Exception("o0 ByteToShort Bad Hardware Data. Current Data: " + b0 + " " + b1);
                if (Math.Abs(CorrectRate0 - CorrectRate1) / MinCount < 0.25)//差距太小分辨不出
                    return false;
                MinCount = 0;
                Reverse = CorrectRate0 < CorrectRate1;
            }
            return false;
            //return p0 > 0.5 || p1 > 0.5;//两个里有一个是正确的概率高于0.5就返回true
        }
    }/**/
    public class ByteToShortByVariance//序列的差值的方差
    {
        public double ThresholdRate;
        protected int MinCount;//最少多少个采样，允许固定顺序//0则说明已经固定了顺序//如果序列化，请务必判断该值为0，如果不为0则给整个类赋null
        public int Count { get => Variance0.Count; set => Variance0.Count = Variance1.Count = value; }
        protected short LastShort0;
        protected short LastShort1;
        protected VarianceMaintainer<double> Variance0;
        protected VarianceMaintainer<double> Variance1;
        public bool Reverse;
        public short Short;
        public ByteToShortByVariance(int MinCount = 10, bool Reverse = false, double ThresholdRate = 2)
        {
            this.MinCount = MinCount;
            this.Reverse = Reverse;
            this.ThresholdRate = ThresholdRate;
            Variance0 = new VarianceMaintainer<double>();
            Variance1 = new VarianceMaintainer<double>();
            Count = -1;
        }
        public bool Update(byte b0, byte b1)
        {
            if (MinCount == 0)//已经固定
            {
                Short = Reverse ? ByteToShort.TwoByteToFloat(b1, b0) : ByteToShort.TwoByteToFloat(b0, b1);
                return true;
            }
            var s0 = ByteToShort.TwoByteToFloat(b0, b1);
            var s1 = ByteToShort.TwoByteToFloat(b1, b0);
            if (Count == -1)
            {
                Count = 0;
                LastShort0 = s0;
                LastShort1 = s1;
                return false;
            }
            var Change0 = s0 - LastShort0;
            var Change1 = s1 - LastShort1;
            Variance0.Add(Change0);
            Variance1.Add(Change1);
            LastShort0 = s0;
            LastShort1 = s1;
            if (Count >= MinCount)
            {
                if (Math.Min(Variance0, Variance1) * 2 > Math.Max(Variance0, Variance1))//差距太小分辨不出
                {
                    Count = MinCount;
                    return false;
                }
                //UnityEngine.Debug.Log(Variance0 + " | " + Variance1);
                MinCount = 0;
                Reverse = Variance1 < Variance0;
            }
            return false;
        }
    }/**/
    public class ByteToVector
    {
        public ByteToShortByVariance[] ByteToShort;
        public bool[] Reverse { get => new bool[] { ByteToShort[0].Reverse, ByteToShort[1].Reverse, ByteToShort[2].Reverse }; }
        public Vector<double> Vector { get => new Vector<double>(ByteToShort[0].Short, ByteToShort[1].Short, ByteToShort[2].Short); }
        public ByteToVector(int MinCount = 10, bool Reverse = false, double ThresholdRate = 2)
        {
            ByteToShort = new ByteToShortByVariance[] { new ByteToShortByVariance(MinCount, Reverse, ThresholdRate),
                new ByteToShortByVariance(MinCount, Reverse, ThresholdRate),
                new ByteToShortByVariance(MinCount, Reverse, ThresholdRate) };
        }

        public bool Update(byte xb0, byte xb1, byte yb0, byte yb1, byte zb0, byte zb1)
        {
            var b0 = ByteToShort[0].Update(xb0, xb1);
            var b1 = ByteToShort[1].Update(yb0, yb1);
            var b2 = ByteToShort[2].Update(zb0, zb1);
            return b0 && b1 && b2;
        }
    }
    public class ByteToShorts
    {
        public ByteToShortByVariance[] ByteToShort;
        public bool[] Reverse
        {
            get
            {
                var reverse = new bool[ByteToShort.Length];
                foreach (var i in ByteToShort.Index())
                    reverse[i] = ByteToShort[i].Reverse;
                return reverse;
            }
        }
        public short Short(int Index) => Index == 0 ? default : Index > 0 ? ByteToShort[Index - 1].Short : (short)-ByteToShort[-Index - 1].Short; //从1开始算index，负则反转正负号

        public Vector<double> Vector(int xIndex = 1, int yIndex = 2, int zIndex = 3) => new Vector<double>(Short(xIndex), Short(yIndex), Short(zIndex));//从1开始算index，负则反转正负号
        public Vector<double> Vector(Vector<int> Index) => new Vector<double>(Short(Index.x), Short(Index.y), Short(Index.z));//从1开始算index，负则反转正负号
        public ByteToShorts(int Count, int MinCount = 10, bool Reverse = false, double ThresholdRate = 2)
        {
            ByteToShort = new ByteToShortByVariance[Count];
            foreach (var i in ByteToShort.Index())
                ByteToShort[i] = new ByteToShortByVariance(MinCount, Reverse, ThresholdRate);
        }
        public bool Update(IEnumerable<byte> b)
        {
            bool done = true;
            foreach (var i in ByteToShort.Index())
            {
                var i2 = i * 2;
                done = done && ByteToShort[i].Update(b.ElementAt(i2), b.ElementAt(i2 + 1));
            }
            return done;
        }
    }
}
