using Aitex.Core.RT.Routine;
using MECF.Framework.Common.CommonData.PUF;
using MECF.Framework.Common.Routine;
using MECF.Framework.Common.TwinCat;
using PunkHPX8_Core;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace PunkHPX8_RT.Devices.AXIS.CANOpen
{
   
    public class CanOpenSwitchOnRoutine : RoutineBase, IRoutine
    {
        private enum SwitchOnStep
        {
            NoneModeOfOperation,
            ClearError,
            WriteTargetPosition,
            CheckSwitchDisabled,
            WriteSwitchOnControlWord,
            SwitchOnDelay,
            CheckSwitchOn,
            End
        }
        #region 常量 
        private const string TARGET_POSITION = "TargetPosition";
        #endregion
        #region 内部变量
        private JetAxisBase _axis;
        private bool _isSwitchDisabled = false;
        private BeckhoffCommonAxis _beckhoffCommonAxis;
        #endregion
        public CanOpenSwitchOnRoutine(string module,JetAxisBase axis,BeckhoffCommonAxis beckhoffCommonAxis) : base(module)
        {
            _axis = axis;
            _beckhoffCommonAxis = beckhoffCommonAxis;
        }

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

        public RState Monitor()
        {
            Runner.Run(SwitchOnStep.NoneModeOfOperation, () => { return _beckhoffCommonAxis.WriteModeOfMode(AxisModeOfOperation.None); },
                ()=> { return CheckModeOfOperation((byte)AxisModeOfOperation.None); }, 1000)
                .Run(SwitchOnStep.ClearError, ClearError,CheckError,1000)
                .Run(SwitchOnStep.WriteTargetPosition, WriteTargetPosition,_delay_1ms)                
                .Run(SwitchOnStep.CheckSwitchDisabled,CheckSwitchDisabled,CheckReadyToSwitchOn,_delay_2s)
                .Run(SwitchOnStep.WriteSwitchOnControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0x0F); }, () => { return CheckControlWord(0x0F); },1000)
                .Delay(SwitchOnStep.SwitchOnDelay,100)
                .Wait(SwitchOnStep.CheckSwitchOn,CheckSwitchOn,500)
                .End(SwitchOnStep.End,NullFun,_delay_1ms);
            return Runner.Status;
        }
        /// <summary>
        /// 清除错误
        /// </summary>
        /// <returns></returns>
        private bool ClearError()
        {
            if(_axis.IsError)
            {
                return _beckhoffCommonAxis.WriteControlWord(128);
            }
            return true;
        }
        /// <summary>
        /// 检验SwitchDisabled
        /// </summary>
        /// <returns></returns>
        private bool CheckSwitchDisabled()
        {
            if(_axis.MotionData.Status== "Switch On Disabled")
            {
                _isSwitchDisabled = true;
                return _beckhoffCommonAxis.WriteControlWord(0x06);
            }
            return true;
        }
        /// <summary>
        /// 当前位置写入Target
        /// </summary>
        /// <returns></returns>
        private bool WriteTargetPosition()
        {
            int targetPosition = _axis.CalculateValueMultiplyScale(_axis.MotionData.MotorPosition);
            return _beckhoffCommonAxis.WriteVariable(TARGET_POSITION, targetPosition);
        }
        /// <summary>
        /// 检验Ready to Switch On
        /// </summary>
        /// <returns></returns>
        private bool CheckReadyToSwitchOn()
        {
            if (_isSwitchDisabled)
            {
                return _axis.MotionData.Status == "Ready to Switch On";
            }
            else
            {
                return true;
            }
        }
        /// <summary>
        /// 检验是否存在错误
        /// </summary>
        /// <returns></returns>
        private bool CheckError()
        {
            return !_axis.IsError;
        }

        public bool CheckModeOfOperation(byte modeOfOperation)
        {
            return _axis.ModeOfOperation == modeOfOperation;
        }

        public bool CheckControlWord(ushort controlWord)
        {
            return _axis.ControlWord == controlWord;
        }

        private bool CheckSwitchOn()
        {
            return _axis.IsSwitchOn;
        }
        public RState Start(params object[] objs)
        {
            _isSwitchDisabled = false;
            return Runner.Start(Module, "Switch On");
        }
    }
}