using CSZTM;
using GTN;
using Rs.Framework;
using Rs.Motion.Base;
using Rs.Motion.Base.Config;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using static System.Runtime.CompilerServices.RuntimeHelpers;

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

        private ZtmCardManager cardManager = ZtmCardManager.Instance;
        private short m_apiResult = 0;
        private double m_dfCommandPosition = 0.0;
        private object m_objLocker = new object();
        private bool m_bIsEncoderSetted = false;
        private short core = 1;
       
        
        /// <summary>
        /// 轴初始化，设置轴参数
        /// </summary>
        /// <returns></returns>
        public override ErrorCode Init()
        {
            LogHelper.Debug($"axis {Config.AxisName} init");
            if (!cardManager.IsInitialized)
            {
                LogHelper.Debug($"axis {Config.AxisName}'s card IsInitialized");
                return ErrorCode.CardNotInit;
            }
            HomeStatus = EHomeStatus.NotStart;
            m_apiResult = ZTM.ZT_SetPOTOn(Config.CardMc, (short)Config.AxisId, (short)((Config.EnableEL&0x01)));
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetPOTOn ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetNOTOn(Config.CardMc, (short)Config.AxisId, (short)((Config.EnableEL>>1)&0x01));
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetNOTOn ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetPOTPol(Config.CardMc, (short)Config.AxisId, (short)Config.ElLogic);
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetPOTPol ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetNOTPol(Config.CardMc, (short)Config.AxisId, (short)Config.ElLogic);
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetNOTPol ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetAlarmOn(Config.CardMc, (short)Config.AxisId, (short)(Config.EnableAlarm));
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetAlarmOn ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetAlarmPol(Config.CardMc, (short)Config.AxisId, (short)Config.AlarmLogic);
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetAlarmPol ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetHomePol(Config.CardMc, (short)Config.AxisId, (short)Config.HomeOrgLogic);
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetHomePol ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetPulseRatio(Config.CardMc, (short)Config.AxisId, 1U, 1U);
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetPulseRatio ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetGearRatio(Config.CardMc, (short)Config.AxisId, 1U, 1U);
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetGearRatio ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetEncoderOn(Config.CardMc, (short)Config.AxisId, (short)(Config.EnableEncoder));
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetEncoderOn ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetPulseMode(Config.CardMc, (short)Config.AxisId, (short)Config.PulseOutMode, 0, 0);
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetPulseMode ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetEncoderPol(Config.CardMc, (short)Config.AxisId, Config.PulseOutDir);
            if (m_apiResult != 0)
            {
                LogHelper.Debug($"axis {Config.AxisName} ZT_SetEncoderPol ret={m_apiResult}");
                return ErrorCode.Fail;
            }
            //m_LastErrorCode = ZTM.ZT_SetEncoderIndexPol(m_axis_config.CardMc, m_axis_config.AxisNum, m_axis_config.encoder_config.pulse_outindexdirection);
            //if (m_LastErrorCode != 0)
            //{
            //    return Err.FAIL;
            //}
            if(Config.EnableEncoder==0)
            {
                m_apiResult = ZTM.ZT_SetDCIAParam(Config.CardMc, (short)Config.AxisId, (short)(Config.PulseOneRound/200), Config.Current, 10, 0, 400, 10);
                LogHelper.Debug($"axis {Config.AxisName} set current {Config.Current} ret={m_apiResult},xifen={(short)(Config.PulseOneRound / 200)}");
                if (m_apiResult != 0)
                {
                    return ErrorCode.Fail;
                }
            }
            else
            {
                ZTM.ZT_SetEncoderIndexPol(Config.CardMc, (short)Config.AxisId, 0);
            }
            if(Config.NeedGoHome==0)
            {
                HomeStatus = EHomeStatus.Finished;
            }
            IsInitialized = true;
            LogHelper.Debug($"axis {Config.AxisName} init ok");
            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
            {
                m_apiResult = ZTM.ZT_SetServoOn(Config.CardMc, (short)Config.AxisId, 1);
                if (m_apiResult != 0)
                    return ErrorCode.Fail;
                return ErrorCode.Ok;
            }
        }

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

        /// <summary>
        /// 停止运动
        /// </summary>
        /// <returns></returns>
        public override ErrorCode Stop()
        {
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            m_apiResult = ZTM.ZT_Stop(Config.CardMc, (uint)(1<<Config.AxisId));
            if (m_apiResult != 0)
                return ErrorCode.Fail;
            else
                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 / Config.AccTime / 1000000.0);
            //float fDec = (float)(maxSpeedPulse / Config.DecTime / 1000000.0);
            //float fAcc = (float)(fMaxVel / (accTime==0?Config.AccTime:accTime * 1000));
            //float fDec = (float)(fMaxVel / (decTime==0?Config.DecTime:decTime * 1000));
             float fAcc = (float)(fMaxVel / Config.AccTime / 1000.0);//pulse/ms2
             float fDec = (float)(fMaxVel / Config.DecTime / 1000.0);//pulse/ms2
            float fStart = (float)(minSpeedPulse / 1000.0);
            float fStop = (float)(stopSpeedPulse / 1000.0);
            m_apiResult = ZTM.ZT_MoveA(Config.CardMc, (short)Config.AxisId, (int)dfPos, fStart, fMaxVel, fAcc, fDec, fStop, (ushort)(Config.STime * 1000.0));
            string msg = $"ZTM.ZT_MoveA({Config.CardMc}, {(short)Config.AxisId}, {(int)dfPos}, {fStart}, {fMaxVel}, {fAcc}, {fDec}, {fStop}, {(ushort)(Config.STime * 1000.0)})";
            LogHelper.MoveLog($"{msg} ret={m_apiResult}");
            //File.AppendAllText("move.txt", msg + "\r\n");
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            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 = GetPrfPosition(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(dfDistVal, 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

            m_apiResult = ZTM.ZT_MoveR(Config.CardMc,
               (short)Config.AxisId,
               (int)dfPos,
               fStart, //起跳速度
               fMaxVel,
               fAcc,
               fDec,
               fStop,
               (ushort)(Config.STime * 1000.0));
            if (m_apiResult != 0)
            {
                return ErrorCode.AxisMoveFail;
            }
            else
            {
                m_dfCommandPosition = 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);//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

                if(dir==0)
                {
                    m_apiResult = ZTM.ZT_JogM(Config.CardMc,
                      (short)Config.AxisId,
                      fStart, //起跳速度
                      fMaxVel,
                      fAcc,
                      fDec,
                      fStop,
                      (ushort)(Config.STime * 1000.0));
                    LogHelper.MoveLog($"ZT_JogM ret={m_apiResult}");
                }
                else
                {
                    m_apiResult = ZTM.ZT_JogP(Config.CardMc,
                     (short)Config.AxisId,
                     fStart, //起跳速度
                     fMaxVel,
                     fAcc,
                     fDec,
                     fStop,
                     (ushort)(Config.STime * 1000.0));
                    LogHelper.MoveLog($"ZT_JogP ret={m_apiResult}");
                }
                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)
        {
            uint uiAllStatus = 0;
            bIsEable = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            m_apiResult = ZTM.ZT_GetServoStatus(Config.CardMc, ref uiAllStatus);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            else
            {
                bIsEable = ((uiAllStatus >> Config.AxisId) & 0x01) == 1;
                return ErrorCode.Ok;
            }
        }

        /// <summary>
        /// 判断轴是否回零
        /// </summary>
        /// <param name="bIsHomed"></param>
        /// <returns></returns>
        public override ErrorCode IsHomed(out bool bIsHomed)
        {
            uint uiAllStatus = 0;
            bIsHomed = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            m_apiResult = ZTM.ZT_GetHomed(Config.CardMc, ref uiAllStatus);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bool bStatus = ((uiAllStatus >> Config.AxisId) & 0x01) == 1;
            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)
        {
            uint uiAllStatus = 0;
            bIsStop = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            ErrorCode errCode = IsEnable(out bool bEnable);
            if (errCode > ErrorCode.Ok)
                return errCode;
            m_apiResult = ZTM.ZT_GetRunning(Config.CardMc, ref uiAllStatus);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsStop = ((uiAllStatus >> Config.AxisId) & 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)
                {
                    int pValue = 0;
                    m_apiResult = ZTM.ZT_GetPosPulse(Config.CardMc, (short)Config.AxisId, ref pValue);
                    if (m_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)
                {
                    int pValue = 0;
                    m_apiResult = ZTM.ZT_GetPosEncoder(Config.CardMc, (short)Config.AxisId, ref pValue);
                    if (m_apiResult != 0)
                    {
                        return ErrorCode.Fail;
                    }
                    err = PulseToMm(pValue, out dfEncPos);
                }
            }
            catch
            {
            }
            return err;
        }

        /// <summary>
        /// 获取编码器位置
        /// </summary>
        /// <param name="dfEncPos"></param>
        /// <returns></returns>
        public override ErrorCode GetDriverPosition(out double dfDriverPos)
        {
            dfDriverPos = 0.0;
            return ErrorCode.Ok;
        }

        /// <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 - m_dfCommandPosition;
            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)
        {
            uint uiAllStatus = 0;
            bIsOn = false;
            m_apiResult = ZTM.ZT_GetPOT(Config.CardMc, ref uiAllStatus);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((uiAllStatus >> Config.AxisId) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取负限位状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetNelStatus(out bool bIsOn)
        {
            uint uiAllStatus = 0;
            bIsOn = false;
            m_apiResult = ZTM.ZT_GetNOT(Config.CardMc, ref uiAllStatus);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((uiAllStatus >> Config.AxisId) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取原点的状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetOrgStatus(out bool bIsOn)
        {
            uint uiAllStatus = 0;
            bIsOn = false;
            m_apiResult = ZTM.ZT_GetHome(Config.CardMc, ref uiAllStatus);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((uiAllStatus >> Config.AxisId) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取报警状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetAlarmStatus(out bool bIsOn)
        {
            uint uiAllStatus = 0;
            bIsOn = false;
            m_apiResult = ZTM.ZT_GetAlarm(Config.CardMc, ref uiAllStatus);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((uiAllStatus >> Config.AxisId) & 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)
        {
            short uiAllStatus = 0;
            short uiAllLevel = 0;
            bIsOn = false;
            m_apiResult = ZTM.ZT_GetEMG(Config.CardMc, ref uiAllLevel, ref uiAllStatus);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((uiAllStatus >> Config.AxisId) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取到位信号
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetInpStatus(out bool bIsOn)
        {
            uint uiAllStatus = 0;
            bIsOn = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            m_apiResult = ZTM.ZT_GetReached(Config.CardMc, ref uiAllStatus);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            bIsOn = ((uiAllStatus >> Config.AxisId) & 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>
        /// <returns></returns>
        public void DefaultHome(object obj)
        {
            HomeStatus = EHomeStatus.Start;
            //MessageQueue.Instance.Insert(Config.AxisName + " Start home");
            MmToPulse(Config.HomeOffset, out double dfHome_offset);
            MmToPulse(Config.HomeSpeed, out double homeSpeedPulse);
            m_apiResult = ZTM.ZT_SetHomeParam(Config.CardMc,
                (short)Config.AxisId,
                (short)Config.HomeDir,//回零方向
                (short)Config.HomeMode,//回零模式
                (float)((double)(homeSpeedPulse / 10) / 1000.0),//回零低速
                (float)((double)homeSpeedPulse / 1000.0),//回零高速
                (float)((homeSpeedPulse / 1000.0) / (Config.AccTime * 1000.0)),//回零加速度
                (int)dfHome_offset, //回原后偏移
                0);
            //LogHelper.Debug($"axis {Config.AxisName} start home ,homedir={Config.HomeDir},homemode={Config.HomeMode}");
            if (m_apiResult != 0)
            {
                HomeStatus = EHomeStatus.Fail;
            }
            
            m_apiResult = ZTM.ZT_Home(Config.CardMc, (uint)(1 << Config.AxisId));
            if (m_apiResult != 0)
            {
                HomeStatus = EHomeStatus.Fail;
            }
            uint homed = 0;
            Stopwatch timer = new Stopwatch();
            timer.Restart();
            uint homing = 0;
            while (HomeStatus!= EHomeStatus.Finished && HomeStatus != EHomeStatus.Abort && timer.ElapsedMilliseconds < 2 * 60 * 1000)
            {
                //等待回原完成
                ZTM.ZT_GetHoming(Config.CardMc, ref homing);
                if(((homing>>Config.AxisId)&1)==0)
                {
                    m_apiResult = ZTM.ZT_GetHomed(Config.CardMc, ref homed);
                    if (((homed >> Config.AxisId) & 1) == 1)
                    {
                        HomeStatus = EHomeStatus.Finished;
                        break;
                    }
                }
            }
        }

        /// <summary>
        /// 轴回原
        /// </summary>
        /// <param name="cat"></param>
        /// <returns></returns>
        public override ErrorCode Home()
        {
            if (!IsInitialized)
            {
                return ErrorCode.AxisNotInit;
            }
            if (HomeStatus == EHomeStatus.Homing || HomeStatus == EHomeStatus.Start)
            {
                //return ErrorCode.Ok;
            }
           
            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;
            }
            Thread t = t = new Thread(new ParameterizedThreadStart(DefaultHome));
            
            t.Name = Config.AxisName + " homing thread";
            t.Start(this);
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 回原完成
        /// </summary>
        /// <param name="iOrig"></param>
        /// <returns></returns>
        public override ErrorCode Zero(int iOrig)
        {
            if (!IsInitialized)
            {
                return ErrorCode.AxisNotInit;
            }
            m_apiResult = ZTM.ZT_SetPosPulse(Config.CardMc, (short)Config.AxisId, iOrig);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            m_apiResult = ZTM.ZT_SetPosEncoder(Config.CardMc, (short)Config.AxisId, iOrig);
            if (m_apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            else
            {
                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 = m_dfCommandPosition;
            return ErrorCode.Ok;
        }

        public override ErrorCode SetPosCompare(ushort channel, double[] postions)
        {
            m_apiResult = ZTM.ZT_SetCompareParam(Config.CardMc, 0, 1, (short)Config.AxisId, (short)channel, 0, 1, (uint)Config.HcmpPulseWidth);
            double[] poses = new double[postions.Length];
            for (int i = 0; i < poses.Length; i++)
            {
                MmToPulse(postions[i], out poses[i]);
            }
            m_apiResult += ZTM.ZT_SetComparePos(Config.CardMc, 0, poses.Select(pos => (int)pos).ToArray(), (short)poses.Length);
            short ret = -1;
            do
            {
                ret = ZTM.ZT_GetHugeDataBusy(Config.CardMc);
            }
            while (ret != 0);
            m_apiResult += ZTM.ZT_CompareBegin(Config.CardMc, 1 << 0);/*00000002,000004,000008*/
            if (m_apiResult == 0)
                return ErrorCode.Ok;
            return ErrorCode.Fail;
        }


        public override ErrorCode ClearCompare(ushort uChannel)
        {
            short a = 0;
            ZTM.ZT_GetCompareNum(Config.CardMc, 0, ref a);

            m_apiResult = ZTM.ZT_CompareEnd(Config.CardMc, 1 << 0);
            if (m_apiResult == 0)
                return ErrorCode.Ok;
            return ErrorCode.Fail;
        }
    }
}