using Aitex.Core.RT.Log;
using Aitex.Core.RT.SCCore;
using Aitex.Sorter.Common;
using MECF.Framework.Common.CommonData;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Text.RegularExpressions;
using System.Threading.Tasks;
using Venus_Core;
using Venus_RT.Devices.VCE;

namespace Venus_RT.Devices.TM
{
    public class SunWayRobot : ITransferRobot
    {
        private readonly AsyncSocket _socket;
        private const string EOF = "\r";
        private RState _status;
        private bool _IsHomed;
        private VRStep _currentStep = VRStep.Idle;
        private Dictionary<string, int> _StationNumbers = new Dictionary<string, int>();
        public RState Status { get { return _status; } }
        public bool IsHomed { get { return _IsHomed; } }
        private string Hand2Arm(Hand hand) => hand == Hand.Blade2 ? "B" : "A";

        //private readonly Regex _rex_check_load = new Regex(@"LOAD\s+(A|B)\s+(\w+)\s*");
        private readonly Regex _rex_check_load = new Regex(@"CHECK LOAD\s*");
        private readonly Regex _rex_rq_load_A = new Regex(@"LOAD\sA\s.*");
        private readonly Regex _rex_rq_load_B = new Regex(@"LOAD\sB\s.*");
        private readonly Regex _rex_error_code = new Regex(@"_ERR\s+(\d+)\s*");
        private readonly Regex _rex_rq_speed = new Regex(@"MOTIONPARA.*");
        private RobotMoveInfo _robotMoveInfo = new RobotMoveInfo();
        private string _WithWaferSpeed = "";
        private string _NoWaferSpeed = "";
        public string WithWaferSpeed => _WithWaferSpeed;
        public string NoWaferSpeed => _NoWaferSpeed;
        public RobotMoveInfo TMRobotMoveInfo { get { return _robotMoveInfo; } }

        public double Offset_X => 0;

        public double Offset_Y => 0;

        public double Offset_D => 0;

        public Dictionary<string, string> _error2msg = new Dictionary<string, string>()
        {
            {"105"  , "数据解析错误" },
            {"221"  , "手臂选择无效,检查指令中的对应参数" },
            //{"222"  , "无效的 Pan 参数"},
            {"233"  , "伸缩信号未使能,检查传输腔传感器信号是否正常" },
            {"304"  , "未识别的错误码" },
            {"305"  , "无法识别的命令,检查指令" },
            {"309"  , "不支持的指令,检查指令" },
            //{"350"  , "解析器错误，堆栈溢出,检查指令" },
            {"401"  , "ZWAFER参数错误 ，检查ZWAFER参数" },
            {"402"  , "非法的slot参数，检查slot设置参数" },
            {"403"  , "运动指令RX参数错误，检查RX参数字段" },
            {"404"  , "晶圆夹取失败，检测传感器是否正常" },
            {"405"  , "晶圆释放失败，检测传感器是否正常" },
            {"406"  , "气缸压力表检测错误，检查气缸传感器" },
            {"407"  , "Mapping传感器伸出失败，检查气缸传感器" },
            {"408"  , "Mapping传感器缩回失败，检查气缸传感器" },
            {"409"  , "Mapping传感器位置数据不正确，检查mapping工位或传感器设置" },
            {"410"  , "Mapping参数值不合理，检查Mapping设置参数" },
            //{"416"  , "无法执行 CheckLoad 操作，检查指令中的对应参数" },
            //{"417"  , "Offset 参数参数超限，检查指令中指定的 offset 值是否在允许范围内" },
            //{"418"  , "AWC 功能检测到误差过大,检查晶圆是否过偏或检查AWC传感器标定参数" },
            //{"420"  , "副臂没有在HOME位置,请检查副臂的位置，可以使用HOME ALL使得所有关节姿态回到HOME位置" },
            {"451"  , "放片时，AWC偏差超过设定最大偏差值(PAN L) ，检测到晶圆偏心过大，或者 AWC 校验失败" },
            {"452"  , "PICK时，AWC偏差超过设定最大偏差值(PAN L) ，检测到晶圆偏心过大，或者 AWC 校验失败" },
            {"453"  , "AWC采集数据个数异常，检查AWC传感器配置" },
            {"455"  , "放片时，AWC偏差超过设定最大偏差值(PAN R) ，检测到晶圆偏心过大，或者 AWC 校验失败" },
            {"456"  , "PICK时，AWC偏差超过设定最大偏差值(PAN R) ，检测到晶圆偏心过大，或者 AWC 校验失败" },
            {"457"  , "AWC检查到破片报警(PAN L)，检查破片设置值，晶圆位置，或者 AWC 校验失败" },
            {"458"  , "AWC检查到破片报警(PAN R)，检查破片设置值，晶圆位置，或者 AWC 校验失败" },
            {"459"  , "AWC未标定，检查AWC标定信息，工位示教后进行AWC标定" },
            {"460"  , "AWC标定失败，检查晶圆SIZE设置是否正确 ，检查AWC传感器是否正常触发 " },
            {"550"  , "Station参数超出范围，检查站点配置参数" },
            //{"551"  , "ARM 不在站点，检查当前手臂实际位置与逻辑位置" },
            //{"560"  , "机械臂处于 HALT 状态，拒绝接受新的指令" },
            //{"561"  , "当前机械臂状态机与指令不匹配，当前状态机不满足执行该指令的条件" },
            {"599"  , "用户急停开启 ，检查用户急停 " },
            {"600"  , "系统未上电，发送上电指令" },
            {"602"  , "指令正在执行中，等待机械手运动完成" },
            //{"603"  , "收到急停指令" },
            {"604"  , "示教器控制中，通过操作示教盒退出示教盒模式 " },
            {"605"  , "机械手运动中停止，确认是否发送了HALT指令" },
            //{"608"  , "机械臂急停中" },
            {"609"  , "系统存在错误，检查错误码或查看Robot内部log文件" },
            {"610"  , "示教盒急停启动，解除示教盒急停按钮" },
            {"611"  , "驱动器报警，检查驱动器状态或对应IO状态" },
            {"629"  , "驱动器存在报警，检查驱动器报警码" },
            //{"692"  , "检测左 PAN 上有片，与逻辑状态不匹配" },
            //{"693"  , "检测右 PAN 上有片，与逻辑状态不匹配" },
            //{"694"  , "检测左 PAN 上无片，与逻辑状态不匹配" },
            //{"695"  , "检测右 PAN 上有片，与逻辑状态不匹配" },
            //{"700"  , "当前手臂有晶圆 ，检查指令动作要求的晶圆状态，与手臂内部存储的晶圆状态是否匹配" },
            //{"701" , "当前手臂无晶圆，检查指令动作要求的晶圆状态，与手臂内部存储的晶圆状态是否匹配"},
            {"705" , "LOAD状态未知，RQ LOAD查询状态 "},
            //{"711" , "站点互锁错误，检查站点的“允许机械臂进入信号”是否就绪"},
            {"712" , "真空吸附动作失败，检查真空吸附传感器是否正常"},
            {"720" , "PAN L PICK RE状态不正确，检查PAN L上是否有晶圆"},
            {"721" , "PAN R PICK RE状态不正确，检查PAN R是否有晶圆 "},
            {"722" , "PAN R PLACE前状态不正确，查询PAN R状态"},
            {"723" , "PAN L PLACE前状态不正确，查询PAN L状态"},
            {"724" , "PAN R PLACE EX状态不正确 ，检查PAN R上是否有晶圆"},
            {"725" , "PAN L PLACE EX状态不正确 ，检查PAN L上是否有晶圆"},
            {"726" , "PAN R PLACE RE状态不正确 ，检查PAN R上是否有晶圆"},
            {"727" , "PAN L PLACE RE状态不正确 ，检查PAN L是否有晶圆"},
            {"730" , "PLACE前LOAD状态错误，RQ LOAD查询状态"},
            {"731" , "PLACE后检测到晶圆，检查手指上晶圆状态"},
            {"734" , "PLACE执行Extend过程中未检测到晶圆，检查Sensor状态和wafer check配置"},
            {"736" , "PLACE执行Retract过程中检测到晶圆，检查Sensor状态和wafer check配置"},
            {"740" , "PICK前LOAD状态错误 ，RQ LOAD查询状态"},
            {"741" , "PICK后未检测到晶圆 ，检查手指是否有晶圆和Sensor状态"},
            {"744" , "PICK执行Extend过程中检测到晶圆，检查Sensor状态和wafer check配置"},
            {"745" , "PICK执行Retract过程中未检测到晶圆，检查Sensor状态和wafer check配置"},
            {"751" , "PAN L晶圆不存在，查询LOAD状态"},
            {"752" , "PAN R晶圆不存在，查询LOAD状态"},
            {"757" , "PAN R晶圆不存在，查询LOAD状态"},
            {"758" , "PAN R PICK EX检测到晶圆，检查Sensor状态和PAN R当前是否晶圆"},
            {"759" , "PAN L PICK RE未检测到晶圆，检查Sensor状态和PAN L当前是否晶圆"},
            {"760" , "PAN R PICK RE未检测到晶圆，检查Sensor状态和PAN R当前是否晶圆"},
            {"761" , "PAN L PLACE EX未检测到晶圆，检查Sensor状态和PAN L当前是否有晶圆"},
            {"762" , "PAN R PLACE EX未检测到晶圆，检查Sensor状态和PAN R当前是否有晶圆"},
            {"763" , "PAN L PLACE RE检测到晶圆，检查Sensor状态和PAN L当前是否有晶圆"},
            {"764" , "PAN R PLACE RE检测到晶圆，检查Sensor状态和PAN R当前是否有晶圆"},
            //{"771" , "取片时 AWC 传感器检测失败，检查 AWC 传感器状态，或者 AWC 配置相关是否正确"},
            //{"791" , "查询 AWC 数据失败，未曾执行过 AWC 相关功能的操作"},
            //{"792" , "AWC 传感器检测失败，检查AWC传感器状态，或者AWC配置相关是否正确"},
            //{"799" , "放片失败，检查手臂晶圆状态，或者晶圆检测传感器状态"},
            {"800" , "PICK执行Retract过程中未检测到晶圆,检查Sensor状态和wafer check配置"},
            {"801" , "改变运动状态发生错误,检查当前运动状态"},
            {"802" , "无法获取下一个状态,检查机械手当前运动状态"},
            {"803" , "没有正在运行的指令 ,指令是否运行完成"},
            {"804" , "没有暂停的动作,检查机械手是否在运动"},
            {"805" , "没有可跳过的动作,检查运动状态"},
            {"806" , "带拐点工位不支持该指令,拐点开关是否打开"},
            {"1006" , "非法的IO端口号,确认配置的IO号"},
            {"1314" , "PITCH轴超界，检查PITCH关节值"},
            {"1315" , "关节位置超界 ，查询当前关节值和软限位"},
            {"1316" , "关节位置超硬限 ，查询当前关节值和硬限位传感器"},
            {"1320" , "机械手跟踪差超限错误 ，查询当前跟踪差配置值"},
            {"1321" , "机械手发生碰撞，检测当前碰撞参数配置值"},
            {"1322" , "机械手发生碰撞，检测当前碰撞参数配置值"},
            {"1323" , "机械手发生碰撞，检测当前碰撞参数配置值"},
            {"2000" , "ALGN偏差超过最大值，检查设置的晶圆最大偏差值 检查晶圆位置 "},
            {"2001" , "AGLN CCD数据异常，确认晶圆位置是否正确 确认CCD传感器位置是否正确 确认内部线路正负正确 "},
            {"2002" , "返回值格式错误，确认返回值格式"},
            {"2003" , "通信已经断开，检查通讯线路是否正常"},
            {"2008" , "FLAT片数据异常，确认晶圆位置是否正确 确认CCD传感器位置是否正确 "},
            {"13007" , "通信断开，检查通讯线路是否正常"},
            {"13033" , "伺服驱动器初始化失败，检查驱动器通信连线或检查通信配置"},
            
        };
        private BlockingCollection<RobotAnimationData> blockingCollection = new BlockingCollection<RobotAnimationData>();
        private ModuleName _module;
        public SunWayRobot(ModuleName module)
        {
            _module = module;
            _socket = new AsyncSocket("", EOF);
            _socket.Connect(SC.GetStringValue($"{_module}.IPAddress"));
            _socket.OnDataChanged += OnReceiveMessage;
            _socket.OnErrorHappened += OnErrorHappen;

            _status = RState.Init;
            _IsHomed = false;

            _StationNumbers[ModuleName.VCE1.ToString()] = SC.GetValue<int>($"{_module}.VCE1StationNumber");
            _StationNumbers[ModuleName.VCEA.ToString()] = SC.GetValue<int>($"{_module}.VCEAStationNumber");
            _StationNumbers[ModuleName.VCEB.ToString()] = SC.GetValue<int>($"{_module}.VCEBStationNumber");
            _StationNumbers[ModuleName.Aligner1.ToString()] = SC.GetValue<int>($"{_module}.Aligner1StationNumber");

            _StationNumbers[ModuleName.PMA.ToString()] = SC.GetValue<int>($"{_module}.PMAStationNumber");
            _StationNumbers[ModuleName.PMB.ToString()] = SC.GetValue<int>($"{_module}.PMBStationNumber");
            _StationNumbers[ModuleName.PMC.ToString()] = SC.GetValue<int>($"{_module}.PMCStationNumber");
            _StationNumbers[ModuleName.PMD.ToString()] = SC.GetValue<int>($"{_module}.PMDStationNumber");
            _StationNumbers["CheckLoad"] = SC.GetValue<int>($"{_module}.CheckStationNumber");

            WaferManager.Instance.SubscribeLocation(ModuleName.TMRobot, 2);
            Task.Run(() =>
            {
                foreach (var data in blockingCollection.GetConsumingEnumerable())
                {
                    _robotMoveInfo.Action = data.Action;
                    _robotMoveInfo.ArmTarget = data.Hand == Hand.Blade1 ? RobotArm.ArmA : (data.Hand == Hand.Both ? RobotArm.Both : RobotArm.ArmB);
                    _robotMoveInfo.BladeTarget = $"{_robotMoveInfo.ArmTarget}.{data.Target}";
                    System.Threading.Thread.Sleep(600);
                }
            });
        }
        //初始化某个轴
        //1.清错
        //2.设备上电
        //3.各轴按顺序运动
        public bool Home()
        {
            _status = RState.Running;
            _currentStep = VRStep.Home;
            return _SendCommand("HOME ALL");
        }
        public bool Halt()
        {
            _status = RState.Running;
            _currentStep = VRStep.Halt;
            return _SendCommand("HALT");
        }
        public bool Release()
        {
            _status = RState.Running;
            _currentStep = VRStep.Release;
            return _SendCommand("RELEASE");
        }
        //public bool MOVE()
        //{
        //}
        public bool Pick(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Pick;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Picking, hand, station);
            return _SendCommand($"PICK {_StationNumbers[station.ToString()]} ARM {Hand2Arm(hand)}");
        }

        public bool PickExtend(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.PickExtend;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Extending, hand, station);
            return _SendCommand($"PICK {_StationNumbers[station.ToString()]} ARM {Hand2Arm(hand)} ENRT NR");
        }

        public bool PickRetract(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.PickRetract;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Retracting, hand, station);
            return _SendCommand($"PICK {_StationNumbers[station.ToString()]} ARM {Hand2Arm(hand)} STRT NR");
        }
        public bool Place(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Place;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Placing, hand, station);
            return _SendCommand($"PLACE {_StationNumbers[station.ToString()]} ARM {Hand2Arm(hand)}");
        }
        public bool PlaceExtend(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Place;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Extending, hand, station);
            return _SendCommand($"PLACE {_StationNumbers[station.ToString()]} ARM {Hand2Arm(hand)} ENRT NR");
        }
        public bool PlaceRetract(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Place;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Retracting, hand, station);
            return _SendCommand($"PLACE {_StationNumbers[station.ToString()]} ARM {Hand2Arm(hand)} STRT NR");
        }
        public bool Transfer(ModuleName fromstation, ModuleName tostation, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Xfer;
            _status = RState.Running;
            return _SendCommand($"XFER ARM {Hand2Arm(hand)} {_StationNumbers[fromstation.ToString()]} {_StationNumbers[tostation.ToString()]}");

        }
        public bool CheckLoad(Hand hand = Hand.Blade1)
        {
            if (_currentStep != VRStep.Home && _currentStep != VRStep.CheckLoad_ArmA && !CheckRobotStatus())
                return false;

            _currentStep = hand == Hand.Blade2 ? VRStep.CheckLoad_ArmB : VRStep.CheckLoad_ArmA;
            _status = RState.Running;
            string arm = hand == Hand.Blade2 ? "B" : "A";

            return _SendCommand($"CHECK LOAD {_StationNumbers["CheckLoad"]} {arm}");
        }


        public bool QueryAwc()
        {
            return true;
        }

        public bool Goto(ModuleName station, int slot, Hand hand)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.Goto;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Rotating, hand, station);
            return _SendCommand($"Goto {_StationNumbers[station.ToString()]} ARM {Hand2Arm(hand)}");
        }

        public bool MoveTo(ModuleName stnFrom, ModuleName stnTo, Hand hand)
        {
            _currentStep = VRStep.Move;
            _status = RState.Running;
            return _SendCommand($"MOVE R ABS 100 ARM {Hand2Arm(hand)}"); ;
        }
        public bool PickWithOffset(ModuleName station, int slot, Hand hand, int Roffset, int Toffset)
        {
            _currentStep = VRStep.Pick;
            _status = RState.Running;
            SetRobotMovingInfo(RobotAction.Picking, hand, station);

            return _SendCommand($"PICK {_StationNumbers[station.ToString()]} ARM {Hand2Arm(hand)} RO {Roffset} TO {Toffset}"); ;
        }
        public bool ReQueryLoadA()
        {
            _currentStep = VRStep.ReQueryLoadA;
            _status = RState.Running;
            return _SendCommand($"RQ LOAD ARM A");
        }
        public bool ReQueryLoadB()
        {
            _currentStep = VRStep.ReQueryLoadB;
            _status = RState.Running;
            return _SendCommand($"RQ LOAD ARM B");
        }

        public bool SetSpeed(string withwafer, float speed)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.SetSpeed;
            _status = RState.Running;
            int setspeed = (int)Math.Floor(speed * 10);
            return _SendCommand($"SET MOTIONPARA {withwafer} SPDPCT ALL {setspeed.ToString().PadLeft(4, '0')}");
        }

        public bool SaveSpeed(string withwafer)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.SaveSpeed;
            _status = RState.Running;
            return _SendCommand($"STORE MOTIONPARA {withwafer} SPDPCT ALL");
        }

        public bool QuerySpeed(string withwafer)
        {
            if (!CheckRobotStatus())
                return false;

            _currentStep = VRStep.RQSpeed;
            _status = RState.Running;
            //MOTIONPARA {} SPDPCT all x t z
            return _SendCommand($"RQ MOTIONPARA {withwafer} SPDPCT ALL");
        }

        public bool ServeOn(bool IsOn)
        {
            _currentStep = VRStep.ServeOn;
            _status = RState.Running;
            if (IsOn)
                return _SendCommand($"SET SERVOS ON");
            else
                return _SendCommand($"SET SERVOS OFF");
        }

        private bool _SendCommand(string cmd)
        {
            LOG.WriteSingeLine(eEvent.INFO_TM_ROBOT, ModuleName.TMRobot, $"Send Command to TMRobot: {cmd},Connect:{_socket.IsConnected}");
            return _socket.Write(cmd + EOF);
        }

        private bool CheckRobotStatus()
        {
            if (Status == RState.Init)
            {
                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, "TMRobot is not homed, please home first.");
                return false;
            }
            else if (Status == RState.Running)
            {
                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, "TMRobot is busy, please wait a minute");
                return false;
            }
            else if (Status == RState.Failed || Status == RState.Timeout)
            {
                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, "TMRobot has a error, please check and fix the hardware issue and home it");
                return false;
            }

            return true;
        }

        private void OnReceiveMessage(string revMsg)
        {
            string[] ResMsgs = revMsg.Split('\r');
            foreach (string revRawMsg in ResMsgs)
            {
                if (string.IsNullOrWhiteSpace(revRawMsg)) continue;
                string RevMsg = revRawMsg.Trim();
                LOG.WriteSingeLine(eEvent.INFO_TM_ROBOT, ModuleName.TMRobot, $"Receive message from TMRobot: {RevMsg}, while {_currentStep}");
                if (_rex_error_code.IsMatch(RevMsg))
                {
                    _IsHomed = false;
                    _status = RState.Failed;

                    var results = _rex_error_code.Match(RevMsg);
                    ErrorMessageHandler(results.Groups[1].Value);
                    return;
                }

                switch (_currentStep)
                {
                    case VRStep.Goto:
                    case VRStep.Halt:
                    case VRStep.Move:
                    case VRStep.Xfer:
                    case VRStep.SetSpeed:
                    case VRStep.SaveSpeed:
                    case VRStep.Pick:
                    case VRStep.PickExtend:
                    case VRStep.PickRetract:
                    case VRStep.Place:
                    case VRStep.PlaceExtend:
                    case VRStep.PlaceRetract:
                        {
                            if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                            {
                                _currentStep = VRStep.Idle;
                                _status = RState.End;
                            }
                            else
                            {
                                ReportWrongMsg(RevMsg);
                            }

                            if (_currentStep != VRStep.PickExtend && _currentStep != VRStep.PlaceExtend)
                                SetRobotMovingInfo(RobotAction.None, Hand.Both, ModuleName.TMRobot);
                        }
                        break;
                    case VRStep.Home:
                        {

                            if (RevMsg.Trim() == "_RDY")
                            {
                                //CheckLoad(Hand.Blade1);
                                _currentStep = VRStep.Idle;
                                _status = RState.End;
                                _IsHomed = true;
                                SetRobotMovingInfo(RobotAction.Homing, Hand.Both, ModuleName.TMRobot);
                            }
                            else
                                ReportWrongMsg(RevMsg);
                        }
                        break;
                    case VRStep.CheckLoad_ArmA:
                        {
                            if (_rex_check_load.IsMatch(RevMsg))
                            {
                                GetCheckLoadResult(RevMsg);

                                //CheckLoad(Hand.Blade2);
                            }

                            if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                            {
                                _currentStep = VRStep.Idle;
                                _status = RState.End;
                            }
                        }
                        break;
                    case VRStep.CheckLoad_ArmB:
                        {
                            if (_rex_check_load.IsMatch(RevMsg))
                            {
                                GetCheckLoadResult(RevMsg);

                                //_currentStep = VRStep.Idle;
                                //_status = RState.End;
                                //_IsHomed = true;
                            }
                            if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                            {
                                _currentStep = VRStep.Idle;
                                _status = RState.End;
                            }
                        }
                        break;
                    case VRStep.ReQueryLoadA:
                        if (_rex_rq_load_A.IsMatch(RevMsg))
                        {
                            string WaferStatus = RevMsg.Split(' ')[2];
                            //LOG.Write(eEvent.WARN_DEFAULT_WARN, ModuleName.TMRobot, WaferStatus);
                            if (WaferStatus.Contains("ON") && WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot, 0))
                            {
                                WaferManager.Instance.CreateWafer(ModuleName.TMRobot, 0, Aitex.Core.Common.WaferStatus.Normal);
                            }
                            if (WaferStatus.Contains("OFF"))
                            {
                                //LOG.Write(eEvent.WARN_DEFAULT_WARN, ModuleName.TMRobot, "contains off");
                                WaferManager.Instance.DeleteWafer(ModuleName.TMRobot, 0);
                            }

                        }
                        else
                        {
                            if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                            {
                                _currentStep = VRStep.Idle;
                                _status = RState.End;
                            }
                            else
                            {
                                ReportWrongMsg(RevMsg);
                            }
                        }
                        break;

                    case VRStep.ReQueryLoadB:
                        if (_rex_rq_load_B.IsMatch(RevMsg))
                        {

                            string WaferStatus = RevMsg.Split(' ')[2];
                            //LOG.Write(eEvent.WARN_DEFAULT_WARN, ModuleName.TMRobot, WaferStatus);
                            if (WaferStatus.Contains("ON") && WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot, 1))
                            {
                                WaferManager.Instance.CreateWafer(ModuleName.TMRobot, 1, Aitex.Core.Common.WaferStatus.Normal);
                            }
                            if (WaferStatus.Contains("OFF"))
                            {
                                WaferManager.Instance.DeleteWafer(ModuleName.TMRobot, 1);
                            }

                        }
                        else
                        {
                            if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                            {
                                _currentStep = VRStep.Idle;
                                _status = RState.End;
                            }
                            else
                            {
                                ReportWrongMsg(RevMsg);
                            }
                        }
                        break;
                    case VRStep.RQSpeed:
                        //MOTIONPARA {} SPDPCT all r t z
                        if (_rex_rq_speed.IsMatch(RevMsg))
                        {

                            string[] _msgs = RevMsg.Split(' ');
                            if (_msgs[1] == "WITHWAFER")
                            {
                                _WithWaferSpeed = _msgs[4];
                            }

                            if (_msgs[1] == "NOWAFER")
                            {
                                _NoWaferSpeed = _msgs[4];
                            }

                            if (_msgs[1] == "MIDDLE")
                            {
                                _WithWaferSpeed = _msgs[4];
                            }

                        }
                        else
                        {
                            if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
                            {
                                _currentStep = VRStep.Idle;
                                _status = RState.End;
                            }
                            else
                            {
                                ReportWrongMsg(RevMsg);
                            }
                        }
                        break;
                    default:
                        if (!RevMsg.Contains("_EVENT"))
                            ReportWrongMsg(RevMsg);
                        break;
                }
            }

        }

        private void GetCheckLoadResult(string revMsg)
        {
            Match result = _rex_check_load.Match(revMsg);

            string Arm = result.Groups[1].Value;
            string WaferStatus = result.Groups[2].Value;
            if (WaferStatus == "ON")
            {
                WaferManager.Instance.CreateWafer(ModuleName.TMRobot, Arm == "A" ? 0 : 1, Aitex.Core.Common.WaferStatus.Unknown);
            }
        }

        private void ErrorMessageHandler(string errorcode)
        {
            _status = RState.Failed;
            string ErrorInfo;
            if (_error2msg.ContainsKey(errorcode))
            {
                ErrorInfo = _error2msg[errorcode];
                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, ErrorInfo);
            }
            else
            {
                LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, $"Dictionary Not Contains error code:{errorcode}");
            }

            _IsHomed = false;
        }

        private void OnErrorHappen(ErrorEventArgs args)
        {
            //Singleton<RouteManager>.Instance.SETM.PostMsg(SETMEntity.MSG.Error);
            LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.TMRobot, $"HongHu TMRobot Error: {args.Reason} while {_currentStep}");
        }

        private void ReportWrongMsg(string revMsg)
        {
            LOG.Write(eEvent.WARN_DEVICE_INFO, ModuleName.TMRobot, $"Receive wrong message:{revMsg} while {_currentStep}");
        }

        public void SetRobotMovingInfo(RobotAction action, Hand hand, ModuleName target)
        {
            //_robotMoveInfo.Action = action;
            //_robotMoveInfo.ArmTarget = hand == Hand.Blade1 ? RobotArm.ArmA : (hand == Hand.Both ? RobotArm.Both : RobotArm.ArmB);
            //_robotMoveInfo.BladeTarget = $"{_robotMoveInfo.ArmTarget}.{target}";
            blockingCollection.Add(new RobotAnimationData(action, hand, target));
        }

    }
}