Virgo_Mask/VirgoRT/Modules/EFEMs/EfemEntity.cs

using System;
using Aitex.Core.Common;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.OperationCenter;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Utilities;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.Schedulers;
using MECF.Framework.Common.SubstrateTrackings;
using VirgoCommon;
using VirgoRT.Device;
using VirgoRT.Device.YASKAWA;
using VirgoRT.Devices.EFEM;
using VirgoRT.Modules.LPs;
using BAI.Systems.Common.Substrates;

namespace VirgoRT.Modules
{
    class EfemEntity : Entity, IEntity, IModuleEntity
    {
        private int _bigWafer = 0;
        private int _midWafer = 0;
        private int _smallWafer = 0;
        private bool _hasPlacementError = false;
        private bool _EnableResetEFEMError = false;

        public enum STATE
        {
            Unknown,            // 0
            Initializing,       // 1
            Idle,               // 2
            Error,              // 3
            Picking,            // 4
            Placing,            // 5
            Aligning,           // 6
            Mapping,            // 7
            Init,               // 8
            Orgshing,           // 9
            Lifting,            // 10
            InitingAL,          // 11
            InitingRB,          // 12
            Extending,          // 13
            Retracting,         // 14
            //SettingLamp,        // 15
            Swapping,
            Gotoing,
            Gripping,
            Ungripping,
            Fliping,
            EmsStoping,
            SetSpeed,
            Loading,
        }

        public enum MSG
        {
            HomeAll,            // 0
            Pick,               // 1
            Place,              // 2
            Align,              // 3
            ActionDone,         // 4
            RecHwMsg,           // 5
            MoveCmd,            // 6
            //LED,                // 7
            Recover,            // 8
            Goto,               // 9
            Error,              // 10
            Online,             // 11
            CommReady,          // 12
            Lift,               // 13
            HomeAL,             // 14
            HomeRB,             // 15
            Extend,             // 16
            Retract,            // 17
            PMLiftPinUp,        // 18
            PMLiftPinDown,      // 19
            TurnOffBuzzer, 
            //SwitchOnBuzzerAndRed,
            Abort,
            Map,
			ToInit,
			Cool,
            PickAndPlace,
            Grip,
            Ungrip,
            Flip,
            LiftActionDone,
            EmsStop,
            SetRobotSpeed,
            Load,
        }

        public enum EfemType
        {
            FutureEfem = 1,
            JetEfem = 2,
            BrooksEFEM = 3,
            BeamSUNWAY = 4,
        }

        public bool Check(int msg, out string reason, params object[] args)
        {
            throw new NotImplementedException();
        }

        // Fields
        //
        private readonly string Name;
        private readonly Efem _efem;
        private readonly LoadPortModule[] _lpms = new LoadPortModule[2];
        private readonly EfemType _efemType;
        public Efem EfemDevice => _efem;
        public EfemType EFEMType => _efemType;

        // Constructor
        //
        public EfemEntity()
        {
            _smallWafer = SC.GetValue<int>($"System.SmallWafer");
            _midWafer = SC.GetValue<int>($"System.MidWafer");
            _bigWafer = SC.GetValue<int>($"System.BigWafer");
            _efemType = (EfemType)SC.GetValue<int>($"EFEM.EfemType");
            _EnableResetEFEMError = SC.GetValue<bool>($"EFEM.EnableResetError");

            _efem = new Efem();

            Name = ModuleName.EFEM.ToString();

            InitFsmMap();
        }

        public void NotifyLP(ModuleName mod, LoadportEntity.MSG msg)
        {
            _lpms[mod - ModuleName.LP1].PostMsg(msg);
        }
        public void NotifyLPError(ModuleName mod )
        {
            _lpms[mod - ModuleName.LP1].PostMsg(LoadportEntity.MSG.ActionDone);

            //_lpms[mod - ModuleName.LP1]..OnError();
        }

        public void NotifyLP(ModuleName mod, LoadPortModule.MSG msg)
        {
            _lpms[mod - ModuleName.LP1].PostMsg(msg);
        }

        public bool IsLPIdle(ModuleName mod)
        {
            return _lpms[mod - ModuleName.LP1].IsReady;
        }

        protected override bool Init()
        {
            _lpms[0] = new LoadPortModule(ModuleName.LP1, _efem);
            _lpms[1] = new LoadPortModule(ModuleName.LP2, _efem);

            _lpms[0].Initialize();
            _lpms[1].Initialize();

            OP.Subscribe($"{ModuleName.EFEM}.{EfemOperation.Home}",         (cmd, args) => { PostMsg(MSG.HomeAll); return true; });
            OP.Subscribe($"{ModuleName.EFEM}.{EfemOperation.ClearError}",        (cmd, args) => { PostMsg(MSG.Recover); return true; });
            OP.Subscribe($"{ModuleName.EFEM}.{EfemOperation.TurnOffBuzzer}", (cmd, args) => { PostMsg(MSG.TurnOffBuzzer); return true; });
            //OP.Subscribe($"{ModuleName.EFEM}.{EfemOperation.SwitchOnBuzzerAndRed}", (cmd, args) => { PostMsg(MSG.SwitchOnBuzzerAndRed); return true; });
            OP.Subscribe($"{ModuleName.EFEM}.Online",                       (cmd, args) => { PostMsg(MSG.Online); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Pick}",    (cmd, args) => { PostMsg(MSG.Pick, args[0], args[1], args[3], args[2]); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Place}",   (cmd, args) => { PostMsg(MSG.Place, args[0], args[1], args[3], args[2]); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Extend}",  (cmd, args) => { PostMsg(MSG.Extend, args[0], args[1], args[2]); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Retract}", (cmd, args) => { PostMsg(MSG.Retract, args[0], args[1]); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Abort}",   (cmd, args) => { PostMsg(MSG.Abort); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Home}",    (cmd, args) => { PostMsg(MSG.HomeRB, args); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Grip}", (cmd, args) =>
            {
                bool isGrip = ((string)args[0]).ToLower() == "on";

                PostMsg(isGrip ? MSG.Grip : MSG.Ungrip,  args[1]); 
                
                return true;
            });

            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Flip}", (cmd, args) => { PostMsg(MSG.Flip, args[0]); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.EmsStop}", (cmd, args) => { PostMsg(MSG.EmsStop); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.SetRobotSpeed}", (cmd, args) => { PostMsg(MSG.SetRobotSpeed, args[0]); return true; });

            OP.Subscribe($"{ModuleName.Aligner1}.{EfemOperation.Home}",     (cmd, args) => { PostMsg(MSG.HomeAL, ModuleName.Aligner1); return true; });
            OP.Subscribe($"{ModuleName.Aligner2}.{EfemOperation.Home}",     (cmd, args) => { PostMsg(MSG.HomeAL, ModuleName.Aligner2); return true; });
            OP.Subscribe($"{ModuleName.Cooling1}.{EfemOperation.Home}", (cmd, args) => { PostMsg(MSG.HomeAL, ModuleName.Cooling1); return true; });
            OP.Subscribe($"{ModuleName.Cooling2}.{EfemOperation.Home}", (cmd, args) => { PostMsg(MSG.HomeAL, ModuleName.Cooling2); return true; });
            OP.Subscribe($"{ModuleName.Aligner1}.{EfemOperation.Align}",    (cmd, args) => { PostMsg(MSG.Align, ModuleName.Aligner1, args[0]); return true; });
            OP.Subscribe($"{ModuleName.Aligner2}.{EfemOperation.Align}",    (cmd, args) => { PostMsg(MSG.Align, ModuleName.Aligner2, args[0]); return true; });
            OP.Subscribe($"{ModuleName.Cooling1}.{EfemOperation.Align}", (cmd, args) => { PostMsg(MSG.Align, ModuleName.Cooling1, args[0]); return true; });
            OP.Subscribe($"{ModuleName.Cooling2}.{EfemOperation.Align}", (cmd, args) => { PostMsg(MSG.Align, ModuleName.Cooling2, args[0]); return true; });
            OP.Subscribe($"{ModuleName.Aligner1}.{EfemOperation.Lift}",     (cmd, args) => { PostMsg(MSG.Lift, ModuleName.Aligner1); return true; });
            OP.Subscribe($"{ModuleName.Aligner2}.{EfemOperation.Lift}",     (cmd, args) => { PostMsg(MSG.Lift, ModuleName.Aligner2); return true; });
            OP.Subscribe($"{ModuleName.Cooling1}.{EfemOperation.Lift}", (cmd, args) => { PostMsg(MSG.Lift, ModuleName.Cooling1); return true; });
            OP.Subscribe($"{ModuleName.Cooling2}.{EfemOperation.Lift}", (cmd, args) => { PostMsg(MSG.Lift, ModuleName.Cooling2); return true; });

            DATA.Subscribe($"{Name}.FsmState",     () => ((STATE)fsm.State).ToString());
            DATA.Subscribe($"{Name}.FsmPrevState", () => ((STATE)fsm.PrevState).ToString());
            DATA.Subscribe($"{Name}.FsmLastMessage", GetFsmLastMessage);
            DATA.Subscribe($"{Name}.SmallWafer", () => _smallWafer);
            DATA.Subscribe($"{Name}.BigWafer", () => _bigWafer);
            DATA.Subscribe($"{Name}.MidWafer", () => _midWafer);
            DATA.Subscribe($"{Name}.HasPlacementError", () => _hasPlacementError);

            return true;
        }

        private void InitFsmMap()
        {
            fsm = new StateMachine<EfemEntity>("EFEM", (int)STATE.Unknown, 50);
            fsm.EnableRepeatedMsg(true);

            AnyStateTransition(FSM_MSG.TIMER,                       fnMonitor,      FSM_STATE.SAME);
            AnyStateTransition(MSG.RecHwMsg,                        fnRecMsg,       FSM_STATE.SAME);
            //AnyStateTransition(MSG.LED,                           fnSetLED,       STATE.SettingLamp);
            AnyStateTransition(MSG.TurnOffBuzzer,                   fnTurnOffBuzzer, FSM_STATE.SAME);
            //AnyStateTransition(MSG.SwitchOnBuzzerAndRed,          fnSwitchOnBuzzerAndRed, FSM_STATE.SAME);
            AnyStateTransition(MSG.Recover,                         fnRecover,      FSM_STATE.SAME);
            AnyStateTransition(MSG.Error,                           fnError,        STATE.Error);
            AnyStateTransition(MSG.Online,                          fnOnline,       FSM_STATE.SAME);
            AnyStateTransition(MSG.Abort,                           fnAbortRobot,   STATE.Idle);
            AnyStateTransition(MSG.ToInit,                          fnToInit,       STATE.Init);

            EnterExitTransition<STATE, FSM_MSG>(STATE.Initializing, fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.InitingRB,    fnEnterExecute, FSM_MSG.NONE, null);
            EnterExitTransition<STATE, FSM_MSG>(STATE.InitingAL,    fnEnterExecute, FSM_MSG.NONE, null);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Picking,      fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Gotoing,      fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Placing,      fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Swapping,     fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Extending,    fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Retracting,   fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Lifting,      fnEnterExecute, FSM_MSG.NONE, null);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Aligning,     fnEnterExecute, FSM_MSG.NONE, null);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Mapping,      fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Gripping,     fnEnterExecute, FSM_MSG.NONE, null);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Ungripping,   fnEnterExecute, FSM_MSG.NONE, null);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Fliping,      fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.EmsStoping,   fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.SetSpeed,     fnEnterExecute, FSM_MSG.NONE, fnExitExecute);
            EnterExitTransition<STATE, FSM_MSG>(STATE.Loading,      fnEnterExecute, FSM_MSG.NONE, fnExitExecute);

            Transition(STATE.Unknown, MSG.CommReady,                null, STATE.Init);

            // Home
            Transition(STATE.Init,      MSG.HomeAll,                fnHomeAll,      STATE.Initializing);
            Transition(STATE.Idle,      MSG.HomeAll,                fnHomeAll,      STATE.Initializing);    // 暂时加，出错的时候做 HOME
            Transition(STATE.Error,     MSG.HomeAll,                fnHomeAll,      STATE.Initializing);
            Transition(STATE.Initializing, MSG.ActionDone,          fnActionDone,   STATE.Orgshing);
            Transition(STATE.Orgshing,  MSG.ActionDone,             fnActionDone,   STATE.Idle);
            Transition(STATE.Idle,      MSG.HomeRB,                 fnHomeRobot,    STATE.InitingRB);
            Transition(STATE.InitingRB, MSG.ActionDone,             null,           STATE.Idle);
            //Transition(STATE.SettingLamp, MSG.ActionDone,         fnSetLampDone,  STATE.Idle);

            // Pick
            Transition(STATE.Idle,      MSG.Pick,                   fnPick,         STATE.Picking);
            Transition(STATE.Picking,   MSG.ActionDone,             fnActionDone,   STATE.Idle);
            Transition(STATE.Picking,   MSG.PMLiftPinUp,            fnPmPinUp,      STATE.Idle);
            Transition(STATE.Picking,   MSG.PMLiftPinDown,          fnPmPinDown,    STATE.Idle);
            
            // Place
            Transition(STATE.Idle,      MSG.Place,                  fnPlace,        STATE.Placing);
            Transition(STATE.Placing,   MSG.ActionDone,             fnActionDone,   STATE.Idle);
            Transition(STATE.Placing,   MSG.PMLiftPinUp,            fnPmPinUp,      STATE.Idle);
            Transition(STATE.Placing,   MSG.PMLiftPinDown,          fnPmPinDown,    STATE.Idle);

            // PickAndPlace
            Transition(STATE.Idle,      MSG.PickAndPlace,           fnSwap,         STATE.Swapping);
            Transition(STATE.Swapping,  MSG.ActionDone,             fnActionDone,   STATE.Idle);
            Transition(STATE.Swapping,  MSG.PMLiftPinUp,            fnPmPinUp,      STATE.Idle);
            Transition(STATE.Swapping,  MSG.PMLiftPinDown,          fnPmPinDown,    STATE.Idle);
            
            // Goto
            Transition(STATE.Idle,      MSG.Goto,                   fnGoto,         STATE.Gotoing);
            Transition(STATE.Gotoing,   MSG.ActionDone,             fnActionDone,   STATE.Idle);

            // Extend
            Transition(STATE.Idle,      MSG.Extend,                 fnExtend,       STATE.Extending);
            Transition(STATE.Extending, MSG.ActionDone,             fnActionDone,   STATE.Idle);

            // Retract
            Transition(STATE.Idle,      MSG.Retract,                fnRetract,      STATE.Retracting);
            Transition(STATE.Retracting, MSG.ActionDone,            fnActionDone,   STATE.Idle);

            // Map
            Transition(STATE.Idle,      MSG.Map,                    fnMap,          STATE.Mapping);
            Transition(STATE.Mapping,   MSG.ActionDone,             fnActionDone,   STATE.Idle);

            // Grip
            Transition(STATE.Idle,      MSG.Grip,                   fnGrip,         STATE.Gripping);
            Transition(STATE.Gripping,  MSG.ActionDone,             fnActionDone,   STATE.Idle);

            // Ungrip
            Transition(STATE.Idle,      MSG.Ungrip,                 fnUngrip,       STATE.Ungripping);
            Transition(STATE.Ungripping, MSG.ActionDone,            fnActionDone,   STATE.Idle);

            // Aligner
            Transition(STATE.Idle,      MSG.HomeAL,                 fnHomeAligner,  STATE.InitingAL);
            Transition(STATE.InitingAL, MSG.ActionDone,             fnActionDone,   STATE.Idle);
            Transition(STATE.Idle,      MSG.Lift,                   fnLift,         STATE.Lifting);
            Transition(STATE.Lifting,   MSG.LiftActionDone,         fnActionDone,   STATE.Idle);
            Transition(STATE.Idle,      MSG.Align,                  fnAlign,        STATE.Aligning);
            Transition(STATE.Aligning,  MSG.ActionDone,             fnActionDone,   STATE.Idle);

            // Flip
            Transition(STATE.Idle,      MSG.Flip,                   fnFlip,         STATE.Fliping);
            Transition(STATE.Fliping,   MSG.ActionDone,             fnActionDone,   STATE.Idle);

            // EmsStop
            Transition(STATE.Idle,      MSG.EmsStop,                fnEmsStop,      STATE.EmsStoping);
            Transition(STATE.EmsStoping,    MSG.ActionDone,         fnActionDone,   STATE.Idle);

            // Set Robot Speed
            Transition(STATE.Idle,      MSG.SetRobotSpeed,          fnSetRobotSpeed,    STATE.SetSpeed);
            Transition(STATE.SetSpeed,  MSG.ActionDone,             fnActionDone,   STATE.Idle);

            // Load Foup
            Transition(STATE.Idle,      MSG.Load,                   fnLoadFoup,     STATE.Loading);
            Transition(STATE.Loading,   MSG.ActionDone,             fnActionDone,   STATE.Idle);

            EnumLoop<STATE>.ForEach((item) => { fsm.MapState((int)item, item.ToString()); });

            EnumLoop<MSG>.ForEach((item) => { fsm.MapMessage((int)item, item.ToString()); });
        }

        private bool fnHomeAll(object[] param)
        {
            _efem.ClearActions();
            _efem.HomeAll();
            return true;
        }

        private bool fnHomeRobot(object[] param)
        {
            _efem.ClearActions();
            _efem.Home(ModuleName.EfemRobot, (string)param[0]);
            return true;
        }

        private bool fnHomeAligner(object[] param)
        {
            // module
            ModuleName unit = ModuleName.EFEM;
            if (param[0] is string s1)
                unit = ModuleNameString.ToEnum(s1);
            else if (param[0] is ModuleName mod)
                unit = mod;
            else
                throw new ArgumentException("Argument error");

            _efem.Home(unit, "");
            return true;
        }

        private bool fnEnterExecute(object[] param)
        {
            if(!_efem.HasUncompleteActions)
                _efem.ExecuteAction();
            return false;
        }

        private bool fnExitExecute(object[] param)
        {
            if (!_efem.HasUncompleteActions)
                _efem.ExecuteAction();
            return false;
        }

        private bool fnActionDone(object[] param)
        {
            try
            {
                EfemOperation actionType = (EfemOperation)param[0];

                if (((fsm.State == (int)STATE.Orgshing) && (actionType == EfemOperation.Orgsh || actionType == EfemOperation.Home)) ||
                    (fsm.State == (int)STATE.Initializing && actionType == EfemOperation.ClearError) ||
                    (fsm.State == (int)STATE.Extending) ||
                    (fsm.State == (int)STATE.Picking) ||
                    (fsm.State == (int)STATE.Placing) ||
                    (fsm.State == (int)STATE.Swapping) ||
                    (fsm.State == (int)STATE.Aligning) ||
                    (fsm.State == (int)STATE.Fliping))
                {
                    if (_efem.HasActions || _efem.HasUncompleteActions)
                    {
                        if(!_efem.HasUncompleteActions)
                        {
                            _efem.ExecuteAction();
                        }
                        return false;
                    }
                }

                return true;
            }
            catch (Exception ex)
            {
                EV.PostAlarmLog(ModuleName.EFEM.ToString(), ex.Message);
                return true;
            }
        }

        public bool CheckToPostMessage(int msg, params object[] args)
        {
            if (!fsm.FindTransition(fsm.State, msg))
            {
                EV.PostWarningLog(Name, $"{Name} is in {(STATE)fsm.State} state，can not do {(MSG)msg}");
                return false;
            }

            Running = true;
            fsm.PostMsg(msg, args);

            return true;
        }

        private bool fnRecMsg(object[] param)
        {
            if (param == null) return false;
            string strHwMsg = param[0] as string;

            if (_efem is EfemBase device)
            {
                device.ReceiveMessage(strHwMsg);
                return true;
            }
            return false;
        }

        private bool fnMonitor(object[] param)
        {
            STATE curSt = (STATE)fsm.State;
            if (curSt == STATE.Initializing || curSt == STATE.Mapping || curSt == STATE.Picking || curSt == STATE.Placing
                || curSt == STATE.Orgshing || curSt == STATE.Lifting || curSt == STATE.Extending || curSt == STATE.Retracting
                || curSt == STATE.InitingAL || curSt == STATE.InitingRB)
            {
                int time = SC.GetValue<int>("EFEM.MotionTimeout");

                if (fsm.ElapsedTime > time * 1000)
                {
                    EV.PostAlarmLog("EFEM", $"Can not complete motion {curSt} in {time} seconds. ");
                    PostMsg(MSG.Error);
                }
            }
            else if (curSt == STATE.Aligning)
            {
                int _timeout = SC.GetValue<int>("EFEM.AlignerTimeout");
                if (fsm.ElapsedTime > _timeout * 1000)
                {
                    EV.PostAlarmLog(ModuleName.Aligner.ToString(), "Align timeout");
                    PostMsg(MSG.Error);
                }
            }
            else if (curSt == STATE.Idle)
            {
                if (_efem.HasActions && !_efem.HasUncompleteActions)
                {
                    _efem.ExecuteAction();
                }
            }

            if (_efemType == EfemType.BrooksEFEM && (SC.GetValue<bool>("EFEM.LoadPort.EnableDockUndock") == false))
            {
                _efem.QueryLPState();
            }

            // update Loadport wafer placement parity error status
            _hasPlacementError = _efem[ModuleName.LP1].HasPlacementError || _efem[ModuleName.LP2].HasPlacementError;

            return true;
        }
 
        private bool fnOnline(object[] param)
        {
            bool bOnlineFlag = (bool)param[0];

            if (_efem is EfemBase efem)
            {
                efem.SetOnline(bOnlineFlag);
            }
            return true;
        }

        private string GetFsmLastMessage()
        {
            int msg = fsm.LastMsg;
            if (msg >= (int)MSG.HomeAll && msg <= (int)MSG.Error)
                return ((MSG)msg).ToString();

            if (msg == (int)FSM_MSG.TIMER)
                return "Timer";

            return msg.ToString();
        }

        private bool fnError(object[] param)
        {
            return true;
        }

        private bool fnToInit(object[] param)
        {
            return true;
        }

        private bool fnRecover(object[] param)
        {
            _efem.ClearActions();
            _efem.ClearError();
            //_efem.ExecuteAction();
            return true;
        }

        private bool fnAbortRobot(object[] param)
        {
            _efem.ClearActions();
            _efem.AbortRobot();
            //_efem.ExecuteAction();
            return true;
        }

        private bool fnSetLED(object[] param)
        {
            LightType light = (LightType)param[0];
            LightStatus st = (LightStatus)param[1];

            _efem.SetLamp(light, st);
            return true;
        }

        private bool fnTurnOffBuzzer(object[] param)
        {
            _efem.TurnOffBuzzer();
            return false;
        }

        //private bool fnSwitchOnBuzzerAndRed(object[] param)
        //{
        //    _efem.SwitchOnBuzzerAndRed();
        //    return false;
        //}
        //

        private bool fnSetLampDone(object[] param)
        {
            if (_efem.HasActions && !_efem.HasUncompleteActions)
            {
                _efem.ExecuteAction();
                return false;
            }
            else
            {
                return true;
            }
        }

        private bool fnPick(object[] param)
        {
            // module
            ModuleName unit = ModuleName.EFEM;
            if (param[0] is string s1)
                unit = ModuleNameString.ToEnum(s1);
            else if (param[0] is ModuleName mod)
                unit = mod;
            else
                throw new ArgumentException("Argument error");

            // slot
            byte slot = (byte)(int)param[1];

            // hand
            Hand arm = (Hand)Enum.Parse(typeof(Hand),  param[2].ToString());

            // wafer size
            WaferSize ws1 = WaferSize.WS0;
            if (param[3] is string s2)
            {
                if (Enum.TryParse(s2, out WaferSize p5))
                    ws1 = p5;
            }
            else if (param[3] is WaferSize p6)
            {
                ws1 = p6;
            }

            MoveParam mp = new MoveParam(unit, slot, ModuleName.EfemRobot, (byte)arm, arm, ws1);
            if (!_efem.Pick(mp))
                return false;

            return true;
        }

        private bool fnPlace(object[] param)
        {
            // module
            ModuleName unit = ModuleName.EFEM;
            if (param[0] is string s1)
                unit = ModuleNameString.ToEnum(s1);
            else if (param[0] is ModuleName mod)
                unit = mod;
            else
                throw new ArgumentException("Argument error");

            // slot
            byte slot = (byte)(int)param[1];

            // hand
            Hand arm = (Hand)Enum.Parse(typeof(Hand), param[2].ToString());

            // wafer size
            WaferSize ws1 = WaferSize.WS0;
            if (param[3] is string s2)
            {
                if (Enum.TryParse(s2, out WaferSize p5))
                    ws1 = p5;
            }
            else if (param[3] is WaferSize p6)
            {
                ws1 = p6;
            }

            MoveParam mp = new MoveParam(ModuleName.EfemRobot, (byte)arm, unit, slot, arm, ws1);
            if (!_efem.Place(mp))
                return false;
		   
            return true;
        }

        private bool fnGoto(object[] param)
        {
            // module
            ModuleName unit = ModuleName.EFEM;
            if (param[0] is string s1)
                unit = ModuleNameString.ToEnum(s1);
            else if (param[0] is ModuleName mod)
                unit = mod;
            else
                throw new ArgumentException("Argument error");

            // slot
            byte slot = (byte)(int)param[1];


            Hand arm = Hand.Blade1;

            WaferSize ws = WaferSize.WS6;

            if (WaferManager.Instance.CheckHasWafer(ModuleName.EfemRobot, 0))
            {
                arm = Hand.Blade1;
                ws = WaferManager.Instance.GetWafer(ModuleName.EfemRobot, 0).Size;
            }
            else if (WaferManager.Instance.CheckHasWafer(ModuleName.EfemRobot, 1))
            {
                arm = Hand.Blade2;
                ws = WaferManager.Instance.GetWafer(ModuleName.EfemRobot, 1).Size;
            }

            MoveParam mp = new MoveParam(unit, slot, unit, slot, arm, ws);
            _efem.Goto(mp);
            return true;
        }

        private bool fnSwap(object[] param)
        {
            // module
            ModuleName unit = ModuleName.EFEM;
            if (param[0] is string s1)
                unit = ModuleNameString.ToEnum(s1);
            else if (param[0] is ModuleName mod)
                unit = mod;
            else
                throw new ArgumentException("Argument error");

            // pickSlot
            byte pickSlot = (byte)(int)param[1];

            // pick hand
            Hand pickArm = (Hand)Enum.Parse(typeof(Hand), param[2].ToString());

            // wafer size
            WaferSize ws1 = WaferSize.WS0;
            if (param[5] is string s2)
            {
                if (Enum.TryParse(s2, out WaferSize p5))
                    ws1 = p5;
            }
            else if (param[5] is WaferSize p6)
            {
                ws1 = p6;
            }

            MoveParam pickParam = new MoveParam(unit, pickSlot, ModuleName.EfemRobot, (byte)pickArm, pickArm, ws1);

            // place slot
            byte placeSlot = (byte)(int)param[4];

            // hand
            Hand placeArm = (Hand)Enum.Parse(typeof(Hand), param[3].ToString());
 
            MoveParam placeParam = new MoveParam(ModuleName.EfemRobot, (byte)placeArm, unit, placeSlot, placeArm, ws1);

            _efem.PickAndPlace(pickParam, placeParam);
            return true;
        }

        private bool fnExtend(object[] param)
        {
            // module
            ModuleName unit = ModuleName.EFEM;
            if (param[0] is string s1)
                unit = ModuleNameString.ToEnum(s1);
            else if (param[0] is ModuleName mod)
                unit = mod;
            else
                throw new ArgumentException("Argument error");

            Hand arm = (Hand)Enum.Parse(typeof(Hand), (string)param[2]);

            ExtendParam mp = new ExtendParam
            {
                Module = unit,
                Arm = arm,
                Pos = (ExtendPos)Enum.Parse(typeof(ExtendPos), param[1] as string)
            };

            if (!_efem.Extend(mp))
                return false;

            return true;
        }

        private bool fnRetract(object[] param)
        {
            // module
            ModuleName unit = ModuleName.EFEM;
            if (param[0] is string s1)
                unit = ModuleNameString.ToEnum(s1);
            else if (param[0] is ModuleName mod)
                unit = mod;
            else
                throw new ArgumentException("Argument error");

            Hand arm = (Hand)Enum.Parse(typeof(Hand), (string)param[2]);

            ExtendParam mp = new ExtendParam
            {
                Module = unit,
                Arm = arm,
                Pos = (ExtendPos)Enum.Parse(typeof(ExtendPos), param[1] as string)
            };

            if (!_efem.Retract(mp))
                return false;

            return true;
        }

        private bool fnPmPinUp(object[] param)
        {
            _efem.UpdateStatus((ushort)param[0], ActionStatus.Completed);
            if (_efem.HasActions && !_efem.HasUncompleteActions)
            {
                _efem.ExecuteAction();
                return false;
            }
            else
            {
                return true;
            }
        }

        private bool fnPmPinDown(object[] param)
        {
            _efem.UpdateStatus((ushort)param[0], ActionStatus.Completed);
            if (_efem.HasActions && !_efem.HasUncompleteActions)
            {
                _efem.ExecuteAction();
                return false;
            }
            else
            {
                return true;
            }
        }

        private bool fnLift(object[] param)
        {
            // module
            ModuleName unit = ModuleName.EFEM;
            if (param[0] is string s1)
                unit = ModuleNameString.ToEnum(s1);
            else if (param[0] is ModuleName mod)
                unit = mod;
            else
                throw new ArgumentException("Argument error");

            bool isUp = true;

            if (param.Length > 1)
            {
                isUp = (bool) param[1];
            }

            if (isUp)
            {
                if (!_efem.SetPinUp(unit))
                    return false;
            }
            else
            {
                if (!_efem.SetPinDown(unit))
                    return false;
            }

            return true;
        }

        private bool fnAlign(object[] param)
        {
            // module
            ModuleName unit = ModuleName.EFEM;
            if (param[0] is string s1)
                unit = ModuleNameString.ToEnum(s1);
            else if (param[0] is ModuleName mod)
                unit = mod;
            else
                throw new ArgumentException("Argument error");

            // wafer size
            WaferSize ws1 = WaferSize.WS0;
            if (param[1] is string s2)
            {
                if (Enum.TryParse(s2, out WaferSize p5))
                    ws1 = p5;
            }
            else if (param[1] is WaferSize p6)
            {
                ws1 = p6;
            }

            if (!_efem.Align(unit, 1000, ws1))
                return false;
            return true;
        }

        private bool fnMap(object[] param)
        {
            // module
            ModuleName unit = ModuleName.EFEM;
            if (param[0] is string s1)
                unit = ModuleNameString.ToEnum(s1);
            else if (param[0] is ModuleName mod)
                unit = mod;
            else
                throw new ArgumentException("Argument error");

            if (!_efem.Map(unit))
                return false;

            return true;
        }

        private bool fnGrip(object[] param)
        {

            Hand arm = (Hand)Enum.Parse(typeof(Hand), (string)param[0]);

            if (!_efem.Grip(arm, true))
                return false;

            return true;
        }
        private bool fnUngrip(object[] param)
        {
 
            Hand arm = (Hand)Enum.Parse(typeof(Hand), (string)param[0]);

            if (!_efem.Grip(arm, false))
                return false;

            return true;
        }
        private bool fnFlip(object[] param)
        {
            string orient = (string)param[0];

            if (!_efem.Flip(orient))
                return false;

            return true;
        }

        private bool fnEmsStop(object[] param)
        {
            if (!_efem.EmsStop())
                return false;

            return true;
        }

        private bool fnSetRobotSpeed(object[] param)
        {
            string speed = (string)param[0];

            if (!_efem.SetSpeed(speed))
                return false;

            return true;
        }

        private bool fnLoadFoup(object[] param)
        {
            ModuleName lp = ModuleHelper.Converter((string)param[0]);
            if (!_efem.Load(lp))
                return false;

            return true;
        }

        public bool IsIdle
        {
            get { return fsm.State == (int)STATE.Idle; }
        }

        public bool IsError
        {
            get { return fsm.State == (int)STATE.Error; }
        }

        public bool IsInit
        {
            get { return fsm.State == (int)STATE.Unknown || fsm.State==(int)STATE.Init; }
        }

        public bool IsBusy
        {
            get { return !IsInit && !IsError && !IsIdle; }
        }

        public bool IsOnline { get; internal set; }

        public int Invoke(string function, params object[] args)
        {
            switch (function)
            {
            case "Home":
                CheckToPostMessage((int)MSG.HomeAll);
                return (int)MSG.HomeAll;
            }

            return (int)FSM_MSG.NONE;
        }

        public bool CheckAcked(int msg)
        {
            return fsm.CheckExecuted(msg);
        }

        internal void InvokeReset()
        {
            if (_EnableResetEFEMError && fsm.State == (int) STATE.Error)
            {
                PostMsg((int)MSG.Recover);
            }
        }

        public int InvokeAlign(string module, float time)
        {
            
            if (CheckToPostMessage((int)MSG.Align, module, time))
                return (int)MSG.Align;

            return (int)FSM_MSG.NONE;
        }

        public int InvokeLiftDown(string module)
        {
            if (CheckToPostMessage((int)MSG.Lift, module, false))
                return (int)MSG.Lift;

            return (int)FSM_MSG.NONE;
        }

        public int InvokePick(ModuleName source, int slot, Hand hand, WaferSize size)
        {
            if (CheckToPostMessage((int)MSG.Pick, source, slot, hand, size))
                return (int)MSG.Pick;

            return (int)FSM_MSG.NONE;
        }

        public int InvokeGoto(ModuleName source, int slot)
        {
            if (CheckToPostMessage((int)MSG.Goto, source, slot))
                return (int)MSG.Goto;

            return (int)FSM_MSG.NONE;
        }

        public int InvokePlace(ModuleName target, int slot, Hand hand, WaferSize size)
        {
            if (CheckToPostMessage((int)MSG.Place, target, slot, hand, size))
                return (int)MSG.Place;

            return (int)FSM_MSG.NONE;
        }


        public int InvokePickAndPlace(ModuleName targetModule, Hand pickHand, int pickSlot,  Hand placeHand, int placeSlot, WaferSize size)
        {
            if (CheckToPostMessage((int)MSG.PickAndPlace, targetModule, pickSlot, pickHand, placeHand, placeSlot, size))
                return (int)MSG.PickAndPlace;

            return (int)FSM_MSG.NONE;
        }

        public int InvokeMap(string target )
        {
            if (CheckToPostMessage((int)MSG.Map, target ))
                return (int)MSG.Map;

            return (int)FSM_MSG.NONE;
        }

        public int InvokeFlip(string orient)
        {
            if (CheckToPostMessage((int)MSG.Flip, orient))
                return (int)MSG.Flip;

            return (int)FSM_MSG.NONE;
        }

        public bool IsPrepareTransferReady(ModuleName module, EnumTransferType type, int slot)
        {
            if (type == EnumTransferType.Pick)
            {
                //需要补充：判断LP 放好了，而且已经map过。
                return _efem[module].HasCassette && _efem[module].IsMapped;
            }
            else if (type == EnumTransferType.Place)
            {
                //需要补充：判断LP 放好了，而且已经map过。
                return _efem[module].HasCassette && _efem[module].IsMapped;
            }

            return false;
        }

        internal bool CheckReadyRunNewJob(ModuleName module)
        {
            //???
            return true;
        }

        internal bool CheckReadyTransfer(ModuleName module)
        {
            return _efem[module].HasCassette && _efem[module].IsMapped;
        }
        internal bool CheckPlaced(ModuleName module)
        {
            return _efem[module].HasCassette;
        }
		
        internal void NoteJobStart(ModuleName module)
        {
            _efem[module].NoteJobStart();
        }

        internal void NoteJobComplete(ModuleName module)
        {
            _efem[module].NoteJobComplete();
        }

        internal string GetCarrierID(ModuleName module)
        {
            return _efem[module].CarrierId;
        }
    }

    /// <summary>
    /// LP entity
    /// </summary>
    class LoadportEntity : Entity, IEntity
    {
        private enum STATE
        {
            Unknown,
            Idle,            // 1
            Initializing,    // 2
            Initialized,     // 3
            Mapping,         // 4
            Mapped,          // 5
        }

        public enum MSG
        {
            Home,            // 0
            Map,             // 1
            ActionDone,      // 2
            RecHwMsg,        // 3
            Recover,         // 4
            Abort,           // 5
            Online,          // 6
            Error            // 7
        }

        private readonly Efem _efem;

        private ModuleName Module { get; }

        public LoadportEntity(ModuleName mod, Efem efem)
        {
            this.Module = mod;
            _efem = efem;

            InitFsmMap();

            OP.Subscribe($"{Module}.Home",   (cmd, args) => { PostMsg(MSG.Home); return true; });
            OP.Subscribe($"{Module}.Abort",  (cmd, args) => { PostMsg(MSG.Abort); return true; });
            //OP.Subscribe($"{Module}.Map",    (cmd, args) => { PostMsg(MSG.Map); return true; });
            OP.Subscribe($"{Module}.Online", (cmd, args) => { PostMsg(MSG.Online); return true; });

            DATA.Subscribe($"{Module}.Status",       () => ((STATE)fsm.State).ToString());
            DATA.Subscribe($"{Module}.FsmState",     () => ((STATE)fsm.State).ToString());
            DATA.Subscribe($"{Module}.FsmPrevState", () => ((STATE)fsm.PrevState).ToString());
            DATA.Subscribe($"{Module}.FsmLastMessage", GetFsmLastMessage);
        }

        private string GetFsmLastMessage()
        {
            int msg = fsm.LastMsg;
            if (msg >= (int)MSG.Home && msg <= (int)MSG.Error)
                return ((MSG)msg).ToString();

            if (msg == (int)FSM_MSG.TIMER)
                return "Timer";

            return msg.ToString();
        }

        private void InitFsmMap()
        {
            fsm = new StateMachine<LoadportEntity>($"LPM_{Module}", (int)STATE.Idle, 500);

            AnyStateTransition(FSM_MSG.TIMER,                         fnMonitor, FSM_STATE.SAME);
            //AnyStateTransition(MSG.RecHwMsg,                        fnRecMsg, FSM_STATE.SAME);
            AnyStateTransition(MSG.Online,                            fnOnline, FSM_STATE.SAME);
            AnyStateTransition(MSG.Recover,                           fnRecover, STATE.Idle);
            AnyStateTransition(MSG.Abort,                             fnRecover, STATE.Idle);

            EnterExitTransition<STATE, FSM_MSG>(STATE.Initializing,   fnEnterExecute, FSM_MSG.NONE, fnExitExecute);

            // Home
            AnyStateTransition(MSG.Home,                              fnHome, STATE.Initializing);
            Transition(STATE.Initializing, MSG.ActionDone,            fnActionDone, STATE.Idle);
        }

        private bool fnOnline(object[] param)
        {
            bool online = (bool)param[0];
            _efem.SetOnline(Module, online);
            return true;
        }

        private bool fnRecover(object[] param)
        {
            return true;
        }

        private bool fnMonitor(object[] param)
        {
            STATE curSt = (STATE)fsm.State;
            if (curSt == STATE.Initializing || curSt == STATE.Mapping)
            {
                if (fsm.ElapsedTime > 20 * 1000)
                {
                    PostMsg(MSG.Recover);
                }
            }
            return true;
        }

        private bool fnEnterExecute(object[] param)
        {
            _efem.ExecuteAction();
            return true;
        }

        private bool fnExitExecute(object[] param)
        {
            return true;
        }

        private bool fnActionDone(object[] param)
        {
            return true;
        }

        private bool fnHome(object[] param)
        {
            _efem.Home(Module, ModuleName.EFEM.ToString());
            return true;
        }

        public bool Check(int msg, out string reason, params object[] args)
        {
            throw new NotImplementedException();
        }
    }
}