using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using UnityEngine;
using o0;
using o0.Geometry;
//using o0._9Axis;

namespace o0.IMU
{
    public class _9AxisPreProcessor
    {
        public _9AxisPreProcessor(Vector<int> GyrByteIndex = default, Vector<int> AccByteIndex = default, Vector<int> MagByteIndex = default, bool AccCorrection = true)
        {
            //Debug.Log("Attitude Init");
            this.AccCorrection = AccCorrection;
            ByteReverseMS = null;
            /*
            this.GyrByteIndex = GyrByteIndex != default ? GyrByteIndex : new Vector<int>(-1, 2, -3);
            this.AccByteIndex = AccByteIndex != default ? AccByteIndex : new Vector<int>(-1, 2, -3);
            this.MagByteIndex = MagByteIndex != default ? MagByteIndex : new Vector<int>(-3, 1, 2);/**///第一个6+3硬件
            /*
            this.GyrByteIndex = GyrByteIndex != default ? GyrByteIndex : new Vector<int>(3, 1, 2);
            this.AccByteIndex = AccByteIndex != default ? AccByteIndex : new Vector<int>(3, 1, 2);
            this.MagByteIndex = MagByteIndex != default ? MagByteIndex : new Vector<int>(-3, 1, 2);/**///第二个6+3硬件
            /*	
            "GyrByteIndex": [-3, 2, -1],
            "AccByteIndex": [3, -2, 1],
            "MagByteIndex": [-3, 2, 1]
            /**/
            this.GyrByteIndex = GyrByteIndex != default ? GyrByteIndex : new Vector<int>(3, -2, 1);
            this.AccByteIndex = AccByteIndex != default ? AccByteIndex : new Vector<int>(3, -2, 1);
            this.MagByteIndex = MagByteIndex != default ? MagByteIndex : new Vector<int>(-3, 2, 1);/**///九轴
                                                                                                       //ByteTo9Axis = new ByteToShorts(9);
        }

        //[Serialize]
        [Serialize]
        protected bool AccCorrection;
        protected double LastMS;
        protected ByteToShort ByteToShortMS;// = new ByteToShort(Short => Short < 0 || Short > 999 ? 0 : 1);//合并 并自动判断高低位
        [Serialize]
        protected bool? ByteReverseMS
        {
            get => ByteToShortMS.MinCount != 0 ? (bool?)null : ByteToShortMS.Reverse;
            set
            {
                ByteToShortMS = value == null ? new ByteToShort(Short => Short < 0 || Short > 999 ? 0 : 1) : new ByteToShort(null, 0, value ?? false);

                TimeGapFilter = new MedianFilter<double>(10);//顺便初始化以下内容
                distanceToAxis = new DistanceToAxis();
                //GyrHardwareVariance = new HardwareVariance();
                //AccHardwareVariance = new HardwareVariance();
                //MagHardwareVariance = new HardwareVariance();
                //Bow9Axis = new o09AxisCS();
                IMUInfo = default;
                //Debug.Log("Attitude Load");
            }
        }
        protected MedianFilter<double> TimeGapFilter = new MedianFilter<double>(10);

        public (Vector<double>, Vector<double>, Vector<double>, double) Update(IEnumerable<byte> gyrByte, IEnumerable<byte> accByte, IEnumerable<byte> magByte,
            byte min, byte sec, byte ms1, byte ms2)
        {
            if (!ByteToShortMS.Update(ms2, ms1))
                return IMUInfo;
            var CurrentMS = (((double)min) * 60 + sec) * 1000 + ByteToShortMS.Short;
            //Debug.Log(LastMS + " | " + CurrentMS);
            if (LastMS == default)
            {
                LastMS = CurrentMS;
                return IMUInfo;
            }
            var GapMS = CurrentMS - LastMS;
            LastMS = CurrentMS;
            //Debug.Log(new int[] { min, sec, ms1, ms2, (int)GapMS, TwoByteToFloat(ms2, ms1) }.ToJson());
            //Debug.Log(new int[] { min, sec, ms1, ms2, (int)GapMS, ByteToShortMS.Short }.ToJson());
            return Update(gyrByte, accByte, magByte, GapMS);
        }
        protected static Func<short, double> GyrCorrectRate = Short =>
        {
            //if (Math.Abs(Short) < 8192) return 1;
            if (Math.Abs(Short) < 16384) return 1;
            if (Math.Abs(Short) < 24576) return 0.5;
            return 0.5;
        };
        [Serialize]
        protected ByteToShorts ByteToGyr = new ByteToShorts(3);//合并 并自动判断高低位
        [Serialize]
        protected ByteToShorts ByteToAcc = new ByteToShorts(3);//合并 并自动判断高低位
        [Serialize]
        protected ByteToShorts ByteToMag = new ByteToShorts(3);//合并 并自动判断高低位
        private bool _GyrCalibrate;
        [Serialize]
        public MeanMaintainer<Vector<double>> GyrCalibrater = new MeanMaintainer<Vector<double>>();
        public bool GyrCalibrate
        {
            get => _GyrCalibrate; set
            {
                _GyrCalibrate = value;
                if (value)
                    GyrCalibrater = new MeanMaintainer<Vector<double>>();
                //else
                //Debug.Log("o0 GyrCalibrate Done: " + GyrCalibrater.Mean);
            }
        }
        [Serialize]
        //protected _9Axis.MagnetometerAutoCalibrater MagCalibrater = new _9Axis.MagnetometerAutoCalibrater();
        public MagnetometerAutoCalibrater MagCalibrater = new MagnetometerAutoCalibrater();
        [Serialize]
        protected Vector<int> GyrByteIndex;
        [Serialize]
        protected Vector<int> AccByteIndex;
        [Serialize]
        protected Vector<int> MagByteIndex;
        public Vector<double> LastGyr;
        //[Serialize]
        protected (Vector<double>, Vector<double>, Vector<double>, double) IMUInfo;
        //Vector<double> LastAcc;
        //Vector<double> LastMag;
        //double LastGapMS;
        //Bow9Axis Bow9Axis = new Bow9Axis();
        DistanceToAxis distanceToAxis;
        //public HardwareVariance GyrHardwareVariance;
        //public HardwareVariance AccHardwareVariance;
        //public HardwareVariance MagHardwareVariance;
        //o09AxisCS Bow9Axis;

        private double Map(Vector<double> Vector, int Index) => Index == 0 ? default : Index > 0 ? Vector[Index - 1] : -Vector[-Index - 1];
        private Vector<double> Map(Vector<double> Vector, Vector<int> Index) => new Vector<double>(Map(Vector, Index.x), Map(Vector, Index.y), Map(Vector, Index.z));//从1开始算index，负则反转正负号

        //bool test = true;
        public (Vector<double>, Vector<double>, Vector<double>, double) Update(IEnumerable<byte> gyrByte, IEnumerable<byte> accByte, IEnumerable<byte> magByte,
            double GapMS)
        {
            var GyrReady = ByteToGyr.Update(gyrByte);
            var AccReady = ByteToAcc.Update(accByte);
            var MagReady = ByteToMag.Update(magByte);
            //Debug.Log("GyrReady: " + GyrReady + " AccReady: " + AccReady + " MagReady: " + MagReady + " GapMS: " + GapMS);
            if (!GyrReady || !AccReady || !MagReady)
                return IMUInfo;
            if (GapMS <= 0 || !TimeGapFilter.Enqueue(GapMS))
                return IMUInfo;//负 或者 与10帧中位数相差2倍以上//后面这个主要用来稳定帧率，如果帧率不稳则不会通过
            //x俯仰角 y航向角 z翻滚角
            var Gyr = ByteToGyr.Vector() / 32768 * 2;
            if (GyrCalibrate)
                GyrCalibrater.Add(Gyr);
            Gyr = Map(Gyr - GyrCalibrater.Mean, GyrByteIndex);

            var Mag = ByteToMag.Vector() / 32768 * 256;
            if (!MagCalibrater.Update(Mag))
                return IMUInfo;
            //Mag = MagCalibrater.EllipsoidFitting.Map(Mag);
            Mag = Map(MagCalibrater.EllipsoidFitting.Map(Mag), MagByteIndex);

            var Acc = ByteToAcc.Vector(AccByteIndex) / 32768 * 16;
            //Debug.Log(AccByteIndex+" "+Acc);


            if (AccCorrection)
            {
                distanceToAxis.Update(Gyr, Acc, Mag, GapMS);
                Acc = distanceToAxis.AccCorrection(Gyr, Acc, GapMS);/**///轴心偏离矫正
            }


            //var Quaternion = Bow9Axis.Update((LastGyr + Gyr) / 2, Acc, Mag, GapMS);
            IMUInfo = ((LastGyr + Gyr) / 2, Acc, Mag, GapMS);
            LastGyr = Gyr;
            return IMUInfo;
        }
    }
}
