using GTN;
using gts;
using Newtonsoft.Json;
using Rs.Framework;
using Rs.Motion.Base;
using Rs.Motion.Base.Config;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace Rs.Motion.GugaoEcat
{
    public class GugaoAxis:IAxis
    {
        public GugaoAxis(AxisConfig config)
        {
            Config = config;
        }

        private GugaoCardManager cardManager = GugaoCardManager.Instance;
        //private Logger<AxisGugaoGENImpl> logger = new Logger<AxisGugaoGENImpl>();
        private short apiResult = 0;
        private object m_objLocker = new object();
        private bool m_bIsEncoderSetted = false;
        private short core = 1;

        
        /// <summary>
        /// 轴初始化，设置轴参数
        /// </summary>
        /// <returns></returns>
        public override ErrorCode Init()
        {
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            HomeStatus = EHomeStatus.NotStart;
            if (Config.EnableEncoder == 1)
            {
                //同步EtherCat的位置信息到编码器和规划器
                apiResult = mc_ecat.GTN_GetEcatEncPos(core, (short)Config.AxisId, out int pEncPos);
                if (apiResult != 0)
                {
                    return ErrorCode.AxisInitFail;
                }
                apiResult = mc_ecat.GTN_SetPrfPos(core, (short)Config.AxisId, pEncPos);
                if (apiResult != 0)
                {
                    return ErrorCode.AxisInitFail;
                }
                apiResult = mc_ecat.GTN_SetEncPos(core, (short)Config.AxisId, pEncPos);
                if (apiResult != 0)
                {
                    return ErrorCode.AxisInitFail;
                }

                double auEncPos = (double)pEncPos;
                if(Config.AssistEncoder>0)
                {
                    apiResult = mc_ecat.GTN_WriteAuEncPos(core, (short)Config.AssistEncoder, ref auEncPos, 1);
                    if (apiResult != 0)
                    {
                        return ErrorCode.AxisInitFail;
                    }
                }
                
                //同步
                apiResult = mc_ecat.GTN_SynchAxisPos(core, 1 << Config.AxisId - 1);
                if (apiResult != 0)
                {
                    return ErrorCode.AxisInitFail;
                }
                HomeStatus = EHomeStatus.Finished;
            }
            IsInitialized = true;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 反初始化轴，停止轴
        /// </summary>
        /// <returns></returns>
        public override ErrorCode Deinit()
        {
            IsInitialized = false;
            m_bIsEncoderSetted = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            else
            {
                Stop();
                Disable();
                return ErrorCode.Ok;
            }
        }

        /// <summary>
        /// 毫米转脉冲
        /// 总长度/电机转一圈走的长度*电机转一圈的脉冲数
        /// </summary>
        /// <param name="mm"></param>
        /// <param name="pulse"></param>
        /// <returns></returns>
        public override ErrorCode MmToPulse(double mm, out double pulse)
        {
            pulse = 0;

            if (Config.DistOneRound == 0.0)
            {
                return ErrorCode.UndefineDistanceOneRound;
            }
            else
            {
                pulse = mm / Config.DistOneRound * (double)Config.PulseOneRound;
                return ErrorCode.Ok;
            }
        }

        /// <summary>
        /// 脉冲转毫米
        /// 总脉冲数/电机转一圈的脉冲数*电机转一圈走的长度
        /// </summary>
        /// <param name="iVal"></param>
        /// <param name="dfDist"></param>
        /// <returns></returns>
        public override ErrorCode PulseToMm(double pulse, out double mm)
        {
            mm = 0;
            if (Config.PulseOneRound == 0)
            {
                return ErrorCode.UndefinePulseOneRound;
            }
            else
            {
                mm = pulse / (double)Config.PulseOneRound * Config.DistOneRound;
                return ErrorCode.Ok;
            }
        }

        /// <summary>
        /// 使能电机
        /// </summary>
        /// <returns></returns>
        public override ErrorCode Enable()
        {
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            else
            {
                apiResult = mc_ecat.GTN_ClrSts(core, (short)Config.AxisId, 1);
                if (apiResult != 0)
                    return ErrorCode.Fail;
                apiResult = mc_ecat.GTN_AxisOn(core, (short)Config.AxisId);
                if (apiResult != 0)
                    return ErrorCode.Fail;
                else
                    return ErrorCode.Ok;
            }
        }

        /// <summary>
        /// 去使能
        /// </summary>
        /// <returns></returns>
        public override ErrorCode Disable()
        {
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            apiResult = mc_ecat.GTN_AxisOff(core, (short)Config.AxisId);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 停止运动
        /// </summary>
        /// <returns></returns>
        public override ErrorCode Stop()
        {
            bool smoothStop = true;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            //int axes = 1 << m_axis_config.AxisId;
            //int option = 1 << m_axis_config.AxisId;
            if(smoothStop)
            {
                apiResult = mc_ecat.GTN_Stop(core, 1 << Config.AxisId - 1, 0 << Config.AxisId - 1);//option 对应位1表示紧急停止 0 为平滑停止
            }
            else
            {
                apiResult = mc_ecat.GTN_Stop(core, 1 << Config.AxisId - 1, 1 << Config.AxisId - 1);//option 对应位1表示紧急停止 0 为平滑停止
            }
            
            if (apiResult != 0)
                return ErrorCode.Fail;
            else
                return ErrorCode.Ok;
        }

        /// <summary>
        /// 运动控制卡回零
        /// </summary>
        /// <returns></returns>
        public override ErrorCode Default_Home_Move()
        {
            //if (!m_motion_card.IsInitialized)
            //{
            //    return ErrorCode.CARD_NOT_INIT;
            //}
            //if (!m_bIsInitialized)
            //{
            //    return ErrorCode.AXIS_NOT_INIT;
            //}
            //ErrorCode err = MmToPulse(m_axis_config.HomeOffset, out double dfHome_offset);
            //if (err > ErrorCode.OK)
            //{
            //    return err;
            //}
            //err = MmToPulse(1.0, out double dfPulseRaio);
            //m_LastErrorCode = ZTM.ZT_SetHomePol(m_axis_config.CardMc, (short)m_axis_config.AxisId, (short)m_axis_config.HomeOrgLogic);
            //if (m_LastErrorCode != 0)
            //{
            //    return ErrorCode.FAIL;
            //}
            //MmToPulse(m_axis_config.HomeSpeed, out double homeSpeedPulse);

            ////acc vel/s mm/s  pulse/s  pulse/ms    mm
            //MmToPulse(m_axis_config.MaxSpeed, out double maxSpeedPulse);

            //m_LastErrorCode = ZTM.ZT_SetHomeParam(m_axis_config.CardMc,
            //    (short)m_axis_config.AxisId,
            //    (short)m_axis_config.HomeDir,
            //    (short)m_axis_config.HomeMode,
            //    (float)((double)(homeSpeedPulse / 4) / 1000.0),
            //    (float)((double)homeSpeedPulse / 1000.0),
            //    (float)((maxSpeedPulse / 1000.0) / (m_axis_config.AccTime * 1000.0)),
            //    (int)dfHome_offset, 0);
            //if (m_LastErrorCode != 0)
            //{
            //    return ErrorCode.FAIL;
            //}
            ////int axes = SetBit(0, (int)m_axis_config.AxisId, 1);
            ////m_LastErrorCode = ZTM.ZT_Home(m_axis_config.CardMc, (uint)axes);
            //if (m_LastErrorCode != 0)
            //{
            //    return ErrorCode.MOVE_FAIL;
            //}
            //else
            //{
            //    return ErrorCode.OK;
            //}
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 绝对位置运动
        /// </summary>
        /// <param name="dfPosVal"></param>
        /// <param name="nSpeedPercent"></param>
        /// <param name="bypassEL"></param>
        /// <returns></returns>
        public override ErrorCode MovePos(double dfPosVal, int nSpeedPercent = 100)
        {
            if (!cardManager.IsInitialized)
                return ErrorCode.CardNotInit;
            if (!IsInitialized)
                return ErrorCode.AxisNotInit;
            if (!SafeCheck())
                return ErrorCode.Unsafe;
            nSpeedPercent = nSpeedPercent <= 0 ? 10 : nSpeedPercent;
            nSpeedPercent = nSpeedPercent > 100 ? 100 : nSpeedPercent;
            if (HomeStatus != EHomeStatus.Finished)
                return ErrorCode.AxisNotHome;
            if (Config.EnableSoftwareEL == 1 && (dfPosVal > Config.PelSoftwarePosition || dfPosVal < Config.NelSoftwarePosition))//启用软限位
            {
                return ErrorCode.OutOfLimit;
            }
            ErrorCode err = GetEmgStatus(out bool bEmg);
            if (err > ErrorCode.Ok)
                return err;
            if (bEmg)
                return ErrorCode.Emergency;
            err = GetAlarmStatus(out bool bAlarm);
            if (err > ErrorCode.Ok)
                return err;
            if (bAlarm)
                return ErrorCode.Alarm;
            err = IsEnable(out bool bEnable);
            if (err > ErrorCode.Ok)
                return err;
            if (!bEnable)
                return ErrorCode.ServoOff;
            err = IsStop(out bool bStop);
            if (err > ErrorCode.Ok)
                return err;
            if (!bStop)
                return ErrorCode.Moving;
            err = MmToPulse(dfPosVal, out double dfPos);
            if (err > ErrorCode.Ok)
                return err;
            double dfPercent = (double)nSpeedPercent / 100.0;

            MmToPulse(Config.MaxSpeed, out double maxSpeedPulse);
            MmToPulse(Config.MinSpeed, out double minSpeedPulse);
            MmToPulse(Config.StopSpeed, out double stopSpeedPulse);

            float fMaxVel = (float)(maxSpeedPulse * dfPercent / 1000.0);//pulse/ms
            //float fAcc = (float)(maxSpeedPulse / (accTime == 0 ? Config.AccTime : accTime) / 1000000.0);
            //float fDec = (float)(maxSpeedPulse / (decTime == 0 ? Config.DecTime : decTime) / 1000000.0);

            float fAcc = (float)(maxSpeedPulse / Config.AccTime / 1000000.0);
            float fDec = (float)(maxSpeedPulse / Config.DecTime / 1000000.0);
            float fStart = (float)(minSpeedPulse / 1000.0);
            float fStop = (float)(stopSpeedPulse / 1000.0);
            apiResult = mc_ecat.GTN_PrfTrap(core, (short)Config.AxisId);//设置运动模式为点位运动
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = mc_ecat.GTN_GetTrapPrm(core, (short)Config.AxisId, out mc_ecat.TTrapPrm trapPrm);//获取点位运动参数
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            trapPrm.acc = fAcc;
            trapPrm.dec = fDec;
            trapPrm.velStart = fStart;
            apiResult = mc_ecat.GTN_SetTrapPrm(core, (short)Config.AxisId, ref trapPrm);//设置点位运动参数
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = mc_ecat.GTN_SetPos(core, (short)Config.AxisId, (int)dfPos);//设置目标位置
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = mc_ecat.GTN_SetVel(core, (short)Config.AxisId, fMaxVel);//设置目标速度
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = mc_ecat.GTN_Update(core, (1 << Config.AxisId - 1));
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            TargetPosition = dfPosVal;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 相对当前位置运动
        /// </summary>
        /// <param name="dfDistVal"></param>
        /// <param name="nSpeedPercent"></param>
        /// <param name="bypassEL"></param>
        /// <returns></returns>
        public override ErrorCode MoveOffset(double dfDistVal, int nSpeedPercent = 100)
        {
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            if (!IsInitialized)
            {
                return ErrorCode.AxisNotInit;
            }
            if (!SafeCheck())
                return ErrorCode.Unsafe;
            nSpeedPercent = nSpeedPercent <= 0 ? 10 : nSpeedPercent;
            nSpeedPercent = nSpeedPercent > 100 ? 100 : nSpeedPercent;
            double curPos = 0.0;
            ErrorCode err;
            if (Config.EnableEncoder == 1)
            {
                err = GetEncoderPosition(out curPos);
                if (err > ErrorCode.Ok)
                {
                    return err;
                }
            }
            else
            {
                err = GetPrfPosition(out curPos);
                if (err > ErrorCode.Ok)
                {
                    return err;
                }
            }
            double dstPos = curPos + dfDistVal;
            return MovePos(dstPos, nSpeedPercent);
            //bool isNotSafe = !bypassEL && (dstPos > Config.PelSoftwarePosition || dstPos < Config.NelSoftwarePosition)
            //    && Config.PelSoftwarePosition != Config.NelSoftwarePosition;
            //if (isNotSafe)
            //{
            //    return ErrorCode.POSITION_OUT_OF_RANGE;
            //}

            err = GetEmgStatus(out bool bEmg);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (bEmg)
            {
                return ErrorCode.Emergency;
            }
            err = GetAlarmStatus(out bool bAlarm);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (bAlarm)
            {
                return ErrorCode.Alarm;
            }
            err = IsEnable(out bool bEnable);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (!bEnable)
            {
                return ErrorCode.ServoOff;
            }
            err = IsStop(out bool bStop);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (!bStop)
            {
                return ErrorCode.Moving;
            }
            //return MovePos(dstPos, nSpeedPercent, bypassEL);
            err = MmToPulse(dstPos, out double dfPos);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            double dfPercent = (double)nSpeedPercent / 100.0;
            MmToPulse(Config.MaxSpeed, out double maxSpeedPulse);
            MmToPulse(Config.MinSpeed, out double minSpeedPulse);
            MmToPulse(Config.StopSpeed, out double stopSpeedPulse);

            float fMaxVel = (float)(maxSpeedPulse * dfPercent / 1000.0);//pulse/ms
            float fAcc = (float)(maxSpeedPulse / Config.AccTime / 1000000.0);//pulse/ms2
            float fDec = (float)(maxSpeedPulse / Config.DecTime / 1000000.0);//pulse/ms2
            float fStart = (float)(minSpeedPulse / 1000.0);//pulse/ms
            float fStop = (float)(stopSpeedPulse / 1000.0);//pulse/ms


            apiResult = mc_ecat.GTN_PrfTrap(core, (short)Config.AxisId);//设置运动模式为点位运动
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = mc_ecat.GTN_GetTrapPrm(core, (short)Config.AxisId, out mc_ecat.TTrapPrm trapPrm);//获取点位运动参数
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            trapPrm.acc = fAcc;
            trapPrm.dec = fDec;
            trapPrm.velStart = fStart;
            apiResult = mc_ecat.GTN_SetTrapPrm(core, (short)Config.AxisId, ref trapPrm);//设置点位运动参数
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = mc_ecat.GTN_SetPos(core, (short)Config.AxisId, (int)dfPos);//设置目标位置
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = mc_ecat.GTN_SetVel(core, (short)Config.AxisId, fMaxVel);//设置目标速度
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = mc_ecat.GTN_Update(core, (1 << Config.AxisId - 1));
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            TargetPosition = dstPos;
            return ErrorCode.Ok;
        }

        public override ErrorCode MoveJog(short dir, int nSpeedPercent = 10)
        {
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            if (!IsInitialized)
            {
                return ErrorCode.AxisNotInit;
            }
            if (!SafeCheck())
                return ErrorCode.Unsafe;
            nSpeedPercent = nSpeedPercent <= 0 ? 10 : nSpeedPercent;
            nSpeedPercent = nSpeedPercent > 100 ? 100 : nSpeedPercent;
            ErrorCode err = IsEnable(out bool bEnable);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (!bEnable)
            {
                return ErrorCode.ServoOff;
            }
            err = IsStop(out bool bStop);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (!bStop)
            {
                return ErrorCode.Moving;
            }
            err = GetEmgStatus(out bool bEmg);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (bEmg)
            {
                return ErrorCode.Emergency;
            }
            err = GetAlarmStatus(out bool bAlarm);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (bAlarm)
            {
                return ErrorCode.Alarm;
            }
            else
            {
                double dfPercent = (double)nSpeedPercent / 100.0;

                MmToPulse(Config.MaxSpeed, out double maxSpeedPulse);
                MmToPulse(Config.MinSpeed, out double minSpeedPulse);
                MmToPulse(Config.StopSpeed, out double stopSpeedPulse);

                float fMaxVel = (float)(maxSpeedPulse * dfPercent / 1000.0);//pulse/ms
                float fAcc = (float)(maxSpeedPulse / Config.AccTime / 1000000.0);
                float fDec = (float)(maxSpeedPulse / Config.DecTime / 1000000.0);
                float fStart = (float)(minSpeedPulse / 1000.0);
                float fStop = (float)(stopSpeedPulse / 1000.0);


                apiResult = mc_ecat.GTN_PrfJog(core, (short)Config.AxisId);//设置运动模式为JOG运动模式
                if (apiResult != 0)
                {
                    return ErrorCode.Fail;
                }
                apiResult = mc_ecat.GTN_GetJogPrm(core, (short)Config.AxisId, out mc_ecat.TJogPrm jogPrm);
                if (apiResult != 0)
                {
                    return ErrorCode.Fail;
                }
                jogPrm.acc = fAcc;
                jogPrm.dec = fDec;
                apiResult = mc_ecat.GTN_SetJogPrm(core, (short)Config.AxisId, ref jogPrm);
                if (apiResult != 0)
                {
                    return ErrorCode.Fail;
                }
                if (dir == 0)
                {
                    fMaxVel = -1 * fMaxVel;
                }
                apiResult = mc_ecat.GTN_SetVel(core, (short)Config.AxisId, fMaxVel);
                if (apiResult != 0)
                {
                    return ErrorCode.Fail;
                }
                apiResult = mc_ecat.GTN_Update(core, 1<<Config.AxisId-1);
                if (apiResult != 0)
                {
                    return ErrorCode.Fail;
                }
                return ErrorCode.Ok;
            }
        }

        public override ErrorCode UpdateTargetPosition(double dfNewPosVal)
        {
            //if (!m_motion_card.IsInitialized)
            //{
            //    return ErrorCode.CARD_NOT_INIT;
            //}
            //if (!m_bIsInitialized)
            //{
            //    return ErrorCode.AXIS_NOT_INIT;
            //}
            //if (_eHomeStatus != EHomeStatus.FINISH)
            //{
            //    return ErrorCode.NOT_GO_HOME;
            //}
            //bool isNotSafe = (dfNewPosVal > m_axis_config.PelSoftwarePosition || dfNewPosVal < m_axis_config.NelSoftwarePosition)
            //    && m_axis_config.PelSoftwarePosition != m_axis_config.NelSoftwarePosition;
            //if (isNotSafe)
            //{
            //    return ErrorCode.POSITION_OUT_OF_RANGE;
            //}
            //ErrorCode err = GetEmgStatus(out bool bEmg);
            //if (err > ErrorCode.OK)
            //{
            //    return err;
            //}
            //if (bEmg)
            //{
            //    return ErrorCode.EMERGENCY;
            //}
            //err = GetAlarmStatus(out bool bAlarm);
            //if (err > ErrorCode.OK)
            //{
            //    return err;
            //}
            //if (bAlarm)
            //{
            //    return ErrorCode.ALARM;
            //}
            //err = IsEnable(out bool bEnable);
            //if (err > ErrorCode.OK)
            //{
            //    return err;
            //}
            //if (bEnable)
            //{
            //    return ErrorCode.SERVO_OFF;
            //}
            //err = MmToPulse(dfNewPosVal, out double dfPos);
            //if (err > ErrorCode.OK)
            //{
            //    return err;
            //}
            //else
            //{
            //    m_dfCommandPosition = dfNewPosVal;
            //    bool flag19 = m_LastErrorCode != 0;
            //    if (flag19)
            //    {
            //        return ErrorCode.FAIL;
            //    }
            //    else
            //    {
            //        return ErrorCode.OK;
            //    }
            //}
            return ErrorCode.Ok;
        }

        // Token: 0x06000256 RID: 598 RVA: 0x0000373C File Offset: 0x0000193C
        public override ErrorCode ChangeSpeed(double dfNewSpeed)
        {
            //if (!m_motion_card.IsInitialized)
            //{
            //    return ErrorCode.CARD_NOT_INIT;
            //}
            //if (!m_bIsInitialized)
            //{
            //    return ErrorCode.AXIS_NOT_INIT;
            //}
            //if (dfNewSpeed <= m_axis_config.MinSpeed)
            //{
            //    dfNewSpeed = m_axis_config.MinSpeed;
            //}
            //if (dfNewSpeed >= m_axis_config.MaxSpeed)
            //{
            //    dfNewSpeed = m_axis_config.MaxSpeed;
            //}
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 判断轴是否使能
        /// </summary>
        /// <param name="bIsEable"></param>
        /// <returns></returns>
        public override ErrorCode IsEnable(out bool bIsEable)
        {
            bIsEable = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            else
            {
                bIsEable = ((pSts >> 9) & 0x01) == 1;
                return ErrorCode.Ok;
            }
        }

        /// <summary>
        /// 判断轴是否回零
        /// </summary>
        /// <param name="bIsHomed"></param>
        /// <returns></returns>
        public override ErrorCode IsHomed(out bool bIsHomed)
        {
            bIsHomed = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            apiResult = mc_ecat.GTN_GetEcatHomingStatus(core, 1, out ushort homingStatus);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bool bStatus = homingStatus == 3;
            if (HomeStatus == EHomeStatus.Finished && bStatus)
            {
                bIsHomed = true;
            }
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 判断轴是否已停止
        /// </summary>
        /// <param name="bIsStop"></param>
        /// <returns></returns>
        public override ErrorCode IsStop(out bool bIsStop)
        {
            bIsStop = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsStop = ((pSts >> 10) & 0x01) == 0;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 判断运动是否到位
        /// </summary>
        /// <param name="bIsInPosition"></param>
        /// <returns></returns>
        public override ErrorCode IsInPosition(out bool bIsInPosition)
        {
            bIsInPosition = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            if (0 == Config.EnableEncoder)//步进
            {
                return IsStop(out bIsInPosition);
            }
            bool bIsStop = false;
            ErrorCode err = IsStop(out bIsStop);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (!bIsStop)
            {
                return ErrorCode.Ok;
            }
            bool bINP = false;
            err = GetInpStatus(out bINP);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (!bINP)
            {
                return ErrorCode.Ok;
            }
            double dfCurrentTorlance = 0.0;
            double dfTorlance = Math.Abs(Config.Tolerance);
            err = GetTolerance(out dfCurrentTorlance);
            if (Math.Abs(dfCurrentTorlance) <= dfTorlance)
            {
                bIsInPosition = true;
            }
            return err;
        }

        /// <summary>
        /// 获取规划位置
        /// </summary>
        /// <param name="dfCmdPos"></param>
        /// <returns></returns>
        public override ErrorCode GetPrfPosition(out double dfCmdPos)
        {
            dfCmdPos = 0.0;
            ErrorCode err = ErrorCode.Fail;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            try
            {
                lock (m_objLocker)
                {
                    apiResult = mc_ecat.GTN_GetPrfPos(core, (short)Config.AxisId, out double pValue, 1, out uint pClock);
                    //apiResult = mc_ecat.GTN_ReadAuEncPos(core, (short)Config.AxisId, out double pValue, 1);
                    if (apiResult != 0)
                    {
                        return ErrorCode.Fail;
                    }
                    err = PulseToMm(pValue, out dfCmdPos);
                }
            }
            catch
            {
            }
            return err;
        }

        /// <summary>
        /// 获取编码器位置
        /// </summary>
        /// <param name="dfEncPos"></param>
        /// <returns></returns>
        public override ErrorCode GetEncoderPosition(out double dfEncPos)
        {
            dfEncPos = 0.0;
            ErrorCode err = ErrorCode.Fail;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            if (0 == Config.EnableEncoder)
            {
                return GetPrfPosition(out dfEncPos);
            }
            try
            {
                lock (m_objLocker)
                {
                    apiResult = mc_ecat.GTN_GetEncPos(core, (short)Config.AxisId, out double pValue, 1, out uint pClock);// ZTM.ZT_GetPosEncoder(m_axis_config.CardMc, (short)m_axis_config.AxisId, ref iPos);
                    if (apiResult != 0)
                    {
                        return ErrorCode.Fail;
                    }
                    err = PulseToMm(pValue, out dfEncPos);
                }
            }
            catch
            {
            }
            return err;
        }

        public override ErrorCode GetAuEncoderPosition(out double dfCmdPos)
        {
            dfCmdPos = 0.0;
            ErrorCode err = ErrorCode.Fail;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            try
            {
                lock (m_objLocker)
                {
                    apiResult = mc_ecat.GTN_ReadAuEncPos(core, (short)Config.AssistEncoder, out double pValue, 1);
                    if (apiResult != 0)
                    {
                        return ErrorCode.Fail;
                    }
                    err = PulseToMm(pValue, out dfCmdPos);
                }
            }
            catch
            {
            }
            return err;
        }


        /// <summary>
        /// 获取编码器位置
        /// </summary>
        /// <param name="dfEncPos"></param>
        /// <returns></returns>
        public override ErrorCode GetDriverPosition(out double dfDriverPos)
        {
            dfDriverPos = 0.0;
            ErrorCode err = ErrorCode.Fail;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            try
            {
                lock (m_objLocker)
                {
                    apiResult = mc_ecat.GTN_GetEcatEncPos(core, (short)Config.AxisId, out int pValue);// ZTM.ZT_GetPosEncoder(m_axis_config.CardMc, (short)m_axis_config.AxisId, ref iPos);
                    if (apiResult != 0)
                    {
                        return ErrorCode.Fail;
                    }
                    err = PulseToMm(pValue, out dfDriverPos);
                }
            }
            catch
            {
            }
            return err;
        }

        /// <summary>
        /// 获取轴允许的公差值
        /// </summary>
        /// <param name="dfTolerance"></param>
        /// <returns></returns>
        public override ErrorCode GetTolerance(out double dfTolerance)
        {
            dfTolerance = 0.0;
            ErrorCode err;
            if (0 == Config.EnableEncoder)
            {
                return ErrorCode.Ok;
            }
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            err = GetPrfPosition(out double dfTargetPos);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            err = GetEncoderPosition(out double dfCurrentPos);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            dfTolerance = dfCurrentPos - dfTargetPos;
            double dfTolerance2 = dfCurrentPos - TargetPosition;
            if (Math.Abs(dfTolerance) < Math.Abs(dfTolerance2))
            {
                dfTolerance = dfTolerance2;
            }
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取正限位状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetPelStatus(out bool bIsOn)
        {
            bIsOn = false;
            apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((pSts >> 5) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取负限位状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetNelStatus(out bool bIsOn)
        {
            bIsOn = false;
            apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((pSts >> 6) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取原点的状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetOrgStatus(out bool bIsOn)
        {
            bIsOn = false;
            apiResult = mc_ecat.GTN_GetDi(core, mc_ecat.MC_HOME, out int pValue);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = (pValue >> Config.AxisId-1 & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取报警状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetAlarmStatus(out bool bIsOn)
        {
            bIsOn = false;
            apiResult = mc_ecat.GTN_GetDi(core, mc_ecat.MC_ALARM, out int pValue);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((pValue >> Config.AxisId - 1) & 0x01) == 1;
            if (bIsOn)
            {
                HomeStatus = EHomeStatus.NotStart;
            }
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取EMG状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetEmgStatus(out bool bIsOn)
        {
            bIsOn = false;
            apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((pSts >> 8) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取到位信号
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetInpStatus(out bool bIsOn)
        {
            bIsOn = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((pSts >> 11) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取EZ状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetEzStatus(out bool bIsOn)
        {
            bIsOn = false;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 轴回原
        /// </summary>
        /// <param name="cat"></param>
        /// <returns></returns>
        public override ErrorCode Home()
        {
            //if (_eHomeStatus == EHomeStatus.HOMING || _eHomeStatus == EHomeStatus.START_TO_GO_HOME)
            //{
            //    return ErrorCode.OK;
            //}
            //if (!m_bIsInitialized)
            //{
            //    return ErrorCode.AXIS_NOT_INIT;
            //}
            //ErrorCode err = GetEmgStatus(out bool bEmg);
            //if (err > ErrorCode.OK)
            //{
            //    return err;
            //}
            //if (bEmg)
            //{
            //    return ErrorCode.EMERGENCY;
            //}
            //err = GetAlarmStatus(out bool bAlarm);
            //if (err > ErrorCode.OK)
            //{
            //    return err;
            //}
            //if (bAlarm)
            //{
            //    return ErrorCode.ALARM;
            //}
            //err = IsEnable(out bool bEnable);
            //if (err > ErrorCode.OK)
            //{
            //    return err;
            //}
            //if (!bEnable)
            //{
            //    return ErrorCode.SERVO_OFF;
            //}
            //err = IsStop(out bool bStop);
            //if (err > ErrorCode.OK)
            //{
            //    return err;
            //}
            //if (!bStop)
            //{
            //    return ErrorCode.NOT_STOP;
            //}
            //Thread t = t = new Thread(new ParameterizedThreadStart(DealHome));
            //_eHomeStatus = EHomeStatus.START_TO_GO_HOME;
            //t.Name = base.AxisConfig.AxisName + " homing thread";
            //t.Start(this);
            return ErrorCode.Ok;
        }

        /// <summary>
        /// XY轴回原
        /// </summary>
        /// <param name="obj"></param>
        protected override void DealHome(object obj)
        {
            //try
            //{
            //    int step = 0;
            //    ErrorCode err;
            //    TimeoutUtil timer = new TimeoutUtil();
            //    _eHomeStatus = EHomeStatus.HOMING;
            //    bool bGoHomePositive = m_axis_config.HomeDir == 1;
            //    while (_eHomeStatus != EHomeStatus.ABORT)
            //    {
            //        Thread.Sleep(50);
            //        switch (step)
            //        {
            //            case 0:
            //                {
            //                    if (IsStop(out bool bStop) > ErrorCode.OK)
            //                    {
            //                        Abort_Go_Home();
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    if (!bStop)
            //                    {
            //                        Abort_Go_Home();
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    step++;
            //                    timer.Restart();
            //                    break;
            //                }
            //            case 1:
            //                {
            //                    bool bELSensorOn = false;
            //                    err = GetOrgStatus(out bool bHomeSensorOn);
            //                    if (err > ErrorCode.OK)
            //                    {
            //                        Abort_Go_Home();
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    if (bGoHomePositive)
            //                    {
            //                        err = GetPelStatus(out bELSensorOn);
            //                    }
            //                    else
            //                    {
            //                        err = GetNelStatus(out bELSensorOn);
            //                    }
            //                    if (err > ErrorCode.OK)
            //                    {
            //                        Abort_Go_Home();
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    if (Default_Home_Move() > ErrorCode.OK)
            //                    {
            //                        Abort_Go_Home();
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    step = 2;
            //                    timer.Restart();
            //                    break;
            //                }
            //            case 2:
            //                {
            //                    err = IsStop(out bool bStop3);
            //                    if (err > ErrorCode.OK)
            //                    {
            //                        Abort_Go_Home();
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    if (bStop3)
            //                    {
            //                        step++;
            //                        timer.Restart();
            //                    }
            //                    else
            //                    {
            //                        if (timer.IsTimeout(50000L))
            //                        {
            //                            Abort_Go_Home();
            //                            _eHomeStatus = EHomeStatus.FAIL;
            //                            return;
            //                        }
            //                    }
            //                    break;
            //                }
            //            case 3:
            //                {
            //                    Thread.Sleep(50);
            //                    if (Zero(0) > ErrorCode.OK)
            //                    {
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    double speedRatio = 0.2;
            //                    if (MoveOffset(m_axis_config.HomeOffset, (int)(speedRatio * 100.0), true) > ErrorCode.OK)
            //                    {
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    step++;
            //                    timer.Restart();
            //                    break;
            //                }
            //            case 4:
            //                {
            //                    err = IsStop(out bool bStop4);
            //                    if (err > ErrorCode.OK)
            //                    {
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    err = GetPelStatus(out bool bELSensorOn3);
            //                    if (err > ErrorCode.OK)
            //                    {
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    err = GetPelStatus(out bELSensorOn3);
            //                    if (err > ErrorCode.OK)
            //                    {
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    if (bELSensorOn3)
            //                    {
            //                        _eHomeStatus = EHomeStatus.FAIL;
            //                        return;
            //                    }
            //                    if (bStop4)
            //                    {
            //                        Thread.Sleep(50);
            //                        if (Zero(0) > ErrorCode.OK)
            //                        {
            //                            _eHomeStatus = EHomeStatus.FAIL;
            //                            return;
            //                        }
            //                        m_dfCommandPosition = 0.0;
            //                        _eHomeStatus = EHomeStatus.FINISH;
            //                        return;
            //                    }
            //                    else
            //                    {
            //                        if (timer.IsTimeout(50000L))
            //                        {
            //                            _eHomeStatus = EHomeStatus.FAIL;
            //                            return;
            //                        }
            //                    }
            //                    break;
            //                }
            //            default:
            //                _eHomeStatus = EHomeStatus.FAIL;
            //                return;
            //        }
            //    }
            //}
            //catch (Exception ex)
            //{
            //    Debug.WriteLine("IAxis::fn_go_home " + ex.Message);
            //}
            //_eHomeStatus = EHomeStatus.FAIL;
        }

        /// <summary>
        /// 回原完成
        /// </summary>
        /// <param name="iOrig"></param>
        /// <returns></returns>
        public override ErrorCode Zero(int iOrig)
        {
            //if (!IsInitialized)
            //{
            //    return ErrorCode.AxisNotInit;
            //}
            //m_dfCommandPosition = 0.0;
            //m_LastErrorCode = ZTM.ZT_SetPosPulse(Config.CardMc, (short)Config.AxisId, iOrig);
            //if (m_LastErrorCode != 0)
            //{
            //    return ErrorCode.FAIL;
            //}
            //m_LastErrorCode = ZTM.ZT_SetPosEncoder(Config.CardMc, (short)Config.AxisId, iOrig);
            //if (m_LastErrorCode != 0)
            //{
            //    return ErrorCode.FAIL;
            //}
            //else
            //{
            //    return ErrorCode.OK;
            //}
            return ErrorCode.Ok;
        }


        public void SetHomeFinished()
        {
            Zero(0);
            //base.Set_Home_Finished();
        }


        /// <summary>
        /// 获取运动的目标位置
        /// </summary>
        /// <param name="dfTargetPos"></param>
        /// <returns></returns>
        public override ErrorCode Get_Target_Position(out double dfTargetPos)
        {
            dfTargetPos = TargetPosition;
            return ErrorCode.Ok;
        }

        public override ErrorCode SetPosCompare(ushort channel, double[] postions)
        {
            //apiResult  = mc_ecat.GTN_PosCompareStatus(core, (short)channel, out mc_ecat.TPosCompareStatus pstatus);
            ClearCompare(channel);
            //apiResult = mc_ecat.GTN_PosCompareClear(core, (short)channel);
            //if(apiResult!=0)
            //return ErrorCode.Fail;
            //short permit = (short)(1 << channel);
            short[] permit = new short[2];
            permit[0] = 0x02;//第一路位置比较
            permit[1] = 0x04;//第二路位置比较
            apiResult  = mc_ecat.GTN_SetTerminalPermitEx(core,0, mc_ecat.MC_HSO, ref permit[0],1, 2);
            LogHelper.Debug($"mc_ecat.GTN_SetTerminalPermitEx({core},0, {mc_ecat.MC_HSO}, {permit},{(short)channel},0x01) ret={apiResult}");
            //if (apiResult != 0)
                //return ErrorCode.Fail;
            //apiResult = mc_ecat.GTN_SetTerminalPermit(core, 0, mc_ecat.MC_HSO, (ushort)(1 << channel));
            //if (apiResult != 0)
               // return ErrorCode.Fail;
            mc_ecat.TPosCompareMode cmpMode;
            apiResult = mc_ecat.GTN_GetPosCompareMode(core, (short)channel, out cmpMode);
            if (apiResult != 0)
                return ErrorCode.Fail;
            cmpMode.dimension = 1;
            cmpMode.mode = 0;
            cmpMode.outputMode = 0;
           cmpMode.outputPulseWidth =(ushort)Config.HcmpPulseWidth;
            cmpMode.sourceMode = 2;
            cmpMode.sourceX =(short) Config.AssistEncoder;
            string json=JsonConvert.SerializeObject(cmpMode);
            
            apiResult = mc_ecat.GTN_SetPosCompareMode(core, (short)channel, ref cmpMode);
            LogHelper.Debug($"mc_ecat.GTN_SetPosCompareMode({core}, 1 ,ref cmpMode); json={json} ret={apiResult}");
            if (apiResult != 0)
                return ErrorCode.Fail;
            List<double> posPulse = new List<double>();

            foreach (var data in postions)
            {
                GetAuEncoderPosition(out double curPos);
                //GetEncoderPosition(out double curPos);
                //double targetPos = curPos + data;
                double offset = data - curPos;
                MmToPulse(offset, out double pulse);
                posPulse.Add(pulse);
            }
            uint seg = 0;

            foreach (double data in posPulse)
            {
                mc_ecat.TPosCompareData cmpData = new mc_ecat.TPosCompareData();
                cmpData.hso = 0x03;
                cmpData.gpo = 0x03;
                cmpData.pos = (int)data;
                cmpData.segmentNumber = seg++;
                mc_ecat.GTN_PosCompareData(1, (short)channel, ref cmpData);
            }

            apiResult = mc_ecat.GTN_PosCompareStart(core, (short)channel);
            if(apiResult!=0)
                return ErrorCode.Fail;
            mc_ecat.GTN_PosCompareSpace(core, (short)channel, out short space);
            return ErrorCode.Ok;
        }

        public override ErrorCode CompareStatus(out short _pStatus, out int _pCount)
        {
            _pStatus = 0;
            _pCount = 0;
            apiResult = mc_ecat.GTN_PosCompareStatus(core, 2, out mc_ecat.TPosCompareStatus status);
            string json = JsonConvert.SerializeObject(status);
            LogHelper.Debug($"mc_ecat.GTN_PosCompareStatus({core}, 2, out mc_ecat.TPosCompareStatus status);status_json{json}");
            //apiResult = mc_pulse.GT_CompareStatus((short)Config.CardId, out _pStatus, out _pCount);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            return ErrorCode.Ok;
        }

        public override ErrorCode ComparePulse(ushort channel, bool dp = true)
        {
            short value = (short)channel;
            if (value != 0)
            {
                value = (short)(1 << channel);

            }
            apiResult = mc_ecat.GTN_PosComparePulse(core, 1, 1, 1, 0);
            LogHelper.Debug($"mc_ecat.GTN_PosComparePulse({core}, 1, 1, 1, 0) ret={apiResult}");
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            return ErrorCode.Ok;
        }

        public override ErrorCode ClearCompare(ushort uChannel)
        {
            apiResult = mc_ecat.GTN_PosCompareStop(core, (short)uChannel);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }

            apiResult = mc_ecat.GTN_PosCompareClear(core, (short)uChannel);
            if(apiResult!=0)
            {
                return ErrorCode.Fail;
            }
            
            return ErrorCode.Ok;
        }

        public override ErrorCode ClearAlarm()
        {
            apiResult = mc_ecat.GTN_ClrSts(core, (short)Config.AxisId, 1);//清除轴状态
            if(apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            return ErrorCode.Ok;
        }

    }
}