Kepler-Release/Venus/Venus_RT/Modules/TM/TMEntity.cs

using System;
using System.Collections.Generic;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.Log;
using Aitex.Core.Util;
using Venus_Core;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using Venus_RT.Devices;
using Venus_RT.Modules.TM;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.OperationCenter;
using MECF.Framework.Common.Schedulers;
using MECF.Framework.Common.CommonData;

namespace Venus_RT.Modules
{
    class TMEntity : Entity, IModuleEntity
    {
        public enum STATE
        {
            Unknown,
            Init,
            Initializing,  
            Idle,        
            Error,       
            Pumping,
            Venting,
            Purging,
            Leakchecking,
            Picking,         
            Placing,  
            Swaping,
            PMPicking,
            PMPlacing,
            PMSwaping,
            Aligning,      
            Mapping,     
            Extending,    
            Retracting,   
            Swapping,
            Gotoing,
        }

        public enum MSG
        {
            Home,  
            Online,
            Offline,
            Pump,
            Vent,
            Purge,
            CyclePurge,
            LeakCheck,
            Pick, 
            Place,
            Swap,
            DoublePick,
            DoublePlace,
            DoubleSwap,
            PMPick,
            PMPlace,
            PMSwap,
            Extend,
            Retract,
            Error,
            Abort,
        }

        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 IsLLSlitDoorClosed(ModuleName ll) => ll == ModuleName.LLA ? _tm.IsLLAESlitDoorClosed : _tm.IsLLBESlitDoorClosed;
        public bool IsLLSlitDoorOpen(ModuleName ll) => ll == ModuleName.LLA ? _tm.IsLLAESlitDoorOpen : _tm.IsLLBESlitDoorOpen;

        public bool IsOnline { get; internal set; }

        private readonly JetTM _tm;
        private readonly ITransferRobot _robot;

        private readonly MFHomeRoutine _homeRoutine;
        private readonly MFPumpRoutine _pumpingRoutine;
        private readonly MFVentRoutine _ventingRoutine;
        private readonly MFLeakCheckRoutine _leakCheckRoutine;
        private readonly MFPurgeRoutine _purgeRoutine;
        private readonly MFPickRoutine _pickRoutine;
        private readonly MFPlaceRoutine _placeRoutine;
        private readonly MFSwapRoutine _swapRoutine;
        private readonly MFPMPickRoutine _pmPickRoutine;
        private readonly MFPMPlaceRoutine _pmPlaceRoutine;
        private readonly MFPMSwapRoutine _pmSwapRoutine;

        private readonly MFPMRetractRoutine _pmRetractRoutine;
        private readonly MFPMExtendRoutine  _pmExtendRoutine;
        public TMEntity()
        {
            _tm                 = Singleton<JetTM>.Instance;
            _robot              = new SIASUNRobot();

            _homeRoutine    = new MFHomeRoutine(_tm, _robot);
            _pickRoutine    = new MFPickRoutine(_tm, _robot);
            _placeRoutine   = new MFPlaceRoutine(_tm, _robot);
            _swapRoutine    = new MFSwapRoutine(_tm, _robot);

            _pmPickRoutine  = new MFPMPickRoutine(_tm, _robot);
            _pmPlaceRoutine = new MFPMPlaceRoutine(_tm, _robot);
            _pmSwapRoutine  = new MFPMSwapRoutine(_tm, _robot);

            _pumpingRoutine     = new MFPumpRoutine(_tm, ModuleName.TM);
            _ventingRoutine     = new MFVentRoutine(_tm, ModuleName.TM);
            _leakCheckRoutine   = new MFLeakCheckRoutine(_tm, ModuleName.TM);
            _purgeRoutine       = new MFPurgeRoutine(_tm, ModuleName.TM);

            _pmRetractRoutine = new MFPMRetractRoutine(_tm, _robot);
            _pmExtendRoutine  = new MFPMExtendRoutine(_tm, _robot);

            WaferManager.Instance.SubscribeLocation(ModuleName.TM, 2);

            InitFsmMap();
        }

        protected override bool Init()
        {
            OP.Subscribe("TM.Home", (cmd, args) => CheckToPostMessage((int)MSG.Home));
            OP.Subscribe($"TM.{RtOperation.LLPick}", (cmd, args) => CheckToPostMessage((int)MSG.Pick, args));
            OP.Subscribe($"TM.{RtOperation.LLPlace}", (cmd, args) => CheckToPostMessage((int)MSG.Place, args));
            OP.Subscribe($"TM.{RtOperation.PMPick}", (cmd, args) => CheckToPostMessage((int)MSG.PMPick, args));
            OP.Subscribe($"TM.{RtOperation.PMPlace}", (cmd, args) => CheckToPostMessage((int)MSG.PMPlace, args));

            OP.Subscribe($"TM.{RtOperation.Extend}", (cmd, args) => CheckToPostMessage((int)MSG.Extend, args));
            OP.Subscribe($"TM.{RtOperation.Retract}", (cmd, args) => CheckToPostMessage((int)MSG.Retract, args));

            

            //OP.Subscribe($"TM.{RtOperation.LLPlace}", (cmd, args) => CheckToPostMessage((int)MSG.Place, args));
            OP.Subscribe($"TM.{RtOperation.Pump}", (cmd, args) => CheckToPostMessage((int)MSG.Pump));
            OP.Subscribe($"TM.{RtOperation.Vent}", (cmd, args) => CheckToPostMessage((int)MSG.Vent));
            OP.Subscribe($"TM.{RtOperation.LeakCheck}", (cmd, args) => CheckToPostMessage((int)MSG.LeakCheck));
            OP.Subscribe($"TM.{RtOperation.Purge}", (cmd, args) => CheckToPostMessage((int)MSG.Purge));
            OP.Subscribe($"TM.{RtOperation.Abort}", (cmd, args) => CheckToPostMessage((int)MSG.Abort));



            DATA.Subscribe("TM.FsmState", () => (((STATE)fsm.State).ToString()));
            DATA.Subscribe("TM.FsmPrevState", () => (((PMState)fsm.PrevState).ToString()));
            DATA.Subscribe("TM.FsmLastMessage", () => (((MSG)fsm.LastMsg).ToString()));


            DATA.Subscribe("TM.RobotMoveAction", () => _robot.TMRobotMoveInfo);
            DATA.Subscribe("TM.RobotMoveAction.ArmTarget", () =>   _robot.TMRobotMoveInfo.ArmTarget.ToString()) ;
            DATA.Subscribe("TM.RobotMoveAction.BladeTarget", () => _robot.TMRobotMoveInfo.BladeTarget);
            DATA.Subscribe("TM.RobotMoveAction.RobotAction", () => _robot.TMRobotMoveInfo.Action.ToString());

            return true;
        }

        private void InitFsmMap()
        {
            fsm = new StateMachine<TMEntity>("TM", (int)STATE.Init, 50);

            //AnyStateTransition(FSM_MSG.TIMER,   fnMonitor,      FSM_STATE.SAME);


            AnyStateTransition(MSG.Error,       fnError,        STATE.Error);
            AnyStateTransition(MSG.Online,      fnOnline,       FSM_STATE.SAME);
            AnyStateTransition(MSG.Offline,     fnOffline,      FSM_STATE.SAME);
            AnyStateTransition(MSG.Home,        fnHome,         STATE.Initializing);

            // Home
            Transition(STATE.Initializing,      FSM_MSG.TIMER,      fnHoming,           STATE.Idle);

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

            // Vent sequence
            Transition(STATE.Idle,              MSG.Vent,           FnStartVent,        STATE.Venting);
            Transition(STATE.Venting,           FSM_MSG.TIMER,      FnVentTimeout,      STATE.Idle);
            Transition(STATE.Venting,           MSG.Abort,          FnAbortVent,        STATE.Idle);

            // Pump sequence
            Transition(STATE.Idle,              MSG.Pump,           FnStartPump,        STATE.Pumping);
            Transition(STATE.Pumping,           FSM_MSG.TIMER,      FnPumpTimeout,      STATE.Idle);
            Transition(STATE.Pumping,           MSG.Abort,          FnAbortPump,        STATE.Idle);

            // Purge sequence
            Transition(STATE.Idle,              MSG.CyclePurge,     FnStartPurge,       STATE.Purging);
            Transition(STATE.Purging,           FSM_MSG.TIMER,      FnPurgeTimeout,     STATE.Idle);
            Transition(STATE.Purging,           MSG.Abort,          FnAbortPurge,       STATE.Idle);

            // Leak check sequence
            Transition(STATE.Idle,              MSG.LeakCheck,      FnStartLeakCheck,   STATE.Leakchecking);
            Transition(STATE.Leakchecking,      FSM_MSG.TIMER,      FnLeakCheckTimeout, STATE.Idle);
            Transition(STATE.Leakchecking,      MSG.Abort,          FnAbortLeakCheck,   STATE.Idle);

            // Pick wafer from LL sequence
            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 to LL sequence
            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.Swaping);
            Transition(STATE.Swaping,           FSM_MSG.TIMER,      FnSwapTimeout,      STATE.Idle);
            Transition(STATE.Swaping,           MSG.Abort,          FnAbortSwap,        STATE.Idle);

            // Pick wafer from PM sequence
            Transition(STATE.Idle,              MSG.PMPick,         FnStartPMPick,      STATE.PMPicking);
            Transition(STATE.PMPicking,         FSM_MSG.TIMER,      FnPMPickTimeout,    STATE.Idle);
            Transition(STATE.PMPicking,         MSG.Abort,          FnAbortPMPick,      STATE.Idle);

            // Place wafer to PM sequence
            Transition(STATE.Idle,              MSG.PMPlace,        FnStartPMPlace,     STATE.PMPlacing);
            Transition(STATE.PMPlacing,         FSM_MSG.TIMER,      FnPMPlaceTimeout,   STATE.Idle);
            Transition(STATE.PMPlacing,         MSG.Abort,          FnAbortPMPlace,     STATE.Idle);

            // Swap wafer with PM sequence
            Transition(STATE.Idle,              MSG.PMSwap,         FnStartPMSwap,      STATE.PMSwaping);
            Transition(STATE.PMSwaping,         FSM_MSG.TIMER,      FnPMSwapTimeout,    STATE.Idle);
            Transition(STATE.PMSwaping,         MSG.Abort,          FnAbortPMSwap,      STATE.Idle);


            //Retract
            Transition(STATE.Idle,              MSG.Retract,        FnStartRetract,     STATE.Retracting);
            Transition(STATE.Retracting,        FSM_MSG.TIMER,      FnRetract,          STATE.Idle);
            Transition(STATE.Retracting,        MSG.Abort,          FnAbortRetract,     STATE.Idle);

            //Extend
            Transition(STATE.Idle,              MSG.Extend,         FnStartExtend,      STATE.Extending);
            Transition(STATE.Extending,         FSM_MSG.TIMER,      FnExtend,           STATE.Idle);
            Transition(STATE.Extending,         MSG.Abort,          FnAbortExtend,      STATE.Idle);


            Running = true;
        }

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

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

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

        private bool fnOffline(object[] param)
        {
            IsOnline = false;
            return true;
        }

        private bool fnAbort(object[] param)
        {
            _robot.Halt();
            return true;
        }

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

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

            return ret == RState.End;
        }

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

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

            return ret == RState.End;
        }

        private bool FnAbortVent(object[] param)
        {
            _ventingRoutine.Abort();
            return true;
        }

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

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

            return ret == RState.End;
        }

        private bool FnAbortPump(object[] param)
        {
            _pumpingRoutine.Abort();
            return true;
        }

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

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

            return ret == RState.End;
        }

        private bool FnAbortPurge(object[] param)
        {
            _purgeRoutine.Abort();
            return true;
        }

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

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

            return ret == RState.End;
        }

        private bool FnAbortLeakCheck(object[] param)
        {
            _leakCheckRoutine.Abort();
            return true;
        }

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

            return ret == RState.End;
        }

        private bool FnAbortSwap(object[] param)
        {
            _swapRoutine.Abort();
            return true;
        }


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

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

            return ret == RState.End;
        }

        private bool FnAbortPMPick(object[] param)
        {
            _pmPickRoutine.Abort();
            return true;
        }
        private bool FnStartPMPlace(object[] param)
        {
            return _pmPlaceRoutine.Start(param) == RState.Running;
        }

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

            return ret == RState.End;
        }

        private bool FnAbortPMPlace(object[] param)
        {
            _pmPlaceRoutine.Abort();
            return true;
        }

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

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

            return ret == RState.End;
        }

        private bool FnAbortPMSwap(object[] param)
        {
            _pmSwapRoutine.Abort();
            return true;
        }


        private bool FnStartRetract(object[] param)
        {
           return _pmRetractRoutine.Start(param) == RState.Running;        
        }
        private bool FnRetract(object[] param)
        {
            RState ret = _pmRetractRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }
        private bool FnAbortRetract(object[] param)
        {
            _pmRetractRoutine.Abort();
            return true;
        }

        private bool FnStartExtend(object[] param)
        {
            return _pmExtendRoutine.Start(param) == RState.Running;
        }
        private bool FnExtend(object[] param)
        {
            RState ret = _pmExtendRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(MSG.Error);
                return false;
            }

            return ret == RState.End;
        }
        private bool FnAbortExtend(object[] param)
        {
            _pmExtendRoutine.Abort();
            return true;
        }
        public bool Check(int msg, out string reason, params object[] args)
        {
            reason = "";
            return true;
        }

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

            return (int)FSM_MSG.NONE;
        }

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

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

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

            return true;
        }

        public bool TurnEFEMSlitDoor(ModuleName loadlock, bool open, out string reason)
        {
            return _tm.TurnEFEMSlitDoor(loadlock, open, out reason);
        }

        private void _debugRoutine()
        {
            int flag = 0;
            // Test Home routine
            if (flag == 1)
            {
                PostMsg(MSG.Home);
            }
            else if (flag == 2)
            {
                PostMsg(MSG.Vent);
            }
            else if (flag == 3)
            {
                PostMsg(MSG.Pump);
            }
            else if(flag == 4)
            {
                PostMsg(MSG.Pick, ModuleName.LLA, 0, 0);
            }
            else if(flag == 5)
            {
                PostMsg(MSG.Place, ModuleName.LLA, 0, 0);
            }
            else if(flag == 6)
            {
                Queue<MoveItem> items = new Queue<MoveItem>();
                items.Enqueue(new MoveItem(ModuleName.TM, 0, ModuleName.LLA, 0, Hand.Blade1));
                items.Enqueue(new MoveItem(ModuleName.TM, 1, ModuleName.LLA, 1, Hand.Blade2));
                items.Enqueue(new MoveItem(ModuleName.LLA, 0, ModuleName.TM, 0, Hand.Blade1));
                items.Enqueue(new MoveItem(ModuleName.LLA, 1, ModuleName.TM, 1, Hand.Blade2));
                PostMsg(MSG.Swap,items);
            }
            else if(flag == 7)
            {
                PostMsg(MSG.PMPick, ModuleName.PMA, 0, 0);
            }
            else if(flag == 8)
            {
                PostMsg(MSG.PMPlace, ModuleName.PMA, 0, 0);
            }
            else if(flag == 9)
            {
                PostMsg(MSG.PMSwap, ModuleName.PMA, 0, 0, 0, 0);
            }

            //else if (flag == 4)
            //{
            //    PostMsg(MSG.PumpLoadLock);
            //}
            //else if (flag == 5)
            //{
            //    PostMsg(MSG.VentLoadLock);
            //}
            //else if (flag == 6)
            //{
            //    PostMsg(MSG.PurgeLoadLock);
            //}
            //else if (flag == 7)
            //{
            //    PostMsg(MSG.LaunchPump);
            //}
            //else if (flag == 8)
            //{
            //    PostMsg(MSG.LaunchTurboPump);
            //}
            //else if (flag == 9)
            //{
            //    PostMsg(MSG.LoadLockLeakCheck);
            //}
            //else if (flag == 10)
            //{
            //    PostMsg(MSG.CyclePurge);
            //}
            //else if (flag == 11)
            //{
            //    PostMsg(MSG.GasLinePurge);
            //}
            //else if (flag == 12)
            //{
            //    PostMsg(MSG.LeakCheck);
            //}
            //else if (flag == 13)
            //{
            //    PostMsg(MSG.GasLeakCheck);
            //}
            //else if (flag == 14)
            //{
            //    PostMsg(MSG.LLPlace);
            //}
            //else if (flag == 15)
            //{
            //    PostMsg(MSG.LLPick);
            //}
            //else if (flag == 16)
            //{
            //    PostMsg(MSG.RunRecipe, "7777");
            //}
            //else if (flag == 17)
            //{
            //    PostMsg(MSG.MFCVerification, "MFC2", (double)50, 10);
            //}
        }
    }
}