using Aitex.Core.RT.Device;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.Routine;
using MECF.Framework.Common.Utilities;
using CyberX8_Core;
using CyberX8_RT.Devices.AXIS;
using CyberX8_RT.Devices.Loader;
using CyberX8_RT.Devices.PUF;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using CyberX8_RT.Devices.Safety;

namespace CyberX8_RT.Modules.Loader
{
    public class LoaderHomeAllRoutine : RoutineBase, IRoutine
    {
        private enum HomeAllStep
        {
            CheckSafety,
            SideADoorUnlock,
            WaitSideADoorUnlock,
            TiltAHome,
            WaitTiltAHome,
            TiltAGotoVertical,
            TiltAGotoVerticalCheck,
            SideBDoorUnlock,
            WaitSideBDoorUnlock,
            TiltBHome,
            WaitTiltBHome,
            TiltBGotoVertical,
            TiltBGotoVerticalCheck,
            ShuttleAHome,
            WaitShuttleAHome,
            ShuttleAGotoClose,
            ShuttleAGotoCloseCheck,
            ShuttleBHome,
            WaitShuttleBHome,
            ShuttleBGotoClose,
            ShuttleBGotoCloseCheck,
            SideADoorLockOn,
            SideADoorLockOnCheck,
            SideBDoorLockOn,
            SideBDoorLockOnCheck,
            CheckDupfCondition,
            RotationHome,
            WaitRotationHome,
            RotationGotoTrnpa,
            RotationGotoTrnpaCheck,
            CRSAHome,
            WaitCRSAHome,
            CRSAGotoSetUp,
            CRSAGotoSetUpCheck,
            CRSBHome,
            WaitCRSBHome,
            CRSBGotoSetUp,
            CRSBGotoSetUpCheck,
            End
        }

        #region 内部变量
        private JetAxisBase _shuttleAAxis;
        private JetAxisBase _shuttleBAxis;
        private JetAxisBase _tiltAAxis;
        private JetAxisBase _tiltBAxis;
        private JetAxisBase _rotationAxis;
        private JetAxisBase _crsAAxis;
        private JetAxisBase _crsBAxis;
        private JetAxisBase _puf1VerticalAxis;
        private JetAxisBase _puf2VerticalAxis;
        private JetAxisBase _puf2RotationAxis;
        private LoaderSideDevice _sideADevice;
        private bool _isExecuteSideAUnlock = false;
        private LoaderSideDevice _sideBDevice;
        private bool _isExecuteSideBUnlock = false;
        #endregion

        public LoaderHomeAllRoutine(string module) : base(module)
        {
            _shuttleAAxis = DEVICE.GetDevice<JetAxisBase>($"{module}.ShuttleA");
            _shuttleBAxis = DEVICE.GetDevice<JetAxisBase>($"{module}.ShuttleB");

            _tiltAAxis = DEVICE.GetDevice<JetAxisBase>($"{module}.TiltA");
            _tiltBAxis = DEVICE.GetDevice<JetAxisBase>($"{module}.TiltB");

            _crsAAxis = DEVICE.GetDevice<JetAxisBase>($"{module}.LSA");
            _crsBAxis = DEVICE.GetDevice<JetAxisBase>($"{module}.LSB");

            _rotationAxis = DEVICE.GetDevice<JetAxisBase>($"{module}.Rotation");
        }

        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }

        public RState Monitor()
        {
                //Home Tilt Goto VERT
            Runner.Run(HomeAllStep.CheckSafety,CheckSafety,_delay_1ms)
                .Run(HomeAllStep.SideADoorUnlock, () => SideDoorUnlock(_sideADevice, ref _isExecuteSideAUnlock), _delay_1ms)
                .WaitWithStopCondition(HomeAllStep.WaitSideADoorUnlock, () => CheckSideDoorUnlockedEndStatus(_sideADevice, _isExecuteSideAUnlock),
                () => CheckSideDoorUnlockedStopStatus(_sideADevice, _isExecuteSideAUnlock))
                .Run(HomeAllStep.TiltAHome, () => { return _tiltAAxis.Home(); },_delay_1ms)
                .WaitWithStopCondition(HomeAllStep.WaitTiltAHome, () => { return _tiltAAxis.Status == RState.End; }, () => {return _tiltAAxis.Status == RState.Failed; })
                .Run(HomeAllStep.TiltAGotoVertical, () => { return _tiltAAxis.PositionStation("VERT",true); },100)
                .WaitWithStopCondition(HomeAllStep.TiltAGotoVerticalCheck, () => { return _tiltAAxis.Status == RState.End; }, () => { return _tiltAAxis.Status == RState.Failed; })
                .Run(HomeAllStep.SideBDoorUnlock, () => SideDoorUnlock(_sideBDevice, ref _isExecuteSideBUnlock), _delay_1ms)
                .WaitWithStopCondition(HomeAllStep.WaitSideBDoorUnlock, () => CheckSideDoorUnlockedEndStatus(_sideBDevice, _isExecuteSideBUnlock),
                () => CheckSideDoorUnlockedStopStatus(_sideADevice, _isExecuteSideBUnlock))
                .Run(HomeAllStep.TiltBHome, () => { return _tiltBAxis.Home(); }, _delay_1ms)
                .WaitWithStopCondition(HomeAllStep.WaitTiltBHome, () => { return _tiltBAxis.Status == RState.End; }, () => { return _tiltBAxis.Status == RState.Failed; })
                .Run(HomeAllStep.TiltBGotoVertical, () => { return _tiltBAxis.PositionStation("VERT",true); }, 100)
                .WaitWithStopCondition(HomeAllStep.TiltBGotoVerticalCheck, () => { return _tiltBAxis.Status == RState.End; }, () => { return _tiltBAxis.Status == RState.Failed; })
                //Home Shuttle Goto CLOSED
                .Run(HomeAllStep.ShuttleAHome, () => { return _shuttleAAxis.Home(); }, _delay_1ms)
                .WaitWithStopCondition(HomeAllStep.WaitShuttleAHome, () => { return _shuttleAAxis.Status == RState.End; }, () => { return _shuttleAAxis.Status == RState.Failed; })
                .Run(HomeAllStep.ShuttleAGotoClose, () => { return _shuttleAAxis.PositionStation("CLOSED",true); }, 100)
                .WaitWithStopCondition(HomeAllStep.ShuttleAGotoCloseCheck, () => { return _shuttleAAxis.Status == RState.End; }, () => { return _shuttleAAxis.Status == RState.Failed; })
                .Run(HomeAllStep.ShuttleBHome, () => { return _shuttleBAxis.Home(); }, _delay_1ms)
                .WaitWithStopCondition(HomeAllStep.WaitShuttleBHome, () => { return _shuttleBAxis.Status == RState.End; }, () => { return _shuttleBAxis.Status == RState.Failed; })
                .Run(HomeAllStep.ShuttleBGotoClose, () => { return _shuttleBAxis.PositionStation("CLOSED", true); }, 100)
                .WaitWithStopCondition(HomeAllStep.ShuttleBGotoCloseCheck, () => { return _shuttleBAxis.Status == RState.End; }, () => { return _shuttleBAxis.Status == RState.Failed; })
                //Door Lock On
                .Run(HomeAllStep.SideADoorLockOn, () => { return _sideADevice.DoorLockOnAction(); }, 100)
                .WaitWithStopCondition(HomeAllStep.SideADoorLockOnCheck, () => { return _sideADevice.Status==RState.End; }, () => { return _sideADevice.Status==RState.Failed; })
                .Run(HomeAllStep.SideBDoorLockOn, () => { return _sideBDevice.DoorLockOnAction(); }, 100)
                .WaitWithStopCondition(HomeAllStep.SideBDoorLockOnCheck, () => { return _sideBDevice.Status==RState.End; }, () => { return _sideBDevice.Status==RState.Failed; })
                //Home Rotation Goto TRNPA
                .Run(HomeAllStep.CheckDupfCondition,CheckDupufCondition,_delay_1ms)
                .Run(HomeAllStep.RotationHome, () => { return _rotationAxis.Home(); },_delay_1ms)
                .WaitWithStopCondition(HomeAllStep.WaitRotationHome, () => { return _rotationAxis.Status == RState.End; }, () => { return _rotationAxis.Status == RState.Failed; })
                .Run(HomeAllStep.RotationGotoTrnpa, () => { return _rotationAxis.PositionStation("TRNPA",true); }, 100)
                .WaitWithStopCondition(HomeAllStep.RotationGotoTrnpaCheck, () => { return _rotationAxis.Status == RState.End; }, () => { return _rotationAxis.Status == RState.Failed; })
                //Home CRS
                .Run(HomeAllStep.CRSAHome, () => { return _crsAAxis.Home(); }, _delay_1ms)
                .WaitWithStopCondition(HomeAllStep.WaitCRSAHome, () => { return _crsAAxis.Status == RState.End; }, () => { return _crsAAxis.Status == RState.Failed; })
                .Run(HomeAllStep.CRSAGotoSetUp, () => { return _crsAAxis.PositionStation("Setup", true); }, _delay_1ms)
                .WaitWithStopCondition(HomeAllStep.CRSAGotoSetUpCheck, () => { return _crsAAxis.Status == RState.End; }, () => { return _crsAAxis.Status == RState.Failed; })
                .Run(HomeAllStep.CRSBHome, () => { return _crsBAxis.Home(); }, _delay_1ms)
                .WaitWithStopCondition(HomeAllStep.WaitCRSBHome, () => { return _crsBAxis.Status == RState.End; }, () => { return _crsBAxis.Status == RState.Failed; })
                .Run(HomeAllStep.CRSBGotoSetUp, () => { return _crsBAxis.PositionStation("Setup",true); }, 100)
                .WaitWithStopCondition(HomeAllStep.CRSBGotoSetUpCheck, () => { return _crsBAxis.Status == RState.End; }, () => { return _crsBAxis.Status == RState.Failed; })
                .End(HomeAllStep.End,NullFun);

            return Runner.Status; 
        }
        /// <summary>
        /// 检验Safety
        /// </summary>
        /// <returns></returns>
        private bool CheckSafety()
        {
            SafetyDevice safetyDevice = DEVICE.GetDevice<SafetyDevice>("Safety");
            if (safetyDevice == null)
            {
                LOG.WriteLog(eEvent.ERR_LOADER, Module.ToString(), "Safety device is null");
                return false;
            }
            if (safetyDevice.SafetyData.TwincatState != 8)
            {
                LOG.WriteLog(eEvent.ERR_LOADER, Module.ToString(), "Twincat Status is not OP status");
                return false;
            }
            if (safetyDevice.SafetyData.LoaderPufCommErr)
            {
                LOG.WriteLog(eEvent.ERR_LOADER, Module.ToString(), "Twincat Loader Communication status is error");
                return false;
            }
            if (safetyDevice.SafetyData.LoaderPufFunctionBlockErr)
            {
                LOG.WriteLog(eEvent.ERR_LOADER, Module.ToString(), "Twincat Loader function block status is error");
                return false;
            }
            return true;
        }
        /// <summary>
        /// Side Door Unlock
        /// </summary>
        /// <returns></returns>
        private bool SideDoorUnlock(LoaderSideDevice sideDevice,ref bool execute)
        {
            if (sideDevice.SideData.DoorLowerLocked || sideDevice.SideData.DoorUpperLocked)
            {
                execute = true;
                return sideDevice.DoorLockOffAction();
            }
            else
            {
                return true;
            }
        }
        /// <summary>
        /// 校验Side Unlocked
        /// </summary>
        /// <param name="sideDevice"></param>
        /// <param name="execute"></param>
        /// <returns></returns>
        private bool CheckSideDoorUnlockedEndStatus(LoaderSideDevice sideDevice,bool execute)
        {
            if(execute)
            {
                return sideDevice.Status == RState.End;
            }
            else
            {
                return true;
            }
        }
        /// <summary>
        /// 校验Side Unlocked
        /// </summary>
        /// <param name="sideDevice"></param>
        /// <param name="execute"></param>
        /// <returns></returns>
        private bool CheckSideDoorUnlockedStopStatus(LoaderSideDevice sideDevice, bool execute)
        {
            if (execute)
            {
                return sideDevice.Status == RState.Failed;
            }
            else
            {
                return true;
            }
        }
        /// <summary>
        /// 检验puf条件
        /// </summary>
        /// <returns></returns>
        private bool CheckDupufCondition()
        {
            if (ModuleHelper.IsInstalled(ModuleName.PUF1))
            {
                if (!_puf1VerticalAxis.IsHomed)
                { 
                    LOG.WriteLog(eEvent.ERR_LOADER, Module.ToString(), "Puf1 Vertical is not homed");
                    return false;
                }
                JetAxisBase puf1VerticalAxis = DEVICE.GetDevice<JetAxisBase>($"{ModuleName.PUF1}.Vertical");
                double puf1VerticalPosition = puf1VerticalAxis.MotionData.MotorPosition;
                if (!puf1VerticalAxis.CheckPositionIsInStation(puf1VerticalPosition, "Park"))
                {
                    LOG.Write(eEvent.ERR_LOADER, Module, $"PUF1 Vertical {puf1VerticalPosition} is not in Park station,Cannot execute GotoSavedPosition");
                    return false;
                }
            }
            if (ModuleHelper.IsInstalled(ModuleName.PUF2))
            {
                if (!_puf2VerticalAxis.IsHomed)
                {
                    LOG.WriteLog(eEvent.ERR_LOADER, Module.ToString(), "Puf2 Vertical is not homed");
                    return false;
                }

                JetAxisBase puf2VerticalAxis = DEVICE.GetDevice<JetAxisBase>($"{ModuleName.PUF2}.Vertical");
                double puf2VerticalPosition = puf2VerticalAxis.MotionData.MotorPosition;
                if (!puf2VerticalAxis.CheckPositionIsInStation(puf2VerticalPosition, "Park") && 
                    !puf2VerticalAxis.CheckPositionIsInStation(puf2VerticalPosition, "Robot"))
                {
                    LOG.Write(eEvent.ERR_LOADER, Module, $"PUF2 Vertical {puf2VerticalPosition} is not in Park and Robot station,Cannot execute GotoSavedPosition");
                    return false;
                }

                if (!_puf2RotationAxis.IsHomed)
                {
                    LOG.WriteLog(eEvent.ERR_LOADER, Module.ToString(), "Puf2 Rotation is not homed");
                    return false;
                }

                JetAxisBase puf2RotationAxis = DEVICE.GetDevice<JetAxisBase>($"{ModuleName.PUF2}.Rotation");
                double puf2RotationPosition = puf2RotationAxis.MotionData.MotorPosition;
                if (!puf2RotationAxis.CheckPositionIsInStation(puf2RotationPosition, "Park") && 
                    !puf2RotationAxis.CheckPositionIsInStation(puf2RotationPosition, "Robot"))
                {
                    LOG.Write(eEvent.ERR_LOADER, Module, $"PUF2 Rotation {puf2RotationPosition} is not in Park and Robot station,Cannot execute GotoSavedPosition");
                    return false;
                }
            }
            return true;
        }
        public RState Start(params object[] objs)
        {
            _sideADevice = DEVICE.GetDevice<LoaderSideDevice>($"{Module}.SideA");
            _sideBDevice = DEVICE.GetDevice<LoaderSideDevice>($"{Module}.SideB");
            _puf1VerticalAxis = DEVICE.GetDevice<JetAxisBase>($"{ModuleName.PUF1}.Vertical");
            _puf2VerticalAxis = DEVICE.GetDevice<JetAxisBase>($"{ModuleName.PUF2}.Vertical");
            _puf2RotationAxis = DEVICE.GetDevice<JetAxisBase>($"{ModuleName.PUF2}.Rotation");
            _isExecuteSideAUnlock = false;
            _isExecuteSideBUnlock = false;
            if(!CheckPreCondition())
            {
                return RState.Failed;
            }
            Runner.Start(Module, "Home All");
            return RState.Running;
        }
        /// <summary>
        /// 检验前置条件
        /// </summary>
        /// <returns></returns>
        private bool CheckPreCondition()
        {
            //if(_sideADevice.SideData.WaferPresent)
            //{
            //    LOG.WriteLog(eEvent.ERR_AXIS, Module.ToString(), "Side A wafer present,cannot home");
            //    return false;
            //}
            //if (_sideBDevice.SideData.WaferPresent)
            //{
            //    LOG.WriteLog(eEvent.ERR_AXIS, Module.ToString(), "Side B wafer present,cannot home");
            //    return false;
            //}
            return true;
        }
    }
}