using ConfigHelper;
using Newtonsoft.Json;
using OnlineStore;
using OnlineStore.Common;
using OnlineStore.LoadCSVLibrary;
using RemoteSheardObject;
using Robot.UR;
using System;
using System.Collections.Generic;
using System.Collections.Specialized;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace DeviceLibrary
{
    public class MI : DeviceBase, IDevice
    {
        public static Dictionary<string, MI> DeviceList = new Dictionary<string, MI>();

        public static void Init(Robot_Config config, Dictionary<string, DeviceGroup> devices,out string msg) {
            msg = "";
            MI sideMove = new MI(devices["AMH-MI1"], out string m);
            DeviceList.Add(devices["AMH-MI1"].GroupName, sideMove);
            msg += m;
            
        }
        DeviceGroup DeviceGroup;
        public MoveInfo MoveInfo;
        MoveInfo RoboMoveInfo;
        //CylinderManger Location;
        public AxisBean Comp;
        public AxisBean Rotate;
        URRobotControl Robot;
        RobotHelper robotHelper;
        string POS_Start = "MI1";
        public MI(DeviceGroup device,out string msg) : base()
        {
            msg = "";
            Name = device.Name+"("+ device.GroupName + ")";
            DeviceGroup = device;
            GroupName = DeviceGroup.GroupName;
            Msg = new MsgService(GroupName+"-T");
            MoveInfo = new MoveInfo(GroupName + "-T");
            RoboMoveInfo = new MoveInfo(GroupName + "-Robo");
            if (device.GroupName == "AMH-MI1")
            {
                Robot = RobotManage.Robot_MI1;
            }
            else
            {
                Robot = RobotManage.Robot_MI2;
                POS_Start = "MI2";
            }
            robotHelper = new RobotHelper(Robot);
            Comp = AxisBean.List[DeviceGroup.GroupName][0];
            Rotate = AxisBean.List[DeviceGroup.GroupName][1];

            
        }
        

        public DeviceStateE DeviceState { get; set; }
        public void Start()
        {
            OpenAllServo();
            robotHelper.Start();
            DeviceState = DeviceStateE.HomeReset;
            MoveInfo.NewMove(MoveStep.H01_HomeReset);
            MoveInfo.log("开始回原");
        }

        public void Stop()
        {
            robotHelper.Stop();
            Robot.StopRobot();
            DeviceState = DeviceStateE.Stop;
            MoveInfo.log("停止运行");
        }
        public void Process()
        {
            try
            {
                MsgService.MSList[GroupName + "-T"].clear();
                LogUtil.OutputDebugString("DeviceState:" + DeviceState.ToString());

                if (!DeviceCheck())
                {
                    return;
                }
                if (DeviceState == DeviceStateE.HomeReset)
                    ResetProcess();
                else if (DeviceState == DeviceStateE.Run)
                {                    
                    OneWayProcess();
                    RobotMoveProcess();
                }
            }
            finally {
                MsgService.MSList[GroupName + "-T"].Show();
            }
        }
        private bool DeviceCheck()
        {

            if (!Robot.ClientIsConnected)
            {
                Msg.add(POS_Start + crc.GetString("Res0011", "机器人未连接MT,无法操作."), MsgLevel.alarm);
                return false;
            }
            if (!robotHelper.RobotStatus) {
                Msg.add(POS_Start + "机器人当前不可用", MsgLevel.alarm);
                return false;
            }
            return true;
        }

        int isalivetrytimes = 0;
        int StopBufDelayMS = 500;
        int StopDelayMS = 1500;

        RobotPosition FromPos;
        RobotPosition ToPos;
        VStore CurrentVStore;
        JobInfo CurrentJobInfo;

        VStore CurrntOutReadyStore;
        public void OneWayProcess()
        {
            Config.Set("RuntimeRobot_MoveStep", MoveInfo.MoveStep.ToString());
            if (CheckWait(MoveInfo))
                return;
            
            switch (MoveInfo.MoveStep)
            {
                case MoveStep.Wait:
                    LogUtil.OutputDebugString("InStoreJobList:" + InStoreJobList.Count.ToString());
                    if (InStoreJob != null)
                    {
                        CurrentJobInfo = InStoreJob.Clone();
                        InStoreJob = null;
                        MoveInfo.log("收到物料入库任务:" + CurrentJobInfo.ToStr());
                        MoveInfo.MoveParam.WareCode = CurrentJobInfo.WareNum;
                        MoveInfo.MoveParam.PosID = CurrentJobInfo.PosId;
                        MoveInfo.MoveParam.PlateW = CurrentJobInfo.plateW;
                        MoveInfo.MoveParam.PlateH = CurrentJobInfo.plateH;
                        MoveInfo.MoveParam.cid = CurrentJobInfo.CID;
                        MoveInfo.NextMoveStep(MoveStep.MI_01);
                        FromPos = GetMIPosition(GroupName, POS_Start + "_MT");
                        var cid = MoveInfo.MoveParam.PosID;//.Split('_')[0];
                        CurrentVStore = VStoreCollection.VStoreList[cid];
                        ToPos = GetMIPosition(GroupName, TowerList.List[cid].PosID);
                        Setting_Init.Runtime_Robot_ReelInfo = JsonConvert.SerializeObject(MoveInfo.MoveParam);
                        Setting_Init.Runtime_Robot_FromPos = FromPos.PositionNum;
                        Setting_Init.Runtime_Robot_ToPos = ToPos.PositionNum;

                        MoveInfo.log($"开始转移物料:{FromPos.PositionNum}=>{ToPos.PositionNum}");
                    }
                    break;
                case MoveStep.MI_01:
                    MoveInfo.NextMoveStep(MoveStep.MI_02);
                    //Location.ToHigh(MoveInfo);
                    RoboMoveInfo.MoveParam = MoveInfo.MoveParam.clone();
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_01);
                    MoveInfo.log("机器人开始取料");
                    break;
                case MoveStep.MI_02:
                    if (RoboMoveInfo.MoveStep >= MoveStep.MI_40)
                    {
                        MoveInfo.NextMoveStep(MoveStep.MI_03);
                        if (FromPos.PositionNum.EndsWith("MT"))
                        {
                            RemoteLoad remoteLoad = new RemoteLoad();
                            remoteLoad.Action = "";
                            remoteLoad.GroupName = GroupName;
                            remoteLoad.RequestLoadInfo = new RequestLoadInfo();
                            remoteLoad.RequestLoadInfo.DeviceGroupName = GroupName;
                            remoteLoad.RequestLoadInfo.IsEmpty = true;
                            remoteLoad.RequestLoadInfo.LoadParam = RoboMoveInfo.MoveParam.clone();
                            TrayManager.TrayRelease(remoteLoad);
                            TrayStop.DeviceList[GroupName].TrayRelease();
                            RoboMoveInfo.log("取料完成托盘放行");
                            TheLine.UpdateLocInfo("", remoteLoad.RequestLoadInfo.LoadParam.WareCode, TheLine.LineStatusE.INROBOT, GroupName);
                        }
                        else
                            RoboMoveInfo.log("取料完成");

                    }
                    break;
                case MoveStep.MI_03:
                    if (RoboMoveInfo.MoveStep >= MoveStep.MI_44)
                    {
                        MoveInfo.NextMoveStep(MoveStep.MI_04);
                        CurrentVStore.RequestInStore(new JobInfo(RoboMoveInfo.MoveParam.WareCode, "", RoboMoveInfo.MoveParam.PlateW, RoboMoveInfo.MoveParam.PlateH));

                    }
                    break;
                case MoveStep.MI_04:
                    if (RoboMoveInfo.MoveStep == MoveStep.Wait)
                    {
                        MoveInfo.log("放入料仓口完成:"+ RoboMoveInfo.MoveParam.ToStr());
                        MoveInfo.NewMove(MoveStep.Wait);
                    }
                    break;

                case MoveStep.MI_50:
                    CurrntOutReadyStore = VStoreCollection.VStoreList.Values.ToList().Find(vs => vs.RTStoreStatus == RTStoreStatus.OutStoreReady);
                    if (CurrntOutReadyStore != null)
                    {
                        if (string.IsNullOrEmpty(CurrntOutReadyStore.CurrentTerminalReelID))
                        {
                            MoveInfo.log("当前出库料仓:" + CurrntOutReadyStore.CID + "已是出库完成状态,但是还没有读到物料完成通知");
                            return;
                        }
                        if (!VStoreCollection.OutStoreReelInfo.ContainsKey(CurrntOutReadyStore.CurrentTerminalReelID))
                        {
                            MoveInfo.log("当前出库料仓:" + CurrntOutReadyStore.CID + ",没有找到料盘信息:" + CurrntOutReadyStore.CurrentTerminalReelID);
                            var result = TheLine.GetReelSize<ResultData>(CurrntOutReadyStore.CurrentTerminalReelID);
                            if (result != null && result.code == 0)
                            {
                                CurrentJobInfo = new JobInfo(CurrntOutReadyStore.CurrentTerminalReelID, "", int.Parse(result.data["plateW"]), int.Parse(result.data["plateH"]));
                                MoveInfo.log("从服务器检索到物料数据:" + CurrentJobInfo.ToStr());

                            }
                            else
                                return;
                        }
                        else
                        {
                            var job = VStoreCollection.OutStoreReelInfo[CurrntOutReadyStore.CurrentTerminalReelID];
                            CurrentJobInfo = job.Clone();
                        }
                    }
                    else
                    {
                        var InStoreError = VStoreCollection.VStoreList.Values.ToList().Find(vs => vs.RTStoreStatus == RTStoreStatus.InStoreError ||vs.RTStoreStatus == RTStoreStatus.InStoreDataTimeOut);

                        if (InStoreError != null)
                        {
                            var result = TheLine.GetReelSize<ResultData>(InStoreError.CurrentTerminalReelID);
                            if (result != null && result.code == 0)
                            {
                                CurrntOutReadyStore = InStoreError;
                                InStoreError.ServerCM.cancelPutInTask(InStoreError.CurrentTerminalReelID);
                                CurrentJobInfo = new JobInfo(InStoreError.CurrentTerminalReelID, "", int.Parse(result.data["plateW"]), int.Parse(result.data["plateH"]));
                                InStoreError.InStoreJobInfo = null;
                                MoveInfo.log("从服务器检索到物料数据:" + CurrentJobInfo.ToStr());
                                MoveInfo.MoveParam.IsNg = true;
                                MoveInfo.MoveParam.NgMsg = InStoreError.CID + "入库异常";
                                MoveInfo.log("检测到有料仓入库NG:" + InStoreError.CID);
                            }
                            else
                            {
                                MoveInfo.log("没有找到,在等待出库的料仓");
                                MoveInfo.NextMoveStep(MoveStep.Wait);
                                TrayStop.DeviceList[GroupName].TrayRelease();
                                return; }
                            
                        }
                        else
                        {
                            MoveInfo.log("没有找到,在等待出库的料仓");
                            MoveInfo.NextMoveStep(MoveStep.Wait);
                            TrayStop.DeviceList[GroupName].TrayRelease();
                            return;
                        }
                    } 
                    MoveInfo.log("收到物料出库任务:" + CurrentJobInfo.ToStr());
                    MoveInfo.MoveParam.WareCode = CurrentJobInfo.WareNum;
                    MoveInfo.MoveParam.PosID = CurrentJobInfo.PosId;
                    MoveInfo.MoveParam.PlateW = CurrentJobInfo.plateW;
                    MoveInfo.MoveParam.PlateH = CurrentJobInfo.plateH;
                    MoveInfo.MoveParam.cid = CurrntOutReadyStore.CID;
                    FromPos = GetMIPosition(GroupName, TowerList.List[MoveInfo.MoveParam.cid].PosID);
                    ToPos = GetMIPosition(GroupName, POS_Start + "_MT");
                    Setting_Init.Runtime_Robot_ReelInfo = JsonConvert.SerializeObject(MoveInfo.MoveParam);
                    Setting_Init.Runtime_Robot_FromPos = FromPos.PositionNum;
                    Setting_Init.Runtime_Robot_ToPos = ToPos.PositionNum;
                    MoveInfo.NextMoveStep(MoveStep.MI_51);
                    MoveInfo.log($"开始转移物料:{FromPos.PositionNum}=>{ToPos.PositionNum}");                    
                    break;
                case MoveStep.MI_51:
                    MoveInfo.NextMoveStep(MoveStep.MI_52);
                    RoboMoveInfo.MoveParam = MoveInfo.MoveParam.clone();
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_01);
                    MoveInfo.log("机器人开始取放料");                    
                    break;
                case MoveStep.MI_52:
                    if (RoboMoveInfo.MoveStep >= MoveStep.MI_40)
                    {
                        MoveInfo.NextMoveStep(MoveStep.MI_53);
                        lock (VStoreCollection.OutStoreReelInfo)
                        {
                            VStoreCollection.OutStoreReelInfo.Remove(MoveInfo.MoveParam.WareCode);
                            //CurrntOutReadyStore.CurrentTerminalReelID = "";
                        }
                        TheLine.UpdateLocInfo("", MoveInfo.MoveParam.WareCode, TheLine.LineStatusE.INROBOT, GroupName);

                        MoveInfo.log("机器人已取到料");
                    }
                    break;
                case MoveStep.MI_53:                    
                    if (RoboMoveInfo.MoveStep >= MoveStep.MI_44)
                    {                        
                        MoveInfo.log("机器人完成放料");
                        RemoteLoad remoteLoad = new RemoteLoad();
                        remoteLoad.GroupName = "AMH-" + POS_Start;
                        remoteLoad.RequestLoadInfo = new RequestLoadInfo();
                        remoteLoad.RequestLoadInfo.DeviceGroupName = "AMH-ML5-2";
                        remoteLoad.RequestLoadInfo.IsEmpty = false;
                        remoteLoad.RequestLoadInfo.TrayType = TrayTypeE.MTP1.ToString();
                        remoteLoad.RequestLoadInfo.LoadParam = MoveInfo.MoveParam.clone();// 填充料盘信息
                        TrayManager.TrayRelease(remoteLoad); //这条会自动放行托盘并且写入托盘目的地
                        TheLine.UpdateLocInfo("", MoveInfo.MoveParam.WareCode, TheLine.LineStatusE.INLINE, CurrrentRFID);

                        MoveInfo.NextMoveStep(MoveStep.MI_54);
                    }
                    break;
                case MoveStep.MI_54:
                    if (RoboMoveInfo.MoveStep == MoveStep.Wait)
                    {
                        MoveInfo.log("机器人任务结束");
                        MoveInfo.NewMove(MoveStep.Wait);
                    }
                    break;
            }
            
        }


        public void RobotMoveProcess()
        {
            Config.Set("RuntimeRobot_Arm_MoveStep", RoboMoveInfo.MoveStep.ToString());

            if (CheckWait(RoboMoveInfo))
                return;
            switch (RoboMoveInfo.MoveStep)
            {
                case MoveStep.Wait:
                    break;
                //机器人取料
                case MoveStep.MI_01:
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_02);
                    CompTOP1(RoboMoveInfo);
                    RoboMoveInfo.log("压紧轴到待机点");
                    break;
                case MoveStep.MI_02:
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_03);
                    robotHelper.Move(RoboMoveInfo, FromPos.Take_P5);
                    RoboMoveInfo.log("机器人到Take_P5");
                    break;
                case MoveStep.MI_03:
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_04);
                    TrayStop.DeviceList[GroupName].Location.ToLow(MoveInfo);
                    CompTOCompression(RoboMoveInfo);
                    RoboMoveInfo.log("压紧轴都下压");
                    break;
                case MoveStep.MI_04:
                    if (!Comp.IsBusy)
                    {
                        RoboMoveInfo.NextMoveStep(MoveStep.MI_05);
                        robotHelper.Move(RoboMoveInfo, FromPos.Take_P6);
                        RoboMoveInfo.log("压紧轴到位");
                        RoboMoveInfo.log("机器人到Take_P6");
                    }
                    break;
                case MoveStep.MI_05:

                    RoboMoveInfo.NextMoveStep(MoveStep.MI_06);
                    CompTOCompression(RoboMoveInfo);
                    RoboMoveInfo.log("压紧轴都下压");

                    break;
                case MoveStep.MI_06:
                    if (!Comp.IsBusy)
                    {
                        RoboMoveInfo.log("压紧轴到位脉冲值:" + Comp.GetAclPosition());
                        RoboMoveInfo.NextMoveStep(MoveStep.MI_07);
                        robotHelper.Move(RoboMoveInfo, FromPos.Take_P7);
                        RoboMoveInfo.log("机器人到Take_P7");
                    }
                    break;
                case MoveStep.MI_07:
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_08);
                    robotHelper.Move(RoboMoveInfo, FromPos.P1);
                    RoboMoveInfo.log("机器人到P1");
                    break;
                case MoveStep.MI_08:
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_40);
                    RoboMoveInfo.log("机器人取料完成");
                    break;

                //放料到缓存位置
                case MoveStep.MI_40:
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_41);
                    robotHelper.Move(RoboMoveInfo, ToPos.Put_P2);
                    RoboMoveInfo.log("机器人到Put_P2");
                    break;
                case MoveStep.MI_41:
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_42);
                    //Comp.AbsMove(RoboMoveInfo, RobotManage.Config.AMH_RoboMI1_Comp_P1, RobotManage.Config.AMH_RoboMI1_Comp_P1_speed);
                    CompTOP1(RoboMoveInfo);
                    robotHelper.Move(RoboMoveInfo, ToPos.Put_P3);
                    RoboMoveInfo.log("压紧轴释放");
                    RoboMoveInfo.log("机器人到Put_P3");
                    break;
                case MoveStep.MI_42:
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_43);
                    robotHelper.Move(RoboMoveInfo, ToPos.Put_P4);
                    RoboMoveInfo.log("机器人到Put_P4");
                    break;
                case MoveStep.MI_43:
                    RoboMoveInfo.NextMoveStep(MoveStep.MI_44);
                    robotHelper.Move(RoboMoveInfo, ToPos.P1);
                    RoboMoveInfo.log("机器人到P1");
                    break;
                case MoveStep.MI_44:
                    RoboMoveInfo.NextMoveStep(MoveStep.Wait);
                    RoboMoveInfo.log("放料完成");
                    break;
            }
            
        }



        StoreJobList InStoreJobList = new StoreJobList(crc.GetString("Res0013","入库"));
        volatile JobInfo InStoreJob;
        StoreJobList OutStoreJobList = new StoreJobList(crc.GetString("Res0015","出库"));
        internal void StartInStore(ReelParam trayParam)
        {
            //PosID== 料仓CID
            InStoreJob = new JobInfo(trayParam.WareCode, trayParam.PosID, trayParam.PlateW, trayParam.PlateH);
            InStoreJob.CID = trayParam.PosID;
            InStoreJobList.Enqueue(new JobInfo(trayParam.WareCode, trayParam.PosID, trayParam.PlateW, trayParam.PlateH));
            MoveInfo.log("写入入库队列:" + trayParam.ToStr());
        }
        internal void EnqueueOutStore(JobInfo trayParam)
        {
            OutStoreJobList.Enqueue(trayParam.Clone());
            MoveInfo.log("写入出库队列:" + trayParam.ToStr());
        }
        internal void StartOutStore()
        {
            MoveInfo.NewMove(MoveStep.MI_50);
            MoveInfo.log("开始出库");
        }
        /// <summary>
        /// 释放托盘
        /// </summary>
        public void TrayRelease() {
            //MoveInfo.log("释放托盘");
           // MoveInfo.NextMoveStep(MoveStep.TrayStop_LoadProcessed);
        }
        void CompTOP1(MoveInfo MoveInfo)
        {
            if (Comp.HasHome())
                Comp.AbsMove(MoveInfo, RobotManage.Config.AMH_RoboMI1_Comp_P1, RobotManage.Config.AMH_RoboMI1_Comp_P1_speed);
            else
                Comp.HomeMove(MoveInfo);
        }
        void CompTOCompression(MoveInfo MoveInfo)
        {
            if (!Comp.HasHome())
                return;

            Comp.AbsMove(null, DeviceGroup.p3, RobotManage.Config.AMH_RoboMI1_Comp_P2_speed);
            Comp.BatchAxisStartCheck(IO_Type.MI_Robot_Clamp_Check, IO_VALUE.HIGH);
        }
        bool compdetereel = false;
        int lastCmd = 0;
        int lastStep = 0;
        int lastPrefix = 0;
        public void ResetProcess()
        {
            if (CheckWait(MoveInfo))
                return;

            switch (MoveInfo.MoveStep)
            {
                case MoveStep.Wait:
                    //MoveInfo.NextMoveStep(MoveStep.H01_HomeReset);
                    
                    break;
                case MoveStep.H01_HomeReset:
                    var m1step = (MoveStep)Enum.Parse(typeof(MoveStep), Config.Get("RuntimeRobot_MoveStep"));
                    var m2step = (MoveStep)Enum.Parse(typeof(MoveStep), Config.Get("RuntimeRobot_Arm_MoveStep"));
                    if (IOValue(IO_Type.MI_Robot_Clamp_Check).Equals(IO_VALUE.LOW)
                        && IOValue(IO_Type.MI_Robot_Reel_Check).Equals(IO_VALUE.LOW)
                        && m1step == MoveStep.Wait)
                    {
                        MoveInfo.NextMoveStep(MoveStep.H09_HomeReset);
                        Robot.SendMoveCmd(1, Setting_Init.URRobot_MI1_Speed_Rate);
                        MoveInfo.log("没有最后的动作, 机器人回待机点");
                    }
                    else
                    {
                        MoveInfo.MoveParam = JsonConvert.DeserializeObject<ReelParam>(Setting_Init.Runtime_Robot_ReelInfo);
                        RoboMoveInfo.MoveParam = MoveInfo.MoveParam.clone();
                        FromPos = GetMIPosition(GroupName, Setting_Init.Runtime_Robot_FromPos);
                        ToPos = GetMIPosition(GroupName, Setting_Init.Runtime_Robot_ToPos);
                        CurrentVStore = VStoreCollection.VStoreList[MoveInfo.MoveParam.cid];
                        MoveInfo.NextMoveStep(m1step);
                        RoboMoveInfo.NextMoveStep(m2step);
                        DeviceState = DeviceStateE.Run;
                    }
                    break;
                case MoveStep.H02_HomeReset:                    
                    if (!Robot.StartRobot()) {
                        MoveInfo.CanWhileCount--;
                        MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(5000));
                        MoveInfo.WaitList.Add(WaitResultInfo.WaitMsg(POS_Start + crc.GetString("Res0016","机器人连接失败"), MoveInfo.CanWhileCount<=0 ? MsgLevel.alarm: MsgLevel.warning));
                    }else
                        MoveInfo.NextMoveStep(MoveStep.H03_HomeReset);
                    break;
                case MoveStep.H03_HomeReset:
                    
                    if (Robot.IsRun)
                    {
                        MoveInfo.NextMoveStep(MoveStep.H04_HomeReset);
                        Robot.PlayProgram();
                        MoveInfo.log("Robot.ProgramState:" + Robot.ProgramState);
                        MoveInfo.log("Robot.ClientIsConnected:" + Robot.ClientIsConnected);
                        MoveInfo.log("Robot.IsRun:" + Robot.IsRun);
                        MoveInfo.log("Robot.IsStartConnect:" + Robot.IsStartConnect);
                        MoveInfo.log("Robot.CurCmdReponse:" + Robot.CurCmdReponse);
                        MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(2000));

                    } else if (MoveInfo.IsTimeOut(30)) {
                        MoveInfo.NextMoveStep(MoveStep.H02_HomeReset);
                    }
                    MoveInfo.WaitList.Add(WaitResultInfo.WaitMsg(POS_Start + crc.GetString("Res0017", "机器人启动失败"), MoveInfo.CanWhileCount <= 0 ? MsgLevel.alarm : MsgLevel.warning));
                    break;
                case MoveStep.H04_HomeReset:                    
                    if (Robot.ClientIsConnected && Robot.CurCmdReponse.Contains("Start Program"))
                    {
                        MoveInfo.NextMoveStep(MoveStep.H05_HomeReset);
                    }
                    else if (MoveInfo.IsTimeOut(10))
                    {
                        MoveInfo.NextMoveStep(MoveStep.H10_HomeReset);
                    }
                    MoveInfo.WaitList.Add(WaitResultInfo.WaitMsg(POS_Start + crc.GetString("Res0018", "机器人未连接MT"), MoveInfo.CanWhileCount <= 0 ? MsgLevel.alarm : MsgLevel.warning));
                    break;
                case MoveStep.H05_HomeReset:
                    MoveInfo.NextMoveStep(MoveStep.H09_HomeReset);
                    return;
                    m1step = (MoveStep)Enum.Parse(typeof(MoveStep), Setting_Init.RuntimeRobot_MoveStep);
                    m2step = (MoveStep)Enum.Parse(typeof(MoveStep), Setting_Init.RuntimeRobot_Arm_MoveStep);
                    if (IOValue(IO_Type.MI_Robot_Clamp_Check).Equals(IO_VALUE.LOW)
                        && IOValue(IO_Type.MI_Robot_Reel_Check).Equals(IO_VALUE.LOW)
                        && m1step == MoveStep.Wait)
                    {
                        MoveInfo.NextMoveStep(MoveStep.H09_HomeReset);
                        Robot.SendMoveCmd(1, Setting_Init.URRobot_MI1_Speed_Rate);
                        MoveInfo.log("没有最后的动作, 机器人回待机点");
                    }
                    else
                    {
                        MoveInfo.MoveParam = JsonConvert.DeserializeObject<ReelParam>(Setting_Init.Runtime_Robot_ReelInfo);
                        FromPos = GetMIPosition(GroupName, Setting_Init.Runtime_Robot_FromPos);
                        ToPos = GetMIPosition(GroupName, Setting_Init.Runtime_Robot_ToPos);
                        CurrentVStore = VStoreCollection.VStoreList[MoveInfo.MoveParam.cid];
                        MoveInfo.NextMoveStep(m1step);
                        RoboMoveInfo.NextMoveStep(m2step);
                        DeviceState = DeviceStateE.Run;
                    }
                    //else if (int.TryParse(Robot.LastMoveCmd, out int cmdid))
                    //{
                    //    lastCmd = cmdid;
                    //    lastStep = cmdid % 10;
                    //    lastPrefix = lastStep * 10;
                    //    Robot.SendMoveCmd(lastCmd, Setting_Init.URRobot_MI1_Speed_Rate);
                    //    MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(1500));
                    //    MoveInfo.log("机器人重做最后的动作:" + lastCmd);

                    //}                    
                    break;
                case MoveStep.H06_HomeReset:
                    if (Robot.MoveCmdOk())
                    {
                        //如果上一步是回到待机点,并且料叉上无物料
                        if (lastStep==1 && !compdetereel && !IOValue(IO_Type.MI_Robot_Reel_Check).Equals(IO_VALUE.HIGH))
                        {
                            //回原结束
                            return;
                        }
                        RoboMoveInfo.MoveParam = MoveInfo.MoveParam.clone();

                        //如果上一步是回待机点,但料叉上有物料
                        if (lastStep == 1)
                        {
                            MoveInfo.NextMoveStep(MoveStep.MI_02);
                            RoboMoveInfo.NextMoveStep(MoveStep.MI_07);
                        }
                        else if (lastStep == 0)//P5 取料低点
                        {
                            MoveInfo.NextMoveStep(MoveStep.MI_02);
                            RoboMoveInfo.NextMoveStep(MoveStep.MI_02);
                        }
                        else if (lastStep == 2)//P6 取料高点
                        {
                            MoveInfo.NextMoveStep(MoveStep.MI_02);
                            RoboMoveInfo.NextMoveStep(MoveStep.MI_04);
                        }
                        else if (lastStep == 5)//P7 取料退出过度点
                        {
                            MoveInfo.NextMoveStep(MoveStep.MI_02);
                            RoboMoveInfo.NextMoveStep(MoveStep.MI_06);
                        }
                        else if (lastStep == 1)//P2 放料高点P2
                        {
                            MoveInfo.NextMoveStep(MoveStep.MI_02);
                            RoboMoveInfo.NextMoveStep(MoveStep.MI_40);
                        }
                    }
                    break;
                case MoveStep.H09_HomeReset:
                    if (Robot.MoveCmdOk())
                    {
                        MoveInfo.EndMove();
                        DeviceState = DeviceStateE.Run;
                    }
                    break;

                    //尝试解除安全保护
                case MoveStep.H10_HomeReset:
                    MoveInfo.NextMoveStep(MoveStep.H11_HomeReset);
                    Robot.SendCMD("safetymode", 0);
                    MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(500));
                    break;
                case MoveStep.H11_HomeReset:                    
                    MoveInfo.log("机器人反馈安全模式为:" + Robot.CurDashboardReponse);
                    if (Robot.CurDashboardReponse.Contains("PROTECTIVE_STOP"))
                    {
                        MoveInfo.NextMoveStep(MoveStep.H12_HomeReset);
                        MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(5000));
                    }
                    else if (Robot.CurDashboardReponse.Contains("NORMAL")) {
                        MoveInfo.NextMoveStep(MoveStep.H02_HomeReset);
                    }
                    else if (Robot.CurDashboardReponse.Contains("Starting program"))
                    {
                        MoveInfo.NextMoveStep(MoveStep.H02_HomeReset);
                    }
                    else {
                        Msg.add("机器人状态异常:" + Robot.CurDashboardReponse, MsgLevel.alarm);
                    }
                        break;
                case MoveStep.H12_HomeReset:
                    MoveInfo.NextMoveStep(MoveStep.H13_HomeReset);
                    MoveInfo.log("机器人反馈安全模式为:"+Robot.CurCmdReponse);
                    Robot.SendCMD("unlock protective stop", 0);
                    MoveInfo.log("尝试解除机器人保护");
                    MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(500));
                    break;
                case MoveStep.H13_HomeReset:
                    if (Robot.CurCmdReponse.Contains("Protective stop releasing")) {
                        MoveInfo.log("机器人成功解除保护");
                        Robot.StopProgram();
                        MoveInfo.NextMoveStep(MoveStep.H02_HomeReset);
                        MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(500));
                    }
                    else if (MoveInfo.IsTimeOut(5))
                    {
                        MoveInfo.NextMoveStep(MoveStep.H10_HomeReset);
                    }                    
                    break;
            }
        }

        /// <summary>
        /// 设备是否有任务
        /// </summary>
        /// <param name="reelParam"></param>
        /// <returns></returns>
        public bool HasJob(ReelParam reelParam)
        {
            if (DeviceState != DeviceStateE.Run)
                return false;
            if (MoveInfo.MoveStep != MoveStep.Wait)
                return false;

            if (reelParam == null)
            {
                //空托盘,判断有没有出库任务
                var sc = VStoreCollection.VStoreList.Values.ToList().Find(vs=>vs.RTStoreStatus==RTStoreStatus.OutStoreReady || vs.RTStoreStatus == RTStoreStatus.InStoreError);
                return (sc != null);
            }

            if (!VStoreCollection.VStoreList.ContainsKey(reelParam.SubCID)) {
                MoveInfo.log("RT料仓CID不存在:"+ reelParam.SubCID);
                return false;
            }
            //有料托盘,判断目的地料仓是否空闲
            if (VStoreCollection.VStoreList[reelParam.SubCID].RTStoreStatus == RTStoreStatus.Ready)
            {
                VStoreCollection.VStoreList[reelParam.SubCID].RTStoreStatus = RTStoreStatus.LockToInStore;
                return true;
            }
            MoveInfo.log("RT料仓当前状态:" + VStoreCollection.VStoreList[reelParam.SubCID].RTStoreStatus);

            return false;
        }
        int bufferstoresindex = 0;
        string[] bufferstores = new string[] { "B01", "B02", "B03", "B04", "B05", "B06", "B07", "B08" };

        string GetNextBufferStore() {
            if (bufferstoresindex >= bufferstores.Length)
                bufferstoresindex = 0;

            var c = POS_Start + "_"+bufferstores[bufferstoresindex];
            bufferstoresindex++;

            return c;
        }

        public bool FrontCheck(int curaddr)
        {
            throw new NotImplementedException();
        }

        public bool IsFree()
        {
            throw new NotImplementedException();
        }
        public RobotPosition GetMIPosition(string device, string pos) {

            if (device == "AMH-MI1")
                return RobotManage.MI1Postion[pos];
            else if (device == "AMH-MI2")
                return RobotManage.MI2Postion[pos];
            else
                return RobotManage.CIPostion[pos];

        }
    }
}