#define SPEED_BY_MySelf
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Net;
using System.ComponentModel;
using FObjBase;
using System.IO;
using Misc;
using System.Collections.ObjectModel;


namespace FlyADBase
{
    /// <summary>
    /// 执行顺序
    /// 1. IFlyAD flyad = new FlyAD7();//加载速度参数，IP地址
    /// 2. flyad.Connect();//flyad.Connect("192.168.251.10:20006");
    /// 3. 当连接上，会设置推送，获取flyad7控制器里面所有数据，设置速度参数到控制器
    /// </summary>
    public partial class FlyAD7 : FObj, IFlyAD
    {
        #region MARKNO
        public const int MARKNO_SET_VELOCITY = 1;
        public const int MARKNO_SET_ZEROPOS = 2;
        public const int MARKNO_SET_SYSPARAM = 3;
        #endregion
        #region 成员变量
        /// <summary>
        /// 不设置服务器
        /// </summary>
        private bool bShieldSetValueEx = false;
        private DateTime dt_surplus = DateTime.MinValue;

        class DtAndBool
        {
            public bool bNeedGet;
            public DateTime dt;
            public void Clear()
            {
                bNeedGet = true;
                dt = DateTime.Now;
            }
        }

        DtAndBool driveman_wait = new DtAndBool();
        DtAndBool pos_wait = new DtAndBool();
        DtAndBool sysinfo_wait = new DtAndBool();

        /// <summary>
        /// 正反转 grid数据 缓存区
        /// </summary>
        class SGrid
        {
            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;
                }
            }
        }
        SGrid fGrid = new SGrid();

        TCPCConn conn = null;

        #endregion

        /// <summary>
        /// 没有收到timegrid的次数， 1秒加1次
        /// </summary>
        int noTGridCnt = 0;
        /// <summary>
        /// 3次收不到timegrid, 重连
        /// </summary>
        const int ReConnectTGridMaxCnt = 3;

        public FlyAD7()
        {
            Now = DateTime.Now;
            Position = 0;
            Speed = 0;
            Position2 = 0;
            Speed2 = 0;
            AD = 0;
            IStatus = 0xffff;
            OStatus = 0xffff;
            MotorType = MOTORTYPE.VF0;
            PosOffset = 0;
            JogVelocity = 5000;
            PosOfGrid = 10;
            PosLen = 8900;
            Ratio01 = 4;
            Ratio02 = 1;
            Velocity = 8000;
            SVelocity = 200;
            ATime = 300;
            DTime = 300;
            HVelocity1 = 4000;
            HVelocity2 = 500;
            Marker = 0;

            Pos2Comp = 1;
            Pos1LCShift = 0;
            IsSync = false;

            fGrid.SetSize(PosLen / PosOfGrid);

            this.PropertyChanged += new PropertyChangedEventHandler(FlyAD7_PropertyChanged);
            PollModule.Current.Poll_Config(PollModule.POLL_CONFIG.ADD, new PollModule.PollHandler(OnPoll));

            //3秒内收不到1个timegrid ，就会重连
            PollModule.Current.Poll_Config(PollModule.POLL_CONFIG.ADD,
                () =>
                {
                    if (!IsConnected)
                    {
                        noTGridCnt = 0;
                    }
                    else
                    {
                        noTGridCnt++;
                        if (noTGridCnt >= ReConnectTGridMaxCnt)
                        {
                            noTGridCnt = 0;
                            ReConnect();
                        }
                    }

                }, TimeSpan.FromSeconds(1));

#if SPEED_BY_MySelf
            //更新线速度
            PollModule.Current.Poll_Config(PollModule.POLL_CONFIG.ADD,
                () =>
                {
                    if (pos1_last_for_speed != int.MinValue)
                    {
                        Speed = Position - pos1_last_for_speed;
                    }
                    pos1_last_for_speed = Position;


                    if (pos2_last_for_speed != int.MinValue)
                    { 
                        Speed2 = Position2 - pos2_last_for_speed;
                    }
                    pos2_last_for_speed = Position2;
                }, TimeSpan.FromSeconds(1));
#endif


            mTimeGridAdvHelper.Init();
        }

        int pos1_last_for_speed = int.MinValue;
        int pos2_last_for_speed = int.MinValue;


        void FlyAD7_PropertyChanged(object sender, PropertyChangedEventArgs e)
        {
            if (e.PropertyName == "HasCRC")
            {
                if (conn != null)
                    conn.HasCRC = HasCRC;
            }
            else if (e.PropertyName == "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 == "PosLen") ||
                (e.PropertyName == "PosOfGrid"))
            {
                fGrid.SetSize(PosLen / PosOfGrid);
            }
            else if (e.PropertyName == "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);
                            }
                        }
                    }
                }
            }
            if (bShieldSetValueEx)
                return;

            if ((e.PropertyName == "Velocity") ||
                (e.PropertyName == "SVelocity") ||
                (e.PropertyName == "ATime") ||
                (e.PropertyName == "DTime") ||
                (e.PropertyName == "HVelocity1") ||
                (e.PropertyName == "HVelocity2"))
            {
                FObjBase.PollModule.Current.Poll_JustOnce(
                    delegate()
                    {
                        //TODO,不应该直接修改Velocity，SVelocity。。。。。。
                        //所以下面也不可能存在！！！
                        SetPosParam(Velocity, SVelocity, ATime, DTime, HVelocity1, HVelocity2);
                    }, this, MARKNO_SET_VELOCITY);
            }
            else if ((e.PropertyName == "PosOffset") ||
                (e.PropertyName == "JogVelocity"))
            {
                FObjBase.PollModule.Current.Poll_JustOnce(
                    delegate()
                    {
                        CurrObjSys.SetValueEx(conn, FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID,
                            ID, FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.SET_ZERO_POS,
                            new FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.Pack_ZeroPos()
                            {
                                zero_pos = PosOffset,
                                jog_velocity = JogVelocity
                            }.ToBytes());
                    }, this, MARKNO_SET_ZEROPOS);
            }
            else if ((e.PropertyName == "MotorType") ||
                    (e.PropertyName == "PosOfGrid") ||
                    (e.PropertyName == "Ratio01") ||
                    (e.PropertyName == "Ratio02"))
            {
                FObjBase.PollModule.Current.Poll_JustOnce(
                    delegate()
                    {
                        SetSysParam(PosOfGrid, MotorType, Ratio01, Ratio02);
                    }, this, MARKNO_SET_SYSPARAM);
            }
            else if (e.PropertyName == "Pos1LCShift")
            {
                
                CurrObjSys.CallFunctionEx(
                   conn,
                   FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                   ID,
                   FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                   new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_Pos1LCShift()
                   {
                       pos1_LCShift = Pos1LCShift
                   }.ToBytes());
            }
            else if (e.PropertyName == "Position2")
            {
                CurrObjSys.CallFunctionEx(
                    conn,
                    FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                    ID,
                    FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                    new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_Pos2()
                    {
                        pos2 = Position2
                    }.ToBytes());
            }
            else if (e.PropertyName == "Pos2Comp")
            {
                CurrObjSys.CallFunctionEx(
                    conn,
                    FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                    ID,
                    FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                    new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_Pos2Comp()
                    {
                        comp = Pos2Comp
                    }.ToBytes());
            }

        }


        private void Init()
        {
            CurrObjSys.SenseConfigEx(
                conn,
                FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.ID,
                ID,
                Misc.MyBase.BIT(FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_POS1)
                | Misc.MyBase.BIT(FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_POS2)
                | Misc.MyBase.BIT(FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_AD1)
                | Misc.MyBase.BIT(FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_IO)
                | Misc.MyBase.BIT(FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_TIMEGRID)
                | Misc.MyBase.BIT(FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_GRID)
                | Misc.MyBase.BIT(FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_STATUS)
                | Misc.MyBase.BIT(FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_MINIGRID)
                ,
                SENSE_CONFIG.ADD);

            UpdateParam();



            //DRIVE_MAN
            CurrObjSys.GetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_STATE);

            CurrObjSys.GetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_SYNC_ORDER_CNT);

            CurrObjSys.GetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_SYNC_STATUS);

            //FLYADC
            CurrObjSys.GetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.GET_POS1AD1);

            CurrObjSys.GetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.GET_POS2);

            //FLYIO
            CurrObjSys.GetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.FLYIO_OBJ_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.FLYIO_OBJ_INTERFACE.GET_IO);

            NotifyPropertyChanged("Pos1LCShift");
            NotifyPropertyChanged("Pos2Comp");

            SyncPos2Clear();
            SyncClear();
            SyncEnd();


            mSysTick.Reset();

            mTimeGridAdvHelper.Clear();
            preTimeGrid_start = DateTime.MinValue;
            preTimeGrid_end = DateTime.MinValue;
        }

        #region FObj
        public override void ConnectNotify(IFConn from)
        {
            IsConnected = from.IsConnected;
            if (IsConnected)
            {
                ConnectCnt++;
                
                
                Init();
            }
            else
            {
                DriveStatus = DRIVE_MAN_STATUS.STOP;
            }
        }
        public override void PushGetValue(IFConn from, uint srcid, ushort memid, byte[] infodata)
        {
            bShieldSetValueEx = true;
            switch (srcid)
            {
                case FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.ID:
                    {
                        switch (memid)
                        {
                            case FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.GET_POS1AD1:
                                {
                                    FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.Pack_GetPos1AD1 pack = new FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.Pack_GetPos1AD1();
                                    if (!pack.TryParse(infodata))
                                        break;

                                    Position = pack.pos;
                                    mTimeGridAdvHelper.AddPos_Default(Position, Now);

                                    if (CorrectADs != null)
                                    {
                                        pack.ad = CorrectAD(Position / PosOfGrid, pack.ad);
                                    }
                                    AD = pack.ad;
                                } break;
                            case FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.GET_POS2:
                                {
                                    FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.Pack_GetPos2 pack = new FLYAD7_OBJ_INTERFACE.FLYADC_OBJ_INTERFACE.Pack_GetPos2();
                                    if (!pack.TryParse(infodata))
                                        break;

                                    Position2 = pack.pos2;
#if !SPEED_BY_MySelf
                                    Speed2 = (int)(pack.speed2 * SpeedScale);
#endif
                                } break;
                        }
                    } break;
                case FLYAD7_OBJ_INTERFACE.FLYIO_OBJ_INTERFACE.ID:
                    {
                        switch (memid)
                        {
                            case FLYAD7_OBJ_INTERFACE.FLYIO_OBJ_INTERFACE.GET_IO:
                                {
                                    FLYAD7_OBJ_INTERFACE.FLYIO_OBJ_INTERFACE.Pack_GetIO pack = new FLYAD7_OBJ_INTERFACE.FLYIO_OBJ_INTERFACE.Pack_GetIO();
                                    if (!pack.TryParse(infodata))
                                        break;

                                    IStatus = pack.istatus;
                                    OStatus = pack.ostatus;
                                    mTimeGridAdvHelper.AddIStatus_Default(IStatus,Position, Now);
                                } break;
                        }
                    } break;
                case FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID:
                    {
                        switch (memid)
                        {
                            case FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_DRIVEPARAM:
                                {
                                    FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_DriveParam pack = new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_DriveParam();

                                    if (!pack.TryParse(infodata))
                                        break;
                                    Velocity = pack.Velocity;
                                    SVelocity = pack.SV;
                                    ATime = pack.ATime;
                                    DTime = pack.DTime;
                                    HVelocity1 = pack.hspeed1;
                                    HVelocity2 = pack.hspeed2;

                                } break;
                            case FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_STATE:
                                {
                                    //TODO ？？？？？
                                    FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_DriveState p = new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_DriveState();
                                    if (!p.TryParse(infodata))
                                        break;
                                    DriveOrder = p.order;//TODO
                                    DriveStatus = p.status;
                                    Marker = p.marker;
                                } break;
                            case FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_SYNC_STATUS:
                                {
                                    FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncStatus p = new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncStatus();
                                    if (!p.TryParse(infodata))
                                        break;
                                    //TODO
                                }
                                break;
                            case FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_SYNC_ORDER_CNT:
                                {
                                    FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncOrderCnt p = new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncOrderCnt();
                                    if (!p.TryParse(infodata))
                                        break;
                                    //TODO

                                }
                                break;
                        }
                    } break;
                case FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID:
                    {
                        switch (memid)
                        {
                            case FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.GET_IP:
                                break;
                            case FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.GET_SYSPARAM:
                                {
                                    FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.Pack_SysParam pack = new FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.Pack_SysParam();

                                    if (!pack.TryParse(infodata))
                                        break;
                                    PosOfGrid = pack.posOfGrid;//1个Grid=?脉冲数(2B)
                                    if ((pack.func_enable & 0x03) == 0x02)
                                        MotorType = MOTORTYPE.SERVO;
                                    else if ((pack.func_enable & 0x03) == 0x01)
                                        MotorType = MOTORTYPE.VF0;
                                    else
                                        MotorType = MOTORTYPE.NULL;

                                    Ratio01 = pack.ratio01;
                                    Ratio02 = pack.ratio02;
                                } break;
                            case FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.GET_SYSINFO:
                                {
                                    FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.Pack_SysInfo pack = new FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.Pack_SysInfo();

                                    if (!pack.TryParse(infodata))
                                        break;

                                    AreaStatus = pack.status;
                                    AreaRet = pack.ret;

                                    Array.Copy(pack.code, Code, pack.code.Length);
                                    Code[6] = 0x06;
                                    NotifyPropertyChanged("Code");

                                    Surplus = pack.surplus;

                                    Array.Copy(pack.access, Access, pack.access.Length);
                                    NotifyPropertyChanged("Access");

                                } break;
                            case FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.GET_ZERO_POS:
                                {
                                    FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.Pack_ZeroPos pack = new FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.Pack_ZeroPos();

                                    if (!pack.TryParse(infodata))
                                        break;

                                    PosOffset = pack.zero_pos;
                                    JogVelocity = pack.jog_velocity;
                                } break;
                        }
                    } break;
            }
            bShieldSetValueEx = false;
        }
        /// <summary>
        ///  systick 转 DateTime
        ///  不断把 从flyad7 获取的 systick 放入。  以每次systick 的差 加上 上一次 dt_1st, 得到时间。
        ///  如： 当前 获得 systick1, 之前是 systick0,   所以 dt = dt_1st+ (systick1-systick0);  dt_1st=dt;
        ///  
        /// 第1个 dt_1st, 是 从 DateTime.Now. 获取的。
        /// 
        /// 不会管 当前DateTime.Now 与 dt 差多少。 所以如果 flyad7 时间比较慢，可能会出现 3天后， DateTime.Now 与 dt 差了 1个小时。
        /// </summary>
        class TSysTick
        {
            Int32 systick_1st = 0;
            DateTime dt_1st = DateTime.MinValue;
            Int32 systick_last = 0;

            public void Reset()
            {
                dt_1st = DateTime.MinValue;
            }
            /// <summary>
            /// 输入从 flyad7 获取的 timemark, 转换为 本地 DateTime
            /// </summary>
            /// <param name="systick"></param>
            /// <returns></returns>
            public DateTime ToDateTime(int systick)
            {
                if (dt_1st == DateTime.MinValue)
                {
                    dt_1st = DateTime.Now;
                    systick_1st = systick;
                    systick_last = systick;
                    return dt_1st;
                }
                //记录与新的timemark 相差10秒，太大了,重新生成 timemark 与 dt 的关系
                else if (Math.Abs(systick - systick_last) > 10000)
                {
                    dt_1st = DateTime.Now;
                    systick_1st = systick;
                    systick_last = systick;
                    return dt_1st;
                }
                else
                {
                    int ms = systick - systick_1st;
                    systick_last = systick;


                    DateTime dt = dt_1st.AddMilliseconds(ms);

                    systick_1st = systick;
                    dt_1st = dt;

                    return dt;
                }

            }
        }
        TSysTick mSysTick = new TSysTick();
        public override void PushInfo(IFConn from, uint srcid, ushort infoid, byte[] infodata)
        {
            bShieldSetValueEx = true;
            switch (infoid)
            {
                case FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_POS1:
                    {
                        FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushPos1_2 pack = new FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushPos1_2();
                        int version;
                        if (!pack.TryParse_version(infodata,out version))
                            break;
                        if(version == 1)
                        {
                            Position = pack.pos1;
#if !SPEED_BY_MySelf
                            Speed = (int)(pack.speed1 * SpeedScale);
#endif
                            mTimeGridAdvHelper.AddPos(Position, DateTime.Now);
                        }
                        else if(version == 2)
                        {
                            Position = pack.pos1;
#if !SPEED_BY_MySelf
                            Speed = (int)(pack.speed1 * SpeedScale);
#endif
                            DateTime dt = mSysTick.ToDateTime(pack.systick);
                            Now = dt;
                            
                            mTimeGridAdvHelper.AddPos(Position, dt);
                        }
                    } break;
                case FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_POS2:
                    {
                        FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushPos2_2 pack = new FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushPos2_2();
                        int version;
                        if (!pack.TryParse_version(infodata,out version))
                            break;
                       
                        Position2 = pack.pos2;
 #if !SPEED_BY_MySelf
                        Speed2 = (int)(pack.speed2 * SpeedScale);
#endif                
                    } break;
                case FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_AD1:
                    {
                        FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushAD1 pack = new FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushAD1();
                        if (!pack.TryParse(infodata))
                            break;

                        if (CorrectADs != null)
                            pack.AD = CorrectAD(Position / PosOfGrid, pack.AD);
                        
                        AD = pack.AD;
                    } break;
                case FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_IO:
                    {
                        FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushIO_2 pack = new FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushIO_2();
                        int version;
                        if (!pack.TryParse_version(infodata, out version))
                            break;

                        if (version == 1) 
                        {
                            UInt16 inchange = (UInt16)(IStatus ^ pack.istatus);
                            IStatus = pack.istatus;
                            OStatus = pack.ostatus;
                            Now = DateTime.Now;

                            mTimeGridAdvHelper.AddIStatus(IStatus, Position, Now);

                            IStatusChangedEvent?.Invoke(
                                this,
                                new IStatusChangedEventArgs(
                                    Now,
                                    IStatus,
                                    inchange,
                                    Position,
                                    Position2));
                        }
                        else if(version == 2)
                        {
                            IStatus = pack.istatus;
                            OStatus = pack.ostatus;
                            DateTime dt = mSysTick.ToDateTime(pack.systick);
                            Now = dt;
                            
                            mTimeGridAdvHelper.AddIStatus(IStatus,pack.pos1, Now);


                            IStatusChangedEvent?.Invoke(
                                this,
                                new IStatusChangedEventArgs(
                                    dt,
                                    pack.istatus,
                                    pack.inChange,
                                    pack.pos1,
                                    pack.pos2));
                            
                        }
                    } break;

                case FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_AD2:
                    break;
                case FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_TIMEGRID:
                    {
                        noTGridCnt = 0;//收到timegrid!!!!!!!!!!!

                        int version;
                        FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushTGrid_2 pack = new FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushTGrid_2();

                        if (!pack.TryParse_version(infodata, out version))
                        {
                            break;
                        }


                        DateTime dt;
                        int[] data;
                        //怕数据发送出错
                        if (pack.data.Any(d => (d >> 16) != 0))
                        {
                            //异常
                            goto _end_push_info;
                        }

                        if (version == 1)
                        {
                            //怕数据发送出错
                            dt = DateTime.Now;
                        }
                        else// if (version == 2)
                        {
                            dt = mSysTick.ToDateTime(pack.systick);
                            Now = dt;
                        }
                        data = pack.data;
                        double ts_ms = 1.28;
                        long ticks = (long)((ts_ms * data.Length) * TimeSpan.TicksPerMillisecond);
                        dt = dt - TimeSpan.FromTicks(ticks);

                        if (TimeGridEvent != null)
                        {
                            if (CorrectADs != null)
                            {
                                if (PosOfGrid > 0)
                                {
                                    int grid = Position / PosOfGrid;
                                    int[] d = new int[1];
                                    for (int i = 0; i < data.Count(); i++)
                                    {
                                        d[0] = data[i];
                                        CorrectADs(Misc.DIRECTION.FIX, grid, d);
                                        data[i] = d[0];
                                    }
                                }
                            }

                            TimeGridEvent(
                                this,
                                new TimeGridEventArgs(
                                    dt,
                                    TimeSpan.FromTicks((long)(ts_ms * TimeSpan.TicksPerMillisecond)),
                                    data));
                        }

                        #region 高级 timegrid
                        if (preTimeGrid_start != DateTime.MinValue)
                        {
                            if (TimeGridAdvEvent != null)
                            {
                                var gridAdvUnits = mTimeGridAdvHelper.GetGridAdv(preTimeGrid_start, preTimeGrid_end);
                                
                                if (CorrectADs != null)
                                { 
                                    foreach(var gridAdvUnit in gridAdvUnits)
                                    {
                                        gridAdvUnit.ad = CorrectAD(gridAdvUnit.pos / PosOfGrid, gridAdvUnit.ad);
                                    }
                                }
                                TimeGridAdvEvent(this, new TimeGridAdvEventArgs() { Data = gridAdvUnits });
                            }
                        }

                        mTimeGridAdvHelper.AddAD(data, dt - TimeSpan.FromMilliseconds(ADLag), ts_ms);

                        preTimeGrid_start = dt;
                        preTimeGrid_end = dt + TimeSpan.FromTicks(ticks);
                        #endregion


                    } break;
                case FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_STATUS:
                    {
                        int version;
                        FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushStatus_2 pack = new FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushStatus_2();
                        if (!pack.TryParse_version(infodata, out version))
                            break;
                        if(version == 2)
                        {
                            DriveOrder = pack.order;
                            
                            DriveStatus = pack.status;
                            Marker = pack.marker;
                            DateTime dt = mSysTick.ToDateTime(pack.systick);
                            Now = dt;
                        }
                        else if(version ==1)
                        {
                            DriveOrder = pack.order;
                            DriveStatus = pack.status;
                        }

                    } break;
                case FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_GRID:
                    {
                        int version;
                        FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushGrid_2 pack = new FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushGrid_2();
                        if (!pack.TryParse_version(infodata,out version))
                            break;

                        DateTime dt;
                        int marker;

                        if (version == 2)
                        {
                            dt = mSysTick.ToDateTime(pack.systick);
                            marker = pack.marker;
                        }
                        else
                        {
                            dt = DateTime.Now;
                            marker = Marker;
                        }

                        mTimeGridAdvHelper.NotifyGridIStatusEvent(
                            pack.direction, pack.grid_start * PosOfGrid, pack.data.Length * PosOfGrid, marker, dt, this );
  
                        {
                            int index = 0;
                            if (pack.direction == Misc.DIRECTION.BACKWARD)
                                index = 1;

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

                            int grid_end = pack.data.Length + pack.grid_start - 1;

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

                            int len = grid_end - pack.grid_start + 1;


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

                            //清空其它数据
                            for (int i = 0; i < pack.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(pack.direction, pack.grid_start, pack.data);

                                GridEvent(this, new MiniGridEventArgs()
                                {
                                    direction = pack.direction,
                                    grid_start = pack.grid_start,
                                    posOfGrid = PosOfGrid,
                                    buf = pack.data,
                                    marker = marker
                                });
                            }
                        }

                    } break;
                case FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.PUSH_MINIGRID:
                    {
                        FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushGrid pack = new FLYAD7_OBJ_INTERFACE.PUSH_DATA_INTERFACE.Pack_PushGrid();
                        if (!pack.TryParse(infodata))
                            break;

                        int index = 0;
                        if (pack.direction == Misc.DIRECTION.BACKWARD)
                            index = 1;

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

                        int grid_end = pack.data.Length + pack.grid_start - 1;



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

                        int len = grid_end - pack.grid_start + 1;

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

                        //清空后面的数据
                        if (pack.direction == Misc.DIRECTION.BACKWARD)
                        {
                            for (int i = 0; i < pack.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(pack.direction, pack.grid_start, pack.data);

                        MiniGridEvent?.Invoke(this, new MiniGridEventArgs()
                        {
                            direction = pack.direction,
                            grid_start = pack.grid_start,
                            posOfGrid = PosOfGrid,
                            buf = pack.data,
                            marker = Marker
                        });

                    } break;
            }
        _end_push_info:
            bShieldSetValueEx = false;
        }

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



        #endregion

        /// <summary>
        /// 100ms 周期
        /// </summary>
        private void OnPoll()
        {
            if (!IsConnected)
                return;

            DateTime now = DateTime.Now;

            if (Surplus != 65535)
            {
                if (dt_surplus == DateTime.MinValue)
                {
                    dt_surplus = now - TimeSpan.FromMinutes(40);//再过20分钟，就会 执行 使用时间+1
                }
                else if ((now - dt_surplus).TotalHours >= 1) //每隔一个小时 执行 使用时间+1
                {
                    dt_surplus = now;
                    if (Surplus > 0)
                    {
                        Surplus--;
                        CurrObjSys.CallFunctionEx(conn, FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID, ID,
                            FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.CALL_SURPLUS_SUB,
                            null);
                    }

                }
            }

            if (sysinfo_wait.bNeedGet)
            {
                if ((now - sysinfo_wait.dt).TotalSeconds >= 1)
                {
                    sysinfo_wait.bNeedGet = false;
                    CurrObjSys.GetValueEx(
                        conn,
                        FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID,
                        ID,
                        FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.GET_SYSINFO);

                }
            }

            if (DriveStatus == DRIVE_MAN_STATUS.RUNNING)
            {
                //if (Position == TargetPos) //已经到达位置，问AD卡是不是？
                //{
                //    if ((now - driveman_wait.dt).TotalSeconds >= 1)//每次发送必须间隔1s
                //    {
                //        driveman_wait.Clear();
                //        CurrObjSys.GetValueEx(
                //            conn,
                //            FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                //            ID,
                //            FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_STATE);
                //    }
                //}
                //else 
                if ((now - driveman_wait.dt).TotalSeconds >= 3)
                {
                    driveman_wait.Clear();
                    if (Position == last_position)//当前正在running，但脉冲在3秒内都没变化，问AD卡，现在什么情况。
                    {
                        CurrObjSys.GetValueEx(
                            conn,
                            FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                            ID,
                            FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_STATE);
                    }
                    last_position = Position;
                }
            }

        }
        int last_position = int.MinValue;
        #region IFlyAD 成员
        private bool isconnected;

        public bool IsConnected
        {
            get { return isconnected; }
            protected set
            {
                if (isconnected != value)
                {
                    isconnected = value;
                    NotifyPropertyChanged("IsConnected");
                }
            }
        }
        private int connectcnt = 0;
        public int ConnectCnt
        {
            get { return connectcnt; }
            protected set
            {
                if (connectcnt != value)
                {
                    connectcnt = value;
                    NotifyPropertyChanged("ConnectCnt");
                }
            }
        }

        #endregion

        #region INotifyPropertyChanged 成员
        protected void NotifyPropertyChanged(string propertyName)
        {
            if (this.PropertyChanged != null)
                this.PropertyChanged.Invoke(this, new PropertyChangedEventArgs(propertyName));
        }
        public event PropertyChangedEventHandler PropertyChanged;

        #endregion





        #region IFlyAD 成员
        private IPEndPoint localep = new IPEndPoint(IPAddress.Parse("192.168.251.10"), 20006);
        public IPEndPoint LocalEP
        {
            get
            {
                return localep;
            }
            set
            {
                if (value == null)
                    return;
                if (!IPEndPoint.Equals(localep, value))
                {
                    localep = new IPEndPoint(value.Address, value.Port);
                }
            }
        }

        public DateTime Now { get; set; }

        
        public int Position { get; set; }


        public int Speed { get; set; }

        public int AD { get; set; }

        public int ADMax { get; } = 65535;

        public UInt16 IStatus { get; set; } = 0xffff;


        public UInt16 OStatus { get; set; }

        public void SetOutputBit(int index, bool is1) 
        {
            if (index > 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));
            }
        }
        public void SetOutput(ushort mask, ushort enable)
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.FLYIO_OBJ_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.FLYIO_OBJ_INTERFACE.CALL_OUTPORT,
                new FLYAD7_OBJ_INTERFACE.FLYIO_OBJ_INTERFACE.Pack_CallOutPort()
                {
                    mask = mask,
                    port = enable
                }.ToBytes());
        }

        /// <summary>
        /// 更新参数
        /// </summary>
        public void UpdateParam()
        {
            //OBJ_SYS_DATA
            CurrObjSys.GetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.GET_SYSPARAM);

            CurrObjSys.GetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.GET_ZERO_POS);

            CurrObjSys.GetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.GET_SYSINFO);

            //DRIVE_MAN
            CurrObjSys.GetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_DRIVEPARAM);
        }

        #region 配置参数
        /// <summary>
        /// 配置参数，请不要再用
        /// </summary>
        /// <param name="posOfGrid"></param>
        /// <param name="motortype"></param>
        /// <param name="ratio01"></param>
        /// <param name="ratio02"></param>
        public void SetSysParam(UInt16 posOfGrid, MOTORTYPE motortype, UInt16 ratio01, UInt16 ratio02)
        {
            byte func_enable = 0x0e;
            if (motortype == MOTORTYPE.NULL)
                func_enable = 0x0e;
            else if (motortype == MOTORTYPE.SERVO)
                func_enable = 0x0e;
            else// if (motortype == MOTORTYPE.VF0)
                func_enable = 0x0d;

            CurrObjSys.SetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.SET_SYSPARAM,
                new FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.Pack_SysParam()
                {
                    posOfGrid = posOfGrid,
                    ad1_interval = 1000,
                    ad2_interval = 1000,
                    push_interval = 100,
                    func_setting = 0,
                    func_enable = func_enable,
                    ratio01 = ratio01,
                    ratio02 = ratio02
                }.ToBytes());
            //CurrObjSys.GetValueEx(
            //    conn,
            //    FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID,
            //    ID,
            //    0x1234,
            //    FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.GET_SYSPARAM);
        }

        public MOTORTYPE MotorType { get; set; } = MOTORTYPE.SERVO;

        private UInt16 posofgrid;
        /// <summary>
        /// // 单位 pulse/grid
        /// </summary>
        public UInt16 PosOfGrid 
        {
            get { return posofgrid; }
            set
            {
                if (value < 1)
                    value = 1;
                if (posofgrid != value)
                {
                    posofgrid = value;
                }
            }
        }
        


        public UInt16 Ratio01 { get; set; }

        public UInt16 Ratio02 { get; set; }

        /// <summary>
        /// Speed1 = Velocity * Speed1Scale
        /// </summary>
        public double Speed1Scale => (double)Ratio02 / Ratio01;
        #endregion


        /// <summary>
        /// 脉冲平移
        /// </summary>
        public Int16 PosOffset { get; set; }

        public UInt32 JogVelocity { get; set; }

        #region 速度
        public void SetVelocity(UInt32 velocity) 
        {
            SetPosParam(velocity, 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff);
        }
        public void SetPosParam(UInt32 velocity, UInt32 sv, UInt32 atime, UInt32 dtime, UInt32 hspeed1, UInt32 hspeed2)
        {
            bShieldSetValueEx = true;
            if (velocity != 0xffffffff)
                Velocity = velocity;
            if (sv != 0xffffffff)
                SVelocity = sv;
            if (atime != 0xffffffff)
                ATime = atime;
            if (dtime != 0xffffffff)
                DTime = dtime;
            if (hspeed1 != 0xffffffff)
                HVelocity1 = hspeed1;
            if (hspeed2 != 0xffffffff)
                HVelocity2 = hspeed2;
            bShieldSetValueEx = false;
            CurrObjSys.SetValueEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.SET_DRIVEPARAM,
                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_DriveParam()
                {
                    Velocity = velocity,
                    SV = sv,
                    ATime = atime,
                    DTime = dtime,
                    hspeed1 = hspeed1,
                    hspeed2 = hspeed2
                }.ToBytes());
            //CurrObjSys.GetValueEx(
            //    conn,
            //    FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
            //    ID,
            //    0x1234,
            //    FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.GET_DRIVEPARAM);
        }

        public UInt32 Velocity { get; set; }


        public UInt32 SVelocity { get; set; }

        public UInt32 ATime { get; set; }


        public UInt32 DTime { get; set; }


        public UInt32 HVelocity1 { get; set; }

        public UInt32 HVelocity2 { get; set; }

        #endregion

        #region 密码
        public AREA_STATUS AreaStatus { get; set; }


        public AREA_ERR AreaRet { get; set; }


        public byte[] Code { get; } = new byte[7];


        /// <summary>
        /// //系统剩余时间
        /// </summary>
        public int Surplus { get; set; } = 0xffff;
        /// <summary>
        /// 系统授权码
        /// </summary>
        public byte[] Access { get; } = new byte[8];

        /// <summary>
        /// 输入系统授权码
        /// </summary>
        /// <param name="access"></param>
        public void SetAccess(byte[] access)
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.CALL_ACCESS,
                access);
            sysinfo_wait.bNeedGet = true;
            sysinfo_wait.dt = DateTime.Now;
        }
        /// <summary>
        /// 初始化系统信息区
        /// </summary>
        public void InitArea()
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.CALL_INIT,
                new FLYAD7_OBJ_INTERFACE.SYS_DATA_INTERFACE.Pack_Init().ToBytes());
            sysinfo_wait.bNeedGet = true;
            sysinfo_wait.dt = DateTime.Now;
        }
        #endregion
        #endregion
        #region IFlyAD 成员

        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;
        }
        public void GetGrid(Misc.DIRECTION direction, out int[] dat)
        {
            GetGrid(direction, 0, fGrid.data[0].Length, out dat);
        }
        public event TimeGridEventHandler TimeGridEvent;

        public event MiniGridEventHandler MiniGridEvent;

        public event MiniGridEventHandler GridEvent;

        public event IStatusChangedEventHandler IStatusChangedEvent;

        public void Runto(int to)
        {
            Drive(DRIVE_MAN_ORDER.RUNTO, to);
        }

        public void Origin()
        {
            Drive(DRIVE_MAN_ORDER.ORIGIN);
        }
        public void Stop()
        {
            Drive(DRIVE_MAN_ORDER.STOP);
        }
        public void Backward()
        {
            Drive(DRIVE_MAN_ORDER.BACKWORD);
        }
        public void Forward()
        {
            Drive(DRIVE_MAN_ORDER.FORWORD);
        }

        public DRIVE_MAN_ORDER DriveOrder { get; set; } = DRIVE_MAN_ORDER.IDLE;

        public DRIVE_MAN_STATUS DriveStatus { get; set; }=DRIVE_MAN_STATUS.STOP;


        void Drive(DRIVE_MAN_ORDER order, object param)
        {
            
            switch (order)
            {
                case DRIVE_MAN_ORDER.RUNTO:
                    {
                        int TargetPos = (int)param;
                        if (Position == TargetPos)
                        {
                            //已经到了位置！！！！！， FLYAD7 是没有动作，没有推送的！！！
                            DriveStatus = DRIVE_MAN_STATUS.STOP;
                        }
                        else
                        {
                            DriveStatus = DRIVE_MAN_STATUS.RUNNING;
                            CurrObjSys.CallFunctionEx(
                                conn,
                                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                                ID,
                                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_CallRunto() { to = TargetPos }.ToBytes());

                        }
                    } break;
                case DRIVE_MAN_ORDER.ORIGIN:
                case DRIVE_MAN_ORDER.BACKWORD:
                case DRIVE_MAN_ORDER.FORWORD:
                case DRIVE_MAN_ORDER.STOP:
                    {
                        DriveStatus = DRIVE_MAN_STATUS.RUNNING;
                        CurrObjSys.CallFunctionEx(
                            conn,
                            FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                            ID,
                            FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                            new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_CallPXX() { order = order }.ToBytes());
                    } break;
                case DRIVE_MAN_ORDER.SYNC:
                    {
                        if (param != null)
                        {
                            CurrObjSys.CallFunctionEx(
                                conn,
                                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                                ID,
                                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncBeginWithPos2()
                                {
                                    pos2 = (int)param
                                }.ToBytes());
                        }
                        else
                        {

                            CurrObjSys.CallFunctionEx(
                                conn,
                                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                                ID,
                                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncBegin().ToBytes());
                        }
                    } break;
            }

            driveman_wait.Clear();
        }
        void Drive(DRIVE_MAN_ORDER order)
        {
            Drive(order, null);
        }
        
        public bool IsFinish { get; set; }


        #endregion

        #region 同步

        /// <summary>
        /// 同步用标示
        /// </summary>
        public Int32 Marker { get; protected set; }

        #region 横向脉冲
        /// <summary>
        /// 逻辑横向脉冲偏移  Pos1 + Pos1LCShift = Pos1LC
        /// </summary>
        public int Pos1LCShift { get; set; }

        #endregion
        #region 主轴脉冲

        /// <summary>
        /// 纵向脉冲速度, pps, 1秒脉冲数
        /// </summary>
        public int Speed2 { get; set; }


        /// <summary>
        /// 纵向脉冲，也叫主轴脉冲
        /// </summary>
        public int Position2 { get; set; }

        /// <summary>
        /// 纵向偏移
        /// </summary>
        public void SetPos2Offset(int offset) 
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_Pos2Shift()
                {
                    pos2_shift = offset
                }.ToBytes());
        }

        /// <summary>
        /// 纵向值补偿系数，补偿时，先乘除后加减
        /// </summary>
        public float Pos2Comp { get; set; } = 1;

        /// <summary>
        /// 纵向同步事件，0-1事件
        /// </summary>
        /// <param name="pos2"></param>
        public void SetPos2At01(int pos2,bool immediately)
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_Pos2_01()
                {
                    pos2 = pos2,
                    immediately = immediately
                }.ToBytes());
            LastPos2At01 = pos2;
        }

        #endregion
        #region 同步控制 同步状态转换规则
        /// <summary>
        /// 进入同步状态
        /// </summary>
        /// <param name="pos2"></param>
        public void SyncBegin(int pos2)
        {
            Drive(DRIVE_MAN_ORDER.SYNC, pos2);
            IsSync = true;
        }
        /// <summary>
        /// 进入同步状态
        /// </summary>
        public void SyncBegin()
        {
            Drive(DRIVE_MAN_ORDER.SYNC, null);
            IsSync = true;
        }
        /// <summary>
        /// 退出同步状态
        /// </summary>
        public void SyncEnd()
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncEnd().ToBytes());
            IsSync = false;
        }
        /// <summary>
        /// 清空同步指令
        /// </summary>
        public void SyncClear()
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncClear().ToBytes());
            
            SyncOrders.Clear();

        }
        public void SyncPos2Clear()
        {
            CurrObjSys.CallFunctionEx(
              conn,
              FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
              ID,
              FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
              new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncPos2Clear().ToBytes());
        }
        #endregion
        #region 同步扫描 脚本指令

        #region IFlyADClient 同步

        public bool IsSync { get; protected set; }

        public int LastPos2At01 { get; protected set; }
        /// <summary>
        /// 同步列表，完成后，会删除
        /// </summary>
        public ObservableCollection<SyncOrder> SyncOrders { get; } = new ObservableCollection<SyncOrder>();

        #endregion
        //同步扫描至
        //D+0xE0+开始主轴位置+结束主轴位置+结束横向脉冲位置（逻辑位置）+脉冲开关（1B）+命令识标号（4B）
        public void SyncRunAtLC(int pos2_begin, int pos2_end, int pos1lc, bool hasDataGrid, Int32 marker)
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncRunAtLC()
                {
                    pos2_begin = pos2_begin,
                    pos2_end = pos2_end,
                    pos1lc = pos1lc,
                    hasDataGrid = hasDataGrid,
                    marker = marker
                }.ToBytes());

            SyncOrders.Add(new SyncOrder_SyncRunAtLC()
            {
                Pos2Begin = pos2_begin,
                Pos2End = pos2_end,
                Pos1LC = pos1lc,
                HasDataGrid = hasDataGrid,
                Marker = marker
            });
        }

        //7.2	位于队列头时运行，归零
        //D+0xE1+命令识标号（4B）
        public void SyncOrigin(Int32 marker)
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncOrigin()
                {
                    marker = marker
                }.ToBytes());

            SyncOrders.Add(new SyncOrder_SyncOrigin()
            {
                Marker = marker
            });
        }


        //7.3	位于队列头时运行，以速度运行至物理位置
        //D+0xE2+横向脉冲位置（4B：int32，物理位置）+速度（4B：int32）+脉冲开关（1B）+命令识标号（4B）
        public void SyncRunTo(int pos1, UInt32 velocity, bool hasDataGrid, Int32 marker)
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncRunto()
                {
                    pos1 = pos1,
                    velocity = velocity,
                    hasDataGrid = hasDataGrid,
                    marker = marker
                }.ToBytes());

            SyncOrders.Add(new SyncOrder_SyncRunTo()
            {
                Pos1 = pos1,
                Velocity = velocity,
                HasDataGrid = hasDataGrid,
                Marker = marker
            });
        }

        //7.3	位于队列头时运行，以速度运行至逻辑位置
        //D+0xE3+横向脉冲位置（4B：int32，逻辑位置）+速度（4B：int32）+脉冲开关（1B）+命令识标号（4B）
        public void SyncRunToLC(int pos1lc, UInt32 velocity, bool hasDataGrid, Int32 marker)
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncRuntoLC()
                {
                    pos1 = pos1lc,
                    velocity = velocity,
                    hasDataGrid = hasDataGrid,
                    marker = marker
                }.ToBytes());
            SyncOrders.Add(new SyncOrder_SyncRunToLC()
            {
                Pos1 = pos1lc,
                Velocity = velocity,
                HasDataGrid = hasDataGrid,
                Marker = marker
            });
        }


        //7.4	等待，ms
        //D+0xE4+毫秒数（4B：int32）+命令识标号（4B）
        public void SyncWait(int ms, Int32 marker)
        {
            CurrObjSys.CallFunctionEx(
                conn,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.ID,
                ID,
                FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.CALL_PXX,
                new FLYAD7_OBJ_INTERFACE.DRIVE_MAN_INTERFACE.Pack_SyncWait()
                {
                    ms = ms,
                    marker = marker
                }.ToBytes());
            SyncOrders.Add(new SyncOrder_SyncWait()
            {
                MS = ms,
                Marker = marker
            });
        }


        #endregion
        #endregion


        public CorrectADsHandler CorrectADs { get; set; }


        #region 滞后处理
        //一共有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 推送出去。

        /// <summary>
        /// ad滞后修正  单位ms
        /// </summary>
        public int ADLag { get; set; }
        public event TimeGridAdvEventHandler TimeGridAdvEvent;
        TimeGridAdvHelper mTimeGridAdvHelper = new TimeGridAdvHelper();
        /// <summary>
        /// 上一次接收到timegrid 的时间
        /// </summary>
        DateTime preTimeGrid_start;
        DateTime preTimeGrid_end;
        #endregion
        #region runto 推送 istatus 变化

        public void GridIStatusEventAdd(int istatus_no, GridIStatusEventHander func) 
        {
            mTimeGridAdvHelper.GridIStatusEventAdd(istatus_no, func);
        }
        public void GridIStatusEventDel(int istatus_no, GridIStatusEventHander func) 
        {
            mTimeGridAdvHelper.GridIStatusEventDel(istatus_no, func);
        }


        #endregion
    }

    public partial class FlyAD7: IFlyADClient
    {

        public bool HasCRC { get; set; }

        private int gridsmooth = 0;
        /// <summary>
        /// grid数据平滑
        /// </summary>
        public int GridSmooth
        {
            get { return gridsmooth; }
            set
            {
                if (value < 0)
                    value = 0;

                if (gridsmooth != value)
                {
                    gridsmooth = value;
                }
            }
        }

        #region IFlyADClient
        public void Connect()
        {
            Connect(LocalEP);
        }
        public void Connect(IPEndPoint ep)
        {
            LocalEP = ep;

            if (conn != null)
            {
                if (!conn.RemoteEP.Equals(LocalEP))
                {
                    //断开之前的连接
                    CurrObjSys.Disconnect_to_Another_OBJSys(conn.RemoteEP, ID);
                }
            }

            conn = CurrObjSys.Connect_to_Another_OBJSys(LocalEP, ID);
            conn.HasCRC = HasCRC;
        }
        public void ReConnect()
        {
            if (conn != null)
            {
                //断开之前的连接
                CurrObjSys.Disconnect_to_Another_OBJSys(conn.RemoteEP, ID);
            }

            conn = CurrObjSys.Connect_to_Another_OBJSys(LocalEP, ID);
            conn.HasCRC = HasCRC;
        }
        #endregion


        public int PosLen { get; set; } = 8900;

        public void RuntoMin()
        {
            Runto(0);
        }

        public void RuntoMax()
        {
            Runto(PosLen);
        }
    }


}
