Kepler-Release/Venus/Venus_RT/Modules/EFEM/EfemEntity.cs

using System;
using System.Diagnostics;
using System.Collections.Generic;
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.Core.Util;
using Aitex.Core.RT.Log;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.Schedulers;
using MECF.Framework.Common.SubstrateTrackings;
using Venus_Core;
using Venus_RT;
using Venus_RT.Devices;
using Venus_RT.Devices.YASKAWA;
using Venus_RT.Devices.EFEM;
using Venus_RT.Modules.LPs;
using Venus_RT.Modules.EFEM;
using System.Collections.Concurrent;

namespace Venus_RT.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,
            Fliping,
        }

        public enum MSG
        {
            HomeAll,            // 0
            Pick,               // 1
            Place,              // 2
            Align,              // 3
            ActionDone,         // 4
            MoveCmd,            // 6
            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, 
            Abort,
			ToInit,
			Cool,
            Swap,
            Grip,
            Ungrip,
            Flip,
            LiftActionDone,
            Offline,

            BackroundCmd = 100,
            Light,
            SetThickness,
            HomeLP,
            Load,
            Unload,
            Dock,
            Undock,
            Clamp,
            Unclamp,
            Map,
            ReadCarrierId,
            WriteCarrierID,
            ReadTagData,
            WriteTagData,
            MaxMsgValue,
        }

        public enum EfemType
        {
            FutureEfem = 1,
            JetEfem = 2,
            BrooksEFEM = 3,
        }
        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 RState RobotStatus
        {
            get
            {
                if (_efem.Status != RState.Running)
                {
                    if (_robotWatch.ElapsedMilliseconds < 200)
                        return RState.Running;
                    else
                        return _efem.Status;
                }
                else
                    return RState.Running;
            }
        }

        public bool Check(int msg, out string reason, params object[] args)
        {
            throw new NotImplementedException();
        }
        // Fields
        //
        private readonly string Name;
        private readonly EfemBase _efem;
        private readonly LoadPortModule[] _lpms = new LoadPortModule[3];
        private readonly EfemBackroundMsgProcessor _backroundMsgPrococessor;

        private readonly EfemType _efemType;
        public EfemBase EfemDevice => _efem;
        public EfemType EFEMType => _efemType;

        // routine
        private readonly EfemPickRoutine _pickRoutine;
        private readonly EfemPlaceRoutine _placeRoutine;
        private readonly EfemSwapRoutine _swapRoutine;
        private readonly EfemHomeRoutine _homeRoutine;
        private readonly EFEMAlignRoutine _alignRoutine;
        private string LiftMessage;

        private Stopwatch _robotWatch = new Stopwatch();
        private R_TRIG _robotIdleTrigger = new R_TRIG();

        public LoadPortModule GetLoadportModule(int lpNumber)
        {
            if(lpNumber<=0)
            {
                return null;
            }
            if(_lpms.Length>=lpNumber)
            {
                return _lpms[lpNumber-1];
            }
            return null;
        }
        // Constructor
        //
        public EfemEntity()
        {
            _efemType = (EfemType)SC.GetValue<int>($"EFEM.EfemType");

            _efem = new JetEfem();
            _backroundMsgPrococessor = new EfemBackroundMsgProcessor(_efem);

            Name = ModuleName.EFEM.ToString();

            _homeRoutine    = new EfemHomeRoutine(_efem);
            _pickRoutine    = new EfemPickRoutine(_efem);
            _placeRoutine   = new EfemPlaceRoutine(_efem);
            _swapRoutine    = new EfemSwapRoutine(_efem);
            _alignRoutine = new EFEMAlignRoutine(_efem);

            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();
        }
        protected override bool Init()
        {
            _lpms[0] = new LoadPortModule(ModuleName.LP1, _efem);
            _lpms[1] = new LoadPortModule(ModuleName.LP2, _efem);
            _lpms[2] = new LoadPortModule(ModuleName.LP3, _efem);


            _lpms[0].Initialize();
            _lpms[1].Initialize();
            _lpms[2].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.Abort}",            (cmd, args) => { PostMsg(MSG.Abort); 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, args[0]); return true; 
            });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Pick}",        (cmd, args) => { PostMsg(MSG.Pick, args[0]); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Place}",       (cmd, args) => { PostMsg(MSG.Place, args[0]); return true; });
            OP.Subscribe($"{ModuleName.EfemRobot}.{EfemOperation.Swap}",        (cmd, args) => { PostMsg(MSG.Swap, args[0]); 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.EfemRobot}.{EfemOperation.Flip}", (cmd, args) => { PostMsg(MSG.Flip, 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], args[1]); return true; });
            OP.Subscribe($"{ModuleName.Aligner2}.{EfemOperation.Align}",    (cmd, args) => { PostMsg(MSG.Align, ModuleName.Aligner2); return true; });
            OP.Subscribe($"{ModuleName.Cooling1}.{EfemOperation.Align}",    (cmd, args) => { PostMsg(MSG.Align, ModuleName.Cooling1); return true; });
            OP.Subscribe($"{ModuleName.Cooling2}.{EfemOperation.Align}",    (cmd, args) => { PostMsg(MSG.Align, ModuleName.Cooling2); 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(),Aitex.Core.Util.SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Name}.FsmPrevState", () => ((STATE)fsm.PrevState).ToString(), Aitex.Core.Util.SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Name}.FsmLastMessage", GetFsmLastMessage, Aitex.Core.Util.SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Name}.RobotMoveAction", () => (_efem.TMRobotMoveInfo), Aitex.Core.Util.SubscriptionAttribute.FLAG.IgnoreSaveDB);

            DATA.Subscribe($"{Name}.IsOnline", () => (IsOnline), Aitex.Core.Util.SubscriptionAttribute.FLAG.IgnoreSaveDB);


            _robotWatch.Restart();
            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.TurnOffBuzzer,           fnTurnOffBuzzer,    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);
            AnyStateTransition(MSG.BackroundCmd,            fnBackroundCommand, FSM_STATE.SAME);


            AnyStateTransition(MSG.CommReady,          fnCommReady,               STATE.Init);


            Transition(STATE.Init, FSM_MSG.TIMER, fnMonitor, STATE.Init);
            Transition(STATE.Idle, FSM_MSG.TIMER, fnMonitor, STATE.Idle);
            Transition(STATE.Error, FSM_MSG.TIMER, fnMonitor, STATE.Error);

            // Home
            Transition(STATE.Init,          MSG.HomeAll,            fnHomeAll,          STATE.Initializing);
            Transition(STATE.Idle,          MSG.HomeAll,            fnHomeAll,          STATE.Initializing);    
            Transition(STATE.Error,         MSG.HomeAll,            fnHomeAll,          STATE.Initializing);
            Transition(STATE.Initializing,  FSM_MSG.TIMER,          fnHomingTimeout,    STATE.Idle);

            // Home Robot
            Transition(STATE.Idle,          MSG.HomeRB,             fnHomeRobot,        STATE.InitingRB);
            Transition(STATE.InitingRB,     FSM_MSG.TIMER,          fnHomingTimeout,    STATE.Idle);

            // Home Aligner
            Transition(STATE.Idle,          MSG.HomeAL,             fnHomeAligner,      STATE.InitingAL);
            Transition(STATE.InitingAL,     FSM_MSG.TIMER,          fnHomingTimeout,    STATE.Idle);

            // Pick wafer
            Transition(STATE.Idle,          MSG.Pick,               FnStartPick,        STATE.Picking);
            Transition(STATE.Picking,       FSM_MSG.TIMER,          FnPickTimeout,      STATE.Idle);
            Transition(STATE.Picking,       MSG.Abort,              FnAbortPick,        STATE.Idle);

            // Place wafer
            Transition(STATE.Idle,          MSG.Place,              FnStartPlace,       STATE.Placing);
            Transition(STATE.Placing,       FSM_MSG.TIMER,          FnPlaceTimeout,     STATE.Idle);
            Transition(STATE.Placing,       MSG.Abort,              FnAbortPlace,       STATE.Idle);

            // Swap wafer with LL sequence
            Transition(STATE.Idle,          MSG.Swap,               FnStartSwap,        STATE.Swapping);
            Transition(STATE.Swapping,      FSM_MSG.TIMER,          FnSwapTimeout,      STATE.Idle);
            Transition(STATE.Swapping,      MSG.Abort,              FnAbortSwap,        STATE.Idle);

            // Goto
            Transition(STATE.Idle,          MSG.Goto,               fnGoto,             STATE.Gotoing);
            Transition(STATE.Gotoing,       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.Lift,               fnLift,             STATE.Lifting);
            //Transition(STATE.Lifting,       MSG.LiftActionDone,     fnActionDone,       STATE.Idle);
            Transition(STATE.Lifting,       FSM_MSG.TIMER,          fnLiftTimeout,    STATE.Idle);
            Transition(STATE.Idle,          MSG.Align,              fnAlign,            STATE.Aligning);
            Transition(STATE.Aligning,      FSM_MSG.TIMER,          fnAlignTimeout, STATE.Idle);
            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()); });

            Running = true;
        }


        private bool fnCommReady(object[] param)
        {
            return true;
        }
        private bool fnHomeAll(object[] param)
        {
            return _homeRoutine.Start(ModuleName.EFEM) == RState.Running;
        }

        private bool fnHomingTimeout(object[] param)
        {
            RState ret = _homeRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        private bool fnHomeRobot(object[] param)
        {
            return _homeRoutine.Start(ModuleName.EfemRobot) == RState.Running;
        }

        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");

            return _homeRoutine.Start(unit) == RState.Running;
        }

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

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

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

            return true;
        }

        private bool fnMonitor(object[] param)
        {
            // robot idle check
            _robotIdleTrigger.CLK = _efem.Status != RState.Running;
            if (_robotIdleTrigger.Q)
            {
                _robotWatch.Restart();
            }

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

            if (_efem is EfemBase efem)
            {
                efem.SetOnline(bOnlineFlag);
            }
            IsOnline = 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.ClearError();
            //_efem.ExecuteAction();
            return true;
        }

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

        private bool fnBackroundCommand(object[] param)
        {
            _backroundMsgPrococessor.SendBackroundCommand(param);
            return true;
        }

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

        private bool FnStartPick(object[] param)
        {
            return _pickRoutine.Start(param) == RState.Running;
        }

        private bool FnPickTimeout(object[] param)
        {
            RState ret = _pickRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                Singleton<RouteManager>.Instance.InvokeAbort(new object[] { ModuleName.EFEM });
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortPick(object[] param)
        {
            _pickRoutine.Abort();
            return true;
        }
        private bool FnStartPlace(object[] param)
        {
            return _placeRoutine.Start(param) == RState.Running;
        }

        private bool FnPlaceTimeout(object[] param)
        {
            RState ret = _placeRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                Singleton<RouteManager>.Instance.InvokeAbort(new object[] { ModuleName.EFEM });
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortPlace(object[] param)
        {
            _placeRoutine.Abort();
            return true;
        }

        private bool FnStartSwap(object[] param)
        {
            return _swapRoutine.Start(param) == RState.Running;
        }

        private bool FnSwapTimeout(object[] param)
        {
            RState ret = _swapRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                Singleton<RouteManager>.Instance.InvokeAbort(new object[] { ModuleName.EFEM });
                return false;
            }

            return ret == RState.End;
        }

        private bool FnAbortSwap(object[] param)
        {
            _swapRoutine.Abort();
            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");

            _efem.Goto(unit, Hand.Blade1);
            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;
            }
            LiftMessage = isUp ? "Up" : "Down";

            return true;
        }
        private bool fnLiftTimeout(object[] param)
        {
            if (LiftMessage == "Up")
            {
                return _efem.LiftIsDown == false && _efem.LiftIsUp == true;
            }
            else if (LiftMessage == "Down")
            { 
                return _efem.LiftIsDown == true && _efem.LiftIsUp == false;

            }
            return false;
        }
        private bool fnAlign(object[] param)
        {
            return _alignRoutine.Start(param) == RState.Running;
        }


        private bool fnAlignTimeout(object[] param)
        {
            RState ret = _alignRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }

        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 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,  int reserv,  float angle)
        {
            if (CheckToPostMessage((int)MSG.Align, module, reserv, angle))
                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.Swap, targetModule, pickSlot, pickHand, placeHand, placeSlot, size))
                return (int)MSG.Swap;

            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(Hand hand)
        {
            if (CheckToPostMessage((int)MSG.Flip, hand))
                return (int)MSG.Flip;

            return (int)FSM_MSG.NONE;
        }

        public int SendEfemBackroundCommand(params object[] args)
        {
            if(CheckToPostMessage((int)MSG.BackroundCmd, args))
                return (int)MSG.BackroundCmd;

            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;
            return true;
        }
        internal bool CheckPlaced(ModuleName module)
        {
            //return _efem[module].HasCassette;
            return true;
        }
		
        internal void NoteJobStart(ModuleName module)
        {
            //_efem[module].NoteJobStart();

        }

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

        private void _debugRoutine()
        {
            int flag = 0;
            // Test Home routine
            if (flag == 1)
            {
                PostMsg(MSG.HomeAll);
            }
            else if (flag == 2)
            {
                WaferManager.Instance.CreateWafer(ModuleName.Aligner1, 0, WaferStatus.Normal);
                WaferManager.Instance.DeleteWafer(ModuleName.EfemRobot, 0);
                var item = new MoveItem(ModuleName.Aligner1, 0, ModuleName.EfemRobot, 0, Hand.Blade1);
                var items = new Queue<MoveItem>();
                items.Enqueue(item);
                PostMsg(MSG.Pick, items);
            }
            else if (flag == 3)
            {
                var item = new MoveItem( ModuleName.EfemRobot, 0, ModuleName.Aligner1, 0, Hand.Blade1);
                var items = new Queue<MoveItem>();
                items.Enqueue(item);
                PostMsg(MSG.Place, items);
            }
            else if (flag == 4)
            {
                WaferManager.Instance.CreateWafer(ModuleName.LLA, 0, WaferStatus.Normal);
                WaferManager.Instance.CreateWafer(ModuleName.LLA, 1, WaferStatus.Normal);
                WaferManager.Instance.DeleteWafer(ModuleName.LLA, 2);
                WaferManager.Instance.DeleteWafer(ModuleName.LLA, 3);
                WaferManager.Instance.CreateWafer(ModuleName.EfemRobot, 0, WaferStatus.Normal);
                WaferManager.Instance.CreateWafer(ModuleName.EfemRobot, 1, WaferStatus.Normal);
                var item = new MoveItem(ModuleName.EfemRobot, 0, ModuleName.LLA, 2, Hand.Blade1);
                var items = new Queue<MoveItem>();
                items.Enqueue(item);
                item = new MoveItem(ModuleName.EfemRobot, 1, ModuleName.LLA, 3, Hand.Blade2);
                items.Enqueue(item);
                item = new MoveItem(ModuleName.LLA, 0, ModuleName.EfemRobot, 0, Hand.Blade1);
                items.Enqueue(item);
                item = new MoveItem(ModuleName.LLA, 1, ModuleName.EfemRobot, 1, Hand.Blade2);
                items.Enqueue(item);
                PostMsg(MSG.Swap, items);
            }
            else if (flag == 5)
            {
                PostMsg(MSG.HomeRB);
            }
            else if (flag == 6)
            {
                PostMsg(MSG.HomeAL, ModuleName.Aligner1);
            }
            else if (flag == 8)
            {
                OP.DoOperation("LP1.Load");
            }
            else if(flag == 9)
            {
                OP.DoOperation("LP1.Unload");
            }
        }
        public string LPCarrierId(ModuleName moduleName)
        {
            return _lpms[moduleName - ModuleName.LP1].LPDevice.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 EfemBase _efem;

        private ModuleName Module { get; }

        public LoadportEntity(ModuleName mod, EfemBase 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)
        {
            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();
        }

        
    }

    public class EfemBackroundMsgProcessor
    {
        private EfemBase _efem;
        private ConcurrentQueue<object[]> _pendingCmds = new ConcurrentQueue<object[]>();
        private ConcurrentQueue<object[]> _signalTowerCmds = new ConcurrentQueue<object[]>();
        public EfemBackroundMsgProcessor(EfemBase efem)
        {
            _efem = efem;
        }

        public void Monitor()
        {
            if(Singleton<RouteManager>.Instance.EFEM.RobotStatus != RState.Running)
            {
                // process backround commmand first
                if(Singleton<RouteManager>.Instance.EFEM.RobotStatus == RState.End)
                {
                    if (_pendingCmds.Count > 0 && _pendingCmds.TryDequeue(out object[] args))
                    {
                        EfemEntity.MSG cmd = (EfemEntity.MSG)args[0];
                        if ((int)cmd > (int)EfemEntity.MSG.BackroundCmd && (int)cmd < (int)EfemEntity.MSG.MaxMsgValue)
                        {
                            processBackroundCommand(cmd, args);
                            return;
                        }
                        else
                        {
                            LOG.Write(eEvent.ERR_EFEM_ROBOT, ModuleName.EFEM, $"Invalid EFEM backround command {cmd}");
                        }
                    }
                }


                // process signal tower command
                if(_signalTowerCmds.Count > 0 && _signalTowerCmds.TryDequeue(out object[] letCmds))
                {
                    _efem.SetLamp((LightType)letCmds[1], (LightStatus)letCmds[2]);
                    return;
                }
            }
        }

        public void SendBackroundCommand(object[] args)
        {
            EfemEntity.MSG cmd = (EfemEntity.MSG)args[0];
            if(cmd == EfemEntity.MSG.Light)
            {
                _signalTowerCmds.Enqueue(args);
            }
            else
            {
                _pendingCmds.Enqueue(args);
            }
        }


        private void processBackroundCommand(EfemEntity.MSG cmd, object[] args)
        {
            switch(cmd)
            {
                case EfemEntity.MSG.SetThickness:
                    _efem.SetThickness((ModuleName)args[1], (string)args[2]);
                    break;
                case EfemEntity.MSG.HomeLP:
                    _efem.Home((ModuleName)args[1]);
                    break;
                case EfemEntity.MSG.Load:
                    _efem.Load((ModuleName)args[1]);
                    break;
                case EfemEntity.MSG.Unload:
                    _efem.Unload((ModuleName)args[1]);
                    break;
                case EfemEntity.MSG.Dock:
                    _efem.Dock((ModuleName)args[1]);
                    break;
                case EfemEntity.MSG.Undock:
                    _efem.Undock((ModuleName)args[1]);
                    break;
                case EfemEntity.MSG.Clamp:
                    _efem.Clamp((ModuleName)args[1], (bool)args[2]);
                    break;
                case EfemEntity.MSG.Unclamp:
                    _efem.Unclamp((ModuleName)args[1]);
                    break;
                case EfemEntity.MSG.Map:
                    _efem.Map((ModuleName)args[1]);
                    break;
                case EfemEntity.MSG.ReadCarrierId:
                    _efem.ReadCarrierId((ModuleName)args[1]);
                    break;
                case EfemEntity.MSG.WriteCarrierID:
                    _efem.WriteCarrierId((ModuleName)args[1], (string)args[2]);
                    break;
                case EfemEntity.MSG.ReadTagData:
                    _efem.ReadCarrierId((ModuleName)args[1]);
                    break;
                case EfemEntity.MSG.WriteTagData:
                    _efem.WriteCarrierId((ModuleName)args[1], (string)args[2]);
                    break;
                default:
                    LOG.Write(eEvent.ERR_EFEM_ROBOT, ModuleName.EFEM, $"unprocessed Efem command {cmd}");
                    break;
            }
        }
    }
}