FlyAd2021Adv.cs

using FObjBase;
using Misc;
using System;
using System.Diagnostics;
using System.Linq;

namespace FlyADBase
{
    public partial class FlyAd2021 : IFlyADClientAdv
    {
        #region IFlyADClientAdv property
        /// <summary>
        /// 通过脉冲计算速度，不使用AD盒的输出
        /// </summary>
        [Obsolete("已经无效，必须通过电脑计算速度")]
        public bool IsCalSpeed { get; set; } = true;

        /// <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>
        /// 机架修正
        /// </summary>
        public CorrectADsHandler CorrectADs { get; set; }

        /// <summary>
        /// ad滞后修正  单位ms
        /// </summary>
        public int ADLag { 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>
        /// 以timegrid 为单位，推送数据
        /// </summary>
        public event TimeGridAdv2EventHandler TimeGridAdv2Event;
        #endregion


        DateTime lastUpdateADTime;
        int lastGrid = Misc.MyBase.NULL_VALUE;
        DateTime lastGridTime;
        Stopwatch stopwatch_gridTime = new Stopwatch();

        SGrid fGrid = new SGrid();
        void advConstructor()
        {
            fGrid.SetSize(PosLen / PosOfGrid);

            mTimeGridAdvHelper.Init();

            this.PropertyChanged += FlyAD7_PropertyChanged1;


            //更新线速度
            PollModule.Current.Poll_Config(PollModule.POLL_CONFIG.ADD,
                () =>
                {

                    if (calSpeed.Cal(Now, out int speed1, out int speed2))
                    {
                        Speed = speed1;
                        Speed2 = speed2;
                    }

                }, TimeSpan.FromSeconds(1));

            //轮询事件触发
            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(PosLen)) ||
                (e.PropertyName == nameof(PosOfGrid)))
            {
                fGrid.SetSize(PosLen / PosOfGrid);
            }
            else if (e.PropertyName == nameof(Marker))
            {
                //if (DriveOrder == DRIVE_MAN_ORDER.SYNC)
                {
                    //同步运行中
                    //检测与哪个指令一致, 
                    //该指令置为 Doing， 前面的全部置为 Finish，并且删除
                    for (int i = 0; i < SyncOrders.Count(); i++)
                    {
                        if (SyncOrders[i].Marker == Marker)
                        {
                            SyncOrders[i].State = SyncOrderState.Doing;

                            for (int j = 0; j < i; j++)
                            {
                                SyncOrders[0].State = SyncOrderState.Finish;
                                SyncOrders.RemoveAt(0);
                            }
                        }
                    }
                }
            }
        }

        void advAfterContected()
        {
            mTimeGridAdvHelper.Clear();
            lastUpdateADTime = DateTime.MinValue;
            lastGrid = Misc.MyBase.NULL_VALUE;
            lastGridTime = 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));
            }
        }

        /// <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, int grid_start, int grid_len, out int[] dat)
        {
            int index = 0;
            if (direction == Misc.DIRECTION.BACKWARD)
                index = 1;

            dat = new int[grid_len];

            for (int i = 0; i < dat.Length; i++)
            {
                int grid_num = grid_start + i;
                if (grid_num >= fGrid.data[index].Length)
                    break;

                if (GridSmooth > 0)
                {
                    int sum = 0;
                    int cnt = 0;
                    for (int j = 0; j < (GridSmooth * 2 + 1); j++)
                    {
                        int idx = grid_num - GridSmooth + j;
                        if (idx < 0)
                            continue;
                        if (idx >= fGrid.data[index].Length)
                            break;
                        if (Misc.MyBase.ISVALIDATA(fGrid.data[index][idx]))
                        {
                            sum += fGrid.data[index][idx];
                            cnt++;
                        }
                    }
                    if (cnt > 0)
                        dat[i] = sum / cnt;
                    else
                        dat[i] = Misc.MyBase.NULL_VALUE;
                }
                else
                {
                    dat[i] = fGrid.data[index][grid_num];
                }
            }

            CorrectADs?.Invoke(direction, grid_start, dat);
            return;
        }

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


        void advPushPosAll()
        {
            advPushPos1();
            advPushPos2();
        }

        void advPushPos1()
        {
            if (mTimeGridAdvHelper.AddPos(Now, Position))
            {
                PositionChangedEvent?.Invoke(this, new PositionChangedEventArgs() { Time = Now, Position = Position });
            }
        }

        void advPushPos2()
        {
            if (mTimeGridAdvHelper.AddPos2(Now, Position2))
            {
                Position2ChangedEvent?.Invoke(this, new PositionChangedEventArgs() { Time = Now, Position = Position2 });
            }
        }

        void advPushIn()
        {
            if (isReadyContext.isReadyGetPos)
            {
                mTimeGridAdvHelper.AddIStatus(Now, IStatus);
            }
            else
            {
                if (mTimeGridAdvHelper.AddIStatus(Now, IStatus, Position, Position2, out UInt16 ichanged))
                {
                    IStatusChangedEvent?.Invoke(this,
                        new IStatusChangedEventArgs(Now, IStatus, ichanged, Position, Position2));
                }
            }
        }

        void advPushAd(int ad)
        {
            mTimeGridAdvHelper.AddAD(Now - TimeSpan.FromMilliseconds(ADLag), new int[] { ad });
        }

        void advPushStatus()
        {
            if (mTimeGridAdvHelper.AddDriveStatus(Now, DriveOrder, DriveStatus, Marker))
            {

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

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

        /// <summary>
        /// ScanMotion 使用。给定按扫描的时间点。 当探头停下来后，获取数据
        /// </summary>
        /// <param name="beginTime"></param>
        /// <returns></returns>
        public TimeGridAdv2EventArgs GetTimeGridAdv2Event(DateTime beginTime)
        {
            //获取ad列表
            var adList = mTimeGridAdvHelper.GetAD(beginTime);
            if (adList.Count() == 0)
                return null;
            DateTime endTime = mTimeGridAdvHelper.NewestTime;

            //获取pos
            var posList = mTimeGridAdvHelper.GetPos(mTimeGridAdvHelper.NewestTime, adList.Count());



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


            //机架修正
            if (CorrectADs != null)
            {
                for (int i = 0; i < adList.Count(); i++)
                {
                    adList[i] = CorrectAD(posList[i] / PosOfGrid, adList[i]);
                }
            }

            TimeGridAdv2EventArgs eventArgs = new TimeGridAdv2EventArgs();
            eventArgs.Direction = direction;
            eventArgs.EndTime = endTime;
            eventArgs.AdList = adList;
            eventArgs.PosList = posList;

            return eventArgs;
        }

        void OnPoll()
        {
            if (!IsConnected)
                return;
            if (!IsReady)
                return;

            OnPoll_TimeGrid();
            OnPoll_TimeGridAdv();
            OnPoll_miniGrid();


        }
        
        void OnPoll_TimeGrid()
        {
            if (lastUpdateADTime == DateTime.MinValue)
            {
                lastUpdateADTime = Now;
                return;
            }


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

            //获取0.2s到现在的数据
            var adList = mTimeGridAdvHelper.GetAD(lastUpdateADTime);
            
            //算AD均值
            
            //机架修改处理
            //ad = CorrectAD(Position / PosOfGrid, ad);
            //AD = ad;
            for (int i = 0; i < adList.Count(); i++) {
                adList[i] = CorrectAD(Position / PosOfGrid, adList[i]);
            }
            int ad = (int)(adList.Average());
            AD = ad;

            //推送timegrid事件
            TimeGridEvent?.Invoke(this,
                new TimeGridEventArgs()
                {
                    Time = lastUpdateADTime,
                    Data = adList.ToArray(),
                    Ts = TimeSpan.FromMilliseconds(1)
                });

            lastUpdateADTime = Now;

        }


        void OnPoll_miniGrid()
        {
            int grid = Position / PosOfGrid;

            if (!Misc.MyBase.ISVALIDATA(lastGrid))
            {
                lastGrid = grid;
                lastGridTime = Now;
                stopwatch_gridTime.Restart();
                return;
            }

            if (lastGrid == grid)
            {
                if (Now - lastGridTime > TimeSpan.FromSeconds(2))
                {
                    //太慢，超过2秒还没走完一个grid
                    //强制缩少数据
                    lastGridTime = Now;
                }
                return;
            }
            if (stopwatch_gridTime.Elapsed < TimeSpan.FromSeconds(0.2)) 
            {
                //太快，控制推送频率
                return;
            }
            stopwatch_gridTime.Restart();

            //方向
            Misc.DIRECTION direction;
            if (lastGrid < grid)
                direction = Misc.DIRECTION.FORWARD;
            else
                direction = Misc.DIRECTION.BACKWARD;

            //开始位置
            var adList = mTimeGridAdvHelper.GetAD(lastGridTime);
            var posList = mTimeGridAdvHelper.GetPos(Now, adList.Count());
            var gridList = TimeGridAdvHelperExt.ToGrid(adList, posList, PosOfGrid, GridLen, out int grid_start, out int grid_end, isMiniGrid: true);

            if (gridList == null)
                return;

            int index = (direction == Misc.DIRECTION.BACKWARD) ? 1 : 0;
            
            var grid_data = fGrid.data[index];

            if (grid_start >= grid_data.Length)
                return;

            if (grid_end >= grid_data.Length)
                grid_end = grid_data.Length - 1;

            int len = grid_end - grid_start + 1;

            Array.Copy(gridList, 0, grid_data, grid_start, len);

            //清空后面的数据
            if (direction == Misc.DIRECTION.BACKWARD)
            {
                if (grid_data[0] != Misc.MyBase.NULL_VALUE) 
                {
                    for (int i = 0; i < grid_start; i++)
                    {
                        fGrid.data[index][i] = Misc.MyBase.NULL_VALUE;
                    }
                }
            }
            else
            {
                if (grid_data.Last() != Misc.MyBase.NULL_VALUE)
                {
                    for (int i = (grid_end + 1); i < grid_data.Length; i++)
                    {
                        grid_data[i] = Misc.MyBase.NULL_VALUE;
                    }
                }
            }
            
            var data = gridList.ToArray();
            CorrectADs?.Invoke(direction, grid_start, data);


            //需要推送grid
            MiniGridEvent?.Invoke(this, new MiniGridEventArgs()
            {
                direction = direction,
                grid_start = grid_start,
                posOfGrid = PosOfGrid,
                buf = data,
                marker = Marker
            });

            lastGrid = grid;
            lastGridTime = Now;
        }


        void OnPoll_TimeGridAdv()
        {
            if (!IsTimeToPushTimeGridAdv)
                return;

            IsTimeToPushTimeGridAdv = false;

            //触发全部高级版的 timegrid
            bool ret = mTimeGridAdvHelper.GetLastRunningTime(out DateTime beginTime, out DateTime endTime, out int marker, out DRIVE_MAN_ORDER order);
            if (!ret)
                return;

            //获取ad列表
            var adList = mTimeGridAdvHelper.GetAD(beginTime, endTime, out DateTime reponse_endTime);
            int adCnt = adList.Count();
            if (adCnt == 0)
                return;

            //获取pos
            var posList = mTimeGridAdvHelper.GetPos(reponse_endTime, adCnt);

            Misc.DIRECTION direction;
            //判断运动方向
            if (posList.Last() > posList.First())
            {
                //正向
                direction = DIRECTION.FORWARD;
            }
            else
            {
                direction = DIRECTION.BACKWARD;
            }
            
            //生成grid图
            var gridList = TimeGridAdvHelperExt.ToGrid(adList, posList, PosOfGrid, GridLen);
            //放入对应方向缓存区
            int index = (direction == Misc.DIRECTION.BACKWARD) ? 1 : 0;

            var grid_data = fGrid.data[index];

            int len = Math.Min(gridList.Length, grid_data.Length);
            
            Array.Copy(gridList, 0, fGrid.data[index], 0, len);


            //机架修正
            if (CorrectADs != null)
            {
                for (int i = 0; i < adList.Count(); i++)
                {
                    adList[i] = CorrectAD(posList[i] / PosOfGrid, adList[i]);
                }
            }


            TimeGridAdv2Event?.Invoke(this, new TimeGridAdv2EventArgs
            {
                Direction = direction,
                EndTime = reponse_endTime,
                AdList = adList,
                PosList = posList,
                Marker = marker
            });

            GridEvent?.Invoke(this, new MiniGridEventArgs()
            {
                direction = direction,
                marker = Marker,
                buf = gridList,
                posOfGrid = PosOfGrid,
                grid_start = 0
            });
        }





        int CorrectAD(int grid, int ad)
        {
            int[] d = new int[1];
            d[0] = ad;
            CorrectADs?.Invoke(Misc.DIRECTION.FIX, grid, d);
            return d[0];
        }
    }

    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(1))
                return false;

            if (pos1_changed_time != last_pos1_changed_time && last_pos1_changed_time != DateTime.MinValue)
            {
                speed1 = (int)((pos1 - last_pos1) / (pos1_changed_time - last_pos1_changed_time).TotalSeconds);
            }
            else
            {
                //停了
            }

            if (pos2_changed_time != last_pos2_changed_time && last_pos2_changed_time != DateTime.MinValue)
            {
                speed2 = (int)((pos2 - last_pos2) / (pos2_changed_time - last_pos2_changed_time).TotalSeconds);
            }
            else
            {
                //停了
            }

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

            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;
        int size;
        public int[][] data = new int[2][];//data[0]=forword, data[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];
            Clear();
        }
        public void Clear()
        {
            //清空所有数据
            for (int i = 0; i < size; i++)
            {
                data[0][i] = Misc.MyBase.NULL_VALUE;
                data[1][i] = Misc.MyBase.NULL_VALUE;
            }
        }
    }
}