using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.OperationCenter;
using Aitex.Core.RT.Routine;
using Aitex.Core.Utilities;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.Common.Utilities;
using CyberX8_Core;
using CyberX8_RT.Devices.AXIS;
using CyberX8_RT.Devices.PUF;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Interop;
using MECF.Framework.Common.CommonData;
using MECF.Framework.Common.Alarm;
using Aitex.Core.Util;
using MECF.Framework.Common.WaferHolder;

namespace CyberX8_RT.Modules.PUF
{
    public class PUFEntity : Entity, IEntity, IModuleEntity
    {
        #region 常量
        #endregion

        #region 属性
        public bool IsInRobotStation
        {
            get { return CheckPufVerticalAndRotationInRobot(); }
        }
        public ModuleName Module { get; private set; }

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


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

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

        public bool IsAuto { get; } = true;

        /// <summary>
        /// 是否为工程模式
        /// </summary>
        public bool IsEngineering { get; } = false;
        /// <summary>
        /// 是否为产品模式
        /// </summary>
        public bool IsProduction { get; } = true;
        /// <summary>
        /// SideA Wafer情况 
        /// </summary>
        public bool SideAWaferPresent
        {
            get { return _pufVacuum.ChuckAWaferPresent; }
        }
        /// <summary>
        /// SideB Wafer情况 
        /// </summary>
        public bool SideBWaferPresent
        {
            get { return _pufVacuum.ChuckBWaferPresent; }
        }

        public bool IsHomed
        {
            get { return _isHomed; }
        }
        /// <summary>
        /// 是否禁用
        /// </summary>
        public bool IsDisable { get; internal set; }
        /// <summary>
        /// Rotation是否SwitchOn
        /// </summary>
        public bool IsRotationSwitchOn
        {
            get { return _rotationAxis.IsSwitchOn; }
        }
        /// <summary>
        /// Vertical是否SwitchOn
        /// </summary>
        public bool IsVerticalSwitchOn
        {
            get { return _verticalAxis.IsSwitchOn; }
        }
        /// <summary>
        /// Flip是否SwitchOn
        /// </summary>
        public bool IsFlipSwitchOn
        {
            get { return _flipAxis.IsSwitchOn; }
        }
        /// <summary>
        /// 当前状态机状态
        /// </summary>
        public int State { get { return fsm.State; } }
        /// <summary>
        /// 是否回到安全位置
        /// </summary>
        public bool IsBackToParkStation
        {
            get
            {
                double flipPosition = _flipAxis.MotionData.MotorPosition;
                double rotationPosition = _rotationAxis.MotionData.MotorPosition;
                double verticalPosition=_verticalAxis.MotionData.MotorPosition;
                return  (_flipAxis.CheckPositionIsInStation(flipPosition, "SideA")||Module==ModuleName.PUF2)
                    && _rotationAxis.CheckPositionIsInStation(rotationPosition, "Park") &&
                    _verticalAxis.CheckPositionIsInStation(verticalPosition, "Park");
            }
        }
        /// <summary>
        /// 是否Flip到达sideA
        /// </summary>
        public bool IsFlipSideA
        {
            get { return _flipAxis.CheckPositionIsInStation(_flipAxis.MotionData.MotorPosition, "SideA"); }
        }
        /// <summary>
        /// Vacuum状态
        /// </summary>
        public PufVacuum PufVacuum { get { return _pufVacuum; } }
        #endregion

        #region 内部变量
        private bool _isHomed;
        private JetAxisBase _flipAxis;
        private JetAxisBase _rotationAxis;
        private JetAxisBase _verticalAxis;
        private PufVacuum _pufVacuum;
        private IRoutine _currentRoutine;
        #endregion

        #region routine
        private PufHomeAllRoutine _homeAllRoutine;
        private PufSwitchOnRoutine _switchOnRoutine;
        private PufSwitchOffRoutine _switchOffRoutine;
        private PufCalibrateRoutine _calibrateRoutine;
        private PufPickFromLoaderRoutine _pickFromLoaderRoutine;
        private PufPlaceToLoaderRoutine _placeToLoaderRoutine;
        private PufPositionRoutine _positionRoutine;
        private PufGotoRobotForPlaceRoutine _gotoRobotForPlaceRoutine;
        private PufGotoRobotForPickRoutine _gotoRobotForPickRoutine;
        private PufReadyForRobotPlaceRoutine _readyForRobotPlaceRoutine;
        private PufReadyForSwapRoutine _readyForSwapRoutine;
        private PufSwapRoutine _swapRoutine;
        private PufReadyForRobotPickRoutine _readyForRobotPickRoutine;
        private PufBackToParkRoutine _backToParkRoutine;
        #endregion

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public PUFEntity(ModuleName module)
        {
            this.Module = module;
            _flipAxis = DEVICE.GetDevice<JetAxisBase>($"{module}.Flip");
            _rotationAxis=DEVICE.GetDevice<JetAxisBase>($"{module}.Rotation");
            _verticalAxis = DEVICE.GetDevice<JetAxisBase>($"{module}.Vertical");
            _pufVacuum = DEVICE.GetDevice<PufVacuum>($"{module}.Vacuum");
            _pufVacuum.OnVacuumLeak += PufVacuum_OnVacuumLeak;
            WaferManager.Instance.SubscribeLocation(Module, 2);
            InitialFsm();
            InitializeRoutine();
        }
        /// <summary>
        /// Vacuum Leak 异常
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void PufVacuum_OnVacuumLeak(object sender, string e)
        {
            PostMsg((int)PUFMSG.Error);
        }

        protected override bool Init()
        {
            InitialOperation();
            InitialDATA();
            return true;
        }

        private void InitialDATA()
        {
            DATA.Subscribe($"{Module}.FsmState", () => ((PUFSTATE)fsm.State).ToString(), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.IsHomed", () => _isHomed,SubscriptionAttribute.FLAG.IgnoreSaveDB); 
            DATA.Subscribe($"{Module}.IsError", () => IsError, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.IsInRobotStation", () => IsInRobotStation, SubscriptionAttribute.FLAG.IgnoreSaveDB); 
            DATA.Subscribe($"{Module}.State", () => ((PUFSTATE)fsm.State).ToString(), SubscriptionAttribute.FLAG.IgnoreSaveDB);
        }
        /// <summary>
        /// 初始化操作
        /// </summary>
        private void InitialOperation()
        {
            OP.Subscribe($"{Module}.Abort", (cmd, args) => { return CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.Abort); });
            OP.Subscribe($"{Module}.ClearError", (cmd, args) => { return CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.ResumeError); });
            OP.Subscribe($"{Module}.{MotionOperation.HomeAll}", (cmd, args) => { return CheckToPostMessage<PUFSTATE,PUFMSG>(eEvent.ERR_PUF,Module.ToString(),(int)PUFMSG.HomeAll);});
            OP.Subscribe($"{Module}.{MotionOperation.SwitchOff}", (cmd, args) => { return CheckToPostMessage<PUFSTATE,PUFMSG>(eEvent.ERR_PUF,Module.ToString(),(int)PUFMSG.SwitchOff); });
            OP.Subscribe($"{Module}.{MotionOperation.SwitchOn}", (cmd, args) => { return CheckToPostMessage<PUFSTATE,PUFMSG>(eEvent.ERR_PUF,Module.ToString(),(int)PUFMSG.SwitchOn); });
            OP.Subscribe($"{Module}.{MotionOperation.Calibrate}", (cmd, args) => { return CheckToPostMessage<PUFSTATE,PUFMSG>(eEvent.ERR_PUF,Module.ToString(),(int)PUFMSG.Calibrate, args);});
            OP.Subscribe($"{Module}.{MotionOperation.Pick}", (cmd, args) => { return CheckToPostMessage<PUFSTATE,PUFMSG>(eEvent.ERR_PUF,Module.ToString(),(int)PUFMSG.PickFromLoader, args); });
            OP.Subscribe($"{Module}.{MotionOperation.Place}", (cmd, args) => { return CheckToPostMessage<PUFSTATE,PUFMSG>(eEvent.ERR_PUF,Module.ToString(),(int)PUFMSG.PlaceToLoader, args); });
            OP.Subscribe($"{Module}.GotoRobotPositionForPlace", (cmd, args) => { return CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.GotoRobotPositionForPlace,  args); }); 
            OP.Subscribe($"{Module}.GotoRobotPositionForPick", (cmd, args) => { return CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.GotoRobotPositionForPick, args); });
        }
        /// <summary>
        /// 初始化Routine
        /// </summary>
        private void InitializeRoutine()
        {
            _homeAllRoutine = new PufHomeAllRoutine(Module,this);
            _switchOnRoutine = new PufSwitchOnRoutine(Module);
            _switchOffRoutine= new PufSwitchOffRoutine(Module);
            _calibrateRoutine = new PufCalibrateRoutine(Module);
            _pickFromLoaderRoutine = new PufPickFromLoaderRoutine(Module.ToString());
            _placeToLoaderRoutine=new PufPlaceToLoaderRoutine(Module.ToString());
            _positionRoutine = new PufPositionRoutine(Module);
            _gotoRobotForPlaceRoutine = new PufGotoRobotForPlaceRoutine(Module.ToString());
            _gotoRobotForPickRoutine=new PufGotoRobotForPickRoutine(Module.ToString());
            _readyForRobotPlaceRoutine=new PufReadyForRobotPlaceRoutine(Module.ToString());
            _readyForSwapRoutine=new PufReadyForSwapRoutine(Module.ToString());
            _swapRoutine=new PufSwapRoutine(Module.ToString());
            _readyForRobotPickRoutine=new PufReadyForRobotPickRoutine(Module.ToString());
            _backToParkRoutine=new PufBackToParkRoutine(Module.ToString());
        }
        /// <summary>
        /// 初始化状态机
        /// </summary>
        private void InitialFsm()
        {
            fsm = new StateMachine<PUFEntity>(Module.ToString(), (int)PUFSTATE.Init, 20);
            fsm.EnableRepeatedMsg(true);

            AnyStateTransition(PUFMSG.Error, EnterError, PUFSTATE.Error);
            AnyStateTransition(PUFMSG.ReturnIdle, (param) => { return true; }, PUFSTATE.Idle);
            AnyStateTransition(PUFMSG.Abort, Abort, PUFSTATE.Init);
            AnyStateTransition(PUFMSG.HomeAll, HomeAll, PUFSTATE.Homing);
            Transition(PUFSTATE.Error, PUFMSG.ResumeError, ResumeError, PUFSTATE.Init);
            //SwitchOn
            Transition(PUFSTATE.Init, PUFMSG.SwitchOn, SwitchOnAll, PUFSTATE.SwitchOning);
            Transition(PUFSTATE.Idle, PUFMSG.SwitchOn, SwitchOnAll, PUFSTATE.SwitchOning);
            Transition(PUFSTATE.Error, PUFMSG.SwitchOn, SwitchOnAll, PUFSTATE.SwitchOning);
            Transition(PUFSTATE.SwitchOning, FSM_MSG.TIMER, SwitchOnTimeout, PUFSTATE.Init);
            //SwitchOff
            Transition(PUFSTATE.Init, PUFMSG.SwitchOff, SwitchOffAll, PUFSTATE.SwitchOffing);
            Transition(PUFSTATE.Idle, PUFMSG.SwitchOff, SwitchOffAll, PUFSTATE.SwitchOffing);
            Transition(PUFSTATE.Error, PUFMSG.SwitchOff, SwitchOffAll, PUFSTATE.SwitchOffing);
            Transition(PUFSTATE.SwitchOffing, FSM_MSG.TIMER, SwitchOffTimeout, PUFSTATE.Init);
            // Home
            Transition(PUFSTATE.Homing, FSM_MSG.TIMER, HomingTimeout, PUFSTATE.Idle);
            //Calibrate
            Transition(PUFSTATE.Idle, PUFMSG.Calibrate, Calibrate, PUFSTATE.Calibrating);
            Transition(PUFSTATE.Calibrating, FSM_MSG.TIMER, CalibrateTimeout, PUFSTATE.Idle);
            //PickFromLoader
            Transition(PUFSTATE.Idle, PUFMSG.PickFromLoader, PickFromLoader, PUFSTATE.Picking);
            Transition(PUFSTATE.Picking, FSM_MSG.TIMER, PickTimeout, PUFSTATE.Idle);
            //PlaceToLoader
            Transition(PUFSTATE.Idle, PUFMSG.PlaceToLoader, PlaceToLoader, PUFSTATE.Placing);
            Transition(PUFSTATE.Placing, FSM_MSG.TIMER, PlaceToLoaderMonitor, PUFSTATE.Idle);
            //GotoRobotPositionForPlace
            Transition(PUFSTATE.Idle, PUFMSG.GotoRobotPositionForPlace, GotoRobotPositionForPlace, PUFSTATE.GotoRobotPositionForPlace);
            Transition(PUFSTATE.GotoRobotPositionForPlace, FSM_MSG.TIMER, GotoRobotPositionForPlaceMonitor, PUFSTATE.Idle);
            //GotoRobotPositionForPick
            Transition(PUFSTATE.Idle, PUFMSG.GotoRobotPositionForPick, GotoRobotPositionForPick, PUFSTATE.GotoRobotPositionForPick);
            Transition(PUFSTATE.GotoRobotPositionForPick, FSM_MSG.TIMER, GotoRobotPositionForPickMonitor, PUFSTATE.Idle);
            //ready for robot place
            Transition(PUFSTATE.Idle, PUFMSG.ReadyForRobotPlace, ReadyForRobotPlace, PUFSTATE.WaitForRobotPlacing);
            Transition(PUFSTATE.WaitForRobotPlacing, FSM_MSG.TIMER, ReadyForRobotPlaceMonitor, PUFSTATE.WaitForRobotPlace);
            Transition(PUFSTATE.WaitForRobotPlace, PUFMSG.ReadyForSwap, ReadyForSwap, PUFSTATE.WaitForSwaping);
            Transition(PUFSTATE.WaitForSwaping, FSM_MSG.TIMER, ReadyForSwapMonitor, PUFSTATE.WaitForSwap);
            Transition(PUFSTATE.WaitForSwap, PUFMSG.Swap, Swap, PUFSTATE.Swaping);
            Transition(PUFSTATE.Swaping, FSM_MSG.TIMER, SwapMonitor, PUFSTATE.AferSwapParkStation);
            Transition(PUFSTATE.AferSwapParkStation, PUFMSG.ReadyForRobotPick, ReadyForRobotPick, PUFSTATE.WaitForRobotPicking);
            Transition(PUFSTATE.WaitForRobotPicking, FSM_MSG.TIMER, ReadyForRobotPickMonitor, PUFSTATE.WaitForRobotPick);
            Transition(PUFSTATE.WaitForRobotPick, PUFMSG.RobotPickComplete, BackToParkStation, PUFSTATE.BackToPackStationing);
            Transition(PUFSTATE.BackToPackStationing, FSM_MSG.TIMER, BackToParkStationMonitor, PUFSTATE.Idle);
            Transition(PUFSTATE.Idle, PUFMSG.FlipSideA, FlipSideA, PUFSTATE.Fliping);
            Transition(PUFSTATE.Fliping,FSM_MSG.TIMER,FlipSideAMonitor,PUFSTATE.Idle);
            //Retry
            Transition(PUFSTATE.Error, PUFMSG.Retry, NullFunc, PUFSTATE.Retrying);
            Transition(PUFSTATE.Retrying, FSM_MSG.TIMER, PufRetry,PUFSTATE.Retrying);
            Transition(PUFSTATE.Retrying, PUFMSG.ReadyForRobotPlace, RetryReadyForRobotPlace, PUFSTATE.WaitForRobotPlacing);
            Transition(PUFSTATE.Retrying, PUFMSG.ReadyForSwap, RetryReadyForSwap, PUFSTATE.WaitForSwaping);
            Transition(PUFSTATE.Retrying, PUFMSG.Swap, RetrySwap, PUFSTATE.Swaping);
            Transition(PUFSTATE.Retrying, PUFMSG.ReadyForRobotPick, RetryReadyForRobotPick, PUFSTATE.WaitForRobotPicking);
            Transition(PUFSTATE.Retrying, PUFMSG.RobotPickComplete, RetryBackToParkStation, PUFSTATE.BackToPackStationing);
            //ConfirmComplete
            Transition(PUFSTATE.Error, PUFMSG.ConfirmComplete, NullFunc, PUFSTATE.ConfirmCompleting);
            Transition(PUFSTATE.Init, PUFMSG.ConfirmComplete, ClearModuleAlarm, PUFSTATE.Init);
            Transition(PUFSTATE.Idle, PUFMSG.ConfirmComplete, ClearModuleAlarm, PUFSTATE.Idle);
            Transition(PUFSTATE.ConfirmCompleting, FSM_MSG.TIMER, ConfirmComplete, PUFSTATE.ConfirmCompleting);
            Transition(PUFSTATE.ConfirmCompleting, PUFMSG.ReadyForRobotPlace, ConfirmReadyForRobotPlace, PUFSTATE.WaitForRobotPlace);
            Transition(PUFSTATE.ConfirmCompleting, PUFMSG.ReadyForSwap, ConfirmReadyForSwap, PUFSTATE.WaitForSwap);
            Transition(PUFSTATE.ConfirmCompleting, PUFMSG.Swap, ConfirmSwap, PUFSTATE.Swaping);
            Transition(PUFSTATE.ConfirmCompleting, PUFMSG.ReadyForRobotPick, ConfirmReadyForRobotPick, PUFSTATE.WaitForRobotPick);
            Transition(PUFSTATE.ConfirmCompleting, PUFMSG.RobotPickComplete, ConfirmBackToParkStation, PUFSTATE.Idle);


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

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

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

        /// <summary>
        /// 恢复错误
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ResumeError(object[] param)
        {
            if (_isHomed)
            {
                PostMsg(PUFMSG.ReturnIdle);
                return false;
            }
            return true;
        }
        #region Abort

        private bool Abort(object parameter)
        {
            bool preHomed = IsHomed;
            _flipAxis.StopPositionOperation();
            _rotationAxis.StopPositionOperation();
            _verticalAxis.StopPositionOperation();
            if(_currentRoutine!=null)
            {
                _currentRoutine.Abort();
                _currentRoutine = null;
            }
            if (preHomed)
            {
                PostMsg(PUFMSG.ReturnIdle);
                return false;
            }
            return true;
        }
        #endregion

        #region Switch On
        /// <summary>
        /// SwitchAll
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool SwitchOnAll(object[] param)
        {
            return _switchOnRoutine.Start() == RState.Running;
        }
        private bool SwitchOnTimeout(object[] param)
        {
            RState ret = _switchOnRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(PUFMSG.Error);
                return false;
            }

            bool result= ret == RState.End;
            if(result)
            {
                _isHomed = false;
            }
            return result;
        }
        #endregion

        #region Switch Off
        /// <summary>
        /// SwitchAll
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool SwitchOffAll(object[] param)
        {
            return _switchOffRoutine.Start() == RState.Running;
        }
        private bool SwitchOffTimeout(object[] param)
        {
            RState ret = _switchOffRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(PUFMSG.Error);
                return false;
            }

            bool result= ret == RState.End;
            if(result)
            {
                _isHomed= false;
            }
            return result;
        }
        #endregion

        #region Home
        /// <summary>
        /// HomeAll
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool HomeAll(object[] param)
        {
            _isHomed = false;
            bool result= _homeAllRoutine.Start() == RState.Running;
            if(result)
            {
                _currentRoutine = _homeAllRoutine;
            }
            return result;
        }
        /// <summary>
        /// Home超时
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool HomingTimeout(object[] param)
        {
            RState ret = _homeAllRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                _currentRoutine = null;
                PostMsg(PUFMSG.Error);
                _isHomed = false;
                return false;
            }

            bool result = ret == RState.End;
            if (result)
            {
                _currentRoutine = null;
                _isHomed = true;
            }
            return result;
        }
        #endregion

        #region Calibrate
        private bool Calibrate(object[] param)
        {
            _calibrateRoutine.Start(param);
            return true;
        }
        private bool CalibrateTimeout(object[] param)
        {
            RState ret = _calibrateRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                return true;
            }

            return ret == RState.End;
        }
        #endregion

        #region Pick
        private bool PickFromLoader(object[] param)
        {
            bool result= _pickFromLoaderRoutine.Start(param)==RState.Running;
            if(result)
            {
                _currentRoutine = _pickFromLoaderRoutine;
            }
            return result;
        }
        private bool PickTimeout(object[] param)
        {
            //RState ret = _pufNoWaferPickRoutine.Monitor();
            RState ret = _pickFromLoaderRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(PUFMSG.Error);
                return false;
            }

            bool result = ret == RState.End;
            if(result)
            {
                _currentRoutine = null;
            }
            return result;
        }
        #endregion

        #region Place
        /// <summary>
        /// 放片至Loader
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool PlaceToLoader(object[] param)
        {
            bool result = _placeToLoaderRoutine.Start(param) == RState.Running;
            if(result)
            {
                _currentRoutine = _placeToLoaderRoutine;
            }
            return result;
        }
        /// <summary>
        /// 监控放片至Loader
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool PlaceToLoaderMonitor(object[] param)
        {
            //RState ret = _pufNoWaferPickRoutine.Monitor();
            RState ret = _placeToLoaderRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                _currentRoutine = null;
                PostMsg(PUFMSG.Error);
                return false;
            }

            bool result = ret == RState.End;
            if(result)
            {
                _currentRoutine = null;
            }
            return result;
        }
        #endregion
        
        #region GotoRobotPositionForPlace
        /// <summary>
        /// Goto Robot Position For Place
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool GotoRobotPositionForPlace(object[] param) 
        {
            bool result= _gotoRobotForPlaceRoutine.Start(param)==RState.Running;
            if(result)
            {
                _currentRoutine = _gotoRobotForPlaceRoutine;
            }
            return result;
        }
        /// <summary>
        /// Goto Robot Position For Place监控
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool GotoRobotPositionForPlaceMonitor(object[] param)
        {
            RState ret = _gotoRobotForPlaceRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                _currentRoutine = null;
                PostMsg(PUFMSG.Error);
                return false;
            }

            bool result= ret == RState.End;
            if(result)
            {
                _currentRoutine = null;
            }
            return result;
        }
        #endregion

        #region GotoRobotPositionForPick
        /// <summary>
        /// Goto Robot Position For Pick
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool GotoRobotPositionForPick(object[] param)
        {
            bool result= _gotoRobotForPickRoutine.Start(param) == RState.Running;
            if(result)
            {
                _currentRoutine = _gotoRobotForPickRoutine;
            }
            return result;
        }
        /// <summary>
        /// Goto Robot Position For Pick监控
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool GotoRobotPositionForPickMonitor(object[] param)
        {
            RState ret = _gotoRobotForPickRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                _currentRoutine = null;
                PostMsg(PUFMSG.Error);
                return false;
            }

            bool result = ret == RState.End;
            if(result)
            {
                _currentRoutine = null;
            }
            return result;
        }
        #endregion

        #region ReadyForRobotPlace
        /// <summary>
        /// Ready For Robot Place
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ReadyForRobotPlace(object[] param)
        {
            bool result = _readyForRobotPlaceRoutine.Start(param)==RState.Running;
            if(result)
            {
                _currentRoutine = _readyForRobotPlaceRoutine;
            }
            return result;
        }

        /// <summary>
        /// Ready For Robot Place
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool RetryReadyForRobotPlace(object[] param)
        {
            int stepIndex = (int)param[0];
            bool result = _readyForRobotPlaceRoutine.Retry(stepIndex) == RState.Running;
            if (result)
            {
                _currentRoutine = _readyForRobotPlaceRoutine;
            }
            return result;
        }
        /// <summary>
        /// ReadyForRobotPlace监控
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ReadyForRobotPlaceMonitor(object[] param)
        {
            RState ret = _readyForRobotPlaceRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                _currentRoutine = null;

                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmList alarmList = new AlarmList(Module.ToString(), ((PUFSTATE)fsm.State).ToString(), (int)PUFMSG.ReadyForRobotPlace, _readyForRobotPlaceRoutine.ErrorMsg, _readyForRobotPlaceRoutine.ErrorStep, (int)AlarmType.Error);
                    AlarmListManager.Instance.AddAlarm(alarmList);
                }
                PostMsg(PUFMSG.Error);
                return false;
            }

            bool result= ret == RState.End;
            if(result)
            {
                AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), PUFSTATE.WaitForRobotPlacing.ToString());
                _currentRoutine = null;
            }
            return result;
        }
        /// <summary>
        /// 确认ReadyForRobotPlace
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ConfirmReadyForRobotPlace(object[] param)
        {
            bool result = _readyForRobotPlaceRoutine.CheckCompleteCondition();
            if (!result)
            {

                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmList alarmList = new AlarmList(Module.ToString(), ((PUFSTATE)fsm.State).ToString(), (int)PUFMSG.ReadyForRobotPlace,
                    _readyForRobotPlaceRoutine.ErrorMsg, _readyForRobotPlaceRoutine.ErrorStep, (int)AlarmType.Error);
                    AlarmListManager.Instance.AddAlarm(alarmList);
                }
                PostMsg(PUFMSG.Error);
            }
            else
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), PUFSTATE.WaitForRobotPlacing.ToString());
                }
            }
            return result;
        }
        #endregion

        #region Ready for Swap
        /// <summary>
        /// Ready For Swap
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ReadyForSwap(object[] param)
        {
            bool result= _readyForSwapRoutine.Start(param)==RState.Running;
            if(result)
            {
                _currentRoutine = _readyForSwapRoutine;
            }
            return result;
        }
        /// <summary>
        /// Retry Ready For Swap
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool RetryReadyForSwap(object[] param)
        {
            int stepIndex = (int)param[0];
            bool result = _readyForSwapRoutine.Retry(stepIndex) == RState.Running;
            if (result)
            {
                _currentRoutine = _readyForSwapRoutine;
            }
            return result;
        }
        /// <summary>
        /// Ready For Swap 监控
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ReadyForSwapMonitor(object[] param)
        {
            RState ret = _readyForSwapRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                _currentRoutine = null;

                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmList alarmList = new AlarmList(Module.ToString(), ((PUFSTATE)fsm.State).ToString(), (int)PUFMSG.ReadyForSwap,
                    _readyForSwapRoutine.ErrorMsg, _readyForSwapRoutine.ErrorStep, (int)AlarmType.Error);
                    AlarmListManager.Instance.AddAlarm(alarmList);
                }
                PostMsg(PUFMSG.Error);
                return false;
            }

            bool result= ret == RState.End;
            if(result)
            {
                AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), PUFSTATE.WaitForSwaping.ToString());
                _currentRoutine = null;
            }
            return result;
        }

        /// <summary>
        /// 确认Ready For Swap
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ConfirmReadyForSwap(object[] param)
        {
            bool result = _readyForSwapRoutine.CheckCompleteCondition();
            if (!result)
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmList alarmList = new AlarmList(Module.ToString(), ((PUFSTATE)fsm.State).ToString(), (int)PUFMSG.ReadyForSwap,
                    _readyForSwapRoutine.ErrorMsg, _readyForSwapRoutine.ErrorStep, (int)AlarmType.Error);
                    AlarmListManager.Instance.AddAlarm(alarmList);
                }
                PostMsg(PUFMSG.Error);
            }
            else
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), PUFSTATE.WaitForSwaping.ToString());
                }
            }
            return result;
        }
        #endregion

        #region Swap
        /// <summary>
        /// Swap
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool Swap(object[] param)
        {
            bool result= _swapRoutine.Start(param)==RState.Running;
            if(result)
            {
                _currentRoutine = _swapRoutine;
            }
            return result;
        }

        /// <summary>
        /// Retry Swap
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool RetrySwap(object[] param)
        {
            int stepIndex = (int)param[0];
            bool result = _swapRoutine.Retry(stepIndex) == RState.Running;
            if (result)
            {
                _currentRoutine = _swapRoutine;
            }
            return result;
        }
        /// <summary>
        /// Swap监控
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool SwapMonitor(object[] param)
        {
            RState ret = _swapRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                _currentRoutine = null;
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmList alarmList = new AlarmList(Module.ToString(), ((PUFSTATE)fsm.State).ToString(), (int)PUFMSG.Swap,
                    _swapRoutine.ErrorMsg, _swapRoutine.ErrorStep, (int)AlarmType.Error);
                    AlarmListManager.Instance.AddAlarm(alarmList);
                }
                PostMsg(PUFMSG.Error);
                return false;
            }

            bool result = ret == RState.End;
            if(result)
            {
                AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), PUFSTATE.Swaping.ToString());
                _currentRoutine = null;
            }
            return result;
        }

        /// <summary>
        /// 确认ReadyForRobotPick
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ConfirmSwap(object[] param)
        {
            bool result = _swapRoutine.CheckCompleteCondition();
            if (!result)
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmList alarmList = new AlarmList(Module.ToString(), ((PUFSTATE)fsm.State).ToString(), (int)PUFMSG.Swap,
                    _readyForRobotPickRoutine.ErrorMsg, _readyForRobotPickRoutine.ErrorStep, (int)AlarmType.Error);
                    AlarmListManager.Instance.AddAlarm(alarmList);
                }
                PostMsg(PUFMSG.Error);
            }
            else
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), PUFSTATE.Swaping.ToString());
                }
            }
            return result;
        }
        #endregion

        #region ReadyForRobotPIck
        /// <summary>
        /// Ready For Robot Pick
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ReadyForRobotPick(object[] param)
        {
            bool result= _readyForRobotPickRoutine.Start(param) == RState.Running;
            if(result)
            {
                _currentRoutine = _readyForRobotPickRoutine;
            }
            return result;
        }
        /// <summary>
        /// retry Ready For Robot Pick
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool RetryReadyForRobotPick(object[] param)
        {
            int stepIndex = (int)param[0];
            bool result = _readyForRobotPickRoutine.Retry(stepIndex) == RState.Running;
            if (result)
            {
                _currentRoutine = _readyForRobotPickRoutine;
            }
            return result;
        }
        /// <summary>
        /// ReadyForRobotPick监控
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ReadyForRobotPickMonitor(object[] param)
        {
            RState ret = _readyForRobotPickRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmList alarmList = new AlarmList(Module.ToString(), ((PUFSTATE)fsm.State).ToString(), (int)PUFMSG.ReadyForRobotPick,
                    _readyForRobotPickRoutine.ErrorMsg, _readyForRobotPickRoutine.ErrorStep, (int)AlarmType.Error);
                    AlarmListManager.Instance.AddAlarm(alarmList);
                }
                PostMsg(PUFMSG.Error);
                _currentRoutine = _readyForRobotPickRoutine;
                return false;
            }

            bool result= ret == RState.End;
            if(result)
            {
                AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), PUFSTATE.WaitForRobotPicking.ToString());
                _currentRoutine = null;
            }
            return result;  
        }
        /// <summary>
        /// 确认ReadyForRobotPick
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ConfirmReadyForRobotPick(object[] param)
        {
            bool result = _readyForRobotPickRoutine.CheckCompleteCondition();
            if (!result)
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmList alarmList = new AlarmList(Module.ToString(), ((PUFSTATE)fsm.State).ToString(), (int)PUFMSG.ReadyForRobotPick,
                    _readyForRobotPickRoutine.ErrorMsg, _readyForRobotPickRoutine.ErrorStep, (int)AlarmType.Error);
                    AlarmListManager.Instance.AddAlarm(alarmList);
                }
                PostMsg(PUFMSG.Error);
            }
            else
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), PUFSTATE.ConfirmCompleting.ToString());
                }
            }
            return result;
        }
        #endregion

        #region BackToParkStation
        /// <summary>
        /// Back to park station
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool BackToParkStation(object[] param)
        {
            bool result= _backToParkRoutine.Start(param) == RState.Running;
            if(result)
            {
                _currentRoutine = _backToParkRoutine;
            }
            return true;
        }
        /// <summary>
        /// Retry Back to park station
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool RetryBackToParkStation(object[] param)
        {
            int stepIdex = (int)param[0];
            bool result = _backToParkRoutine.Retry(stepIdex) == RState.Running;
            if (result)
            {
                _currentRoutine = _backToParkRoutine;
            }
            return true;
        }
        /// <summary>
        /// BackToParkStation监控
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool BackToParkStationMonitor(object[] param)
        {
            RState ret = _backToParkRoutine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmList alarmList = new AlarmList(Module.ToString(), ((PUFSTATE)fsm.State).ToString(), (int)PUFMSG.RobotPickComplete,
                    _backToParkRoutine.ErrorMsg, _backToParkRoutine.ErrorStep, (int)AlarmType.Error);
                    AlarmListManager.Instance.AddAlarm(alarmList);
                }
                PostMsg(PUFMSG.Error);
                _currentRoutine = null;
                return false;
            }

            bool result= ret == RState.End;
            if(result)
            {
                AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), PUFSTATE.BackToPackStationing.ToString());
                _currentRoutine = null;
            }
            return result;
        }
        /// <summary>
        /// 确认BackToParkStation
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ConfirmBackToParkStation(object[] param)
        {
            bool result = _backToParkRoutine.CheckCompleteCondition();
            if (!result)
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmList alarmList = new AlarmList(Module.ToString(), ((PUFSTATE)fsm.State).ToString(), (int)PUFMSG.RobotPickComplete,
                    _backToParkRoutine.ErrorMsg, _backToParkRoutine.ErrorStep, (int)AlarmType.Error);
                    AlarmListManager.Instance.AddAlarm(alarmList);
                }
                PostMsg(PUFMSG.Error);
            }
            else
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), PUFSTATE.BackToPackStationing.ToString());
                }
            }
            return result;
        }
        #endregion

        #region Retry
        /// <summary>
        /// Retry
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool PufRetry(object[] param)
        {
            AlarmList alarmList = AlarmListManager.Instance.GetAlarmListByModule(Module.ToString());
            if (alarmList != null)
            {
                CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), alarmList.ModuleCmd,
                    alarmList.ModuleStep);
            }
            return false;
        }
        #endregion

        #region ConfirmComplete
        /// <summary>
        /// 确认是否完成
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ConfirmComplete(object[] param)
        {
            AlarmList alarmList = AlarmListManager.Instance.GetAlarmListByModule(Module.ToString());
            if (alarmList != null)
            {
                if (alarmList.ModuleState == PUFSTATE.WaitForRobotPlacing.ToString())
                {
                    CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.ReadyForRobotPlace);
                }
                else if (alarmList.ModuleState == PUFSTATE.WaitForSwaping.ToString())
                {
                    CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.ReadyForSwap);
                }
                else if (alarmList.ModuleState == PUFSTATE.WaitForRobotPicking.ToString())
                {
                    CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.ReadyForRobotPick);
                }
                else if (alarmList.ModuleState == PUFSTATE.BackToPackStationing.ToString())
                {
                    CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.RobotPickComplete);
                }
                else if (alarmList.ModuleState == PUFSTATE.Swaping.ToString())
                {
                    CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.Swap);
                }
                else
                {
                    PostMsg(PUFMSG.Error);
                    LOG.WriteLog(eEvent.ERR_PUF, Module.ToString(), $"error state {alarmList.ModuleState},cannot ConfirmComplete");
                }
            }
            return false;
        }
        /// <summary>
        /// 清除报警
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool ClearModuleAlarm(object[] param)
        {
            AlarmList alarmList = AlarmListManager.Instance.GetAlarmListByModule(Module.ToString());
            if (alarmList != null)
            {
                AlarmListManager.Instance.CheckModuleAlamAndRemove(Module.ToString(), "");
            }
            return true;
        }
        #endregion

        #region FlipSideA
        /// <summary>
        /// Flip SideA
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool FlipSideA(object[] param) 
        {
            return _flipAxis.PositionStation("SideA");
        }
        /// <summary>
        /// Flip SideA监控
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool FlipSideAMonitor(object[] param)
        {
            RState ret = _flipAxis.Status;
            if (ret == RState.End)
            {
                return true;
            }
            else if (ret == RState.Failed || ret == RState.Timeout)
            {
                PostMsg(PUFMSG.Error);
            }
            return false;
        }
        #endregion

        /// <summary>
        /// 校验Puf vertical/Rotation是否到达Robot位置
        /// </summary>
        /// <returns></returns>
        public bool CheckPufVerticalAndRotationInRobot()
        {
            if (_verticalAxis == null)
            {
                return false;
            }
            if (!_verticalAxis.CheckPositionIsInStation(_verticalAxis.MotionData.MotorPosition, "Robot"))
            {
                return false;
            }
            if (!_rotationAxis.CheckPositionIsInStation(_rotationAxis.MotionData.MotorPosition, "Robot"))
            {
                return false;
            }
            return true;
        }
        public bool Check(int msg, out string reason, params object[] args)
        {
            reason = "";
            return false;
        }

        public bool CheckAcked(int msg)
        {
            return false;
        }

        public int Invoke(string function, params object[] args)
        {
            switch(function)
            {
                case "HomeAll":
                    if(IsIdle)
                    {
                        return (int)FSM_MSG.NONE;
                    }
                    if (CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.HomeAll))
                    {
                        return (int)FSM_MSG.NONE;
                    }
                    else
                    {
                        return (int)FSM_MSG.ALARM;
                    }
                case "Abort":
                    CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.Abort);
                    return (int)FSM_MSG.NONE;
                case "Retry":
                    if (CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.Retry, args))
                    {
                        return (int)PUFMSG.Retry;
                    }
                    else
                    {
                        return (int)FSM_MSG.NONE;
                    }
                case "ConfirmComplete":
                    if (CheckToPostMessage<PUFSTATE, PUFMSG>(eEvent.ERR_PUF, Module.ToString(), (int)PUFMSG.ConfirmComplete, args))
                    {
                        return (int)PUFMSG.ConfirmComplete;
                    }
                    else
                    {
                        return (int)FSM_MSG.NONE;
                    }
                default:
                    return (int)FSM_MSG.NONE;
            }
        }
    }
    public enum PUFMSG
    {
        HomeAll,            // 0
        SwitchOn,
        SwitchOff,
        Calibrate,
        Error,
        ResumeError,
        Abort,
        PickFromLoader,
        PlaceToLoader,
        GoToSavedPosition,
        GotoRobotPositionForPlace,
        GotoRobotPositionForPick,
        ReturnIdle,
        ReturnInit,
        ReadyForRobotPlace,
        ReadyForSwap,
        Swap,
        ReadyForRobotPick,
        RobotPickComplete,
        Retry,
        ConfirmComplete,
        FlipSideA
    }
}