using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Sorter.Common;
using Venus_RT.Devices;
using Aitex.Core.Common;
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;
using MECF.Framework.Common.Schedulers;
using System.Collections.Generic;
using System;
using Aitex.Core.RT.Device;
using MECF.Framework.Common.DBCore;

namespace Venus_RT.Modules.TM
{
    class MFPMSwapRoutine : ModuleRoutineBase, IRoutine
    {
        private enum SwapStep
        {
            WaitPMReady,
            PreRotation,
            OpenSlitDoor,
            PickPrepare,
            PickExtend,
            DropDownWafer,
            PickDelay,
            PickRetract,
            PlacePrepare,
            PlaceExtend,
            LiftUpWafer,
            PlaceDelay,
            PlaceRetract,
            CloseSlitDoor,
            NotifyDone,
        }
        private enum SwapWithHeaterStep
        {
            WaitPMReady,
            PreRotation,
            WaitPressreStable,
            OpenSlitDoor,
            PickPrepare,
            Picking,
            QueryPickAWCData,
            SavePickAWCData,
            CheckPickAWCData,
            PlacePrepare,
            Placing,
            QueryPlaceAWCData,
            SavePlaceAWCData,
            CheckPlaceAWCData,
            CloseSlitDoor,
            NotifyDone,
        }
        private readonly JetTM _JetTM;
        private readonly ITransferRobot _robot;

        private int _swapingTimeout = 120 * 1000;
        private int _liftPinTimeout = 20 * 1000;
        private int _placeDelayTime = 0;
        private int _pickDelayTime = 0;
        private ModuleName _targetModule;
        private PMEntity _pmModule;
        private JetPMBase _jetPM;
        private int _targetSlot;
        private Hand _pickHand;
        private Hand _placeHand;
        double maxPressureDifference;


        private bool _pickQueryAWC;
        private bool _placeQueryAWC;
        private Hand _hand;
        private DateTime _starttime;
        private int _robotTransferAWCOffset;
        private int _robotPickDelayTime = 500;

        public MFPMSwapRoutine(JetTM tm, ITransferRobot robot) : base(ModuleName.TMRobot)
        {
            _JetTM = tm;
            _robot = robot;
            Name = "Swap with 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;
            }

            var swapItem = (Queue<MoveItem>)objs[0];
            if (!WaferManager.Instance.CheckDuplicatedWafersBeforeMove(swapItem))
                return RState.Failed;

            _targetModule = swapItem.Peek().SourceModule;
            _targetSlot = swapItem.Peek().SourceSlot;
            _pickHand = swapItem.Peek().RobotHand;
            _placeHand = _pickHand == Hand.Blade2 ? Hand.Blade1 : Hand.Blade2;

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

            if (WaferManager.Instance.CheckNoWafer(ModuleName.TMRobot, (int)_placeHand))
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Cannot Swap Wafer as TM Robot Arm: {_placeHand} has no wafer");
                return RState.Failed;
            }

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

            if (WaferManager.Instance.CheckNoWafer(_targetModule, _targetSlot))
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Cannot Swap Wafer as {_targetModule} Slot {_targetSlot + 1} has no wafer");
                return RState.Failed;
            }

            var wafer = WaferManager.Instance.GetWafer(_targetModule, _targetSlot);
            if (wafer.ChuckState == EnumWaferChuckStatus.Chucked)
            {
                LOG.Write(eEvent.ERR_TM, Module, $"Cannot pick the wafer in {_targetModule} as the wafer is chucked");
                return RState.Failed;
            }

            Reset();
            _swapingTimeout = SC.GetValue<int>($"TM.SwapTimeout") * 1000;
            _pickDelayTime = SC.GetValue<int>($"{_targetModule}.PickDelayTime");
            _placeDelayTime = SC.GetValue<int>($"{_targetModule}.PlaceDelayTime");
            maxPressureDifference = SC.GetValue<double>("System.PMTMMaxPressureDifference");

            int queryAWCType = SC.GetValue<int>($"TM.QueryAWCOption");
            if (queryAWCType == 0)
            {
                _pickQueryAWC = false;
                _placeQueryAWC = false;
            }
            else if (queryAWCType == 1)
            {
                _pickQueryAWC = true;
            }
            else if (queryAWCType == 2)
            {
                _placeQueryAWC = true;
            }
            else if (queryAWCType == 3)
            { 
                _pickQueryAWC = true;
                _placeQueryAWC = true;
            }
            _robotTransferAWCOffset = SC.GetValue<int>("TM.EnterPMAWCAlarmLimit");

            if (RtInstance.ConfigType == ConfigType.Kepler2200)
            {
                _robotPickDelayTime = SC.GetValue<int>($"TM.RobotPickDelayTime");
            }
            return Runner.Start(Module, $"Swap with {_targetModule}");
        }

        public RState Monitor()
        {
            //Runner.Wait(SwapStep.WaitPMReady,       () => _pmModule.IsIdle,                             _delay_60s)
            //    .RunIf(SwapStep.PreRotation,        _JetTM.PreRotateModules.ContainsKey(_targetModule), RotateArm,              WaitRotateDone)
            //    .Run(SwapStep.PickPrepare,          PickPrepare,                                        IsModuleReadyForPick)
            //    .Run(SwapStep.PickExtend,           PickExtend,                                         WaitRobotExtendDone)
            //    .Run(SwapStep.DropDownWafer,        NotifyPMPickWafer,                                  WaitPMWaferDropDown)
            //    .Delay(SwapStep.PickDelay,          _pickDelayTime)
            //    .Run(SwapStep.PickRetract,          PickRetract,                                        WaitRobotRetractDone)
            //    .Run(SwapStep.PlacePrepare,         PlacePrepare,                                       IsModuleReadyForPlace)
            //    .Run(SwapStep.PlaceExtend,          PlaceExtend,                                        WaitRobotExtendDone)
            //    .Run(SwapStep.LiftUpWafer,          NotifyLiftUpWafer,                                  WaitPMWaferLiftUp)
            //    .Delay(SwapStep.PlaceDelay,         _placeDelayTime)
            //    .Run(SwapStep.PlaceRetract,         PlaceRetract,                                       WaitRobotRetractDone)
            //    .End(SwapStep.NotifyDone,           NotifyPMDone,                                       _delay_50ms);


            switch (_pmModule.ChamberType)
            {
                //case JetChamber.Venus:
                case JetChamber.Kepler2300:

                    Runner.Wait(SwapStep.WaitPMReady,     () => _pmModule.IsIdle,                             _delay_60s)
                          .RunIf(SwapStep.PreRotation,    _JetTM.PreRotateModules.ContainsKey(_targetModule), RotateArm, WaitRotateDone)
                          .Run(SwapStep.OpenSlitDoor,     OpenPMSlitDoor,                                     OpenPMSlitDoorIsOK)
                          .Run(SwapStep.PickPrepare,      PickPrepare,                                        IsModuleReadyForPick)
                          .Run(SwapStep.PickExtend,       PickExtend,                                         WaitRobotExtendDone)
                          .Run(SwapStep.DropDownWafer,    NotifyPMPickWafer,                                  WaitPMWaferDropDown)
                          .Delay(SwapStep.PickDelay,                                                          _pickDelayTime)
                          .Run(SwapStep.PickRetract,      PickRetract,                                        WaitRobotRetractDone)
                          .Run(SwapStep.PlacePrepare,     PlacePrepare,                                       IsModuleReadyForPlace)
                          .Run(SwapStep.PlaceExtend,      PlaceExtend,                                        WaitRobotExtendDone)
                          .Run(SwapStep.LiftUpWafer,      NotifyLiftUpWafer,                                  WaitPMWaferLiftUp)
                          .Delay(SwapStep.PlaceDelay,     _placeDelayTime)
                          .Run(SwapStep.PlaceRetract,     PlaceRetract,                                       WaitRobotRetractDone)
                          .Run(SwapStep.CloseSlitDoor,    ClosePMSlitDoor,                                    ClosePMSlitDoorIsOK)

                          .End(SwapStep.NotifyDone, NotifyPMDone, _delay_50ms);
                    break;

                case JetChamber.Kepler2200A:
                case JetChamber.Kepler2200B:

                    Runner.Wait(SwapWithHeaterStep.WaitPMReady,       () => _pmModule.IsIdle,              _delay_60s)
                          .RunIf(SwapWithHeaterStep.PreRotation,      _JetTM.PreRotateModules.ContainsKey(_targetModule), RotateArm, WaitRotateDone)               
                          .Run(SwapWithHeaterStep.PickPrepare,        PickPrepare,                         IsModuleReadyForPick)
                          .Wait(SwapWithHeaterStep.WaitPressreStable, TMPMPressureIsOK, _delay_60s)
                          .Run(SwapWithHeaterStep.OpenSlitDoor,       OpenPMSlitDoor,                      OpenPMSlitDoorIsOK)
                          .Run(SwapWithHeaterStep.Picking,            Picking,                             WaitPickDone)

                          .RunIf(SwapWithHeaterStep.QueryPickAWCData, _pickQueryAWC, QueryAWC,             WaitRobotQueryDone)
                          .RunIf(SwapWithHeaterStep.SavePickAWCData,  _pickQueryAWC, PickRecordAWCData,        NullFun)
                          .RunIf(SwapWithHeaterStep.CheckPickAWCData, _pickQueryAWC, CheckAwc)

                          .Run(SwapWithHeaterStep.PlacePrepare,       PlacePrepare,                        IsModuleReadyForPlace)
                          .Run(SwapWithHeaterStep.Placing,            Placing,                             WaitPlaceDone)

                          .RunIf(SwapWithHeaterStep.QueryPlaceAWCData, _placeQueryAWC, QueryAWC,      WaitRobotQueryDone)
                          .RunIf(SwapWithHeaterStep.SavePlaceAWCData,  _placeQueryAWC, RecordAWCData, NullFun)
                          .RunIf(SwapWithHeaterStep.CheckPlaceAWCData, _placeQueryAWC, CheckAwc)

                          .Run(SwapWithHeaterStep.CloseSlitDoor,      ClosePMSlitDoor,                     ClosePMSlitDoorIsOK)
                          .End(SwapWithHeaterStep.NotifyDone,         NotifyPMDone,                        _robotPickDelayTime * 1000);
                    break;
            }
            return Runner.Status;
        }

        private bool PickPrepare()
        {
            _pmModule.PostMsg(PMEntity.MSG.PreparePick);
            return true;
        }
        private bool TMPMPressureIsOK()
        {
            if (RouteManager.IsATMMode)
            {
                return _JetTM.IsTMATM && _JetTM.IsModuleATM(_targetModule);
            }
            else
            {
                if (Math.Abs((_pmModule.ChamberPressure - _JetTM.ChamberPressure)) < (maxPressureDifference-2))
                {
                    LOG.Write(eEvent.INFO_TM, Module, $"TM Pressure {_JetTM.ChamberPressure},PM Pressure {_pmModule.ChamberPressure}");
                    return true;
                }
                else
                {
                    return false;
                }
            }
        }
        private bool OpenPMSlitDoor()
        {
            return _JetTM.TurnMFSlitDoor(_targetModule, true, out _);
        }
        private bool OpenPMSlitDoorIsOK()
        {
            return _JetTM.IsPMSlitdoorOpened(_targetModule);
        }
        private bool ClosePMSlitDoor()
        {
            return _JetTM.TurnMFSlitDoor(_targetModule, false, out _);
        }
        private bool ClosePMSlitDoorIsOK()
        {
            return _JetTM.IsPMSlitdoorClosed(_targetModule);
        }
        private bool IsModuleReadyForPick()
        {
            return _pmModule.Status == PMEntity.PMStatus.Ready_For_Pick;
        }

        private bool PickExtend()
        {
            return _robot.PickExtend(_targetModule, _targetSlot, _pickHand);
        }
        private bool PickRetract()
        {
            return _robot.PickRetract(_targetModule, _targetSlot, _pickHand);
        }

        private bool PlacePrepare()
        {
            _pmModule.PostMsg(PMEntity.MSG.PreparePlace);
            return true;
        }

        private bool IsModuleReadyForPlace()
        {
            return _pmModule.Status == PMEntity.PMStatus.Ready_For_Place;
        }

        private bool PlaceExtend()
        {
            return _robot.PlaceExtend(_targetModule, _targetSlot, _placeHand);
        }
        private bool PlaceRetract()
        {
            return _robot.PlaceRetract(_targetModule, _targetSlot, _placeHand);
        }

        private bool WaitRobotExtendDone()
        {
            if (_robot.Status == RState.Running)
            {
                return false;
            }
            else if (_robot.Status == RState.End)
            {
                return true;
            }
            else
            {
                Runner.Stop($"TM Robot Place Extend failed, {_robot.Status}");
                return true;
            }
        }
        private bool Picking()
        {
            if (!_jetPM.IsAllowRobotTransfer)
            {
                Runner.Stop($"不允许Pick,传片位置信号不满足");
                return false;
            }
            if (!_jetPM.PreparePlaceIsOK())
            {
                Runner.Stop($"不允许Pick,传片位置信号满足,但不在Position1");
                return false;
            }
            _hand = _pickHand;
            _starttime=DateTime.Now;

            return _robot.Pick(_targetModule, _targetSlot, _pickHand);

            //if (_jetPM.PreparePickIsOK())
            //{
            //}
            //else
            //{
            //    Runner.Stop($"TM Robot Picking failed due to");

            //    return false;
            //}
        }
        private bool QueryAWC()
        {
            return _robot.QueryAwc();
        }
        private bool PickRecordAWCData()
        {

            //已经move后的数据
            string _origin_module = $"LP{WaferManager.Instance.GetWafer(ModuleName.TMRobot, (int)_pickHand).OriginStation}";
            int _origin_slot = WaferManager.Instance.GetWafer(ModuleName.TMRobot, (int)_pickHand).OriginSlot;
            //查询完毕 插入数据
            OffsetDataRecorder.RecordOffsetData(
                Guid.NewGuid().ToString(),
                ModuleName.TMRobot, 0,
                _targetModule, _targetSlot,
                _origin_module, _origin_slot,
                _hand, RobotArmPan.None,
                _robot.Offset_X, _robot.Offset_Y, _robot.Offset_D,
                _starttime, DateTime.Now);
            return true;
        }
        private bool RecordAWCData()
        {

            //已经move后的数据
            string _origin_module = $"LP{WaferManager.Instance.GetWafer(_targetModule, _targetSlot).OriginStation}";
            int _origin_slot = WaferManager.Instance.GetWafer(_targetModule, _targetSlot).OriginSlot;
            //查询完毕 插入数据
            OffsetDataRecorder.RecordOffsetData(
                Guid.NewGuid().ToString(),
                ModuleName.TMRobot, 0,
                _targetModule, _targetSlot,
                _origin_module, _origin_slot,
                _hand, RobotArmPan.None,
                _robot.Offset_X, _robot.Offset_Y, _robot.Offset_D,
                _starttime, DateTime.Now);
            return true;
        }
        private bool CheckAwc()
        {
            if (Math.Abs(_robot.Offset_X) > _robotTransferAWCOffset)
            {
                Stop($"Check AWC 失败， 当前 X AWC [{_robot.Offset_X}um], 高于Alarm最大值: [{_robotTransferAWCOffset}]um");
                return false;
            }
            if (Math.Abs(_robot.Offset_Y) > _robotTransferAWCOffset)
            {
                Stop($"Check AWC 失败， 当前 Y AWC [{_robot.Offset_Y}um], 高于Alarm最大值: [{_robotTransferAWCOffset}]um");
                return false;
            }
            return true;
        }

        private bool WaitRobotQueryDone()
        {
            if (_robot.Status == RState.Running)
            {
                return false;
            }
            else if (_robot.Status == RState.End)
            {
                return true;
            }
            else
            {
                Runner.Stop($"TM Robot Query Awc failed, {_robot.Status}");
                return true;
            }
        }
        private bool WaitPickDone()
        {
            if (_robot.Status == RState.Running)
            {
                return false;
            }
            else if (_robot.Status == RState.End)
            {
                WaferManager.Instance.WaferMoved(_targetModule, _targetSlot, ModuleName.TMRobot, (int)_pickHand);

                return true;
            }
            else
            {
                WaferManager.Instance.CreateDuplicatedWafer(_targetModule, _targetSlot, ModuleName.TMRobot, (int)_pickHand);
                Runner.Stop($"TM Robot Picking failed, {_robot.Status}");
                return true;
            }
        }
        private bool Placing()
        {
            if (!_jetPM.IsAllowRobotTransfer)
            {
                Runner.Stop($"不允许Place,传片位置信号不满足");
                return false;
            }
            if (!_jetPM.PreparePlaceIsOK())
            {
                Runner.Stop($"不允许Place,传片位置信号满足,但不在Position1");
                return false;
            }
            _hand = _placeHand;
            _starttime = DateTime.Now;
            return _robot.Place(_targetModule, _targetSlot, _placeHand);

        }

        private bool WaitPlaceDone()
        {
            if (_robot.Status == RState.Running)
            {
                return false;
            }
            else if (_robot.Status == RState.End)
            {
                WaferManager.Instance.WaferMoved(ModuleName.TMRobot, (int)_placeHand, _targetModule, _targetSlot);
                return true;
            }
            else
            {
                WaferManager.Instance.CreateDuplicatedWafer(ModuleName.TMRobot, (int)_placeHand, _targetModule, _targetSlot);
                Runner.Stop($"TM Robot Place failed, {_robot.Status}");
                return true;
            }
        }

        private bool RotateArm()
        {
            _pmModule.PostMsg(PMEntity.MSG.PreparePick); // Notify PM to Serv pressure in advance for throughput enhancement
            return _robot.Goto(_JetTM.PreRotateModules[_targetModule], 0, _placeHand);
        }

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

        private bool NotifyPMPickWafer()
        {
            _pmModule.PostMsg(PMEntity.MSG.DropDownWafer);
            return true;
        }

        private bool WaitPMWaferDropDown()
        {
            if (_pmModule.Status == PMEntity.PMStatus.Exchange_Ready)
            {
                WaferManager.Instance.WaferMoved(_targetModule, _targetSlot, ModuleName.TMRobot, (int)_pickHand);
                return true;
            }
            else if (Runner.StepElapsedMS > _liftPinTimeout)
            {
                WaferManager.Instance.CreateDuplicatedWafer(_targetModule, _targetSlot, ModuleName.TMRobot, (int)_pickHand);
                Runner.Stop($"Wait {_targetModule} Lift Pin down timeout, {Runner.StepElapsedMS}");
            }

            return false;
        }

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

        private bool NotifyLiftUpWafer()
        {
            _pmModule.PostMsg(PMEntity.MSG.LiftUpWafer);
            return true;
        }
        private bool WaitPMWaferLiftUp()
        {
            if (_pmModule.Status == PMEntity.PMStatus.Exchange_Ready)
            {
                WaferManager.Instance.WaferMoved(ModuleName.TMRobot, (int)_placeHand, _targetModule, _targetSlot);
                return true;
            }
            else if (Runner.StepElapsedMS > _liftPinTimeout)
            {
                WaferManager.Instance.CreateDuplicatedWafer(ModuleName.TMRobot, (int)_placeHand, _targetModule, _targetSlot);
                Runner.Stop($"Wait {_targetModule} Lift Pin Up timeout, {Runner.StepElapsedMS}");
            }

            return false;
        }

        private bool NotifyPMDone()
        {
            _pmModule.PostMsg(PMEntity.MSG.PlaceReady);
            return true;
        }

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