Virgo_B_V82XS/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;

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

        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,
        }

        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,
            PickAndPlace,
            Grip,
            Ungrip,
        }

        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 LoadportEntity[] _lpms = new LoadportEntity[2];
        public Efem EfemDevice => _efem;

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

            _efem = new Efem();
            _lpms[0] = new LoadportEntity(ModuleName.LP1, _efem);
            _lpms[1] = new LoadportEntity(ModuleName.LP2, _efem);

            Name = ModuleName.EFEM.ToString();

            InitFsmMap();

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

        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();
        }
        protected override bool Init()
        {
            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); 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.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);

            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, STATE.Idle);
            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);

            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.Initializing, MSG.ActionDone,          null, 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.ActionDone,               fnActionDone, STATE.Idle);
            Transition(STATE.Idle, MSG.Align,                       fnAlign, STATE.Aligning);
            Transition(STATE.Aligning, 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.Home(ModuleName.EfemRobot);
            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)
        {
            _efem.ExecuteAction();
            return false;
        }

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

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

                if (actionType == EfemOperation.Orgsh)
                {
                    if (_efem.HasActions)
                    {
                        _efem.ExecuteAction();
                        return false;
                    }
                }

                    if (actionType == EfemOperation.Extend)
                {
                    if (fsm.State == (int)STATE.Placing)
                    {
                        _efem.ExecuteAction();
                        return false;
                    }
                    else if (fsm.State == (int)STATE.Picking)
                    {
                        if (_efem.HasActions)
                        {
                            _efem.ExecuteAction();
                            return false;
                        }
                        else
                        {
                            return true;
                        }
                    }
                    else if (fsm.State == (int)STATE.Swapping)
                    {
                        if (_efem.HasActions)
                        {
                            _efem.ExecuteAction();
                            return false;
                        }
                        else
                        {
                            return true;
                        }
                    }
                }
                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);
                }
            }

            if (curSt == STATE.Aligning)
            {
                if (fsm.ElapsedTime > 10 * 1000)
                {
                    EV.PostAlarmLog(ModuleName.Aligner.ToString(), "Align timeout");
                    PostMsg(MSG.ActionDone, "alignment timeout");
                }
            }

            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.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.ExecuteAction();
                return false;
            }
            else
            {
                return true;
            }
        }

        private bool fnPmPinDown(object[] param)
        {
            _efem.UpdateStatus((ushort)param[0], ActionStatus.Completed);
            if (_efem.HasActions)
            {
                _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");

            if (!_efem.SetPinUp(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;
        }

        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 (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 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();
        }
    }

    /// <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);
            return true;
        }

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