using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Sorter.Common;
using Venus_RT.Devices;
using MECF.Framework.Common.Routine;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using Venus_Core;
using Aitex.Core.RT.Log;
using Aitex.Core.Util;
using Venus_RT.Modules.PMs;

namespace Venus_RT.Modules.TM
{
    class MFPMExtendRoutine : ModuleRoutineBase, IRoutine
    {
        private enum ExtendStep
        {
            ArmExtend,
            End,
        }

        private readonly JetTM _JetTM;
        private readonly ITransferRobot _robot;

        private int _extendingTimeout = 120 * 1000;
        private ModuleName _targetModule;
        private PMEntity _pmModule;
        int _targetSlot;
        Hand _hand;

        public MFPMExtendRoutine(JetTM tm, ITransferRobot robot) : base(ModuleName.TMRobot)
        {
            _JetTM = tm;
            _robot = robot;
            Name = "Extend to PM";
        }
        public RState Start(params object[] objs)
        {
            if (!_robot.IsHomed)
            {
                LOG.Write(eEvent.ERR_TM, Module, $"TM Robot is not homed, please home it first");
                return RState.Failed;
            }

            _targetModule = (ModuleName)objs[0];
            _targetSlot = (int)objs[1];
            _hand = (Hand)objs[2];

            if (ModuleHelper.IsPm(_targetModule) && ModuleHelper.IsInstalled(_targetModule))
            {
                _pmModule = Singleton<RouteManager>.Instance.GetPM(_targetModule);
            }
            else
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Invalid target module : {_targetModule} for extending action");
                return RState.Failed;
            }

            if(_pmModule.IsSlitDoorClose)
            {
                LOG.Write(eEvent.ERR_TM, Module, $"{_targetModule} slit door closed, can not extend robot arm");
                return RState.Failed;
            }

            if (WaferManager.Instance.CheckHasWafer(ModuleName.TMRobot, (int)_hand))
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Cannot pick as TM Robot Arm: {_hand} already has a wafer");
                return RState.Failed;
            }

            if (WaferManager.Instance.CheckHasWafer(_targetModule, _targetSlot))
            {
                if(WaferManager.Instance.CheckHasWafer(ModuleName.TMRobot, (int)_hand))
                {
                    LOG.Write(eEvent.ERR_TM, Module, $"Both {_targetModule} and robot arm {_hand} have wafers");
                    return RState.Failed;
                }

                if(_pmModule.LiftPinIsDown)
                {
                    LOG.Write(eEvent.ERR_TM, Module, $"{_targetModule} has a wafer and Lift Pin is down, can not extend robot arm");
                    return RState.Failed;
                }
            }

            Reset();
            _extendingTimeout = SC.GetValue<int>("TM.ExtendTimeout") * 1000;

            return Runner.Start(Module, $"Extend to {_targetModule}");
        }

        public RState Monitor()
        {
            Runner.Run(ExtendStep.ArmExtend, ArmExtend, WaitRobotExtendDone)
                .End(ExtendStep.End, NullFun, _delay_50ms);

            return Runner.Status;
        }

        private bool ArmExtend()
        {
            if (WaferManager.Instance.CheckHasWafer(_targetModule, _targetSlot))
            {
                return _robot.PickExtend(_targetModule, _targetSlot, _hand);
            }
            else
            {
                return _robot.PickExtend(_targetModule, _targetSlot, _hand);
            } 
        }

        private bool WaitRobotExtendDone()
        {
            if (_robot.Status == RState.Running)
            {
                return false;
            }
            else if (_robot.Status == RState.End)
            {
                return true;
            }
            else
            {
                Runner.Stop($"TM Robot Arm Extend failed, {_robot.Status}");
                return true;
            }
        }

        public void Abort()
        {
            //_robot.Halt();
        }
    }
}