using FObjBase;
using Misc;
using NLog.Fluent;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Reflection;
using System.Runtime.Remoting.Channels;

namespace FlyADBase
{
    public partial class FlyAd2021B2 : IFlyADClientAdv
    {
        #region IFlyADClientAdv property
        /// <summary>
        /// grid数据平滑
        /// </summary>
        public int GridSmooth { get; set; }

        /// <summary>
        /// 机架总长
        /// </summary>
        public int PosLen { get; set; } = 8900;

        /// <summary>
        /// 使用独立的脉冲最大最小值
        /// </summary>
        public bool HasPosMaxMin { get; set; }

        /// <summary>
        /// 最小脉冲，默认是0
        /// </summary>
        public int PosMin { get; set; } = 0;

        /// <summary>
        /// 最大脉冲，默认于PosLen 一样
        /// </summary>
        public int PosMax { get; set; } = 8900;

        public int GridLen => PosLen / PosOfGrid;
        /// <summary>
        /// Speed1 = Velocity * Speed1Scale
        /// </summary>
        public double Speed1Scale => (double)Ratio02 / Ratio01;

        /// <summary>
        /// 动作完成
        /// </summary>
        public bool IsFinish { get; set; }

        /// <summary>
        /// ad滞后修正  单位ms
        /// </summary>
        public int ADLag { get; set; }

        /// <summary>
        /// ad2滞后修正  单位ms
        /// </summary>
        public int AD2Lag { get; set; }

        //一共有1.AD数据池（由timegrid提供） 1min
        //2.pos数据池（pos推送提供） 1min
        //4.当接收的grid事件数据。它有 （direction, grid_start，grid_len, systick )
        //systick 就是结束的时间点。 当AD数据池出现了这个时间点

        //pos数据池向前找。 pos 在 grid_start*posOfGrid 范围的数据。 
        //找到开始的systick 后，整合3个数据池的数据。
        //5.最后代替 grid 推送出去。
        public TimeGridAdvHelper mTimeGridAdvHelper { get; } = new TimeGridAdvHelper();

        /// <summary>
        /// 动作指令完成，准备推送 timegridadv 事件
        /// </summary>
        public bool IsTimeToPushTimeGridAdv { get; private set; } = false;

        /// <summary>
        /// 正向 Grid数组 更新时间 
        /// </summary>
        public DateTime GridFrameUpdateTime_Forw { get; private set; }

        /// <summary>
        /// 反向 Grid数组 更新时间 
        /// </summary>
        public DateTime GridFrameUpdateTime_Backw { get; private set; }

        /// <summary>
        /// 以timegrid 为单位，推送数据
        /// </summary>
        public event TimeGridAdv2EventHandler TimeGridAdv2Event;
        #endregion

        SGrid sGrid = new SGrid();

        /// <summary>
        /// 上一次更新AD，及推送timegrid的时间
        /// </summary>
        DateTime lastUpdateADTime = DateTime.MinValue;

        /// <summary>
        /// 上一次检查 更新grid数据 的时刻
        /// </summary>
        DateTime lastCheckUpdateGridTime = DateTime.MinValue;

        /// <summary>
        /// 上一次更新grid数组的最后时刻
        /// </summary>
        DateTime lastUpdateGridEndTime = DateTime.MinValue;

        /// <summary>
        /// 上一次更新grid数组的grid序号
        /// </summary>
        int lastUpdateGrid = -1;

        /// <summary>
        /// 上一次更新grid数组的方向
        /// </summary>
        DIRECTION lastUpdateGridDirection = DIRECTION.FIX;

        /// <summary>
        /// 上一次更新grid的序号
        /// </summary>
        int? lastPushMiniGrid = null;

        /// <summary>
        /// 上一次推送minigrid的方向
        /// </summary>
        DIRECTION lastPushMiniGridDirection = DIRECTION.FIX;

        /// <summary>
        /// 推送grid 的计时器，限制最小推送周期
        /// </summary>
        Stopwatch stopwatch_pushMiniGridTime = new Stopwatch();

        /// <summary>
        /// 记录下推送的时间
        /// </summary>
        DateTime lastPushTimeGridAdvTime = DateTime.MinValue;


        void advConstructor()
        {
            mTimeGridAdvHelper.Init();

            this.PropertyChanged += FlyAD7_PropertyChanged1;

            sGrid.SetSize(GridLen);

            //轮询事件触发
            PollModule.Current.Poll_Config(PollModule.POLL_CONFIG.ADD, OnPoll);
        }
        private void FlyAD7_PropertyChanged1(object sender, System.ComponentModel.PropertyChangedEventArgs e)
        {
            if (e.PropertyName == nameof(DriveStatus))
            {
                switch (DriveStatus)
                {
                    case DRIVE_MAN_STATUS.STOP_MANUAL:
                    case DRIVE_MAN_STATUS.STOP:
                    case DRIVE_MAN_STATUS.LIMIT:
                        IsFinish = true;
                        break;
                    case DRIVE_MAN_STATUS.RUNNING:
                        IsFinish = false;
                        break;
                }
            }
            else if (e.PropertyName == nameof(GridLen)) 
            {
                sGrid.SetSize(GridLen);
            }
        }

        void advAfterContected()
        {
            mTimeGridAdvHelper.Clear();
            lastUpdateADTime = DateTime.MinValue;
            lastCheckUpdateGridTime = DateTime.MinValue;
            lastUpdateGridEndTime = DateTime.MinValue;
            lastUpdateGrid = -1;
            lastUpdateGridDirection = DIRECTION.FIX;
            
            lastPushMiniGrid = null;
            lastPushMiniGridDirection = DIRECTION.FIX;
            stopwatch_pushMiniGridTime.Reset();

            lastPushTimeGridAdvTime = DateTime.MinValue;
        }

        #region IFlyADClientAdv function
        /// <summary>
        /// Runto(0)， 不同于 Backward
        /// </summary>
        public void RuntoMin()
        {
            if (HasPosMaxMin)
                Runto(PosMin);
            else
                Runto(0);
        }

        /// <summary>
        /// Runto(PosLen)， 不同于 Forward
        /// </summary>
        public void RuntoMax()
        {
            if (HasPosMaxMin)
                Runto(PosMax);
            else
                Runto(PosLen);
        }

        /// <summary>
        /// 设置输出
        /// </summary>
        /// <param name="index"></param>
        /// <param name="is1"></param>
        public void SetOutputBit(int index, bool is1)
        {
            if (index > 15)//4)
                return;
            if (index < 0)
                return;
            if (is1)
            {
                SetOutput(
                    (UInt16)Misc.MyBase.BIT(index),
                    (UInt16)Misc.MyBase.BIT(index));
            }
            else
            {
                SetOutput(
                    (UInt16)Misc.MyBase.BIT(index),
                    (UInt16)~Misc.MyBase.BIT(index));
            }
        }

        #endregion

        
        void advPushData(DateTime dt, int ad, int ad2)
        {
            var ret = mTimeGridAdvHelper.AddData(dt, ad, ad2, Position, Position2, IStatus);
            if (ret.isChanged)
            {
                if (ret.isIstatusChanged) {
                    IStatusChangedEvent?.Invoke(this,
                        new IStatusChangedEventArgs(Now, IStatus, ret.ichanged, Position, Position2));
                }
                if (ret.isPosChanged)
                {
                    PositionChangedEvent?.Invoke(this, new PositionChangedEventArgs() { Time = Now, Position = Position });
                }
                if (ret.isPos2Changed) 
                {
                    Position2ChangedEvent?.Invoke(this, new PositionChangedEventArgs() { Time = Now, Position = Position2 });
                }
            }
        }
        
        void advPushStatus()
        {
            if (mTimeGridAdvHelper.AddDriveStatus(Now, DriveOrder, DriveStatus))
            {

                //状态变了
                if (DriveStatus != DRIVE_MAN_STATUS.RUNNING)
                {
                    //通知 mTimeGridAdvHelper 下次触发 timegridadv
                    IsTimeToPushTimeGridAdv = true;
                }

                DriveStatusChangedEvent?.Invoke(this, new DriveStatusChangedEventArgs()
                {
                    Time = Now,
                    DriveOrder = DriveOrder,
                    DriveStatus = DriveStatus
                });
            }
        }

        /// <summary>
        /// <para>ScanMotion 使用。</para> 
        /// <para>给定按扫描的时间点。当探头停下来后，获取数据。</para> 
        /// <para>数据带滞后修正,带运动方向。</para> 
        /// </summary>
        /// <param name="beginTime"></param>
        /// <returns></returns>
        public TimeGridAdv2EventArgs GetTimeGridAdv2Event(DateTime beginTime)
        {
            if (mTimeGridAdvHelper.DataPool.Count() == 0)
                return null;

            //获取ad列表
            var dataList = mTimeGridAdvHelper.GetData(beginTime);
            if (dataList.Count() == 0)
                return null;
            return GetTimeGridAdv2Event(dataList);

        }
        TimeGridAdv2EventArgs GetTimeGridAdv2Event(List<DateTimeUnit5> dataList) 
        {
            DateTime endTime = mTimeGridAdvHelper.NewestTime;
            Misc.DIRECTION direction;

            //判断运动方向
            if (dataList.Last().pos > dataList.First().pos)
            {
                //正向
                direction = DIRECTION.FORWARD;
            }
            else
            {
                direction = DIRECTION.BACKWARD;
            }

            //数据滞后
            int lag = Math.Max(Math.Abs(ADLag), Math.Abs(AD2Lag));

            if (dataList.Count() < lag * 5)
            {
                //数据量太少,无法滞后修正

            }
            else
            {
                TimeGridAdvHelperExt.AdLagCorr(dataList, ref endTime, ADLag, AD2Lag);
            }


            TimeGridAdv2EventArgs eventArgs = new TimeGridAdv2EventArgs();
            eventArgs.Direction = direction;
            eventArgs.EndTime = endTime;
            eventArgs.DataList = dataList;
            return eventArgs;
        }
        public TimeGridAdv2EventArgs GetTimeGridAdv2Event(int dataCnt) 
        {
            if (mTimeGridAdvHelper.DataPool.Count() == 0)
                return null;
            int adLag = Math.Max(Math.Abs(ADLag), Math.Abs(AD2Lag));


            //获取ad列表
            List<DateTimeUnit5> dataList;
            int skipCnt = mTimeGridAdvHelper.DataPool.Count() - dataCnt - adLag;
            if (skipCnt < 0)
            {
                dataList = mTimeGridAdvHelper.DataPool.ToList();
            }
            else 
            {
                dataList = mTimeGridAdvHelper.DataPool.Skip(skipCnt).ToList();
            }
            return GetTimeGridAdv2Event(dataList);
        }
        void OnPoll()
        {
            if (!IsConnected)
                return;
            if (!IsReady)
                return;

            OnPoll_grid();
            OnPoll_TimeGrid();
            OnPoll_TimeGridAdv();
            OnPoll_miniGrid();
        }

        //没有滞后处理，没有机架修正
        void OnPoll_TimeGrid()
        {
            if (lastUpdateADTime == DateTime.MinValue)
            {
                lastUpdateADTime = Now;
                return;
            }


            if (Now - lastUpdateADTime < TimeSpan.FromSeconds(0.1))
                return;

            //获取0.1s到现在的数据
            var dataList = mTimeGridAdvHelper.GetData(lastUpdateADTime);

            //算AD均值
            int[] ad1s = dataList.Select(d => d.ad).ToArray();
            int[] ad2s = dataList.Select(d => d.ad2).ToArray();

            int ad = (int)ad1s.Average();
            AD = ad;

            int ad2 = (int)ad2s.Average();
            AD2 = ad2;

            //推送timegrid事件
            TimeGridEvent?.Invoke(this,
                new TimeGridEventArgs()
                {
                    Time = lastUpdateADTime,
                    Ad1s = ad1s,
                    Ad2s = ad2s
                });

            lastUpdateADTime = Now;
        }

        int GetDataPoolGrid(int index)
        {
            return (int)mTimeGridAdvHelper.DataPool[index].pos / PosOfGrid;
        }

        /// <summary>
        /// 找到上一个grid的最后时刻
        /// </summary>
        /// <param name="lastValidIndex"></param>
        /// <param name="updateGridBeginIndex"></param>
        /// <param name="updateGridEndIndex"></param>
        /// <returns></returns>
        bool FindOutUpdateGridEndIndex(int lastValidIndex, int updateGridBeginIndex, out int updateGridEndIndex)
        {
            updateGridEndIndex = updateGridBeginIndex;

            //最后 有效数据 grid号
            int lastValidGrid = GetDataPoolGrid(lastValidIndex);

            int firstGrid = GetDataPoolGrid(updateGridBeginIndex);
            if (firstGrid == lastValidGrid)
            {
                //整个队列 都是同一个grid, 不处理
                return false;
            }
            else
            {
                updateGridEndIndex = updateGridBeginIndex;

                //从 有效数据 向前找到 grid号不一样的
                //肯定能找到， 最起码 [0] 是不一样的
                for (int i = lastValidIndex; i >= 0; i--)
                {
                    int grid = GetDataPoolGrid(i);
                    if (grid != lastValidGrid)
                    {
                        //找到了
                        updateGridEndIndex = i;
                        break;
                    }
                }
            }
            return true;
        }

        /// <summary>
        /// 停下来了，且数据已经可以获取了
        /// </summary>
        /// <returns></returns>
        public bool IsTimeToGetGridAfterStop()
        {
            if (mTimeGridAdvHelper.DriveStatusPool.Count() == 0)
                return false;//没有状态
            var cell = mTimeGridAdvHelper.DriveStatusPool.Last();
            if (cell.driveStatus == DRIVE_MAN_STATUS.RUNNING)
                return false;//运行中
            if (lastCheckUpdateGridTime >= cell.dt)
                return true;
            return false;
        }
        void OnPoll_grid()
        {
            bool isStopTime = false;

            //最后 时间点
            DateTime newestTime = mTimeGridAdvHelper.NewestTime;

            if (newestTime == DateTime.MinValue)//还没有 有效数据
                return;
            if (mTimeGridAdvHelper.DataPool.Count() == 0)
                return;
            int lastIndex = mTimeGridAdvHelper.DataPool.Count() - 1;

            //当前 更新grid数组 开始的数据池序号
            int updateGridBeginIndex;

            //当前 更新grid数组 结束的数据池序号
            int updateGridEndIndex = -1;

            //上一次更新grid数组的最后时刻
            if (lastUpdateGridEndTime == DateTime.MinValue)
            {
                //从来没有处理完数据
                updateGridBeginIndex = 0;
            }
            else
            {
                if (lastUpdateGridEndTime >= newestTime)
                {
                    return;//不需要更新
                }

                int lastUpdateGridEndIndex = mTimeGridAdvHelper.Time2Index(lastUpdateGridEndTime);
                updateGridBeginIndex = lastUpdateGridEndIndex + 1;
                if (updateGridBeginIndex < 0)
                {
                    //很久以前了，复位
                    updateGridBeginIndex = 0;
                }

                if (mTimeGridAdvHelper.DriveStatusPool.Count() != 0)
                {
                    var cell = mTimeGridAdvHelper.DriveStatusPool.Last();
                    if (cell.driveStatus != DRIVE_MAN_STATUS.RUNNING)
                    {
                        //当前已经停下来
                        DateTime stopTime = cell.dt;
                        if (stopTime > lastUpdateGridEndTime)
                        {
                            //把最后一个grid也导出
                            if (newestTime >= stopTime)
                            {
                                //当前的有效时间已经大于停止时间
                                updateGridEndIndex = lastIndex;

                                isStopTime = true;
                            }
                        }
                    }
                }
            }
            //记录时间，已经调用过了
            lastCheckUpdateGridTime = newestTime;



            if (updateGridEndIndex == -1)
            {
                //找到上一个grid的最后时刻
                bool ret = FindOutUpdateGridEndIndex(lastIndex, updateGridBeginIndex, out updateGridEndIndex);
                if (!ret)//没有不一样的grid序号
                    return;
            }

            int firstGrid = GetDataPoolGrid(updateGridBeginIndex);

            //最后 有效数据 grid号
            int lastValidGrid = GetDataPoolGrid(lastIndex);

            //方向
            Misc.DIRECTION direction;
            if (firstGrid < lastValidGrid)
                direction = Misc.DIRECTION.FORWARD;
            else
                direction = Misc.DIRECTION.BACKWARD;
            int dir_index = direction == Misc.DIRECTION.FORWARD ? 0 : 1;

            if (lastUpdateGridEndTime != DateTime.MinValue)
            {
                if (lastUpdateGridDirection != direction)
                {
                    //方向变化了，清除当前方向数据
                    sGrid.Clear(dir_index);
                }
            }
            //grid号列表
            List<int> gridList = new List<int>();
            //grid ad列表
            List<int>[] adLists = new List<int>[2] { new List<int>(), new List<int>() };
            DateTime endTime = DateTime.MinValue;
            int lastGrid = -1;
            int grid_begin = GetDataPoolGrid(updateGridBeginIndex);
            int grid_end = GetDataPoolGrid(updateGridEndIndex);
            logger.Debug($"OnPoll_grid() dir_index={dir_index} grid=[{grid_begin}~{grid_end}]");
            for (int i = updateGridBeginIndex; i <= updateGridEndIndex; i++)
            {
                int grid = GetDataPoolGrid(i);
                if (i != updateGridBeginIndex)
                {
                    if (grid != lastGrid)
                    {
                        if (lastGrid >= 0 && lastGrid < GridLen)
                        {

                            sGrid.GetData(0)[dir_index][lastGrid] = (int)adLists[0].Average();
                            sGrid.GetData(1)[dir_index][lastGrid] = (int)adLists[1].Average();
                            sGrid.times[dir_index][lastGrid] = endTime;
                            gridList.Add(lastGrid);
                        }
                        adLists[0].Clear();
                        adLists[1].Clear();
                    }
                }
                var ads = new int?[] {
                    mTimeGridAdvHelper.DataPool[i].ad,
                    mTimeGridAdvHelper.DataPool[i].ad2
                };
                if (ads.Any(a => a == null))
                {
                    //居然没有数据
                    throw new Exception($"DataPool[{i}].ad0 或 ad1 没有AD数据,  grid = {grid}");
                }
                for (int j = 0; j < 2; j++)
                {

                    adLists[j].Add((int)ads[j]);
                    //1个grid的数据不能大于2000ms
                    if (adLists[j].Count() > 2000)
                        adLists[j].RemoveAt(0);
                }
                endTime = mTimeGridAdvHelper.Index2Time(i);
                lastGrid = grid;
            }

            {
                //最后一个lastGrid 肯定是满数据的。 下一个时间， grid序号就会改变， 这个是在 FindOutUpdateGridEndIndex 操作的
                if (lastGrid >= 0 && lastGrid < GridLen)
                {
                    sGrid.GetData(0)[dir_index][lastGrid] = (int)adLists[0].Average();
                    sGrid.GetData(1)[dir_index][lastGrid] = (int)adLists[1].Average();
                    sGrid.times[dir_index][lastGrid] = endTime;
                    gridList.Add(lastGrid);
                }
                adLists[0].Clear();
                adLists[1].Clear();
            }

            //上一次 grid 与 当前grid 中间有一段空白
            //线性插值 中间空白区
            if (lastUpdateGridEndTime != DateTime.MinValue)
            {
                if (lastUpdateGridDirection == direction)
                {
                    int grid0 = lastUpdateGrid;
                    int grid1 = firstGrid;
                    if (grid0 >= 0 && grid0 < GridLen &&
                        grid1 >= 0 && grid1 < GridLen)
                    {
                        if (Math.Abs(grid0 - grid1) > 1)//有空白区
                        {
                            FillEmptyGrid(dir_index, grid0, grid1);
                            logger.Debug($"OnPoll_grid() dir_index={dir_index} FillEmptyGrid({dir_index},{grid0},{grid1})");
                        }
                    }

                }

            }

            //这次grid填充， 检查是否连续，如果有留空， 插值填充
            for (int i = 1; i < gridList.Count(); i++)
            {
                int grid1 = gridList[i];
                int grid0 = gridList[i - 1];

                if (Math.Abs(grid1 - grid0) > 1) //有空白区
                {
                    FillEmptyGrid(dir_index, grid0, grid1);
                    logger.Debug($"OnPoll_grid() dir_index={dir_index} FillEmptyGrid({dir_index},{grid0},{grid1})");
                }
            }





            lastUpdateGridEndTime = mTimeGridAdvHelper.Index2Time(updateGridEndIndex);
            lastUpdateGridDirection = direction;
            lastUpdateGrid = lastGrid;
            if (isStopTime) {
                if (direction == DIRECTION.FORWARD)
                {
                    GridFrameUpdateTime_Forw = lastUpdateGridEndTime;
                }
                else {
                    GridFrameUpdateTime_Backw = lastUpdateGridEndTime;
                }
            }
        }
        /// <summary>
        /// 填充 grid0 ~grid1 之间的空白
        /// </summary>
        /// <param name="dir_index"></param>
        /// <param name="grid0"></param>
        /// <param name="grid1"></param>
        void FillEmptyGrid(int dir_index, int grid0, int grid1)
        {
            var adData0 = sGrid.GetData(0)[dir_index];//探头#0 dirIndex 方向 grid数据
            var adData1 = sGrid.GetData(1)[dir_index];//探头#1 dirIndex 方向 grid数据


            int ad0_0 = adData0[grid0];//探头#0 两个grid 数据 开始点
            int ad0_1 = adData0[grid1];//探头#0 两个grid 数据 结束点

            int ad1_0 = adData1[grid0];
            int ad1_1 = adData1[grid1];

            DateTime time0 = sGrid.times[dir_index][grid0];
            DateTime time1 = sGrid.times[dir_index][grid1];

            if (grid0 < grid1)
            {
                for (int i = grid0 + 1; i < grid1; i++)
                {
                    int grid = i;
                    int ad = ad0_0 + (ad0_1 - ad0_0) * (grid - grid0) / (grid1 - grid0);
                    adData0[grid] = ad;

                    ad = ad1_0 + (ad1_1 - ad1_0) * (grid - grid0) / (grid1 - grid0);
                    adData1[grid] = ad;

                    DateTime time = time0 + TimeSpan.FromMilliseconds(((time1 - time0).Milliseconds * (grid - grid0) / (grid1 - grid0)));

                    sGrid.times[dir_index][grid] = time;
                }
            }
            else
            {
                for (int i = grid1 + 1; i < grid0; i++)
                {
                    int grid = i;
                    int ad = ad0_0 + (ad0_1 - ad0_0) * (grid - grid0) / (grid1 - grid0);
                    adData0[grid] = ad;

                    ad = ad1_0 + (ad1_1 - ad1_0) * (grid - grid0) / (grid1 - grid0);
                    adData1[grid] = ad;

                    DateTime time = time0 + TimeSpan.FromMilliseconds(((time1 - time0).Milliseconds * (grid - grid0) / (grid1 - grid0)));

                    sGrid.times[dir_index][grid] = time;
                }
            }
        }

        IEnumerable<T> Slice<T>(IEnumerable<T> list, int startIdx, int endIdx)
        {
            return list.Skip(startIdx).Take(endIdx - startIdx + 1);
        }

        void OnPoll_miniGrid()
        {
            if (MiniGridEvent == null)//没有注册事件
            {
                lastPushMiniGrid = null;//复位最后推送grid号
                stopwatch_pushMiniGridTime.Stop();//停止计时
                return;
            }

            if (lastUpdateGridEndTime == DateTime.MinValue)
                return;//没有生成过一个grid

            if (lastPushMiniGrid == null)
            {
                //从来没有推送
            }
            else
            {
                if (lastPushMiniGrid == lastUpdateGrid)
                {
                    return;//一样不用推送
                }

                if (stopwatch_pushMiniGridTime.IsRunning && stopwatch_pushMiniGridTime.Elapsed < TimeSpan.FromSeconds(0.1))
                {
                    //太快，控制推送频率
                    return;
                }
            }

            if (lastUpdateGrid >= 0 && lastUpdateGrid < GridLen)
            {
                //在范围内

                //需要推送grid
                if (lastPushMiniGrid == null || //从来没有推送
                    lastPushMiniGrid < 0 || lastPushMiniGrid >= GridLen ||//之前在范围外
                    lastPushMiniGridDirection != lastUpdateGridDirection)//之前的方向与现在不一样
                {
                    PushMiniGridEvent(lastUpdateGridDirection, lastUpdateGrid);
                }
                else
                {
                    //之前在范围内
                    if (lastUpdateGridDirection == DIRECTION.FORWARD)
                    {
                        if (lastPushMiniGrid < lastUpdateGrid)
                        {
                            PushMiniGridEvent(lastUpdateGridDirection, (int)lastPushMiniGrid + 1, lastUpdateGrid);
                        }
                        else
                        {
                            //逻辑不对， 之前的推送位置 在 最新位置 右边
                            PushMiniGridEvent(lastUpdateGridDirection, lastUpdateGrid);
                        }
                    }
                    else
                    {
                        if (lastPushMiniGrid > lastUpdateGrid)
                        {
                            PushMiniGridEvent(lastUpdateGridDirection, lastUpdateGrid, (int)lastPushMiniGrid - 1);
                        }
                        else
                        {
                            //逻辑不对， 之前的推送位置 在 最新位置 左边
                            PushMiniGridEvent(lastUpdateGridDirection, lastUpdateGrid);
                        }
                    }
                }
            }
            lastPushMiniGridDirection = lastUpdateGridDirection;
            lastPushMiniGrid = lastUpdateGrid;
            stopwatch_pushMiniGridTime.Restart();
        }

        void PushMiniGridEvent(DIRECTION direction, int grid)
        {
            int dir_index = direction == Misc.DIRECTION.FORWARD ? 0 : 1;

            var buf = new int[] { sGrid.data[dir_index][grid] };
            var buf2 = new int[] { sGrid.data2[dir_index][grid] };

            var times = new DateTime[] { sGrid.times[dir_index][grid] };

            MiniGridEvent(this, new MiniGridEventArgs()
            {
                direction = direction,
                grid_start = grid,
                posOfGrid = PosOfGrid,
                buf = buf,
                buf2 = buf2,
                times = times
            });
        }

        void PushMiniGridEvent(DIRECTION direction, int beginGrid, int endGrid)
        {
            int dir_index = direction == Misc.DIRECTION.FORWARD ? 0 : 1;

            var buf = Slice(sGrid.data[dir_index], beginGrid, endGrid);
            var buf2 = Slice(sGrid.data2[dir_index], beginGrid, endGrid);
            var times = Slice(sGrid.times[dir_index], beginGrid, endGrid);


            MiniGridEvent(this, new MiniGridEventArgs()
            {
                direction = direction,
                grid_start = beginGrid,
                posOfGrid = PosOfGrid,
                buf = buf.ToArray(),
                buf2 = buf2.ToArray(),
                times = times.ToArray()

            });

        }
        
        void OnPoll_TimeGridAdv()
        {            
            if (TimeGridAdv2Event == null)
                return;

            if (mTimeGridAdvHelper.DriveStatusPool.Count() < 2)//起码要有 RUNNING, STOP 状态
                return;

            var cell = mTimeGridAdvHelper.DriveStatusPool.Last();
            if (cell.driveStatus == DRIVE_MAN_STATUS.RUNNING)
                return;//还在运行中

            IsTimeToPushTimeGridAdv = false;

            //当前已经停下来
            DateTime stopTime = cell.dt;
            if (lastPushTimeGridAdvTime !=DateTime.MinValue 
                && lastPushTimeGridAdvTime>=stopTime)
            {
                return;//已经推送了出去
            }

            var cell2 = mTimeGridAdvHelper.DriveStatusPool[mTimeGridAdvHelper.DriveStatusPool.Count() - 2];

            if (cell2.driveStatus != DRIVE_MAN_STATUS.RUNNING) 
            {
                //异常， stop 之前不是running
                throw new Exception($"异常， stop 之前不是running, cell2.driveStatus={cell2.driveStatus}");
            }

            

            //开始RUNNING时间
            DateTime beginTime = cell2.dt;
            var eventArgs = GetTimeGridAdv2Event(beginTime);

            //记录下推送的时间，防止再次推送
            lastPushTimeGridAdvTime = eventArgs.EndTime;

            //获取ad列表
            var dataList = eventArgs.DataList;
            int dataCnt = dataList.Count();
            if (dataCnt == 0)
                return;
            
            var endTime = eventArgs.EndTime;
            var direction = eventArgs.Direction;


            //触发事件
            TimeGridAdv2Event?.Invoke(this, new TimeGridAdv2EventArgs
            {
                Direction = direction,
                EndTime = endTime,
                DataList= dataList
            });
        }


        /// <summary>
        /// 从正反缓存区, 获取grid数据
        /// </summary>
        /// <param name="direction">方向, 只有 正,反</param>
        /// <param name="grid_start">grid 开始位置</param>
        /// <param name="grid_len">grid 长度</param>
        /// <param name="dat">grid 数据</param>
        public void GetGrid(Misc.DIRECTION direction, out int[] buf0, out int[] buf1, out DateTime[] times)
        {
            int dirIndex = (direction == Misc.DIRECTION.FORWARD) ? 0 : 1;

            buf0 = sGrid.GetData(0)[dirIndex].ToArray();
            buf1 = sGrid.GetData(1)[dirIndex].ToArray();

            times = sGrid.times[dirIndex].ToArray();
        }


        /// <summary>
        /// 从正反缓存区, 获取grid数据
        /// </summary>
        /// <param name="direction">方向, 只有 正,反</param>
        /// <param name="grid_start">grid 开始位置</param>
        /// <param name="grid_len">grid 长度</param>
        /// <param name="dat">grid 数据</param>
        public void GetGrid(int adIndex, Misc.DIRECTION direction, int grid_start, int grid_len, out int[] dat, out DateTime[] times)
        {
            int dirIndex = (direction == Misc.DIRECTION.FORWARD) ? 0 : 1;

            dat = new int[grid_len];
            times = new DateTime[grid_len];

            var adData = sGrid.GetData(adIndex);//获取序号为 adIndex 探头的正反向数据
            var dirData = adData[dirIndex];//获取 该方向数据
            var timeData = sGrid.times[dirIndex];

            for (int i = 0; i < dat.Length; i++)
            {
                int grid_num = grid_start + i;
                if (grid_num >= dirData.Length)
                {
                    //剩余全部填入 NULL_VALUE;
                    for (int j = i; j < dat.Length; j++)
                    {
                        dat[i] = Misc.MyBase.NULL_VALUE;
                        times[i] = DateTime.MinValue;
                    }
                    break;
                }
                dat[i] = dirData[grid_num];
                times[i] = timeData[grid_num];
            }

            return;
        }

        public void GetGrid(int adIndex, Misc.DIRECTION direction, int grid_start, int grid_len, out int[] dat)
        {
            GetGrid(adIndex, direction, grid_start, grid_len, out dat, out _);
        }
        /// <summary>
        /// 从正反缓存区, 获取全部grid数据
        /// </summary>
        /// <param name="direction">方向, 只有 正,反</param>
        /// <param name="dat">grid 数据</param>
        public void GetGrid(int adIndex, Misc.DIRECTION direction, out int[] dat)
        {
            GetGrid(adIndex, direction, 0, GridLen, out dat);
        }

        /// <summary>
        /// 从正反缓存区, 获取全部grid数据
        /// </summary>
        /// <param name="direction">方向, 只有 正,反</param>
        /// <param name="dat">grid 数据</param>
        public void GetGrid(int adIndex, Misc.DIRECTION direction, out int[] dat, out DateTime[] times)
        {
            GetGrid(adIndex, direction, 0, GridLen, out dat, out times);
        }
    }

    class CalSpeed
    {
        DateTime last_now;
        DateTime pos1_changed_time;
        DateTime pos2_changed_time;
        DateTime last_pos1_changed_time;
        DateTime last_pos2_changed_time;
        int last_pos1;
        int last_pos2;
        int pos1;
        int pos2;

        public bool Cal(DateTime now, out int speed1, out int speed2)
        {
            speed1 = 0;
            speed2 = 0;
            if (last_now == DateTime.MinValue && now != DateTime.MinValue)
            {
                last_now = now;
                last_pos1_changed_time = pos1_changed_time;
                last_pos2_changed_time = pos2_changed_time;
                last_pos1 = pos1;
                last_pos2 = pos2;
                return false;
            }

            if (now - last_now < TimeSpan.FromSeconds(0.5))
                return false;

            bool isInvalid = false;
            if (pos1_changed_time != last_pos1_changed_time && last_pos1_changed_time != DateTime.MinValue)
            {
                var spd = (int)((pos1 - last_pos1) / (pos1_changed_time - last_pos1_changed_time).TotalSeconds);
                if (Math.Abs(spd) > 100000)
                {
                    //肯定是溢出了，
                    isInvalid = true;
                }
                else
                {
                    speed1 = spd;
                }
            }
            else
            {
                //停了
                speed1 = 0;
            }

            if (pos2_changed_time != last_pos2_changed_time && last_pos2_changed_time != DateTime.MinValue)
            {
                var spd=  (int)((pos2 - last_pos2) / (pos2_changed_time - last_pos2_changed_time).TotalSeconds);
                if (Math.Abs(spd) > 100000)
                {
                    //肯定是溢出了，
                    isInvalid = true;
                }
                else {
                    speed2 = spd;
                }
            }
            else
            {
                //停了
                speed2 = 0;
            }

            last_now = now;
            last_pos1_changed_time = pos1_changed_time;
            last_pos2_changed_time = pos2_changed_time;
            last_pos1 = pos1;
            last_pos2 = pos2;

            if (isInvalid)
                return false;
            else
                return true;

        }

        public void SetPos1(DateTime changedTime, int pos)
        {
            pos1_changed_time = changedTime;
            pos1 = pos;
        }
        public void SetPos2(DateTime changedTime, int pos)
        {
            pos2_changed_time = changedTime;
            pos2 = pos;
        }
        public void Reset()
        {
            last_now = DateTime.MinValue;

            pos1_changed_time = DateTime.MinValue;
            pos1 = 0;
            last_pos1_changed_time = DateTime.MinValue;
            last_pos1 = 0;

            pos2_changed_time = DateTime.MinValue;
            pos2 = 0;
            last_pos2_changed_time = DateTime.MinValue;
            last_pos2 = 0;


        }
    }

    /// <summary>
    /// 正反转 grid数据 缓存区
    /// </summary>
    class SGrid
    {
        public const int GRID_MAX_SIZE = 1000;
        public int Size;


        public int[][] data = new int[2][];//探头#1 data0[0]=forword, data0[1]=backward
        public int[][] data2 = new int[2][];//探头#2 data1[0]=forword, data1[1]=backward

        public DateTime[][] times = new DateTime[2][];//每个grid的时间 times[0]=forword, times[1]=backward
        public SGrid()
        {
            //清空所有数据
            SetSize(GRID_MAX_SIZE);
        }
        public void SetSize(int size)
        {
            this.Size = size;
            data[0] = new int[size];
            data[1] = new int[size];

            data2[0] = new int[size];
            data2[1] = new int[size];

            times[0] = new DateTime[size];
            times[1] = new DateTime[size];

            Clear();
        }
        public int[][] GetData(int dataIndex)
        {
            if (dataIndex == 0)
                return data;
            else
                return data2 ;
        }

        public void Clear()
        {
            //清空所有数据
            Clear(0);
            Clear(1);
        }
        public void Clear(int index)
        {
            //清空所有数据
            for (int i = 0; i < Size; i++)
            {
                data[index][i] = Misc.MyBase.NULL_VALUE;
                data2[index][i] = Misc.MyBase.NULL_VALUE;
                times[index][i] = DateTime.MinValue;
            }
        }
    }
}
