using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.Schedulers;
using MECF.Framework.Common.SubstrateTrackings;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Interop;
using Venus_Core;
using Venus_RT.Modules.TM.VenusEntity;
using Venus_RT.Scheduler;

namespace Venus_RT.Modules.Schedulers
{
    
    public class SchedulerSETMRobot : SchedulerModule
    {
        class SchedulerItem
        {
            public SETMEntity.MSG MoveType { get; set; }
            public ModuleName target { get; set; }

            public Queue<MoveItem> moveList { get; set; }

        }
        public override bool IsAvailable
        {
            get { return _entity.IsIdle && (_entity.IsOnline || !Singleton<RouteManager>.Instance.IsAutoMode) && RunSchedulers(); }
        }
        public override bool IsOnline
        {
            get { return _entity.IsOnline; }

        }
        public override bool IsError
        {
            get { return _entity.IsError; }

        }

        public RState RobotStatus
        {
            get { return _entity.RobotStatus; }
        }

        public bool IsVCESlitDoorClosed => _entity.IsVCESlitDoorClosed;


        private SETMEntity _entity;
        public int _entityTaskToken = (int)FSM_MSG.NONE;
        private Queue<SchedulerItem> _schedulerList = new Queue<SchedulerItem>();
        private SchedulerItem _currentScheduler = null;
        private int _singleArmOption = 0;

        public SchedulerSETMRobot(ModuleName module) : base(module.ToString())
        {
            _entity = Singleton<RouteManager>.Instance.seTM;
            _singleArmOption = SC.GetValue<int>("SETM.SingleArmOption");
        }

        public bool PostMoveItems(MoveItem[] items)
        {
            foreach (var item in items)
            {
                LOG.Write(eEvent.EV_ROUTER, ModuleName.TMRobot, $"Post Moving Item: {item.SourceModule} Slot {item.SourceSlot + 1} => {item.DestinationModule} Slot {item.DestinationSlot + 1}");
            }

            //pm swap
            void PackPMSwapCmds(MoveItem[] swapItems)
            {
                // must swap the item order for PM swap
                if (!ModuleHelper.IsTMRobot(swapItems[1].SourceModule))
                {
                    SchedulerItem item = new SchedulerItem();
                    item.MoveType = SETMEntity.MSG.Pick;
                    item.target = swapItems[1].SourceModule;
                    item.moveList = new Queue<MoveItem>();
                    item.moveList.Enqueue(new MoveItem(swapItems[1].SourceModule, swapItems[1].SourceSlot, ModuleName.TMRobot, 0, (Hand)0));
                    _schedulerList.Enqueue(item);
                }

                SchedulerItem swap = new SchedulerItem();
                swap.MoveType = SETMEntity.MSG.PMSwap;
                swap.target = swapItems[0].SourceModule;
                swap.moveList = new Queue<MoveItem>();
                swap.moveList.Enqueue(new MoveItem(swapItems[0].SourceModule, swapItems[0].SourceSlot, ModuleName.TMRobot, 1, (Hand)1));
                swap.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, 0, swapItems[1].DestinationModule, swapItems[1].DestinationSlot, (Hand)0));
                _schedulerList.Enqueue(swap);

                if (!ModuleHelper.IsTMRobot(swapItems[0].DestinationModule))
                {
                    SchedulerItem last = new SchedulerItem();
                    last.MoveType = SETMEntity.MSG.Place;
                    last.target = swapItems[0].DestinationModule;
                    last.moveList = new Queue<MoveItem>();
                    last.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, 1, swapItems[0].DestinationModule, swapItems[0].DestinationSlot, (Hand)1));
                    _schedulerList.Enqueue(last);
                }
            }

            void PackSwapCmds(MoveItem[] swapItems, SETMEntity.MSG swapType, int swapIndex)
            {
                SchedulerItem swap = new SchedulerItem();
                swap.MoveType = swapType;
                swap.target = swapItems[0].DestinationModule;
                swap.moveList = new Queue<MoveItem>();

                for (int i = 0; i < swapIndex; i++)
                {
                    if (!ModuleHelper.IsTMRobot(swapItems[i].SourceModule))
                    {
                        SchedulerItem item = new SchedulerItem();
                        item.MoveType = !ModuleHelper.IsPm(swapItems[i].SourceModule) ? SETMEntity.MSG.Pick : SETMEntity.MSG.PMPick;
                        item.target = swapItems[i].SourceModule;
                        item.moveList = new Queue<MoveItem>();
                        item.moveList.Enqueue(new MoveItem(swapItems[i].SourceModule, swapItems[i].SourceSlot, ModuleName.TMRobot, i, (Hand)i));
                        _schedulerList.Enqueue(item);

                        swap.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, i, swapItems[i].DestinationModule, swapItems[i].DestinationSlot, (Hand)i));
                    }
                }

                for (int j = swapIndex; j < swapItems.Length; j++)
                {
                    swap.moveList.Enqueue(new MoveItem(swapItems[j].SourceModule, swapItems[j].SourceSlot, ModuleName.TMRobot, j - swapIndex, (Hand)(j - swapIndex)));
                }
                _schedulerList.Enqueue(swap);

                for (int j = swapIndex; j < swapItems.Length; j++)
                {
                    if (!ModuleHelper.IsTMRobot(swapItems[j].DestinationModule))
                    {
                        SchedulerItem item = new SchedulerItem();
                        item.MoveType = !ModuleHelper.IsPm(swapItems[j].DestinationModule) ? SETMEntity.MSG.Place : SETMEntity.MSG.PMPlace;
                        item.target = swapItems[j].DestinationModule;
                        item.moveList = new Queue<MoveItem>();
                        item.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, j - swapIndex, swapItems[j].DestinationModule, swapItems[j].DestinationSlot, (Hand)(j - swapIndex)));
                        _schedulerList.Enqueue(item);
                    }
                }
            }

            //双臂模式 检查是否双臂有wafer
            if (WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot, 0) && WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot, 1) && _singleArmOption == 0)
            {
                //switch (items.Length)
                //{
                //    //3 in && 3 out
                //    //double pick align swap
                //    case 6:
                //
                //        break; 
                //    //2 in && 3 out
                //    case 5:
                //
                //        break;
                //    //1 in && 3 out
                //    //2 in && 2 out
                //    case 4:
                //        break;
                //    //1 in && 2 out
                //    case 3:
                //        if (
                //            ModuleHelper.IsVCE(items[0].SourceModule)&&
                //            ModuleHelper.IsVCE(items[1].SourceModule)&&
                //            ModuleHelper.IsVCE(items[2].SourceModule)&&
                //            ModuleHelper.IsPm(items[0].DestinationModule) &&
                //            ModuleHelper.IsPm(items[1].DestinationModule) &&
                //            ModuleHelper.IsPm(items[2].DestinationModule))
                //        {
                //            SchedulerItem scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.Swap;
                //            scitem.target = items[0].SourceModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem(items[0].SourceModule, items[0].SourceSlot, ModuleName.TMRobot, 0, Hand.Blade1));
                //            scitem.moveList.Enqueue(new MoveItem(items[1].SourceModule, items[1].SourceSlot, ModuleName.TMRobot, 1, Hand.Blade2));
                //            _schedulerList.Enqueue(scitem);
                //
                //            scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.PMPlace;
                //            scitem.target = items[0].DestinationModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, 0, items[0].DestinationModule, items[0].DestinationSlot, Hand.Blade1));
                //            _schedulerList.Enqueue(scitem);
                //
                //            scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.PMPlace;
                //            scitem.target = items[0].DestinationModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, 1, items[1].DestinationModule, items[1].DestinationSlot, Hand.Blade2));
                //            _schedulerList.Enqueue(scitem);
                //
                //            scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.Pick;
                //            scitem.target = items[2].SourceModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem(items[2].SourceModule, items[2].SourceSlot, ModuleName.TMRobot, 0, Hand.Blade1));
                //            _schedulerList.Enqueue(scitem);
                //
                //            scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.PMPlace;
                //            scitem.target = items[2].DestinationModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, 0, items[2].DestinationModule, items[2].DestinationSlot, Hand.Blade1));
                //            _schedulerList.Enqueue(scitem);
                //        }
                //
                //        if (ModuleHelper.IsVCE(items[0].DestinationModule) &&
                //            ModuleHelper.IsVCE(items[1].DestinationModule) &&
                //            ModuleHelper.IsVCE(items[2].DestinationModule) &&
                //            ModuleHelper.IsPm(items[0].SourceModule) &&
                //            ModuleHelper.IsPm(items[1].SourceModule) &&
                //            ModuleHelper.IsPm(items[2].SourceModule))
                //        {
                //            SchedulerItem scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.PMPick;
                //            scitem.target = items[0].SourceModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem(items[0].SourceModule, items[0].SourceSlot, ModuleName.TMRobot, 0, Hand.Blade1));
                //            _schedulerList.Enqueue(scitem);
                //
                //            scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.PMPick;
                //            scitem.target = items[1].SourceModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem(items[1].SourceModule, items[1].SourceSlot, ModuleName.TMRobot, 1, Hand.Blade2));
                //            _schedulerList.Enqueue(scitem);
                //
                //            scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.Place;
                //            scitem.target = items[0].DestinationModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem( ModuleName.TMRobot, 0, items[0].DestinationModule, items[0].DestinationSlot, Hand.Blade1));
                //            _schedulerList.Enqueue(scitem);
                //
                //            scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.Place;
                //            scitem.target = items[1].DestinationModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, 1, items[1].DestinationModule, items[1].DestinationSlot, Hand.Blade2));
                //            _schedulerList.Enqueue(scitem);
                //
                //            scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.PMPick;
                //            scitem.target = items[2].SourceModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem(items[2].SourceModule, items[2].SourceSlot, ModuleName.TMRobot, 0, Hand.Blade1));
                //            _schedulerList.Enqueue(scitem);
                //
                //            scitem = new SchedulerItem();
                //            scitem.MoveType = SETMEntity.MSG.Place;
                //            scitem.target = items[2].DestinationModule;
                //            scitem.moveList = new Queue<MoveItem>();
                //            scitem.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, 0, items[2].DestinationModule, items[2].DestinationSlot, Hand.Blade1));
                //            _schedulerList.Enqueue(scitem);
                //
                //        }
                //            break;
                //    //1 in && 1 out
                //    case 2:
                //        break;
                //    default:
                //        break;
                //}


                if (items.Length == 4)
                {
                    //是回到vce 且下一个也是 再下两个要出来 可以做swap 
                    if (ModuleHelper.IsVCE(items[0].DestinationModule) &&
                        items[0].DestinationModule == items[1].DestinationModule &&
                        items[0].DestinationModule == items[2].SourceModule &&
                        items[0].DestinationModule == items[3].SourceModule)
                    {
                        PackSwapCmds(items, SETMEntity.MSG.Swap, 2);
                    }
                else if (items.Length == 3)
                {
                    if (ModuleHelper.IsVCE(items[0].DestinationModule) &&
                        items[0].DestinationModule == items[1].DestinationModule &&
                        items[0].DestinationModule == items[2].SourceModule)
                    {
                        PackSwapCmds(items, SETMEntity.MSG.Swap, 2);
                    }
                    else if (ModuleHelper.IsVCE(items[0].DestinationModule) &&
                        items[0].DestinationModule == items[1].SourceModule &&
                        items[0].DestinationModule == items[2].SourceModule)
                    {
                        PackSwapCmds(items, SETMEntity.MSG.Swap, 1);
                    }
                    else if (ModuleHelper.IsVCE(items[0].SourceModule) &&
                        items[0].SourceModule == items[1].SourceModule &&
                        ModuleHelper.IsPm(items[1].DestinationModule) &&
                        items[1].DestinationModule == items[2].SourceModule)
                    {
                        SchedulerItem llDoublePick = new SchedulerItem();
                        llDoublePick.MoveType = SETMEntity.MSG.Swap;
                        llDoublePick.target = items[0].SourceModule;
                        llDoublePick.moveList = new Queue<MoveItem>();
                        llDoublePick.moveList.Enqueue(new MoveItem(items[0].SourceModule, items[0].SourceSlot, ModuleName.TMRobot, 0, 0));
                        llDoublePick.moveList.Enqueue(new MoveItem(items[1].SourceModule, items[1].SourceSlot, ModuleName.TMRobot, 1, (Hand)1));
                        _schedulerList.Enqueue(llDoublePick);
                
                        SchedulerItem pm1_place = new SchedulerItem();
                        pm1_place.MoveType = SETMEntity.MSG.PMPlace;
                        pm1_place.target = items[0].DestinationModule;
                        pm1_place.moveList = new Queue<MoveItem>();
                        pm1_place.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, 0, items[0].DestinationModule, items[0].DestinationSlot, 0));
                        _schedulerList.Enqueue(pm1_place);
                
                        SchedulerItem pm2_swap = new SchedulerItem();
                        pm2_swap.MoveType = SETMEntity.MSG.PMSwap;
                        pm2_swap.target = items[2].SourceModule;
                        pm2_swap.moveList = new Queue<MoveItem>();
                        pm2_swap.moveList.Enqueue(new MoveItem(items[2].SourceModule, items[2].SourceSlot, ModuleName.TMRobot, 0, 0));
                        pm2_swap.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, 1, items[1].DestinationModule, items[1].DestinationSlot, (Hand)1));
                        _schedulerList.Enqueue(pm2_swap);
                
                        if (ModuleHelper.IsVCE(items[2].DestinationModule))
                        {
                            SchedulerItem llPlace = new SchedulerItem();
                            llPlace.MoveType = SETMEntity.MSG.Place;
                            llPlace.target = items[2].DestinationModule;
                            llPlace.moveList = new Queue<MoveItem>();
                            llPlace.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, 0, items[2].DestinationModule, items[2].DestinationSlot, 0));
                            _schedulerList.Enqueue(llPlace);
                        }
                    }
                    }
                }
                else if (items.Length == 2)
                {

                    if (ModuleHelper.IsTMRobot(items[0].DestinationModule) 
                        && ModuleHelper.IsTMRobot(items[1].DestinationModule)
                        &&ModuleHelper.IsVCE(items[0].SourceModule)
                        &&ModuleHelper.IsVCE(items[1].SourceModule)
                        &&WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot,0) && WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot, 1))
                    {
                        SchedulerItem llDoublePick = new SchedulerItem();
                        llDoublePick.MoveType = SETMEntity.MSG.Swap;
                        llDoublePick.target = items[0].SourceModule;
                        llDoublePick.moveList = new Queue<MoveItem>();
                        llDoublePick.moveList.Enqueue(new MoveItem(items[0].SourceModule, items[0].SourceSlot, ModuleName.TMRobot, 0, 0));
                        llDoublePick.moveList.Enqueue(new MoveItem(items[1].SourceModule, items[1].SourceSlot, ModuleName.TMRobot, 1, (Hand)1));
                        _schedulerList.Enqueue(llDoublePick);
                    }
                    else if (ModuleHelper.IsVCE(items[0].DestinationModule) &&
                            items[0].DestinationModule == items[1].DestinationModule)
                    {
                        PackSwapCmds(items, SETMEntity.MSG.Swap, 2);
                    }
                    else if (ModuleHelper.IsVCE(items[0].SourceModule) &&
                        items[0].SourceModule == items[1].SourceModule)
                    {
                        PackSwapCmds(items, SETMEntity.MSG.Swap, 0);
                    }
                    else if (ModuleHelper.IsPm(items[0].SourceModule) &&
                        items[0].SourceModule == items[1].DestinationModule)
                    {
                        PackPMSwapCmds(items);
                    }
                }
            }
            Hand freeHand = SelectFreeHand();
            //如果没有空的手臂 直接报错
            if (freeHand == Hand.None && !ModuleHelper.IsTMRobot(items[0].SourceModule))
            {
                LOG.Write(eEvent.WARN_ROUTER, ModuleName.TMRobot, "No Free Arm to transfer wafer");
                return false;
            }

            if (_schedulerList.Count == 0) 
            {
                if (items.Length >= 2 && items[0].SourceModule == items[1].DestinationModule && ModuleHelper.IsPm(items[0].SourceModule))
                {
                    SchedulerItem item = new SchedulerItem();
                    item.MoveType = SETMEntity.MSG.PMSwap;
                    item.target = items[0].SourceModule;
                    item.moveList = new Queue<MoveItem>();
                    item.moveList.Enqueue(new MoveItem(items[0].SourceModule, items[0].SourceSlot, items[1].DestinationModule, items[1].DestinationSlot, (Hand)(items[1].SourceSlot == 1 ? 0 : 1)));
                    _schedulerList.Enqueue(item);

                    item = new SchedulerItem();
                    item.MoveType = SETMEntity.MSG.Place;
                    item.target = items[0].SourceModule;
                    item.moveList = new Queue<MoveItem>();
                    item.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, items[1].SourceSlot, items[0].DestinationModule, items[0].DestinationSlot, (Hand)(items[1].SourceSlot == 1 ? 0 : 1)));
                    _schedulerList.Enqueue(item);
                    items = items.Skip(2).ToArray();
                }

                if (items.Length == 3 
                    && ModuleHelper.IsTMRobot(items[1].DestinationModule)
                    && ModuleHelper.IsTMRobot(items[2].DestinationModule)
                    && items[1].DestinationSlot == items[2].DestinationSlot
                    )
                {
                    items[1].DestinationSlot = 0;
                    items[2].DestinationSlot = 1;
                }

                foreach (var moveitem in items)
                {
                    //如果不是从tm上来去 就要补上tmrobot中间过程
                    if (!ModuleHelper.IsTMRobot(moveitem.SourceModule) && !ModuleHelper.IsTMRobot(moveitem.DestinationModule))
                    {
                        //如何取
                        SchedulerItem item = new SchedulerItem();
                        item.MoveType = !ModuleHelper.IsPm(moveitem.SourceModule) ? SETMEntity.MSG.Pick : SETMEntity.MSG.PMPick;
                        item.target = moveitem.SourceModule;
                        item.moveList = new Queue<MoveItem>();
                        item.moveList.Enqueue(new MoveItem(moveitem.SourceModule, moveitem.SourceSlot, ModuleName.TMRobot, (int)freeHand, freeHand));
                        _schedulerList.Enqueue(item);
                        //如何放
                        item = new SchedulerItem();
                        item.MoveType = !ModuleHelper.IsPm(moveitem.DestinationModule) ? SETMEntity.MSG.Place : SETMEntity.MSG.PMPlace;
                        item.target = moveitem.DestinationModule;
                        item.moveList = new Queue<MoveItem>();
                        item.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, (int)freeHand, moveitem.DestinationModule, moveitem.DestinationSlot, freeHand));
                        _schedulerList.Enqueue(item);
                    }
                    //如果是place
                    else if (ModuleHelper.IsTMRobot(moveitem.SourceModule))
                    {
                        SchedulerItem item = new SchedulerItem();
                        item.MoveType = !ModuleHelper.IsPm(moveitem.DestinationModule) ? SETMEntity.MSG.Place : SETMEntity.MSG.PMPlace;
                        item.target = moveitem.DestinationModule;
                        item.moveList = new Queue<MoveItem>();
                        item.moveList.Enqueue(new MoveItem(ModuleName.TMRobot, moveitem.SourceSlot, moveitem.DestinationModule, moveitem.DestinationSlot, (Hand)moveitem.SourceSlot));
                        _schedulerList.Enqueue(item);
                    }
                    //如果是pick
                    else if (ModuleHelper.IsTMRobot(moveitem.DestinationModule))
                    {
                        SchedulerItem item = new SchedulerItem();
                        item.MoveType = !ModuleHelper.IsPm(moveitem.SourceModule) ? SETMEntity.MSG.Pick : SETMEntity.MSG.PMPick;
                        item.target = moveitem.SourceModule;
                        item.moveList = new Queue<MoveItem>();
                        item.moveList.Enqueue(new MoveItem(moveitem.SourceModule, moveitem.SourceSlot, ModuleName.TMRobot, moveitem.DestinationSlot, (Hand)moveitem.DestinationSlot));
                        _schedulerList.Enqueue(item);
                    }
                }
            }
            RunSchedulers();
            return true;
        }
        bool RunSchedulers()
        {
            //非运行状态
            if (_entity.IsIdle && CheckTaskDone())
            {
                if (_schedulerList.Count == 0)
                    return true;

                _currentScheduler = _schedulerList.Dequeue();
                Queue<MoveItem> moveItems = new Queue<MoveItem>();
                foreach (var item in _currentScheduler.moveList)
                {
                    moveItems.Enqueue(item);
                    LOG.Write(eEvent.INFO_TM, ModuleName.TMRobot, $"TM Moving Items: {item.SourceModule} Slot {item.SourceSlot + 1} => {item.DestinationModule} Slot {item.DestinationSlot + 1}");
                }

                if (_entity.CheckToPostMessage((int)_currentScheduler.MoveType, moveItems))
                {
                    _entityTaskToken = (int)_currentScheduler.MoveType;
                }
                else
                    _entityTaskToken = (int)FSM_MSG.NONE;
            }

            return false;
        }

        //获得没有wafer的手臂
        private Hand SelectFreeHand()
        {
            if (WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot, 0) && _singleArmOption != 2)
                return Hand.Blade1;

            if (WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot, 1) && _singleArmOption != 1)
                return Hand.Blade2;

            return Hand.None;
        }

        private bool CheckTaskDone()
        {
            bool ret = false;
            switch (_entityTaskToken)
            {
                case (int)SETMEntity.MSG.Pick:
                case (int)SETMEntity.MSG.PMPick:
                    ret = WaferManager.Instance.CheckHasWafer(_currentScheduler.moveList.Peek().DestinationModule, _currentScheduler.moveList.Peek().DestinationSlot) &&
                        WaferManager.Instance.CheckNoWafer(_currentScheduler.moveList.Peek().SourceModule, _currentScheduler.moveList.Peek().SourceSlot);
                    break;
                case (int)SETMEntity.MSG.Place:
                case (int)SETMEntity.MSG.PMPlace:
                    ret = WaferManager.Instance.CheckHasWafer(_currentScheduler.moveList.Peek().DestinationModule, _currentScheduler.moveList.Peek().DestinationSlot) &&
                        WaferManager.Instance.CheckNoWafer(_currentScheduler.moveList.Peek().SourceModule, _currentScheduler.moveList.Peek().SourceSlot);
                    break;
                case (int)SETMEntity.MSG.Swap:
                    ret = WaferManager.Instance.CheckHasWafer(_currentScheduler.moveList.Peek().DestinationModule, _currentScheduler.moveList.Peek().DestinationSlot) &&
                        WaferManager.Instance.CheckHasWafer(_currentScheduler.moveList.Last().DestinationModule, _currentScheduler.moveList.Last().DestinationSlot);
                    break;
                case (int)SETMEntity.MSG.PMSwap:
                    ret = WaferManager.Instance.CheckHasWafer(_currentScheduler.moveList.Peek().DestinationModule, _currentScheduler.moveList.Peek().DestinationSlot) &&
                        WaferManager.Instance.CheckHasWafer(_currentScheduler.moveList.Peek().SourceModule, _currentScheduler.moveList.Peek().SourceSlot);
                    break;
                case (int)SETMEntity.MSG.Align:
                    ret = _entity.CheckAcked(_entityTaskToken) && _entity.IsIdle ; 
                    break;
                case (int)FSM_MSG.NONE:
                    ret = true;
                    break;
            }

            if (ret && _entityTaskToken != (int)FSM_MSG.NONE)
            {
                _entityTaskToken = (int)FSM_MSG.NONE;
                LOG.Write(eEvent.EV_ROUTER, Module, $"Scheduler, {_currentScheduler.target} Task done: {_currentScheduler.MoveType}");
            }

            return ret;
        }

        public override bool Align(float angle)
        {
            _task = TaskType.Align;

            if (_entity.CheckToPostMessage((int)SETMEntity.MSG.Align, angle))
            {
                _entityTaskToken = (int)SETMEntity.MSG.Align;
                return true;
            }
            else
            {
                _entityTaskToken = (int)FSM_MSG.NONE;
                return false;
            }

        }
    }
}