using FlyADBase;
using GalaSoft.MvvmLight.Command;
using Microsoft.Win32;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.ComponentModel;
using System.IO;
using System.Linq;
using System.Threading.Tasks;
using System.Windows;

namespace Flyad7_WPF
{
    public class WdGridAdvAnalyzeVm : INotifyPropertyChanged
    {
        public double R { get; set; }
        public double CurrR { get; set; }
        public double CurrGroupR { get; set; }
        public double Speed { get; set; }
        public ObservableCollection<double> KsP { get; } = new ObservableCollection<double>();
        public ObservableCollection<double> Ks { get; } = new ObservableCollection<double>();


        public int CellCnt { get; private set; }
        public int SampleCellIdx { get; set; }



        public RelayCommand LoadCmd { get; private set; }
        public RelayCommand AutoCmd { get; private set; }
        public RelayCommand Auto2Cmd { get; private set; }
        public RelayCommand ManualCmd { get; private set; }
        public RelayCommand NextIdxCmd { get; private set; }
        public RelayCommand PreIdxCmd { get; private set; }
        public RelayCommand ClearRCmd { get; private set; }

        public RelayCommand ConvDebugCmd { get; private set; }
        List<GridAdvAnalyzeCell> gridAdvAnalyzeCells = new List<GridAdvAnalyzeCell>();
        List<GridAdvAnalyzeCellGroup> groups = new List<GridAdvAnalyzeCellGroup>();
        int[] Target;
        int[] Roller;

        System.Windows.Forms.DataVisualization.Charting.Chart chart;
        System.Windows.Forms.DataVisualization.Charting.Chart chart1;
        System.Windows.Forms.DataVisualization.Charting.Chart chart2;
        IFlyADClientAdv flyad;

        public event PropertyChangedEventHandler PropertyChanged;

        public WdGridAdvAnalyzeVm()
        {
            LoadCmd = new RelayCommand(Load);
            AutoCmd = new RelayCommand(AutoCal);
            Auto2Cmd = new RelayCommand(AutoCal2);
            ManualCmd = new RelayCommand(ManualCal);
            ClearRCmd = new RelayCommand(DrawClear);
            NextIdxCmd = new RelayCommand(NextIdx);
            PreIdxCmd = new RelayCommand(PreIdx);
            ConvDebugCmd = new RelayCommand(ConvDebug);


            for (int i = 0; i < 5; i++)
            {
                KsP.Add(0);
                Ks.Add(0);
            }
            Ks[0] = 1;
        }

        public void Init(
            IFlyADClientAdv flyad,
            System.Windows.Forms.DataVisualization.Charting.Chart chart0,
            System.Windows.Forms.DataVisualization.Charting.Chart chart1,
            System.Windows.Forms.DataVisualization.Charting.Chart chart2
            )
        {
            this.flyad = flyad;
            this.chart = chart0;
            this.chart1 = chart1;
            this.chart2 = chart2;
        }

        private void ConvDebug()
        {
            WdConvolution w = new WdConvolution();
            w.Init(Target);
            w.ShowDialog();
        }

        private void PreIdx()
        {
            SampleCellIdx--;
            ManualCal();
        }

        private void NextIdx()
        {
            SampleCellIdx++;
            ManualCal();
        }

        void Load()
        {
            OpenFileDialog ofd = new OpenFileDialog();
            ofd.DefaultExt = ".json";
            if (ofd.ShowDialog() != true)
                return;

            List<TimeGridAdv2EventArgs> timeGridAdv2Datas;
            string path = ofd.FileName;
            try
            {
                string json = File.ReadAllText(path);
                timeGridAdv2Datas = Newtonsoft.Json.JsonConvert.DeserializeObject<List<TimeGridAdv2EventArgs>>(json);
            }
            catch (Exception e)
            {
                MessageBox.Show(e.Message);
                return;
            }
            if (timeGridAdv2Datas == null || timeGridAdv2Datas.Count() < 4)
            {
                MessageBox.Show("数量为空，或者太少了");
                return;
            }

            toGridAdvAnalyzeCells(timeGridAdv2Datas);


            MessageBox.Show(
                $"共加载 {gridAdvAnalyzeCells.Count()}幅数据 " + System.Environment.NewLine
                + $"速度从 {gridAdvAnalyzeCells.First().speed:F1}pps -> {gridAdvAnalyzeCells.Last().speed:F1}pps"
                );
            return;
        }

        //void createRollerView()
        //{
        //    int roller_begin = 20;
        //    int roller_end = 60;
        //    Roller = new int[Target.Count()];



        //    //把 Target 的 测到不锈钢部分删除， 剩下求平均值
        //    int sum =0;
        //    int cnt=0;
        //    for (int i = 0; i < Target.Count(); i++) 
        //    {
        //        if (i >= roller_begin && i <= roller_end)
        //            continue;
        //        if (Misc.MyBase.ISVALIDATA(Target[i]))
        //        {
        //            sum += Target[i];
        //            cnt++;
        //        }
        //    }

        //    airAd = sum / cnt;
        //    int sensorGridWidth = (int)(25 / mmpp / flyad.PosOfGrid);
        //    int rollerGridWidth = (int)(rollerWidth / mmpp / flyad.PosOfGrid);

        //    //通过相关性，找到中心位置
        //    int rollerGridPos = 40;

        //    var roller = new int[rollerGridWidth + sensorGridWidth * 4];
        //    int idx = 0;
        //    for (int i = 0; i < sensorGridWidth*2; i++)
        //    {
        //        roller[idx] = airAd;
        //        idx++;
        //    }
        //    for (int i = 0; i < rollerGridWidth; i++)
        //    {
        //        roller[idx] = airAd;
        //        idx++;
        //    }
        //    for (int i = 0; i < sensorGridWidth * 2; i++)
        //    {
        //        roller[idx] = airAd;
        //        idx++;
        //    }

        //    //找到最大相关性对应的位置
        //    CurrR = Misc.MyMath.Correl(Target, ad_x);
        //}
        async void toGridAdvAnalyzeCells(List<TimeGridAdv2EventArgs> timeGridAdv2Datas)
        {
            gridAdvAnalyzeCells.Clear();
            foreach (var gridAdvUnit in timeGridAdv2Datas)
            {
                var cell = new GridAdvAnalyzeCell()
                {
                    units = gridAdvUnit
                };
                gridAdvAnalyzeCells.Add(cell);
            }

            //计算速度
            foreach (var cell in gridAdvAnalyzeCells)
            {
                //计算中间1/3数据的平均速度
                int cnt = cell.units.AdList.Count();
                int range = cnt / 20;
                int idx_last = (cnt + range) / 2;
                int idx_first = (cnt - range) / 2;
                int delta_pos = cell.units.PosList[idx_last] - cell.units.PosList[idx_first];
                double delta_sec = (idx_last - idx_first) * 1.28 / 1000;
                cell.speed = delta_pos / delta_sec;
            }

            //画出速度
            //await Draw2(gridAdvAnalyzeCells.Select(c => Math.Abs(c.speed)).ToArray());

            //把相同速度的，放在一起
            double speed_diff = 25;
            groups.Clear();
            for (int i = 0; i < gridAdvAnalyzeCells.Count(); i++)
            {
                var cell = gridAdvAnalyzeCells[i];
                GridAdvAnalyzeCellGroup group;
                if (groups.Count() == 0)
                {
                    group = new GridAdvAnalyzeCellGroup() { speed = Math.Abs(cell.speed) };
                    groups.Add(group);
                    if (cell.speed > 0)
                    {
                        group.fCellIdxs.Add(i);
                    }
                    else
                    {
                        group.bCellIdxs.Add(i);
                    }
                    group.range.Begin = i;
                    group.range.End = i;
                }
                else
                {
                    group = groups.Last();
                    if (Math.Abs(group.speed - Math.Abs(cell.speed)) < speed_diff)
                    {
                        //同一个
                        if (cell.speed > 0)
                        {
                            group.fCellIdxs.Add(i);
                        }
                        else
                        {
                            group.bCellIdxs.Add(i);
                        }
                        group.range.End = i;
                    }
                    else
                    {
                        group = new GridAdvAnalyzeCellGroup() { speed = Math.Abs(cell.speed) };
                        groups.Add(group);
                        if (cell.speed > 0)
                        {
                            group.fCellIdxs.Add(i);
                        }
                        else
                        {
                            group.bCellIdxs.Add(i);
                        }
                        group.range.Begin = i;
                        group.range.End = i;
                    }
                }
            }
            GetTarget();
            CellCnt = gridAdvAnalyzeCells.Count();
            SampleCellIdx = CellCnt * 3 / 4;
            ManualCal();
        }
        int GetGroupIdxFromCellIdx(int cellidx)
        {
            return groups.FindIndex(group => group.range.Contain(cellidx));
        }

        int[] CalGroupData(int groupIdx)
        {
            int[] ad_sum = new int[1000];
            int[] ad_cnt = new int[1000];
            foreach (var cal_cell_idx in groups[groupIdx].fCellIdxs)
            {
                Filter2(gridAdvAnalyzeCells[cal_cell_idx].units, out var Xs, Ks);
                
                var ad_x = TimeGridAdvHelperExt.ToGrid(Xs.AdList, Xs.PosList, flyad.PosOfGrid, flyad.GridLen);
                for (int i = 0; i < 1000; i++)
                {
                    if (Misc.MyBase.ISVALIDATA(ad_x[i]))
                    {
                        ad_sum[i] += ad_x[i];
                        ad_cnt[i]++;
                    }
;
                }
            }
            foreach (var cal_cell_idx in groups[groupIdx].bCellIdxs)
            {
                Filter2(gridAdvAnalyzeCells[cal_cell_idx].units, out var Xs, Ks);
                var ad_x = TimeGridAdvHelperExt.ToGrid(Xs.AdList, Xs.PosList, flyad.PosOfGrid, flyad.GridLen);

                for (int i = 0; i < 1000; i++)
                {
                    if (Misc.MyBase.ISVALIDATA(ad_x[i]))
                    {
                        ad_sum[i] += ad_x[i];
                        ad_cnt[i]++;
                    }
                }
            }

            for (int i = 0; i < 1000; i++)
            {
                if (ad_cnt[i] > 0)
                {
                    ad_sum[i] /= ad_cnt[i];
                }
                else
                {
                    ad_sum[i] = Misc.MyBase.NULL_VALUE;
                }
            }
            return ad_sum;
            //await Chart1DrawData(3, ad_sum);
        }

        int[] CalGroupDataAvg(int groupIdx)
        {
            int[] ad_sum = new int[1000];
            int[] ad_cnt = new int[1000];
            foreach (var cal_cell_idx in groups[groupIdx].fCellIdxs)
            {
                var Xs = gridAdvAnalyzeCells[cal_cell_idx].units;
                var ad_x = TimeGridAdvHelperExt.ToGrid(Xs.AdList, Xs.PosList, flyad.PosOfGrid, flyad.GridLen);
                for (int i = 0; i < 1000; i++)
                {
                    if (Misc.MyBase.ISVALIDATA(ad_x[i]))
                    {
                        ad_sum[i] += ad_x[i];
                        ad_cnt[i]++;
                    }
;
                }
            }
            foreach (var cal_cell_idx in groups[groupIdx].bCellIdxs)
            {
                Filter2(gridAdvAnalyzeCells[cal_cell_idx].units, out var Xs, Ks);
                
                var ad_x = TimeGridAdvHelperExt.ToGrid(Xs.AdList, Xs.PosList, flyad.PosOfGrid, flyad.GridLen);

                for (int i = 0; i < 1000; i++)
                {
                    if (Misc.MyBase.ISVALIDATA(ad_x[i]))
                    {
                        ad_sum[i] += ad_x[i];
                        ad_cnt[i]++;
                    }
                }
            }

            for (int i = 0; i < 1000; i++)
            {
                if (ad_cnt[i] > 0)
                {
                    ad_sum[i] /= ad_cnt[i];
                }
                else
                {
                    ad_sum[i] = Misc.MyBase.NULL_VALUE;
                }
            }
            return ad_sum;
            //await Chart1DrawData(3, ad_sum);
        }


        async void DrawClear()
        {
            await Task.Yield();
            //画图
            App.Current.Dispatcher.Invoke(() =>
            {
                System.Windows.Forms.DataVisualization.Charting.Series series = chart.Series[0];
                series.Points.Clear();
                //chart.ChartAreas[0].AxisX.Minimum = double.NaN;
                //chart.ChartAreas[0].AxisX.Maximum = double.NaN;
            });
        }
        async Task DrawR(int index, double r)
        {
            //画图
            await Task.Yield();
            await Task.Factory.StartNew(() =>
            {
                App.Current.Dispatcher.Invoke(() =>
                {
                    System.Windows.Forms.DataVisualization.Charting.Series series = chart.Series[index];
                    series.Points.AddY(r * 10000);
                    double max = series.Points.Max(p => p.YValues[0]);
                    double min = series.Points.Min(p => p.YValues[0]);
                    if (max != min)
                    {
                        double range = (max - min) / 10;
                        chart.ChartAreas[0].AxisY.Minimum = min - range;
                        chart.ChartAreas[0].AxisY.Maximum = max + range;
                    }
                });
            });
        }

        async Task Draw2(double[] data)
        {
            var chart = chart2;
            //画图
            await Task.Yield();
            await Task.Factory.StartNew(() =>
            {
                App.Current.Dispatcher.Invoke(() =>
                {
                    System.Windows.Forms.DataVisualization.Charting.Series series = chart.Series[0];

                    series.Points.Clear();
                    for (int i = 0; i < data.Count(); i++)
                    {
                        series.Points.AddXY(i, data[i]);
                    }

                    double max = series.Points.Max(p => p.YValues[0]);
                    double min = series.Points.Min(p => p.YValues[0]);
                    if (max != min)
                    {
                        double range = (max - min) / 10;
                        chart.ChartAreas[0].AxisY.Minimum = min - range;
                        chart.ChartAreas[0].AxisY.Maximum = max + range;
                    }
                });
            });
        }


        async Task Chart1DrawData(int index, int[] data)
        {
            var chart = chart1;
            await Task.Yield();
            //画图
            await Task.Factory.StartNew(() =>
            {
                App.Current.Dispatcher.Invoke(() =>
                {
                    System.Windows.Forms.DataVisualization.Charting.Series series = chart.Series[index];
                    series.Points.Clear();
                    for (int i = 0; i < data.Count(); i++)
                    {
                        if (Misc.MyBase.ISVALIDATA(data[i]))
                            series.Points.AddXY(i, data[i]);
                    }
                    //if (series.Points.Count() > 0)
                    //{
                    //    double max = series.Points.Max(p => p.YValues[0]);
                    //    double min = series.Points.Min(p => p.YValues[0]);
                    //    if (max != min)
                    //    {
                    //        double range = (max - min) / 10;
                    //        chart.ChartAreas[0].AxisY.Minimum = min - range;
                    //        chart.ChartAreas[0].AxisY.Maximum = max + range;
                    //    }
                    //}
                });
            });
        }

        async Task Chart2DrawData(int index, int[] data)
        {
            var chart = chart2;
            await Task.Yield();
            //画图
            await Task.Factory.StartNew(() =>
            {
                App.Current.Dispatcher.Invoke(() =>
                {
                    System.Windows.Forms.DataVisualization.Charting.Series series = chart.Series[index];
                    series.Points.Clear();
                    for (int i = 0; i < data.Count(); i++)
                    {
                        if (Misc.MyBase.ISVALIDATA(data[i]))
                            series.Points.AddXY(i, data[i]);
                    }
                    //if (series.Points.Count() > 0)
                    //{
                    //    double max = series.Points.Max(p => p.YValues[0]);
                    //    double min = series.Points.Min(p => p.YValues[0]);
                    //    if (max != min)
                    //    {
                    //        double range = (max - min) / 10;
                    //        chart.ChartAreas[0].AxisY.Minimum = min - range;
                    //        chart.ChartAreas[0].AxisY.Maximum = max + range;
                    //    }
                    //}
                });
            });
        }

        async void GetTarget()
        {

            int[] sum = new int[1000];
            int[] cnt = new int[1000];
            int[] targets = new int[1000];

            for (int i = 0; i < 4; i++)
            {
                
                var Xs = gridAdvAnalyzeCells[i].units;
                var datas = TimeGridAdvHelperExt.ToGrid(Xs.AdList, Xs.PosList, flyad.PosOfGrid, flyad.GridLen);

                for (int j = 0; j < 1000; j++)
                {
                    if (Misc.MyBase.ISVALIDATA(datas[j]))
                    {
                        sum[j] += datas[j];
                        cnt[j]++;
                    }
                }
            }

            for (int j = 0; j < 1000; j++)
            {
                if (cnt[j] > 0)
                {
                    targets[j] = sum[j] / cnt[j];
                }
                else
                {
                    targets[j] = Misc.MyBase.NULL_VALUE;
                }
            }
            Target = targets;

            await Chart1DrawData(0, Target);
            await Chart2DrawData(0, Target);
        }
        async void ManualCal()
        {
            if (SampleCellIdx < 0)
                SampleCellIdx = 0;
            else if (SampleCellIdx >= CellCnt)
                SampleCellIdx = CellCnt - 1;

            int cal_cell_idx = SampleCellIdx;
            Speed = gridAdvAnalyzeCells[cal_cell_idx].speed;
            //1. 找中间的图

            var Xs = gridAdvAnalyzeCells[cal_cell_idx].units;
            var orgs0 = TimeGridAdvHelperExt.ToGrid(Xs.AdList, Xs.PosList, flyad.PosOfGrid, flyad.GridLen);


            await Chart1DrawData(1, orgs0);
            await Chart2DrawData(1, orgs0);
            //2. 算出相关性
            R = Misc.MyMath.Correl(Target, orgs0);


            if (Ks.Sum() != 1)
            {
                Ks[4] = 1 - (Ks.Sum() - Ks[4]);
            }
            Filter2(gridAdvAnalyzeCells[cal_cell_idx].units, out Xs, Ks);
            var ad_x = TimeGridAdvHelperExt.ToGrid(Xs.AdList, Xs.PosList, flyad.PosOfGrid, flyad.GridLen);
            CurrR = Misc.MyMath.Correl(Target, ad_x);

            await DrawR(0, CurrR);
            await Chart1DrawData(2, ad_x);
            int groupIdx = GetGroupIdxFromCellIdx(SampleCellIdx);

            int[] group_ad = CalGroupDataAvg(groupIdx);
            await Chart2DrawData(2, group_ad);


            group_ad = CalGroupData(groupIdx);
            CurrGroupR = Misc.MyMath.Correl(Target, group_ad);
            await Chart1DrawData(3, group_ad);
            await Chart2DrawData(3, group_ad);
            //await Draw2(gridAdvAnalyzeCells.Select(c => Math.Abs(c.speed)).ToArray());


        }

        private async void AutoCal()
        {
            if (SampleCellIdx < 4)
                SampleCellIdx = 4;
            else if (SampleCellIdx >= CellCnt)
                SampleCellIdx = CellCnt - 1;

            int cal_cell_idx = SampleCellIdx;

            //1. 找中间的图
            var Xs = gridAdvAnalyzeCells[cal_cell_idx].units;
            var orgs0 = TimeGridAdvHelperExt.ToGrid(Xs.AdList, Xs.PosList, flyad.PosOfGrid, flyad.GridLen);
            await Chart1DrawData(1, orgs0);

            //2. 算出相关性
            R = Misc.MyMath.Correl(Target, orgs0);

            DrawClear();
            Ks[0] = 1;
            for (int i = 0; i < 4; i++)
            {
                await AutoSearchKn(cal_cell_idx, 0, Ks[i], 0.1, 0.000001, Ks.Take(i).ToArray());
            }

            int groupIdx = GetGroupIdxFromCellIdx(SampleCellIdx);
            int[] group_ad = CalGroupData(groupIdx);
            CurrGroupR = Misc.MyMath.Correl(Target, group_ad);
            await Chart1DrawData(3, group_ad);

            //await AutoSearchKn(cal_cell_idx, 0, 1, 0.1, 0.000001);

            //await AutoSearchKn(cal_cell_idx, 0, Ks[1], 0.1, 0.000001, Ks.Take(1).ToArray());

            //await AutoSearchKn(cal_cell_idx, 0, Ks[2], 0.1, 0.000001, Ks.Take(2).ToArray());

            //await AutoSearchKn(cal_cell_idx, 0, Ks[3], 0.1, 0.000001, Ks.Take(3).ToArray());
        }

        private async void AutoCal2()
        {
            
            if (SampleCellIdx < 4)
                SampleCellIdx = 4;
            else if (SampleCellIdx >= CellCnt)
                SampleCellIdx = CellCnt - 1;

            int cal_cell_idx = SampleCellIdx;

            //1. 找中间的图
            var Xs = gridAdvAnalyzeCells[cal_cell_idx].units;
            var orgs0 = TimeGridAdvHelperExt.ToGrid(Xs.AdList, Xs.PosList, flyad.PosOfGrid, flyad.GridLen);
            await Chart1DrawData(1, orgs0);

            //2. 算出相关性
            R = Misc.MyMath.Correl(Target, orgs0);

            DrawClear();
            await AutoSearchKn2(cal_cell_idx, 0.000001, Ks.ToArray());

        }
        async Task<double> AutoSearchKn(int cal_cell_idx, double min, double max, double step, double min_step, params double[] kn)
        {
            double[] Ks = new double[5];
            int cnt = kn.Count();
            if (cnt > 3)
            {
                throw new Exception("AutoSearchKn 输出 参数太多");
            }
            double k_sum = 0;
            for (int i = 0; i < cnt; i++)
            {
                Ks[i] = kn[i];
                k_sum += kn[i];
            }

            double max_r = -1;
            double best_k = -1;
            int[] ad_x = null;
            for (double k = max; k >= min; k -= step)
            {
                Ks[cnt] = k;
                Ks[cnt + 1] = 1 - k_sum - Ks[cnt];

                Filter2(gridAdvAnalyzeCells[cal_cell_idx].units, out var Xs, Ks[0], Ks[1], Ks[2], Ks[3], Ks[4]);
                
                ad_x = TimeGridAdvHelperExt.ToGrid(Xs.AdList, Xs.PosList, flyad.PosOfGrid, flyad.GridLen);
                double r = Misc.MyMath.Correl(Target, ad_x);
                if (r > max_r)
                {
                    max_r = r;
                    CurrR = max_r;
                    best_k = k;
                    for (int i = 0; i < Ks.Count(); i++)
                    {
                        this.Ks[i] = Ks[i];
                    }
                }
            }
            if (ad_x != null)
            {
                await DrawR(0, max_r);
                await Chart1DrawData(2, ad_x);
            }
            if (step <= min_step)
                return best_k;

            step = step / 10;
            double next_min = best_k - step * 2;
            double next_max = best_k + step * 2;
            if (next_max <= min)
                return best_k;
            if (next_min >= max)
                return best_k;
            if (next_min < min)
                next_min = min;
            if (next_max > max)
                next_max = max;
            return await AutoSearchKn(cal_cell_idx, next_min, next_max, step / 10, min_step, kn);
        }


        async Task AutoSearchKn2(int cal_cell_idx, double step, double[] Ks)
        {
            if (Ks.Count() != 5)
            {
                throw new Exception("AutoSearchKn2 初始值数量不为5");
            }
            int step_i = 0;
            int[] ad_x;
            while (true)
            {
                Filter2Avg(gridAdvAnalyzeCells[cal_cell_idx].units, out var Xs0, Ks);
                ad_x = TimeGridAdvHelperExt.ToGrid(Xs0.AdList, Xs0.PosList, flyad.PosOfGrid, flyad.GridLen);
                double r0 = Misc.MyMath.Correl(Target, ad_x);

                bool hasChanged = false;
                double[] ks = Ks.Clone() as double[];
                double r = r0;
                await Task.Factory.StartNew(() =>
                {
                    //每个参数改变 step, 确定方向
                    for (int i = 0; i < Ks.Count(); i++)
                    {
                        double curr_step = step;
                        double r1;
                        double r2;
                        int[] ad_x1;
                        int[] ad_x2;
                        while (true)
                        {
                            ks[i] = Ks[i] + curr_step;
                            Filter2Avg(gridAdvAnalyzeCells[cal_cell_idx].units, out var Xs1, ks);
                            ad_x1 = TimeGridAdvHelperExt.ToGrid(Xs1.AdList, Xs1.PosList, flyad.PosOfGrid, flyad.GridLen);
                            
                            r1 = Misc.MyMath.Correl(Target, ad_x1);


                            ks[i] = Ks[i] - curr_step;
                            Filter2Avg(gridAdvAnalyzeCells[cal_cell_idx].units, out Xs1, ks);
                            
                            ad_x2 = TimeGridAdvHelperExt.ToGrid(Xs1.AdList, Xs1.PosList, flyad.PosOfGrid, flyad.GridLen);
                            r2 = Misc.MyMath.Correl(Target, ad_x2);

                            if ((r1 == r0) || (r2 == r0))
                            {
                                //居然没有变化，把step翻倍
                                curr_step *= 1.5;
                            }
                            else
                            {
                                //有变化了
                                break;
                            }
                        }

                        if (r1 > r0)
                        {
                            //方向正确
                            ks[i] = Ks[i] + curr_step;
                            hasChanged = true;
                            r = r1;
                            ad_x = ad_x1;
                            continue;
                        }
                        else if (r2 > r0)
                        {
                            //方向正确
                            ks[i] = Ks[i] - curr_step;
                            hasChanged = true;
                            r = r2;
                            ad_x = ad_x2;
                            continue;
                        }
                        else
                        {
                            //方向错误，回到以前
                            ks[i] = Ks[i];
                        }
                    }
                });

                step_i++;
                CurrR = r;
                //if(step_i%10==0)
                await DrawR(0, r);

                ks.CopyTo(Ks, 0);
                //K1 = Ks[0];
                //K2 = Ks[1];
                //K3 = Ks[2];
                //K4 = Ks[3];
                //K5 = Ks[4];

                Kp2Ks(ks, out double[] kkkkk);
                for (int i = 0; i < kkkkk.Count(); i++)
                {
                    this.Ks[i] = kkkkk[i];
                };
                if (!hasChanged)
                {
                    //已经计算完毕
                    break;
                }
            }
            if (ad_x != null)
            {
                await Chart1DrawData(2, ad_x);
            }
        }



        /// <summary>
        /// 一阶滤波 Y(n)=k1*X(n) + k2*Y(n-1) 
        /// </summary>
        /// <param name="y"></param>
        /// <param name="x"></param>
        /// <param name=""></param>
        void Filter1(TimeGridAdv2EventArgs Ys, out TimeGridAdv2EventArgs Xs, double k1)
        {
            double k2 = 1 - k1;
            Xs = new TimeGridAdv2EventArgs()
            {
                AdList = new List<int>(),
                PosList = Ys.PosList.ToList(),
                EndTime = Ys.EndTime,
                Direction = Ys.Direction,
                Marker = Ys.Marker
            };
            for (int i = 0; i < Ys.AdList.Count(); i++)
            {
                double y = Ys.AdList[i - 0];
                double y_1 = i >= 1 ? Ys.AdList[i - 1] : y;
                double x = (y - k2 * y_1) / k1;

                Xs.AdList.Add((int)x);
            }
        }

        /// <summary>
        /// 二阶滤波 Y(n)=k1*X(n) + k2*Y(n-1) + k3*X(n-1)+ k4*Y(n-2) + k5*X(n-2)
        /// </summary>
        /// <param name="y"></param>
        /// <param name="x"></param>
        /// <param name=""></param>
        void Filter2(TimeGridAdv2EventArgs Ys, out TimeGridAdv2EventArgs Xs, double k1, double k2, double k3, double k4, double k5)
        {
            Xs = new TimeGridAdv2EventArgs()
            {
                AdList = new List<int>(),
                PosList = Ys.PosList.ToList(),
                EndTime = Ys.EndTime,
                Direction = Ys.Direction,
                Marker = Ys.Marker
            };
            for (int i = 0; i < Ys.AdList.Count(); i++)
            {
                double y = Ys.AdList[i - 0];
                double y_1 = i >= 1 ? Ys.AdList[i - 1] : y;
                double y_2 = i >= 2 ? Ys.AdList[i - 2] : y_1;

                double x_1 = i >= 1 ? Xs.AdList[i - 1] : y_1;
                double x_2 = i >= 2 ? Xs.AdList[i - 2] : x_1;

                double x = (y - (k2 * y_1 + k3 * x_1 + k4 * y_2 + k5 * x_2)) / k1;
                Xs.AdList.Add((int)x);
            }
        }

        /// <summary>
        /// 二阶滤波 Y(n)=k1*X(n) + k2*Y(n-1) + k3*X(n-1)+ k4*Y(n-2) + k5*X(n-2)
        /// </summary>
        /// <param name="y"></param>
        /// <param name="x"></param>
        /// <param name=""></param>
        void Filter2(TimeGridAdv2EventArgs Ys, out TimeGridAdv2EventArgs Xs, IEnumerable<double> ks)
        {
            Xs = new TimeGridAdv2EventArgs()
            {
                AdList = new List<int>(),
                PosList = Ys.PosList.ToList(),
                EndTime = Ys.EndTime,
                Direction = Ys.Direction,
                Marker = Ys.Marker
            };
            for (int i = 0; i < Ys.AdList.Count(); i++)
            {
                double y = Ys.AdList[i - 0];
                double y_1 = i >= 1 ? Ys.AdList[i - 1] : y;
                double y_2 = i >= 2 ? Ys.AdList[i - 2] : y_1;

                double x_1 = i >= 1 ? Xs.AdList[i - 1] : y_1;
                double x_2 = i >= 2 ? Xs.AdList[i - 2] : x_1;

                double x = (y - (ks.ElementAt(1) * y_1 + ks.ElementAt(2) * x_1 + ks.ElementAt(3) * y_2 + ks.ElementAt(4) * x_2)) / ks.ElementAt(0);

                Xs.AdList.Add((int)x);
            }
        }

        void Kp2Ks(double[] kp, out double[] Ks)
        {
            Ks = new double[5];

            if (kp.Count() != 5)
                throw new Exception("Filter2Avg kp.Count()!=5");
            if (kp.All(p => p == 0))
            {
                for (int i = 0; i < 5; i++)
                    Ks[i] = 1.0 / 5;
            }
            else
            {
                for (int i = 0; i < 5; i++)
                    Ks[i] = kp[i] / kp.Sum();
            }
        }
        /// <summary>
        /// 二阶滤波 Y(n)=k1*X(n) + k2*Y(n-1) + k3*X(n-1)+ k4*Y(n-2) + k5*X(n-2)
        /// </summary>
        /// <param name="y"></param>
        /// <param name="x"></param>
        /// <param name=""></param>
        void Filter2Avg(TimeGridAdv2EventArgs Ys, out TimeGridAdv2EventArgs Xs, double[] kp)
        {
            Kp2Ks(kp, out double[] Ks);
            Xs = new TimeGridAdv2EventArgs()
            {
                AdList = new List<int>(),
                PosList = Ys.PosList.ToList(),
                EndTime = Ys.EndTime,
                Direction = Ys.Direction,
                Marker = Ys.Marker
            };
            for (int i = 0; i < Ys.AdList.Count(); i++)
            {
                double y = Ys.AdList[i - 0];
                double y_1 = i >= 1 ? Ys.AdList[i - 1] : y;
                double y_2 = i >= 2 ? Ys.AdList[i - 2] : y_1;

                double x_1 = i >= 1 ? Xs.AdList[i - 1] : y_1;
                double x_2 = i >= 2 ? Xs.AdList[i - 2] : x_1;

                double x = (y - (Ks[1] * y_1 + Ks[2] * x_1 + Ks[3] * y_2 + Ks[4] * x_2)) / Ks[0];
                Xs.AdList.Add((int)x);
            }
        }



    }
    public class GridAdvAnalyzeCell
    {
        public TimeGridAdv2EventArgs units;
        public double speed;// 平均速度


    }
    public class GridAdvAnalyzeCellGroup
    {
        public List<int> fCellIdxs = new List<int>();
        public List<int> bCellIdxs = new List<int>();
        public double speed = 0;
        public Misc.Range range = new Misc.Range();
    }
}