FlyADClientAdv.cs

using FObjBase;
using Misc;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Diagnostics;
using System.Linq;
using System.Net;
using System.Text;
using System.Threading.Tasks;

namespace FlyADBase
{
    public partial class FlyAD7 : IFlyADClientAdv
    {
        NLog.Logger logger = NLog.LogManager.GetCurrentClassLogger();

        #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>
        /// 机架修正
        /// </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;

        public event TimeGridAdv2EventHandler TimeGridAdv2Event;
        #endregion

        SGrid fGrid = new SGrid();




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

            mTimeGridAdvHelper.Init();

            this.PropertyChanged += FlyAD7_PropertyChanged1;
        }

        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();

        }



        #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)
                {
                    //剩余全部填入 NULL_VALUE;
                    for (int j = i; j < dat.Length; j++) { 
                        dat[i] = Misc.MyBase.NULL_VALUE;
                    }
                    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 advGetPos1AD1(int ad)
        {
            mTimeGridAdvHelper.AddPos_Default(Now, Position);

            if (CorrectADs != null)
            {
                ad = CorrectAD(Position / PosOfGrid, ad);
            }
            AD = ad;
        }
        void advGetPos2() 
        {
            mTimeGridAdvHelper.AddPos2_Default(Now, Position2);
        }
        void advGetIo()
        {
            mTimeGridAdvHelper.AddIStatus_Default(Now, IStatus);
        }
        void advGetState()
        {
            mTimeGridAdvHelper.AddDriveStatus_Default(Now, DriveOrder, DriveStatus, Marker);
        }

        void advPushPos1(int version)
        {
            if(version == 2)
                mTimeGridAdvHelper.AddPos(Now, Position);


        }
        void advPushPos2(int version)
        {
            if (version == 2)
                mTimeGridAdvHelper.AddPos2(Now, Position2);

            
        }
        void advPushIo(int version)
        {
            if (version == 2)
            {
                mTimeGridAdvHelper.AddIStatus(Now, IStatus, Position, Position2, out UInt16 ichanged);
            }
        }
        void advPushAd(int ad)
        {
            if (CorrectADs != null)
                ad = CorrectAD(Position / PosOfGrid, ad);

            AD = ad;
        }

        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 advPushTimeGrid(DateTime end_dt, int[] datas)
        {
            #region 高级 timegrid
            //TODO 
            
            mTimeGridAdvHelper.AddAD(end_dt - TimeSpan.FromMilliseconds(ADLag), datas);

            if (IsTimeToPushTimeGridAdv) {
                IsTimeToPushTimeGridAdv = false;

                if (TimeGridAdv2Event != null)
                {
                    //触发全部高级版的 timegrid
                    bool ret = mTimeGridAdvHelper.GetLastRunningTime(out DateTime beginTime, out DateTime endTime, out int marker, out DRIVE_MAN_ORDER order);
                    if (ret)
                    {
                        //获取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, 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 = reponse_endTime;
                        eventArgs.AdList = adList;
                        eventArgs.PosList = posList;
                        eventArgs.Marker = marker;

                        TimeGridAdv2Event(this, eventArgs);
                    }
                }

            }

            #endregion
        }


        void advPushStatus(int version)
        {
            
            mTimeGridAdvHelper.AddDriveStatus(Now, DriveOrder, DriveStatus, Marker);
            if (DriveStatus != DRIVE_MAN_STATUS.RUNNING)
            {
                //检测 mTimeGridAdvHelper.AdPool 的 systick 是否 大于等于 Now
                if (mTimeGridAdvHelper.NewestTime >= Now)
                {
                    IsTimeToPushTimeGridAdv = false;
                }
                else {
                    //通知 mTimeGridAdvHelper 下次触发 timegridadv
                    IsTimeToPushTimeGridAdv = true;

                }
            }
        }

        void advPushGrid(DateTime dt, int marker, DIRECTION direction, int grid_start, int[] data)
        {

            int index = (direction == Misc.DIRECTION.BACKWARD) ? 1 : 0;

            if (grid_start >= fGrid.data[index].Length)
                return;

            int grid_end = data.Length + grid_start - 1;

            if (grid_end >= fGrid.data[index].Length)
                grid_end = fGrid.data[index].Length - 1;

            int len = grid_end - grid_start + 1;


            Array.Copy(data, 0, fGrid.data[index], grid_start, len);

            //清空其它数据
            for (int i = 0; i < grid_start; i++)
            {
                fGrid.data[index][i] = Misc.MyBase.NULL_VALUE;
            }
            for (int i = (grid_end + 1); i < fGrid.data[index].Length; i++)
            {
                fGrid.data[index][i] = Misc.MyBase.NULL_VALUE;
            }
            if (GridEvent != null)
            {
                CorrectADs?.Invoke(direction, grid_start, data);
            }
        }

        void advPushMiniGrid(DIRECTION direction, int grid_start, int[] data) 
        {
            int index = (direction == Misc.DIRECTION.BACKWARD) ? 1 : 0;

            if (grid_start >= fGrid.data[index].Length)
                return;

            int grid_end = data.Length + grid_start - 1;



            if (grid_end >= fGrid.data[index].Length)
                grid_end = fGrid.data[index].Length - 1;

            int len = grid_end - grid_start + 1;

            Array.Copy(data, 0, fGrid.data[index], grid_start, len);

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

        }

        int CorrectAD(int grid, int ad)
        {
            int[] d = new int[1];
            d[0] = ad;
            CorrectADs(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;
            }
        }
    }
}