using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using o0.Geometry;
using o0.Num;
using UnityEngine;

namespace o0.IMU
{

    public class Map<TKey, T> : List<T>
    {
        public Dictionary<TKey, T> Dictionary;//按空间映射，每个空间Block内仅允许一个Vector
        public Func<T, TKey> ValueToKey;
        public Map(Func<T, TKey> ValueToKey)
        {
            Dictionary = new Dictionary<TKey, T>();
            this.ValueToKey = ValueToKey;
        }

        public new void Add(T value)
        {
            var key = ValueToKey(value);
            if (!Dictionary.ContainsKey(key))
            {
                base.Add(value);
                return;
            }
        }
    }



    public class MagnetometerAutoCalibrater
    {
        public int MaxCount;//超出2倍就通过缩网格减少数量
        //public List<Geometry.Vector<double>> Vector;//按输入顺序排序
        public Geometry.Vector<int> Key(Geometry.Vector<double> Vector) => (Vector * Math.Pow(2, CountPerLength)).To<int>();
        public Dictionary<Geometry.Vector<int>, Geometry.Vector<double>> VectorByBlock;//按空间映射，每个空间Block内仅允许一个Vector
        public double CountPerLength;//每单位长度内有多少个vector//大到小收敛

        public int NewBlock;
        public double NewBlockAccumulation;//每秒累加一次newBlock
        public long LastTimestamp;
        public int FitThreshold;//多少个newBlock累加则进行一次拟合  单位为 block * 秒

        public EllipsoidFitting EllipsoidFitting;
        public double Variance;//拟合的方差 小于0.001则认为地磁计拟合完成
        public double CompleteVarianceThreshold;

        public int FitCountLeft;//拟合成功多少次停止拟合
        public bool Lock;//停止校准,锁定fit
        public bool Complete
        {
            get => EllipsoidFitting != null && Variance < CompleteVarianceThreshold;
        }
        /*
        public double MagVectorInfoCount
        {
            get => (VectorByBlock?.Count ?? 0) / Math.Pow(CountPerLength == 0 ? 1 : CountPerLength, 3);
        }/**/
        public void Init()
        {
            VectorByBlock = new Dictionary<Geometry.Vector<int>, Geometry.Vector<double>>();
            CountPerLength = 6;
            EllipsoidFitting = null;
            Variance = double.MaxValue;
            ++FitCountLeft;
        }

        public MagnetometerAutoCalibrater(double CompleteVarianceThreshold = 0.001, int MaxCount = 1000, int FitThreshold = default, int FitCountLeft = 5)
        {
            this.CompleteVarianceThreshold = CompleteVarianceThreshold;
            this.MaxCount = MaxCount;
            this.FitThreshold = FitThreshold == default ? 100 : FitThreshold;
            this.FitCountLeft = FitCountLeft;
            //Vector = new List<Geometry.Vector<double>>();
            NewBlock = 0;
            NewBlockAccumulation = 0;
            LastTimestamp = o0.ToTimestampMilliseconds();
            Lock = false;
            Init();
        }
        protected void Fit()
        {
            NewBlock = 0;
            NewBlockAccumulation = 0;
            var lastComplete = Complete;
            var lastEllipsoidFitting = EllipsoidFitting;
            var lastVariance = Variance;
            EllipsoidFitting = EllipsoidFitting.Fit(VectorByBlock.Values);

            /**/
            if (EllipsoidFitting == null)
            {
                //Debug.Log("o0 EllipsoidFitting Fail");
            }
            else
            {
                var length = new double[VectorByBlock.Count];
                var i = 0;
                foreach (var io in VectorByBlock.Values)
                    length[i++] = EllipsoidFitting.Map(io).Length;
                Variance = length.Variance();
                //Debug.Log("o0 EllipsoidFitting Successed Variance " + Variance);
                if (Complete)
                    --FitCountLeft;

                if (lastComplete && !Complete)//受到磁场干扰，退回之前的拟合
                {
                    //Debug.Log("o0 EllipsoidFitting Interfered, Back To Last Fitting And Lock");
                    Lock = true;
                    Init();
                    EllipsoidFitting = lastEllipsoidFitting;
                    Variance = lastVariance;
                    return;
                    //throw new Exception("o0 EllipsoidFitting 受到磁场干扰，请初始化设备");
                    //Debug.Log("o0 EllipsoidFitting Init");
                    //Init();
                }
                if (FitCountLeft <= 0)
                {
                    //Debug.Log("o0 EllipsoidFitting Finished And Lock");
                    Lock = true;
                    VectorByBlock = null;
                }
            }

        }
        protected void Add(Geometry.Vector<double> Mag, bool TryFitIfNewBlock = true)
        {
            var key = Key(Mag);
            if (VectorByBlock.ContainsKey(key))
                VectorByBlock[key] = Mag;
            else
            {
                VectorByBlock.Add(key, Mag);
                if (TryFitIfNewBlock)
                {
                    ++NewBlock;
                    var newTimestamp = o0.ToTimestampMilliseconds();
                    NewBlockAccumulation += NewBlock * (newTimestamp - LastTimestamp) / 1000.0;
                    LastTimestamp = newTimestamp;
                    //Debug.Log("o0 MagnetometerAutoCalibrater NewBlock: " + NewBlock);
                    if (VectorByBlock.Count > MaxCount / 8 && NewBlockAccumulation >= FitThreshold)
                        Fit();
                }
            }
        }
        public bool Update(Geometry.Vector<double> Mag)
        {
            if (Lock)
            {
                if (EllipsoidFitting == null)
                    throw new Exception("o0 EllipsoidFitting Null");
                else
                    return true;
            }
            if (VectorByBlock.Count < MaxCount)
            {
                if (VectorByBlock == null)
                {
                    //Vector.Add(Mag);
                    return false;
                }
                Add(Mag);
                return Complete;

                //Vector.Remove(VectorByBlock[key]);
                //Vector.Add(Mag);
                ////////////比较一下哪个向量应该留下//暂时没什么好想法
                /*
                Vector.Remove(VectorByBlock[key]);
                Vector.Add(Mag);
                VectorByBlock[key] = Mag;
                return true;/**/
            }
            Fit();
            var Mags = VectorByBlock.Values;
            //Debug.Log("o0 MagnetometerAutoCalibrater CountPerLength " + CountPerLength + " -> " + (CountPerLength - 1));
            --CountPerLength;
            VectorByBlock = new Dictionary<Geometry.Vector<int>, Geometry.Vector<double>>();
            foreach (var i in Mags)
                Add(i, false);
            Add(Mag, false);
            return Complete;
        }
        public Geometry.Vector<double> Map(Geometry.Vector<double> Vector) => EllipsoidFitting?.Map(Vector) ?? Vector;
    }

}
