using Aurora;
using Aurora.Api.Devices;
using Aurora.Api;
using AuroraACS;
using AuroraLaser.Views;
using Hermes;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Documents;
using WeldingCommon;
using WeldingCommon.Settings;
using ACS.SPiiPlusNET;
using ILSOS_PrismaticGun.Process;
using System.Security.RightsManagement;
using static ILSOS_PrismaticGun.Recipes.MainRecipe;
using System.Threading;
using CoreUnit.Control.UserControl.Canvas;
using ILSOS_PrismaticGun.Settings;
using AuroraMotionBoard.Ajin;
using System.Diagnostics;
using VisionCommon;
using ILSOS_PrismaticGun.Tools;
using WeldingCommon.Utils;

using eTeachingPosition = ILSOS_PrismaticGun.Settings.eTeachingPosition;
using TeachingPositionSetting = ILSOS_PrismaticGun.Settings.TeachingPositionSetting;
namespace ILSOS_PrismaticGun.Device.ACS
{
    // ACS 함수가 너무 많아서.. 실제 사용부에서 편하게 사용하려고...
    public interface IPrismaticGunACS
    {
        bool IsActivated();
        bool SetOneShot(int msecShotTime, double secPhzShotPulse1, double secPhzShotPulse2);
        bool SetLineShot(double corePower, double ringPower, double startPosX, double endPosX, double startPosY, double endPosY, double velocity, double acc);
        bool SetWeldingVel(double acc, double svel, double cvel);
        bool SetWeldingPos(double startPosX, double startPosY, double endPosX, double endPosY, double camOffsetX, double camOffsetY);
        bool SetWeldingArc(double point1, double point2, double point3, double point4);

        bool SetWeldingType(eOutType type);       // << 추후 추가 할 예정
        bool SetWeldingPulse(double coreMaxPower, double coreMinPower, double ringMaxPower, double ringMinPower);
        bool SetLaserOnOff(double onPosX, double onOffSet, double offPosX, double offOffSet, double camOffsetX, double camOffsetY);
        bool SetRamppingOffParameter(double pos, double overlap, double endpower, double camOffsetX, double camOffsetY);
        bool SetRamppingOnParameter(double overlap, double startpower);
        bool SetLCILserOn(bool on);
        bool SetWeldingAxes(int axisXnum, int axisYnum);
        bool SetLaserIOUse(bool on);
        bool AllAxisHomeStart();
        bool AllAxisHomeStop();
        bool CheckAllAxisHome();
        bool OneShotStart();
        bool OneShotStop();
        bool CheckOneShotComp();
        bool LineShotStart();
        bool LineShotStop();
        bool CheckLineShotComp();
        bool WeldingStart();
        bool WeldingStop();
        bool CheckWeldingComp();
        bool SetWeldingTeachingPosition1(double PosX, double PosY, double camOffsetX, double camOffsetY);
        bool SetWeldingTeachingPosition2(double PosX, double PosY, double camOffsetX, double camOffsetY);
        bool SetWeldingTeachingPosition3(double PosX, double PosY, double camOffsetX, double camOffsetY);
        bool SetWeldingTeachingPosition4(double PosX, double PosY, double camOffsetX, double camOffsetY);
        bool SetWeldingTeachingPosition5(double PosX, double PosY, double camOffsetX, double camOffsetY);
        bool SetWeldingTeachingPosition6(double PosX, double PosY, double camOffsetX, double camOffsetY);
        bool SetWeldingTeachingPosition7(double PosX, double PosY, double camOffsetX, double camOffsetY);
        bool SetWeldingTeachingPosition8(double PosX, double PosY, double camOffsetX, double camOffsetY);
        bool ResetAlarm();
        bool IsLaserOn();
        void AllAxisServoOff();
    }
    public partial class PrismaticGunACS : ACSDevice, IPrismaticGunACS
    {
        private ACSDeviceSetting _setting;
        private LaserPowerCalParam _calibration;

        private TeachingPositionSetting _teachingSetting;
        private ACSMotor MotorX1, MotorX2, MotorY1, MotorY2, MotorZ;

        #region ACS Parameters
        //public MotionVariableCollection MotionVariables { get => _MotionVariables; }
        //private MotionVariableCollection _MotionVariables = new MotionVariableCollection();
        public MotionVariable _motorVariable;

        public bool HasHeavyAlarm { get; private set; }
        public bool HasLightAlarm { get; private set; }
        public int[] HaveAlarmCodes { get => _HaveAlarmCodes; }
        private int[] _HaveAlarmCodes = new int[11];
        #endregion

        public PrismaticGunACS(bool isSimulation, bool useTerminateUMD = false) : base(isSimulation, useTerminateUMD)
        {
            _motorVariable = new MotionVariable();
        }
        protected override int DoActivate(IDeviceConfig config)
        {
            if (base.DoActivate(config) != E1.OK)
                return E2.ABORTED;

            var env = Application.Current.AuroraEnv();
            _setting = env.SettingEntry[SettingIds.ACS] as ACSDeviceSetting;
            _calibration = env.SettingEntry[CommonSettingIds.LaserPowerCalParam] as LaserPowerCalParam;
            _teachingSetting = env.SettingEntry[SettingIds.TeachingPos] as TeachingPositionSetting;

            //MotorX1 = env.DeviceManager[MotorIds.AxisX1] as ACSMotor;
            MotorX2 = env.DeviceManager[MotorIds.AxisX2] as ACSMotor;
            //MotorY1 = env.DeviceManager[MotorIds.AxisY1] as ACSMotor;
            //MotorY2 = env.DeviceManager[MotorIds.AxisY2] as ACSMotor;
            MotorZ = env.DeviceManager[MotorIds.AxisZ] as ACSMotor;


            SetMonitoringService(UpdateACSParam, 50);
            StartMonitoringService();

            return E1.OK;
        }

        private void UpdateACSParam()
        {
            // Stage A Motor Parameter
            //_motorVariable.X1AxisPosition = MotorX1.GetActualPos();
            //_motorVariable.X1InPosition = MotorX1.IsInpos();
            //_motorVariable.X1AxisVelocity = MotorX1.GetFeedbackVelocity();
            //_motorVariable.X1AxisAcceleration = MotorX1.GetAcceleration();

            _motorVariable.X2AxisPosition = MotorX2.GetActualPos();
            _motorVariable.X2InPosition = MotorX2.IsInpos();
            _motorVariable.X2AxisVelocity = MotorX2.GetFeedbackVelocity();
            _motorVariable.X2AxisAcceleration = MotorX2.GetAcceleration();

            _motorVariable.Y1AxisPosition = MotorY1.GetActualPos();
            _motorVariable.Y1InPosition = MotorY1.IsInpos();
            _motorVariable.Y1AxisVelocity = MotorY1.GetFeedbackVelocity();
            _motorVariable.Y1AxisAcceleration = MotorY1.GetAcceleration();

            _motorVariable.Y2AxisPosition = MotorY2.GetActualPos();
            _motorVariable.Y2InPosition = MotorY2.IsInpos();
            _motorVariable.Y2AxisVelocity = MotorY2.GetFeedbackVelocity();
            _motorVariable.Y2AxisAcceleration = MotorY2.GetAcceleration();

            _motorVariable.ZAxisPosition = MotorZ.GetActualPos();
            _motorVariable.ZInPosition = MotorZ.IsInpos();
            _motorVariable.ZAxisVelocity = MotorZ.GetFeedbackVelocity();
            _motorVariable.ZAxisAcceleration = MotorZ.GetAcceleration();

            _motorVariable.IsHome = MotorX2.GetServoStatus() == 1 && MotorY1.GetServoStatus() == 1
                                             && MotorY2.GetServoStatus() == 1 && MotorZ.GetServoStatus() == 1;

            // Stage A Position Parameter
            _motorVariable.IsLoadPosition = CheckTeachingPosition(eTeachingPosition.Load);
            _motorVariable.IsUnloadPosition = CheckTeachingPosition(eTeachingPosition.UnLoad);
            _motorVariable.IsSafetyPosition = CheckTeachingPosition(eTeachingPosition.SafetyPosition);
            _motorVariable.IsWeldingStartPosition = CheckTeachingPosition(eTeachingPosition.StageWeldingStartPos);

            //if (ChangeChecker.IsChangedBool("ALoadEnable", MotionVariables[eStage.A].IsLoadEnable))
            //    Debug.WriteLine($"A Load Enable [{!MotionVariables[eStage.A].IsLoadEnable} -> {MotionVariables[eStage.A].IsLoadEnable}] >> XPos[{MotionVariables[eStage.A].XAxisPosition}] YPos[{MotionVariables[eStage.A].YAxisPosition}] Home[{MotionVariables[eStage.A].IsHome}] ACS Active[{IsActivated}]");

            //if (ChangeChecker.IsChangedBool("AUnloadEnable", MotionVariables[eStage.A].IsUnloadEnable))
            //    Debug.WriteLine($"A Unload Enable [{!MotionVariables[eStage.A].IsUnloadEnable} -> {MotionVariables[eStage.A].IsUnloadEnable}] >> XPos[{MotionVariables[eStage.A].XAxisPosition}] YPos[{MotionVariables[eStage.A].YAxisPosition}] Home[{MotionVariables[eStage.A].IsHome}] ACS Active[{IsActivated}]");

            HasHeavyAlarm = ReadVariableInt(HeavyAlarm) == 1;
            HasLightAlarm = ReadVariableInt(LightAlarm) == 1;
            ReadVariableIntArrRef(AlarmCodes, ref _HaveAlarmCodes, ProgramBuffer.ACSC_NONE, 0, 10);
        }

        protected override int DoDeactivate()
        {
            StopMonitoringService();
            return base.DoDeactivate();
        }
        bool IPrismaticGunACS.AllAxisHomeStart()
        {
            var Buffer = eBuffer.All_Axis_Home;
            if (RunBuffer((ProgramBuffer)Buffer)) {
                Log.D("SetAllAxisHome >> Complete");
                return true;
            }

            Log.D("SetAllAxisHome >> Fail");
            return false;
        }

        bool IPrismaticGunACS.AllAxisHomeStop()
        {
            var Buffer = eBuffer.All_Axis_Home;
            StopBuffer((ProgramBuffer)Buffer);
            Log.D("StopAllAxisHome >> Complete");
            return true;
        }

        bool IPrismaticGunACS.CheckAllAxisHome()
        {
            return ReadVariableInt(iAllAxisHomeComplete) > 0;
        }

        bool IPrismaticGunACS.OneShotStart()
        {
            var buffer = eBuffer.Welding_OneShot;

            if (RunBuffer((ProgramBuffer)buffer)) {
                Log.D($"{Enum.GetName(typeof(eBuffer), buffer)} >> Complete");
                return true;
            }

            Log.D("SetALineOneShotStart >> Fail");
            return false;
        }

        public bool OneShotStop()
        {
            var buffer = eBuffer.Welding_OneShot;

            IPrismaticGunACS motion = this;
            motion.SetLCILserOn(false);
            StopBuffer((ProgramBuffer)buffer);

            Log.D("StopBuffer");
            return true;
        }

        bool IPrismaticGunACS.CheckOneShotComp()
        {
            return ReadVariableInt(iOneShotComplete) > 0;
        }

        bool IPrismaticGunACS.LineShotStart()
        {
            var buffer = eBuffer.Welding_Line;

            if (RunBuffer((ProgramBuffer)buffer)) {
                Log.D($"SetLineShotStart >> Complete");
                return true;
            }

            Log.D($"SetLineShotStart >> Fail");
            return false;
        }

        bool IPrismaticGunACS.LineShotStop() {
            var buffer = eBuffer.Welding_Line;

            IPrismaticGunACS motion = this;
            motion.SetLCILserOn(false);
            StopBuffer((ProgramBuffer)buffer);

            Log.D("StopBuffer");
            return true;
        }

        bool IPrismaticGunACS.CheckLineShotComp()
        {
            return ReadVariableInt(iLineWeldingComplete) > 0;
        }

        bool IPrismaticGunACS.WeldingStart()
        {
            if (IsSimul)
                return true;

            var buffer = eBuffer.Welding_AllRound;

            if (RunBuffer((ProgramBuffer)buffer)) {
                Log.I($"Set Welding All-Round >> Complete");
                return true;
            }

            Log.D($"Set Welding All-Round  >> Fail");
            return false;
        }
        bool IPrismaticGunACS.WeldingStop()
        {
            var buffer = eBuffer.Welding_AllRound;

            IPrismaticGunACS motion = this;
            motion.SetLCILserOn(false);
            StopBuffer((ProgramBuffer)buffer);

            Log.D("StopBuffer");
            return true;
        }
        bool IPrismaticGunACS.CheckWeldingComp()
        {
            if (IsSimul)
                return true;

            return ReadVariableInt(iWeldingComplete) > 0;
        }

        bool IPrismaticGunACS.SetOneShot(int msecShotTime, double secPhzShotPulse1, double secPhzShotPulse2)
        {
#warning calibration 제어 값이 Percent to watt로 써야하는건지 확인필요
            //var calPower = _calibration.CalibratePercentToWatt(secPhzShotPulse);

            if (WriteRealVariable(rOneShotTime, msecShotTime) &
                WriteRealVariable(rOneShotCoreVoltage, secPhzShotPulse1) &
                WriteRealVariable(rOneShotRingVoltage, secPhzShotPulse2)) {
                Log.D($"SetOneShot >> Complete");
                return true;
            }
            Log.D($"SetOneShot >> Fail");
            return false;
        }

        bool IPrismaticGunACS.SetLineShot(double corePower, double ringPower, double startPosX, double endPosX, double startPosY, double endPosY, double velocity, double acc)
        {
#warning calibration 제어 값이 Percent to watt로 써야하는건지 확인필요
            //var calPower = _calibration.CalibratePercentToWatt(pulse);

            if (WriteRealVariable(rLineWeldingLaserOnX, startPosX) &
              WriteRealVariable(rLineWeldingLaserOnY, startPosY) &
              WriteRealVariable(rLineWeldingLaserOffX, endPosX)&
              WriteRealVariable(rLineWeldingLaserOffY, endPosY)&
              WriteRealVariable(rLineWeldingVel, velocity) &
              WriteRealVariable(rLineWeldingAcc, acc) &
              WriteRealVariable(rLineWeldingCoreVoltage, corePower) &
              WriteRealVariable(rLineWeldingRingVoltage, ringPower)) {
                Log.D($"SetLineShot >> Setting Complete");
                return true;
            }
            Log.D($"SetLineShot >> Fail");
            return false;
        }

        bool IPrismaticGunACS.SetWeldingVel(double acc, double svel, double cvel)
        {
            if (WriteRealVariable(rWeldingAcc, acc) &
                WriteRealVariable(rWeldingSVel, svel) &
                WriteRealVariable(rWeldingCVel, cvel)) {
                Log.D($"SetWeldingVel >> Complete");
                return true;
            }
            Log.D($"SetWeldingVel >> Fail");
            return false;
        }

        bool IPrismaticGunACS.SetWeldingPos(double startPosX, double startPosY, double endPosX, double endPosY, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rWeldingStartPosX, startPosX + camOffsetX) &
                WriteRealVariable(rWeldingStartPosY, startPosY + camOffsetY) &
                WriteRealVariable(rWeldingEndPosX, endPosX + camOffsetX) &
                WriteRealVariable(rWeldingEndPosY, endPosY + camOffsetY)) {
                Log.D($"SetWeldingPos >> Complete");
                return true;
            }
            Log.D($"SetWeldingPos >> Fail");
            return false;
        }

        bool IPrismaticGunACS.SetWeldingArc(double point1, double point2, double point3, double point4)
        {
            if (WriteRealVariable(rWeldingEdgePointRad1, point1) &
                WriteRealVariable(rWeldingEdgePointRad2, point2) &
                WriteRealVariable(rWeldingEdgePointRad3, point3) &
                WriteRealVariable(rWeldingEdgePointRad4, point4)) {
                Log.D($"SetWeldingArc >> Complete");
                return true;
            }
            Log.D($"SetWeldingArc >> Fail");
            return false;
        }
        // 현재 Voltage 고정적으로 사용
        bool IPrismaticGunACS.SetWeldingType(eOutType type)
        {
            if (WriteVariableInt(iALaserShotMode, (int)type)) {
                Log.D($"SetWeldingPulse >> Complete");
                return true;
            }
            Log.D($"SetWeldingPulse >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetWeldingPulse(double coreMaxPower, double coreMinPower, double ringMaxPower, double ringMinPower)
        {
            if (WriteRealVariable(rWeldingCorePowerMax, coreMaxPower) &
                WriteRealVariable(rWeldingCorePowerMin, coreMinPower) &
                WriteRealVariable(rWeldingRingPowerMax, ringMaxPower) &
                WriteRealVariable(rWeldingRingPowerMin, ringMinPower)) {
                Log.D($"SetWeldingPulse >> Complete");
                return true;
            }
            Log.D($"SetWeldingPulse >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetLaserOnOff(double onPosX, double onOffSet, double offPosX, double offOffSet, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rLaserONPosX, onPosX + camOffsetX) &
                WriteRealVariable(rLaserONOffset, onOffSet) &
                WriteRealVariable(rLaserOFFPosX, offPosX + camOffsetX) &
                WriteRealVariable(rLaserOFFOffset, offOffSet)) {
                Log.D($"SetLaserOnOff >> Complete");
                return true;
            }
            Log.D($"SetLaserOnOff >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetLCILserOn(bool on)
        {
            if (WriteRealVariable(iAStageLCType, on ? 1 : 0))
            {
                Log.D($"LCILaserOn >> Complete [LaserUSE {on}]");
                return true;
            }
            Log.D($"LCILaserOn >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetWeldingAxes(int axisXnum, int axisYnum)
        {
            if (WriteVariableInt(iXAxisNumber, axisXnum) & WriteVariableInt(iYAxisNumber, axisYnum)) {
                Log.D("Set Welding Axes(X, Y) >> Complete");
                return true;
            }
            Log.D($"Set Welding Axes(X, Y) >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetLaserIOUse(bool on)
        {
            if (WriteRealVariable(iLaserUse, on ? 1 : 0))
            {
                Log.D($"Set Laser Use Set(Interface IO Use) {on} >> Complete");
                return true;
            }
            Log.D($"Set Laser Use Set(Interface IO Use) >> Fail");
            return false;
        }     

        bool IPrismaticGunACS.SetRamppingOffParameter(double pos, double overlap, double endpower, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rRampingOFFStartPosX, pos + camOffsetX) &
                WriteRealVariable(rRampingOFFOverlapDist, overlap) &
                WriteRealVariable(rRampingOFFEndPower, endpower)) {
                Log.D($"SetRamppingStartPos >> Complete");
                return true;
            }
            Log.D($"SetRamppingStartPos >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetRamppingOnParameter(double overlap, double startpower)
        {
            if (WriteRealVariable(rRampingONOverlapDist, overlap) &
                WriteRealVariable(rRampingONStartPower, startpower)) {
                Log.D($"SetRamppingStartPos >> Complete");
                return true;
            }
            Log.D($"SetRamppingStartPos >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetWeldingTeachingPosition1(double PosX, double PosY, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rWeldingPosition1PosX, PosX + camOffsetX) &
                WriteRealVariable(rWeldingPosition1PosY, PosY + camOffsetY)) {
                Log.D($"Set Welding Pos1 ({PosX} / {PosY}) >> Complete");
                return true;
            }
            Log.D($"Set Welding Pos1 >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetWeldingTeachingPosition2(double PosX, double PosY, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rWeldingPosition2PosX, PosX + camOffsetX) &
                WriteRealVariable(rWeldingPosition2PosY, PosY + camOffsetY)) {
                Log.D($"Set Welding Pos2 ({PosX} / {PosY}) >> Complete");
                return true;
            }
            Log.D($"Set Welding Pos2 >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetWeldingTeachingPosition3(double PosX, double PosY, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rWeldingPosition3PosX, PosX + camOffsetX) &
                WriteRealVariable(rWeldingPosition3PosY, PosY + camOffsetY)) {
                Log.D($"Set Welding Pos3 ({PosX} / {PosY}) >> Complete");
                return true;
            }
            Log.D($"Set Welding Pos3 >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetWeldingTeachingPosition4(double PosX, double PosY, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rWeldingPosition4PosX, PosX + camOffsetX) &
                WriteRealVariable(rWeldingPosition4PosY, PosY + camOffsetY)) {
                Log.D($"Set Welding Pos4 ({PosX} / {PosY}) >> Complete");
                return true;
            }
            Log.D($"Set Welding Pos4 >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetWeldingTeachingPosition5(double PosX, double PosY, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rWeldingPosition5PosX, PosX + camOffsetX) &
                WriteRealVariable(rWeldingPosition5PosY, PosY + camOffsetY)) {
                Log.D($"Set Welding Pos5 ({PosX} / {PosY}) >> Complete");
                return true;
            }
            Log.D($"Set Welding Pos5 >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetWeldingTeachingPosition6(double PosX, double PosY, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rWeldingPosition6PosX, PosX + camOffsetX) &
                WriteRealVariable(rWeldingPosition6PosY, PosY + camOffsetY)) {
                Log.D($"Set Welding Pos6 ({PosX} / {PosY}) >> Complete");
                return true;
            }
            Log.D($"Set Welding Pos6 >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetWeldingTeachingPosition7(double PosX, double PosY, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rWeldingPosition7PosX, PosX + camOffsetX) &
                WriteRealVariable(rWeldingPosition7PosY, PosY + camOffsetY)) {
                Log.D($"Set Welding Pos7 ({PosX} / {PosY}) >> Complete");
                return true;
            }
            Log.D($"Set Welding Pos7 >> Fail");
            return false;
        }
        bool IPrismaticGunACS.SetWeldingTeachingPosition8(double PosX, double PosY, double camOffsetX, double camOffsetY)
        {
            if (WriteRealVariable(rWeldingPosition8PosX, PosX + camOffsetX) &
                WriteRealVariable(rWeldingPosition8PosY, PosY + camOffsetY)) {
                Log.D($"Set Welding Pos8 ({PosX} / {PosY}) >> Complete");
                return true;
            }
            Log.D($"Set Welding Pos8 >> Fail");
            return false;
        }

        bool IPrismaticGunACS.IsActivated() => IsActivated;

        bool IPrismaticGunACS.ResetAlarm()
        {
            TerminalCommand("FCLEAR ALL");
            return WriteVariableInt(AlarmReset, 1);
        }

        bool IPrismaticGunACS.IsLaserOn()
        {
            if (!IsActivated || !IsConnected)
                return false;

            return ReadVariableInt(IsLaserOn) > 0;
        }

        void IPrismaticGunACS.AllAxisServoOff()
        {
            if (!IsActivated || !IsConnected)
                return;
            SetMotorDisableAll();
        }

        private bool CheckTeachingPosition(eTeachingPosition teachingPos)
        {
            if (IsSimul)
                return true;

            var targetPos = _teachingSetting.Get(teachingPos);

            // 수정 필요 Jeongin
            if ((_motorVariable.X2AxisPosition.CheckInside(targetPos.AxisX, 0.5) & _motorVariable.X2InPosition)
                & (_motorVariable.Y1AxisPosition.CheckInside(targetPos.AxisY1, 0.5) & _motorVariable.Y1InPosition)
                & (_motorVariable.Y2AxisPosition.CheckInside(targetPos.AxisY2, 0.5) & _motorVariable.Y2InPosition)
                & (_motorVariable.ZAxisPosition.CheckInside(targetPos.AxisZ, 0.5) & _motorVariable.ZInPosition))
                return true;

            return false;
        }

        internal bool IsReachedTeachingPosition(eTeachingPosition targetPos)
        {
            switch (targetPos) {
                case eTeachingPosition.Load:
                    return _motorVariable.IsLoadPosition;
                case eTeachingPosition.UnLoad:
                    return _motorVariable.IsUnloadPosition;
                case eTeachingPosition.SafetyPosition:
                    return _motorVariable.IsSafetyPosition;
                case eTeachingPosition.StageWeldingStartPos:
                    return _motorVariable.IsWeldingStartPosition;
                default:
                    return false;
            }
        }
       
    }
}