using csLTDMC;
using CSZTM;
using GTN;
using gts;
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;
using static gts.mc_pulse;

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

        private LeisaiCardManager cardManager = LeisaiCardManager.Instance;
        private short apiResult = 0;
        private double m_dfCommandPosition = 0.0;
        private object m_objLocker = new object();

        
        /// <summary>
        /// 轴初始化，设置轴参数
        /// </summary>
        /// <returns></returns>
        public override ErrorCode Init()
        {
            //设置回原点信号的有效电平
            apiResult = LTDMC.dmc_set_home_pin_logic(Config.CardId, Config.AxisId, Config.HomeOrgLogic, 0);
            if(apiResult!=0)
            {
                LogHelper.Debug($"{apiResult} = LTDMC.dmc_set_home_pin_logic({Config.CardId}, {Config.AxisId}, {Config.HomeOrgLogic}, 0);");
            }
            // mc_pulse.GT_ClrSts((short)Config.CardId, (short)Config.AxisId, 1);
            IsInitialized = true;

            return ErrorCode.Ok;
        }

        /// <summary>
        /// 反初始化轴，停止轴
        /// </summary>
        /// <returns></returns>
        public override ErrorCode Deinit()
        {
            IsInitialized = 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 = LTDMC.dmc_write_sevon_pin(Config.CardId, Config.AxisId, 0);
                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 = LTDMC.dmc_write_sevon_pin(Config.CardId, Config.AxisId, 1);
            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;
            }
            if(smoothStop)
            {
                apiResult = LTDMC.dmc_stop(Config.CardId, Config.AxisId, 0);
            }
            else
            {
                apiResult = LTDMC.dmc_stop(Config.CardId, Config.AxisId, 1);
            }
            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 = 4)
        {
            if (!cardManager.IsInitialized)
                return ErrorCode.CardNotInit;
            if (!IsInitialized)
                return ErrorCode.AxisNotInit;
            if (!SafeCheck())
                return ErrorCode.Unsafe;
            mc_pulse.GT_ClrSts((short)Config.CardId, (short)Config.AxisId, 1);//清除轴状态
            //运动之前先清一下状态
            //ClearAlarm();
            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 );//pulse/ms
            float fStart = (float)(minSpeedPulse * dfPercent);
            float fStop = (float)(stopSpeedPulse * dfPercent);
            apiResult = LTDMC.dmc_set_profile(Config.CardId,Config.AxisId, fStart, fMaxVel, Config.AccTime, Config.DecTime, fStop);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = LTDMC.dmc_set_s_profile(Config.CardId, Config.AxisId, 0,Config.STime);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }

            apiResult = LTDMC.dmc_pmove(Config.CardId, Config.AxisId, (int)dfPos, 1);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            else
            {
                m_dfCommandPosition = 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 offset, int nSpeedPercent = 100)
        {
            if (!cardManager.IsInitialized)
                return ErrorCode.CardNotInit;
            if (!IsInitialized)
                return ErrorCode.AxisNotInit;
            if (!SafeCheck())
                return ErrorCode.Unsafe;
            //运动之前先清一下状态
            //ClearAlarm();
            nSpeedPercent = nSpeedPercent <= 0 ? 10 : nSpeedPercent;
            nSpeedPercent = nSpeedPercent > 100 ? 100 : nSpeedPercent;
            //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(offset, 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);//pulse/ms
            float fStart = (float)(minSpeedPulse * dfPercent);
            float fStop = (float)(stopSpeedPulse * dfPercent);
            apiResult = LTDMC.dmc_set_profile(Config.CardId, Config.AxisId, fStart, fMaxVel, Config.AccTime, Config.DecTime, fStop);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = LTDMC.dmc_set_s_profile(Config.CardId, Config.AxisId, 0, Config.STime);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }

            apiResult = LTDMC.dmc_pmove(Config.CardId, Config.AxisId, (int)dfPos, 0);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            else
            {
                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;
            mc_pulse.GT_ClrSts((short)Config.CardId, (short)Config.AxisId, 1);//清除轴状态
            //运动之前先清一下状态
            //ClearAlarm();
            nSpeedPercent = nSpeedPercent <= 0 ? 10 : nSpeedPercent;
            nSpeedPercent = nSpeedPercent > 100 ? 100 : nSpeedPercent;
            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;
            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);//pulse/ms
            float fStart = (float)(minSpeedPulse * dfPercent);
            float fStop = (float)(stopSpeedPulse * dfPercent);
            apiResult = LTDMC.dmc_set_profile(Config.CardId, (ushort)(Config.AxisId), fStart, fMaxVel, Config.AccTime, Config.DecTime, fStop);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            apiResult = LTDMC.dmc_set_s_profile(Config.CardId, (ushort)(Config.AxisId), 0, Config.STime);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }

            apiResult = LTDMC.dmc_vmove(Config.CardId, (ushort)(Config.AxisId), (ushort)dir);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            else
            {
                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 = LTDMC.dmc_read_sevon_pin(Config.CardId, Config.AxisId);
            bIsEable = apiResult == 0;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 运动控制卡回零
        /// </summary>
        /// <returns></returns>
        public void DefaultHome(object obj)
        {
            if (!SafeCheck())
            {
                MessageQueue.Instance.Insert($"{Config.AxisName} go home unsafe");
                return;
            }  
            MessageQueue.Instance.Insert($"{Config.AxisName} go home");
            //判断是否在原点
            GetOrgStatus(out bool bOrg);
            if(bOrg)
            {
                //如果在原点，向回原方向的反方向运动直到离开原点
                if(Config.HomeDir==1)
                {
                    MoveJog(0, 2);
                }
                else
                {
                    MoveJog(1, 2);
                }
                while (bOrg)
                {
                    GetOrgStatus(out bOrg);
                }
                Stop();
                Thread.Sleep(100);
            }
            apiResult = LTDMC.dmc_set_position(Config.CardId, Config.AxisId, 0);
            apiResult = LTDMC.dmc_set_encoder(Config.CardId, Config.AxisId, 0);
            HomeStatus = EHomeStatus.Start;

            MmToPulse(Config.HomeOffset, out double dfHome_offset);
            MmToPulse(Config.HomeSpeed, out double homeSpeedPulse);
            MmToPulse(Config.StopSpeed, out double stopSpeedPulse);

            apiResult = LTDMC.dmc_set_profile(Config.CardId, Config.AxisId, homeSpeedPulse / 2, homeSpeedPulse, Config.AccTime, Config.DecTime, stopSpeedPulse);

            apiResult = LTDMC.dmc_set_homemode(Config.CardId, Config.AxisId, (ushort)Config.HomeDir, Config.HomeVelMode, Config.HomeMode, Config.EnableEncoder);

            LTDMC.dmc_home_move(Config.CardId, Config.AxisId);
            if (apiResult != 0)
            {
                MessageQueue.Instance.Insert($"{Config.AxisName} go home fail ret:{apiResult}");
                HomeStatus = EHomeStatus.Fail;
            }
            Stopwatch timer = new Stopwatch();
            timer.Restart();
            while (HomeStatus != EHomeStatus.Finished && HomeStatus != EHomeStatus.Abort && timer.ElapsedMilliseconds < 2 * 60 * 1000)
            {

                apiResult = LTDMC.dmc_check_done(Config.CardId, Config.AxisId);
                Thread.Sleep(10);
                if(apiResult==1)
                {
                    GetEncoderPosition(out double curPos);
                    MmToPulse(Config.HomeOffset, out double offsetPulse);
                    MoveOffset(Config.HomeOffset, 5);
                    //LTDMC.dmc_pmove(Config.CardId, Config.AxisId, (int)offsetPulse, 0);
                    do
                    {
                        IsStop(out bool stoped);
                        if (stoped)
                            break;
                    }
                    while (true);
                    Thread.Sleep(500);
                    Zero(0);
                    HomeStatus = EHomeStatus.Finished;
                    break;
                }
            }
        }

        /// <summary>
        /// 判断轴是否回零
        /// </summary>
        /// <param name="bIsHomed"></param>
        /// <returns></returns>
        public override ErrorCode IsHomed(out bool bIsHomed)
        {
            bIsHomed = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            if (HomeStatus == EHomeStatus.Finished)
            {
                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 = LTDMC.dmc_check_done(Config.CardId, Config.AxisId);

            bIsStop = apiResult == 1;
            return ErrorCode.Ok;
        }

        public override ErrorCode IsArrived(out bool bIsArrived)
        {
            bIsArrived = false;
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            bool bIsStop = false;
            ErrorCode err = IsStop(out bIsStop);
            if (err > ErrorCode.Ok)
            {
                return err;
            }
            if (!bIsStop)
            {
                return ErrorCode.Ok;
            }
            double currentPos = 0.0;
            if(Config.EnableEncoder==1)
            {
                GetEncoderPosition(out currentPos);
            }
            else
            {
                GetPrfPosition(out currentPos);
            }
            bIsArrived = Math.Abs((currentPos - m_dfCommandPosition)) < 0.05;
            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;
            }
            if(Config.EnableInp==1)
            {
                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)
                {
                    Int32 pulse = LTDMC.dmc_get_position(Config.CardId, Config.AxisId);
                    err = PulseToMm(pulse, 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 paulse = LTDMC.dmc_get_encoder(Config.CardId, Config.AxisId);
                    err = PulseToMm(paulse, 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.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 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 - 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 bIsInPel)
        {
            uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
            bIsInPel = ((status >> 1) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取负限位状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetNelStatus(out bool bIsInNel)
        {
            uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
            bIsInNel = ((status >> 2) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取原点的状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetOrgStatus(out bool bIsInOrg)
        {
            uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
            bIsInOrg = ((status >> 4) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取报警状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetAlarmStatus(out bool bIsAlarm)
        {
            uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
            bIsAlarm = ((status >> 0) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取EMG状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetEmgStatus(out bool bIsEmg)
        {
            uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
            bIsEmg = ((status >> 3) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取到位信号
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetInpStatus(out bool bIsInp)
        {
            uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
            bIsInp = ((status >> 8) & 0x01) == 1;
            return ErrorCode.Ok;
        }

        /// <summary>
        /// 获取EZ状态
        /// </summary>
        /// <param name="bIsOn"></param>
        /// <returns></returns>
        public override ErrorCode GetEzStatus(out bool bIsInEz)
        {
            uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
            bIsInEz = ((status >> 9) & 0x01) == 1;
            return ErrorCode.Ok;
        }

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

        /// <summary>
        /// 
        /// </summary>
        //protected void AutoHomeMode(object obj)
        //{
        //    bool bIsStop = false;
        //    int searchDistance = 1000;//回原搜索距离
        //    double searchPulse = 0;
        //    double curPos = 0.0;
        //    //固高板块的回原点，可以用捕获回原，如果是步进电机采用直接回原的方式

            
        //    int pValue = 0;
        //    if (Config.HomeDir == 0)
        //    {
        //        searchDistance *= -1;
        //    }
        //    MmToPulse(searchDistance, out searchPulse);

        //    MmToPulse(Config.HomeSpeed, out double homeSpeedPulse);
        //    MmToPulse(Config.MinSpeed, out double minSpeedPulse);
        //    MmToPulse(Config.StopSpeed, out double stopSpeedPulse);

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


        //    mc_pulse.GT_HomeInit((short)Config.CardId);
        //    mc_pulse.GT_Home(apiResult, (short)Config.AxisId, (int)searchPulse, fMaxVel, fAcc, 0);
        //    while(true)
        //    {
        //        mc_pulse.GT_HomeSts((short)Config.CardId,(short)Config.AxisId, out ushort pStatus);
        //        if(pStatus==1)
        //        {
        //            ClearAlarm();
        //            HomeStatus = EHomeStatus.Finished;
        //            break;
        //        }    
        //    }
        //}

        /// <summary>
        /// 
        /// </summary>
        /// <param name="obj"></param>
        protected void CaptureHome(object obj)
        {
            if(HomeJudge())
            {
                bool bIsStop = false;
                int searchDistance = 1000;//回原搜索距离
                double searchPulse = 0;
                double curPos = 0.0;
                //固高板块的回原点，可以用捕获回原，如果是步进电机采用直接回原的方式

                short pStatus = 0;
                int pValue = 0;
                if (Config.HomeDir == 0)
                {
                    searchDistance *= -1;
                }
                MmToPulse(searchDistance, out searchPulse);
                mc_pulse.GT_SetCaptureSense((short)Config.CardId, (short)Config.AxisId, mc_pulse.CAPTURE_HOME, (short)Config.HomeOrgLogic);
                //启动捕获回原
                mc_pulse.GT_SetEncPos(0, 1, 0);
                mc_pulse.GT_SetPrfPos(0, 1, 0);
                if (Config.EnableEncoder == 1)
                {
                    mc_pulse.GT_GetEncPos(0, 1, out curPos, 1, out uint pClock);
                }
                else
                {
                    mc_pulse.GT_GetPrfPos(0, 1, out curPos, 1, out uint pClock);
                }
                apiResult = mc_pulse.GT_SetCaptureMode((short)Config.CardId, (short)Config.AxisId, mc_pulse.CAPTURE_HOME);
                if (apiResult != 0)
                {
                    return;
                }

                double targetPos = curPos + searchPulse;
                MmToPulse(Config.HomeSpeed, out double homeSpeedPulse);
                MmToPulse(Config.MinSpeed, out double minSpeedPulse);
                MmToPulse(Config.StopSpeed, out double stopSpeedPulse);

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

                
                apiResult = mc_pulse.GT_PrfTrap((short)Config.CardId, (short)Config.AxisId);//设置运动模式为点位运动
                if (apiResult != 0)
                {
                    return;// ErrorCode.Fail;
                }
                apiResult = mc_pulse.GT_GetTrapPrm((short)Config.CardId, (short)Config.AxisId, out mc_pulse.TTrapPrm trapPrm);//获取点位运动参数
                if (apiResult != 0)
                {
                    return;// ErrorCode.Fail;
                }
                trapPrm.acc = fAcc;
                trapPrm.dec = fDec;
                trapPrm.velStart = fStart;
                apiResult = mc_pulse.GT_SetTrapPrm((short)Config.CardId, (short)Config.AxisId, ref trapPrm);//设置点位运动参数
                if (apiResult != 0)
                {
                    return;// ErrorCode.Fail;
                }
                apiResult = mc_pulse.GT_SetPos((short)Config.CardId, (short)Config.AxisId, (int)targetPos);//设置目标位置
                if (apiResult != 0)
                {
                    return;// ErrorCode.Fail;
                }
                apiResult = mc_pulse.GT_SetVel((short)Config.CardId, (short)Config.AxisId, fMaxVel);//设置目标速度
                if (apiResult != 0)
                {
                    return;// ErrorCode.Fail;
                }
                apiResult = mc_pulse.GT_Update((short)Config.CardId, (1 << Config.AxisId - 1));
                if (apiResult != 0)
                {
                    return;// ErrorCode.Fail;
                }
                do
                {
                    //获取捕获状态
                    apiResult = mc_pulse.GT_GetCaptureStatus((short)Config.CardId, (short)Config.AxisId, out pStatus, out pValue, 1, out uint pClock);
                    IsStop(out bIsStop);
                    if (bIsStop)
                    {
                        HomeStatus = EHomeStatus.Fail;
                        return;
                    }
                } while (pStatus == 0);

                MmToPulse(Config.HomeOffset, out double offsetPulse);
                targetPos = pValue + (int)offsetPulse;
                apiResult = mc_pulse.GT_SetPos((short)Config.CardId, (short)Config.AxisId, (int)targetPos);//设置目标位置
                apiResult = mc_pulse.GT_Update((short)Config.CardId, (1 << Config.AxisId - 1));
                do
                {
                    IsStop(out bIsStop);
                } while (!bIsStop && HomeStatus != EHomeStatus.Abort);
                Thread.Sleep(500);
                Zero(0);
                HomeStatus = EHomeStatus.Finished;
            }
        }

        protected bool HomeJudge()
        {
            int persent = (int)((Config.HomeSpeed / Config.MaxSpeed) * 100);
            if (persent <= 0)
                persent = 2;
            bool bIsOrg = false;
            bool bIsPel = false;
            bool bIsNel = false;
            int homeStep = 0;
            while(homeStep>=0 && HomeStatus!= EHomeStatus.Abort)
            {
                switch (homeStep)
                {
                    case 0://判断是否在原点，在原点，就先离开原点
                        GetOrgStatus(out bIsOrg);
                        if(bIsOrg)
                        {
                            //离开原点
                            if(Config.HomeDir==0)
                            {
                                MoveJog(1, persent);
                            }
                            else
                            {
                                MoveJog(0, persent);
                            }
                            do
                            {
                                GetOrgStatus(out bIsOrg);
                            } while (bIsOrg && HomeStatus!= EHomeStatus.Abort);
                            Stop();
                            Thread.Sleep(500);
                            homeStep = -1;
                        }
                        else
                            homeStep = 1;
                        break;
                    case 1://判断是否在正限位
                        GetPelStatus(out bIsPel);
                        if( bIsPel)
                        {
                            //先去找原点
                            MoveJog(0, persent);
                            do
                            {
                                GetOrgStatus(out bIsOrg);
                            } while (!bIsOrg && HomeStatus != EHomeStatus.Abort);
                            Stop();
                            ClearAlarm();
                            Thread.Sleep(100);
                            homeStep = 0;
                        }
                        else
                            homeStep = 2;
                        break;
                    case 2://判断是否在负限位
                        GetNelStatus(out bIsNel);
                        if( bIsNel )
                        {
                            MoveJog(1, persent);
                            do
                            {
                                GetOrgStatus(out bIsOrg);
                            } while (!bIsOrg && HomeStatus != EHomeStatus.Abort);
                            Stop();
                            ClearAlarm();
                            Thread.Sleep(100);
                            homeStep = 0;
                        }
                        homeStep = 3;
                        break;
                    case 3://不在原点，也不在限位上，先朝回原方向搜索
                        if(Config.HomeDir==0)
                        {
                            MoveJog(0, persent);
                            
                        }
                        else
                        {
                            MoveJog(1, persent);
                        }
                        //搜索的过程中，可能碰到原点，也可能碰到限位,碰到任何一个就停下来
                        do
                        {
                            GetOrgStatus(out bIsOrg);
                            if (bIsOrg)
                            {
                                homeStep = 0;
                            }
                            GetPelStatus(out bIsPel);
                            if (bIsPel)
                            {
                                homeStep = 1;
                            }
                            GetNelStatus(out bIsNel);
                            if (bIsNel)
                            {
                                homeStep = 2;
                            }
                        } while (!bIsOrg && !bIsPel && !bIsNel && HomeStatus != EHomeStatus.Abort);
                        Stop();
                        Thread.Sleep(100);
                        break;
                    default:
                        break;
                }
            }

            if(HomeStatus!= EHomeStatus.Abort)
            {
                return true;
            }
            return false;
        }

        /// <summary>
        /// 回原完成
        /// </summary>
        /// <param name="iOrig"></param>
        /// <returns></returns>
        public override ErrorCode Zero(int iOrig)
        {
            if (!cardManager.IsInitialized)
            {
                return ErrorCode.CardNotInit;
            }
            if(!IsInitialized)
            {
                return ErrorCode.AxisNotInit;
            }
            apiResult = LTDMC.dmc_set_position(Config.CardId, Config.AxisId, 0);
            if(apiResult!=0)
            {
                return ErrorCode.Fail;
            }
            if(Config.EnableEncoder==1)
            {
                apiResult = LTDMC.dmc_set_encoder(Config.CardId, Config.AxisId, 0);
                if (apiResult != 0)
                {
                    return ErrorCode.Fail;
                }
            }
            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)
        {
            Int32 remained_points = 0;
            Int32 current_point = 0;
            Int32 runned_points = 0;
            apiResult = LTDMC.dmc_hcmp_get_current_state(Config.CardId, channel, ref remained_points, ref current_point, ref runned_points);

            apiResult = LTDMC.dmc_hcmp_set_mode(Config.CardId, channel, 4);
            if(apiResult!=0)
            {
                LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareStop({(short)Config.CardId}, {(short)(channel - 1)});");
                return ErrorCode.Fail;
            }
            apiResult = LTDMC.dmc_hcmp_set_config(Config.CardId, channel, Config.AxisId, 1, 0, Config.HcmpPulseWidth);
            if(apiResult!=0)
            {
                LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareMode({(short)Config.CardId}, {(short)(channel - 1)}, mc_pulse.COMPARE2D_MODE_1D);");
                return ErrorCode.Fail;
            }
            apiResult = LTDMC.dmc_hcmp_clear_points(Config.CardId, channel);
            if (apiResult != 0)
            {
                LogHelper.Debug($"{apiResult} = LTDMC.dmc_hcmp_clear_points({Config.CardId}, {channel});");
                return ErrorCode.Fail;
            }

            foreach (double pos in postions)
            {
                double tarPos = 0.0;
                MmToPulse(pos, out tarPos);
                apiResult = LTDMC.dmc_hcmp_add_point(Config.CardId, channel, (int)tarPos);
                if (apiResult != 0)
                    return ErrorCode.Fail;
            }
            return ErrorCode.Ok;
        }

        public override ErrorCode CompareStatus(out short _pStatus,out int _pCount)
        {
            _pStatus = 0;
            _pCount = 0;
           
            return ErrorCode.Ok;
        }

        public override ErrorCode ComparePulse(ushort channel,bool dp=true)
        {

            apiResult = LTDMC.dmc_write_cmp_pin(Config.CardId, channel, 1);
            if (apiResult != 0)
            {
                LogHelper.Debug($"{apiResult} = LTDMC.dmc_write_cmp_pin({Config.CardId}, {channel}, 1);");
                return ErrorCode.Fail;
            }
            return ErrorCode.Ok;
        }

        public override ErrorCode ClearCompare(ushort uChannel)
        {
            apiResult = LTDMC.dmc_hcmp_set_mode(Config.CardId, uChannel, 0);
            if (apiResult != 0)
            {
                return ErrorCode.Fail;
            }
            else
            {
                apiResult = LTDMC.dmc_hcmp_clear_points(Config.CardId, uChannel);
                if (apiResult != 0)
                {
                    LogHelper.Debug($"{apiResult} = LTDMC.dmc_hcmp_clear_points({Config.CardId}, {uChannel});");
                    return ErrorCode.Fail;
                }
            }
            return ErrorCode.Ok;
        }

        public override ErrorCode ClearAlarm()
        {
            apiResult = LTDMC.dmc_write_erc_pin(Config.CardId, Config.AxisId, 1);
            if (apiResult != 0)
            {
                LogHelper.Debug($"{apiResult}= LTDMC.dmc_write_erc_pin({Config.CardId},{Config.AxisId}, 1);");
                return ErrorCode.Fail;
            }

            return ErrorCode.Ok;
        }

    }
}