using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.Schedulers;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.RT.EquipmentLibrary.LogicUnits;
using System;
using System.Collections.Generic;
using FurnaceRT.Equipments.FIMSs;
using FurnaceRT.Equipments.LPs;
using FurnaceRT.Equipments.Stockers;
using FurnaceRT.Equipments.Systems;
using FurnaceRT.Equipments.WaferRobots;
using static FurnaceRT.Equipments.FIMSs.FIMSModule;

namespace FurnaceRT.Equipments.CarrierRobots
{
    public class CarrierLoad : ModuleRoutine, IRoutine
    {
        enum RoutineStep
        {
            Pick,
            DoorOpen,
            DoorClose,
            RobotRequestCassettePresent,
            CheckCassetteInfoByRobotSensor,
            ReadCarrierID,
            CheckCarrierID,
            CheckBeforePick,
            SetRobotActionCommand,
            Goto,
            CheckGotoFinish,
        }

        private CarrierRobotModule _cassetteRobotModule;
        private ModuleName _source;
        private ModuleName _destinationStocker;
        private int _sourceSlot;
        private Hand _blade;
        private int _timeout = 0;
        private RD_TRIG _holdTrig = new RD_TRIG();
        private R_TRIG _emergencyStopTrig = new R_TRIG();
        private R_TRIG _pauseTrig = new R_TRIG();
        private R_TRIG _resumeTrig = new R_TRIG();
        private RoutineStep _routineStep;
        private double _durationTime = 0;
        private bool _isHasAlarm = false;
        private bool _needStartCheck = true;
        private CarrierType _carrierType;
        private string _reservedCarrierId;
        private string _readCarrierId;
        private string _currentCarrierId;
        private string _bypassReadID;
        private string _carrierIDReaderSetting;
        public CarrierLoad(CarrierRobotModule cassetteModule)
        {
            _cassetteRobotModule = cassetteModule;
            Module = cassetteModule.Module;
            Name = "Load";
        }

        public void Init(ModuleName source, int sourceSlot, Hand blade, ModuleName destinationStocker,
            CarrierType carrierType, string reservedCarrierId, bool isHasAlarm)
        {
            _source = source;
            _sourceSlot = sourceSlot;
            _blade = blade;
            _destinationStocker = destinationStocker;
            _carrierType = carrierType;
            _reservedCarrierId = reservedCarrierId;

            var para = new List<object> { _source, _sourceSlot, _blade, destinationStocker, _carrierType, _reservedCarrierId, true };
            _cassetteRobotModule.PickCassetteTimeoutAlarm.RetryMessage = (int)CarrierRobotModule.MSG.LoadRetry;
            _cassetteRobotModule.PickCassetteTimeoutAlarm.RetryMessageParas = para.ToArray();

            _cassetteRobotModule.PickCassetteFailAlarm.RetryMessage = (int)CarrierRobotModule.MSG.LoadRetry;
            _cassetteRobotModule.PickCassetteFailAlarm.RetryMessageParas = para.ToArray();

            _cassetteRobotModule.RequestCassettePresentFailAlarm.RetryMessage = (int)CarrierRobotModule.MSG.LoadRetry;
            _cassetteRobotModule.RequestCassettePresentFailAlarm.RetryMessageParas = para.ToArray();

            _cassetteRobotModule.RequestCassettePresentTimeoutAlarm.RetryMessage = (int)CarrierRobotModule.MSG.LoadRetry;
            _cassetteRobotModule.RequestCassettePresentTimeoutAlarm.RetryMessageParas = para.ToArray();

            _cassetteRobotModule.SetStageValveFailAlarm.RetryMessage = (int)CarrierRobotModule.MSG.LoadRetry;
            _cassetteRobotModule.SetStageValveFailAlarm.RetryMessageParas = para.ToArray();

            _cassetteRobotModule.SetStageValveTimeout.RetryMessage = (int)CarrierRobotModule.MSG.LoadRetry;
            _cassetteRobotModule.SetStageValveTimeout.RetryMessageParas = para.ToArray();

            _cassetteRobotModule.RobotHasError.RetryMessage = (int)CarrierRobotModule.MSG.LoadRetry;
            _cassetteRobotModule.RobotHasError.RetryMessageParas = para.ToArray();

            _cassetteRobotModule.ShutterOpenTimeoutAlarm.RetryMessage = (int)CarrierRobotModule.MSG.LoadRetry;
            _cassetteRobotModule.ShutterOpenTimeoutAlarm.RetryMessageParas = para.ToArray();

            _cassetteRobotModule.ShutterCloseTimeoutAlarm.RetryMessage = (int)CarrierRobotModule.MSG.LoadRetry;
            _cassetteRobotModule.ShutterCloseTimeoutAlarm.RetryMessageParas = para.ToArray();

            _isHasAlarm = isHasAlarm;
            if (!_isHasAlarm)
                _needStartCheck = true;
        }

        public Result Start(params object[] objs)
        {
            // 抛出过alarm就不reset
            if (!_isHasAlarm)
            {
                Reset();
            }
            else
            {
                _historySteps.Remove((int)_routineStep);
                _isHasAlarm = false;
                ResetState();
            }

            _holdTrig.RST = true;
            _emergencyStopTrig.RST = true;
            _pauseTrig.RST = true;
            _resumeTrig.RST = true;
            _currentCarrierId = "";

            _timeout = SC.GetValue<int>($"{Module}.MotionTimeout");
            _bypassReadID = SC.GetStringValue($"LoadPort.{_source}.BypassReadID");
            _carrierIDReaderSetting = SC.GetStringValue($"LoadPort.{_source}.CarrierIDReaderSetting");
            if (_carrierIDReaderSetting == "UseReservedID")
                _currentCarrierId = _reservedCarrierId;

            if ((Singleton<EquipmentManager>.Instance.Modules[_source] as LoadPortModule).LPDevice.IsFoupPresent)
            {
                if (!CarrierManager.Instance.CheckNoCarrier(_source.ToString(), 0))
                    CarrierManager.Instance.DeleteCarrier(_source.ToString());
                CarrierManager.Instance.CreateCarrier(_source.ToString(), _carrierType, _destinationStocker.ToString());
            }

            if (_needStartCheck)
            {
                if (!Singleton<EquipmentManager>.Instance.Modules.ContainsKey(_source))
                {
                    _cassetteRobotModule.PlaceCassetteFailAlarm.Set($"pick from {_source} failed for not install");
                    return Result.FAIL;
                }
                if (ModuleHelper.IsLoadPort(_source))
                {
                    var lpModule = Singleton<EquipmentManager>.Instance.Modules[_source] as LoadPortModule;
                    if (lpModule == null)
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {_source} failed for not install");
                        return Result.FAIL;
                    }
                    if (lpModule != null && !lpModule.CheckReadyForTransfer(ModuleHelper.Converter(Module), _blade, _sourceSlot, EnumTransferType.Pick, out string reason))
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {_source} failed for {reason}");
                        return Result.FAIL;
                    }
                }
                else if (_source == ModuleName.FIMS1 || _source == ModuleName.FIMS2)
                {
                    var fims = Singleton<EquipmentManager>.Instance.Modules[_source] as FIMSModule;
                    if (fims == null)
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {_source} failed for not install");
                        return Result.FAIL;
                    }
                    if (fims != null && !fims.CheckReadyForTransfer(ModuleHelper.Converter(Module), _blade, _sourceSlot, EnumTransferType.Pick, out string reason))
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {_source} failed for {reason}");
                        return Result.FAIL;
                    }

                    if (!SC.GetValue<bool>("System.IsSimulatorMode"))
                    {
                        if (fims != null && !fims.IsFoupExist)
                        {
                            _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {_source} failed for cassette is not present");
                            return Result.FAIL;
                        }
                    }
                }
                else
                {
                    var stockerModule = Singleton<EquipmentManager>.Instance.Modules[_source] as StockerModule;
                    if (stockerModule == null)
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {_source} failed for not install");
                        return Result.FAIL;
                    }
                    if (!SC.GetValue<bool>("System.IsSimulatorMode"))
                    {
                        if (stockerModule != null && !stockerModule.IsFoupPresent)
                        {
                            _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {_source} failed for cassette is not present");
                            return Result.FAIL;
                        }
                    }
                }

                if (SC.GetValue<bool>("System.IsSimulatorMode"))
                {
                    if (!CarrierManager.Instance.CheckNoCarrier(ModuleName.CarrierRobot, 0))
                    {
                        _cassetteRobotModule.BladeCassettePresentWarning.Set($"{ModuleName.CarrierRobot} cassette present");
                        return Result.FAIL;
                    }
                }
                else
                {
                    if (_cassetteRobotModule.CarrierRobotDevice.IsWaferPresenceOnBlade1 || !CarrierManager.Instance.CheckNoCarrier(ModuleName.CarrierRobot, 0))
                    {
                        _cassetteRobotModule.BladeCassettePresentWarning.Set($"{ModuleName.CarrierRobot} cassette present");
                        return Result.FAIL;
                    }
                }

                if (!CarrierManager.Instance.CheckHasCarrier(_source, 0))
                {
                    _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {_source} failed for cassette is not present");
                    return Result.FAIL;
                }
            }

            Notify($"Start");

            return Result.RUN;
        }
        public void Abort()
        {
            _cassetteRobotModule.ResetRobotActionCommand();
            _cassetteRobotModule.Stop();
            (Singleton<EquipmentManager>.Instance.Modules[_source] as ITransferTarget)?.NoteTransferStop(ModuleHelper.Converter(_cassetteRobotModule.Module), _blade, _sourceSlot, EnumTransferType.Pick);
        }

        public override Result Monitor()
        {
            try
            {
                PauseRountine(_cassetteRobotModule.CarrierRobotDevice.IsPause);
                if (_cassetteRobotModule.CarrierRobotDevice.IsPause)
                    return Result.RUN;

                CheckBeforePick((int)RoutineStep.CheckBeforePick, _source, _sourceSlot, _blade);

                //if (_carrierIDReaderSetting == "UseReadID" || _carrierIDReaderSetting == "ReadWithReservedIDCheck")
                //    ReadCarrierID((int)RoutineStep.ReadCarrierID, _timeout);

                //if (_carrierIDReaderSetting == "ReadWithReservedIDCheck")
                //    CheckCarrierID((int)RoutineStep.CheckCarrierID);

                Goto((int)RoutineStep.Goto, _source, _sourceSlot, _blade, true, _timeout);

                if (_source == ModuleName.LP1 || _source == ModuleName.LP2)
                    SaferDoorOpen((int)RoutineStep.DoorOpen, true, _timeout);

                CheckGotoFinish((int)RoutineStep.CheckGotoFinish, _source, _timeout);

                if (_cassetteRobotModule.TrigActionCommand != null)
                    SetRobotActionCommand((int)RoutineStep.SetRobotActionCommand, _source, _timeout);

                Pick((int)RoutineStep.Pick, _source, _sourceSlot, _blade, _carrierType, _currentCarrierId, _destinationStocker, _timeout);

                if (_source == ModuleName.LP1 || _source == ModuleName.LP2)
                    SaferDoorOpen((int)RoutineStep.DoorClose, false, _timeout);

                RobotRequestCassettePresent((int)RoutineStep.RobotRequestCassettePresent, _blade, _timeout);

                CheckCassetteInfoByRobotSensor((int)RoutineStep.CheckCassetteInfoByRobotSensor, _blade, true);
            }
            catch (RoutineBreakException)
            {
                return Result.RUN;
            }
            catch (RoutineFaildException ex)
            {
                _cassetteRobotModule.ResetRobotActionCommand();
                (Singleton<EquipmentManager>.Instance.Modules[_source] as ITransferTarget)?.NoteTransferStop(ModuleHelper.Converter(_cassetteRobotModule.Module), _blade, _sourceSlot, EnumTransferType.Pick);
                return Result.FAIL;
            }

            _cassetteRobotModule.ResetRobotActionCommand();
            (Singleton<EquipmentManager>.Instance.Modules[_source] as ITransferTarget)?.NoteTransferStop(ModuleHelper.Converter(_cassetteRobotModule.Module), _blade, _sourceSlot, EnumTransferType.Pick);

            Notify("Finished");
            return Result.DONE;
        }
        private void SetRobotActionCommand(int id, ModuleName source, int timeout)
        {
            _routineStep = (RoutineStep)Enum.Parse(typeof(RoutineStep), id.ToString());
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Set robot action command target position {source}");

                _cassetteRobotModule.SetRobotActionCommand(source, EnumTransferType.Pick);

                return true;
            }, () =>
            {
                return _cassetteRobotModule.CheckRobotActionCommand(source, EnumTransferType.Pick);
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {source} failed for robot action command interlock");
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    _cassetteRobotModule.PickCassetteTimeoutAlarm.Set($"timeout over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        private void ReadCarrierID(int id, int timeout)
        {
            _routineStep = (RoutineStep)Enum.Parse(typeof(RoutineStep), id.ToString());
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Read carrier ID");
                string reason;

                return true;
            }, () =>
            {
                _readCarrierId = "123";//Carrier ID reader读取到的值
                _currentCarrierId = "123";//Carrier ID reader读取到的值
                return true;
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    //_cassetteRobotModule.SetStageValveTimeout.Description = $"timeout over {timeout} seconds";
                    _cassetteRobotModule.SetStageValveTimeout.Set($"timeout over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        private void Pick(int id, ModuleName source, int slot, Hand hand, CarrierType carrierType, string lotId, ModuleName carrierInternalModuleName, int timeout)
        {
            _routineStep = (RoutineStep)Enum.Parse(typeof(RoutineStep), id.ToString());
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Send pick from {source} command to robot device");
                string reason;
                if (!CarrierManager.Instance.CheckNoCarrier(source.ToString(), 0))
                    CarrierManager.Instance.DeleteCarrier(source.ToString());//要保留
                CarrierManager.Instance.CreateCarrier(source.ToString(), carrierType, carrierInternalModuleName.ToString());
  
                CarrierManager.Instance.GetCarrier(source.ToString(), 0).IsMapOK = false;
                CarrierManager.Instance.GetCarrier(source.ToString(), 0).IsMapped = false;
                (Singleton<EquipmentManager>.Instance.Modules[source] as ITransferTarget)?.NoteTransferStart(ModuleHelper.Converter(_cassetteRobotModule.Module), hand, slot, EnumTransferType.Pick);
                if (!_cassetteRobotModule.RobotPick(source, slot, hand, out reason))
                {
                    //_cassetteRobotModule.PickCassetteFailAlarm.Description = reason;
                    _cassetteRobotModule.PickCassetteFailAlarm.Set(reason);
                    return false;
                }
                return true;
            }, () =>
            {
                if (_cassetteRobotModule.CarrierRobotDevice.IsReady() && !_cassetteRobotModule.CarrierRobotDevice.IsError)
                {
                    var lp = Singleton<EquipmentManager>.Instance.Modules[_source] as LoadPortModule;
                    var readCarrierId = lp != null ? lp.CarrierId : lotId;
                    CarrierManager.Instance.UpdateCarrierId(ModuleName.CarrierRobot.ToString(), readCarrierId);
                    return true;
                }

                return false;
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    //_cassetteRobotModule.PickCassetteFailAlarm.Description = $"{_cassetteRobotModule.CassetteRobotDevice.ErrorCode}";
                    _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {source} failed for robot error, error code={_cassetteRobotModule.CarrierRobotDevice.ErrorCode}");
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    //_cassetteRobotModule.PickCassetteTimeoutAlarm.Description = $"timeout over {timeout} seconds";
                    _cassetteRobotModule.PickCassetteTimeoutAlarm.Set($"timeout over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        private void RobotRequestCassettePresent(int id, Hand hand, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify("Request cassette present");

                if (!_cassetteRobotModule.RequestCassettePresent(hand, out string reason))
                {
                    //_cassetteRobotModule.RequestCassettePresentFailAlarm.Description = reason;
                    _cassetteRobotModule.RequestCassettePresentFailAlarm.Set(reason);
                    return false;
                }

                return true;
            }, () =>
            {
                if (_cassetteRobotModule.CarrierRobotDevice.IsReady() && !_cassetteRobotModule.CarrierRobotDevice.IsError)
                {
                    return true;
                }

                return false;
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    //_cassetteRobotModule.RequestCassettePresentTimeoutAlarm.Description = $"timeout over {timeout} seconds";
                    _cassetteRobotModule.RequestCassettePresentTimeoutAlarm.Set($"timeout over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        private void CheckCassetteInfoByRobotSensor(int id, Hand hand, bool isAfterPick)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                Notify($"Check cassette info by robot RQ present");

                if (SC.GetValue<bool>("System.IsSimulatorMode"))
                    return true;

                if (hand == Hand.Blade1 || hand == Hand.Both)
                {
                    if (!isAfterPick && _cassetteRobotModule.CarrierRobotDevice.IsWaferPresenceOnBlade1)
                    {
                        //_cassetteRobotModule.RobotSensorFoundCassetteOnBladeAfterPlaceAlarm.Description = "Cassette Robot sensor found cassette on blade 1";
                        _cassetteRobotModule.RobotSensorFoundCassetteOnBladeAfterPlaceAlarm.Set("Cassette Robot sensor found cassette on blade 1");
                        return false;
                    }

                    if (isAfterPick && !_cassetteRobotModule.CarrierRobotDevice.IsWaferPresenceOnBlade1)
                    {
                        //_cassetteRobotModule.RobotSensorNotFoundCassetteOnBladeAfterPickAlarm.Description = "Cassette Robot sensor no cassette on blade 1";
                        _cassetteRobotModule.RobotSensorNotFoundCassetteOnBladeAfterPickAlarm.Set("Cassette Robot sensor no cassette on blade 1");
                        return false;
                    }
                }

                if (hand == Hand.Blade2 || hand == Hand.Both)
                {
                    if (!isAfterPick && _cassetteRobotModule.CarrierRobotDevice.IsWaferPresenceOnBlade2)
                    {
                        //_cassetteRobotModule.RobotSensorFoundCassetteOnBladeAfterPlaceAlarm.Description = "Cassette Robot sensor found cassette on blade 2";
                        _cassetteRobotModule.RobotSensorFoundCassetteOnBladeAfterPlaceAlarm.Set("Cassette Robot sensor found cassette on blade 2");
                        return false;
                    }

                    if (isAfterPick && !_cassetteRobotModule.CarrierRobotDevice.IsWaferPresenceOnBlade2)
                    {
                        //_cassetteRobotModule.RobotSensorNotFoundCassetteOnBladeAfterPickAlarm.Description = "Cassette Robot sensor no cassette on blade 2";
                        _cassetteRobotModule.RobotSensorNotFoundCassetteOnBladeAfterPickAlarm.Set("Cassette Robot sensor no cassette on blade 2");
                        return false;
                    }
                }

                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    //_cassetteRobotModule.CheckCassetteInformationFailAlarm.Description = $"check cassette info failed.";
                    _cassetteRobotModule.CheckCassetteInformationFailAlarm.Set($"check cassette info failed.");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }


        private void CheckBeforePick(int id, ModuleName source, int slot, Hand blade)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                Notify($"Check pick from {source} condition");
                string reason = string.Empty;

                if (_cassetteRobotModule.CarrierRobotDevice.IsError)
                {
                    _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {source} failed for robot error, error code={_cassetteRobotModule.CarrierRobotDevice.ErrorCode}");
                    return false;
                }

                if (!_cassetteRobotModule.CarrierRobotDevice.IsReady())
                {
                    _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {source} failed for robot isn't Ready");
                    return false;
                }

                if (!CarrierManager.Instance.CheckHasCarrier(_source, 0))
                {
                    _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {_source} failed for pod is not present");
                    return false;
                }

                if (blade == Hand.Blade1)
                {
                    if (!CarrierManager.Instance.CheckHasCarrier(source, 0))
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {source} failed for {source} no carrrier");
                        return false;
                    }

                    if (!CarrierManager.Instance.CheckNoCarrier(Module, 0))
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {source} failed for blade has carrier");
                        return false;
                    }
                }
                else if (blade == Hand.Blade2)
                {
                    if (!CarrierManager.Instance.CheckHasCarrier(source, 0))
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {source} failed for {source} no carrier");
                        return false;
                    }

                    if (!CarrierManager.Instance.CheckNoCarrier(Module, 0))
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {source} failed for blade has carrier");
                        return false;
                    }
                }
                else
                {
                    if (!CarrierManager.Instance.CheckHasCarrier(source, 0))
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {source} failed for {source} no carrier");
                        return false;
                    }

                    if (!CarrierManager.Instance.CheckNoCarrier(Module, 0))
                    {
                        _cassetteRobotModule.PickCassetteFailAlarm.Set($"pick from {source} failed for blade has carrier");
                        return false;
                    }
                }

                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }

            _needStartCheck = false;
        }
        private void CheckCarrierID(int id)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                Notify($"Check carrier ID");
                string reason = string.Empty;

                if (_readCarrierId != _reservedCarrierId)
                {
                    _cassetteRobotModule.CheckCarrierIDFailAlarm.Set($"read id={_readCarrierId}, reserved id={_reservedCarrierId}, mismatch");
                    return false;
                }
                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }

            _needStartCheck = false;
        }
        private void Goto(int id, ModuleName target, int slot, Hand hand, bool isPickReady, int timeout)
        {
            _routineStep = (RoutineStep)Enum.Parse(typeof(RoutineStep), id.ToString());
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Send goto to {target} command to robot device");
                string reason;
                _cassetteRobotModule.RobotGoto(target, slot, hand, isPickReady, out reason);
                return true;
            }, () =>
            {
                return true;
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    _cassetteRobotModule.GotoFailAlarm.Set($"goto to {target} failed for robot error, error code={_cassetteRobotModule.CarrierRobotDevice.ErrorCode}");
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    _cassetteRobotModule.GotoTimeoutAlarm.Set($"timeout over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        private void CheckGotoFinish(int id, ModuleName target, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Check goto to {target} command finish");

                return true;
            }, () =>
            {
                if (_cassetteRobotModule.CarrierRobotDevice.IsError)
                    return null;

                if (_cassetteRobotModule.CarrierRobotDevice.IsReady() && !_cassetteRobotModule.CarrierRobotDevice.IsError)
                {
                    return true;
                }

                return false;
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    _cassetteRobotModule.GotoFailAlarm.Set($"goto to {target} failed for robot error, error code={_cassetteRobotModule.CarrierRobotDevice.ErrorCode}");
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    _cassetteRobotModule.GotoTimeoutAlarm.Set($"timeout over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        private void SaferDoorOpen(int id, bool isOpen, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Shutter Door {(isOpen ? "Open" : "Close")}");

                if (isOpen)
                    _cassetteRobotModule.DoorDevice.Open();
                else
                    _cassetteRobotModule.DoorDevice.Close();

                if (isOpen)
                    (Singleton<EquipmentManager>.Instance.Modules[_source] as LoadPortModule)?.LPDevice.Unclamp(out _);

                return true;
            }, () =>
            {
                return isOpen ? _cassetteRobotModule.DoorDevice.OpenCloseStatus == Devices.DeviceStatus.Open && (Singleton<EquipmentManager>.Instance.Modules[_source] as LoadPortModule).IsReleased : _cassetteRobotModule.DoorDevice.OpenCloseStatus == Devices.DeviceStatus.Close;
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw new RoutineFaildException();
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    if (isOpen)
                        _cassetteRobotModule.ShutterOpenTimeoutAlarm.Set($"timeout over {timeout} seconds");
                    else
                        _cassetteRobotModule.ShutterCloseTimeoutAlarm.Set($"timeout over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw new RoutineBreakException();
            }
        }
    }
}