#include "cmotordriver_gugao.h"
#include "cmotor_gugao.h"
#include "cmakepath.h"
#include "cmotor_gugao.h"
#include "logic_compdef.h"
#include "gts.h"
#include "gtgl500.h"
#include "logic_local_def.h"
#include "def_param_motor.h"

#define GuGaoFUN(f) do    \
{                \
    int irtn=(f);    \
    if(irtn)   \
    {MQT_THROW_STR(QObject::trUtf8("GuGao  Function %1 exe err,Err Id:%2")\
    .arg(#f).arg(irtn));}    \
    }while(0);

const unsigned int kDefaultGugaoCoreId = 1;
const unsigned int kExtendLinkId = 0;
enum EcatAxisMode {
    kHomeMode = 6,
    kMoveMode = 8,
};

enum EcatHomeStatus {
    kHomeFinished = 0b1, //! 回原点完成
    kHomeSuccess = 0b10,  //! 回原点成功,完成后才成功
    kHomeError = 0b100,
};

enum EcatAxisStatus {
    kRevesed = 0x01,
    kDriverAlarm = 0x02,  //! 驱动器告警
    kFlowError = 0x10,    //! 跟随误差错误
    kPassitiveLimit = 0x20, //! 正限位
    kNegativeLimit = 0x40,    //! 负限位
    kSmoothStop = 0x80,   //! 平滑停止
    kEmgStop = 0x100,     //! 急停
    kEnablePower = 0x200, //! 电机使能状态
    kPropMoveing = 0x400, //! 规划移动中
    kArrived = 0x800,     //! 到达指定位置
};

enum EcatHomeMode {
    kHomePassitive = 0,             //! 回原点 Mode1
    kHomeNegative = 1,
    kHomePassitiveAndOrigin = 2,      //! 回原点 Mode26
    kHomeNegativeAndOrigin = 3,
};
CMotorDriver_GUGAO::CMotorDriver_GUGAO(const QString& name, int32_t id)
    :CMotorDriver_Base(name, id)
{

}

int32_t CMotorDriver_GUGAO::init_driver()
{
    GuGaoFUN(GTN_Open(5, 1));
    //GuGaoFUN(GTN_Reset(kDefaultGugaoCoreId));

    GuGaoFUN(GTN_InitEcatComm(kDefaultGugaoCoreId));


    CPreciseTime timeSpan;
    timeSpan.StartTime();
    short iEcatStatus = 0;
    do {
        //0 == iEcatStatus : not connected
        //1 == iEcatStatus : connected
        GuGaoFUN(GTN_IsEcatReady(kDefaultGugaoCoreId, &iEcatStatus));


        MSleep(200);

        /*if(iEcatStatus != 1) {
            GuGaoFUN(GTN_InitEcatComm(kDefaultGugaoCoreId));
        }*/

    } while(timeSpan.GetTime() < 3000 && iEcatStatus != 1);

    if(iEcatStatus != 1) {
        MQT_THROW_STR(QObject::trUtf8("Ecat can't connected"));
    }

    GuGaoFUN(GTN_StartEcatComm(kDefaultGugaoCoreId));

    {
        QString strPath = CMakePath::make_bin_path(QString("gtn_core1.cfg"));
        QByteArray arrPath = strPath.toLocal8Bit();
        GuGaoFUN(GTN_LoadConfig(kDefaultGugaoCoreId, arrPath.data()));
    }

    GuGaoFUN(GTN_ClrSts(kDefaultGugaoCoreId, 1, 20));

    setdefaultspeed_all();

    GuGaoFUN(GTN_ClrSts(kDefaultGugaoCoreId, 1, 20));

    GuGaoFUN(GT_GLinkInit(kExtendLinkId));
    int irtn = CMotorDriver_Base::init_driver();


	for (auto iter = mchannels.begin(); iter != mchannels.end(); iter++)
	{
		CChannel* pchannel = (*iter);
		if (pchannel->m_itype != MODULE_MOTOR) {
			continue;
		}

        CMotor_GUGAO* pMotor = static_cast<CMotor_GUGAO*>(pchannel);
        int iMotorId = pMotor->m_iID;
        int iCore, iAxisNum;
        pMotor->SerialToAxisID(iCore, iAxisNum);
        long sts;
        GuGaoFUN(GTN_GetSts(iCore, iAxisNum, &sts));


        bool bHasEnabled = (sts & kEnablePower);
        if(bHasEnabled && (!(sts & kDriverAlarm))) {
            double dRefPos = freshAPos(pMotor->m_iID);
            if(dRefPos > 0.1 || dRefPos <= 0.1) {
                long lEcodValue = pMotor->MMToPlus(dRefPos);
                GuGaoFUN(GTN_SetEncPos(iCore, iAxisNum, lEcodValue));
                //设置已经回原点
                //TODO:调试模式时才使用
                pMotor->m_Variants.get_VData(Var_Motor_Hashome_ID)->setValue(true);
            }
        }

		setdefaultspeed(iMotorId);
    }

    return irtn;
}

int32_t CMotorDriver_GUGAO::exit_driver()
{
    stopmove_all();
    int irtn = CMotorDriver_Base::exit_driver();
    GuGaoFUN(GTN_Close());

    return irtn;
}

int32_t CMotorDriver_GUGAO::freshio(int32_t ioid, int32_t ioidx)
{
    try {
        if(ioidx < 0)return 0;
        CIO* pIO = (CIO*)FindChannel(ioid);
        Q_ASSERT(pIO);
        Q_ASSERT(pIO->m_itype != MODULE_MOTOR);
        int iCore = kDefaultGugaoCoreId;
        //模块编号
        int iModuleIndex = pIO->m_io[ioidx].ioIndex / 16;
        //IO 位
        int iIndex = pIO->m_io[ioidx].ioIndex % 16;
        //字节偏移
  //      int iByteOffset = iIndex / 8;
        //int iValueIndex = iIndex % 8;
        switch(ioid) {
        case IO_INPUT:
        {
            uint16_t iValue;
            GuGaoFUN(GT_GetGLinkDi(iModuleIndex, 0, (unsigned char*)&iValue, 2));
            bool bSet = (iValue >> iIndex) & 0x1;
            if(pIO->m_io[ioidx].bReversed) {
                bSet = !bSet;
            }
            pIO->m_io[ioidx].fdata = bSet ? 1 : 0;
        }
        break;

        case IO_OUTPUT:
        {
            uint16_t iValue;
            GuGaoFUN(GT_GetGLinkDo(iModuleIndex, 0, (unsigned char*)&iValue, 2));
            bool bSet = (iValue >> iIndex) & 0x1;
            if(pIO->m_io[ioidx].bReversed) {
                bSet = !bSet;
            }
            pIO->m_io[ioidx].fdata = bSet ? 1 : 0;
        }
        break;
        case IO_INPUT_ANALOG:
        {
            double dValue;
            GTN_GetAuAdc(iCore, iIndex, &dValue);
            pIO->m_io[ioidx].fdata = dValue;
        }
        break;
        case IO_OUTPUT_ANALOG:
        {
            short sValue;
            GTN_GetAuDac(iCore, iIndex, &sValue);
            double dValue = sValue / 32767.0 * 10.0;
            pIO->m_io[ioidx].fdata = dValue;
        }
        break;

        }
    } catch(QString& s) {
        MqDebug() << s;
        UNUSED(s);
    }
    return 0;
}

const int kDefaultIoCount = 32;
int32_t CMotorDriver_GUGAO::freshio_multi(int32_t ioid, int32_t idxstart,
                                          int32_t idxend)
{
    try {
        Q_ASSERT(idxend >= idxstart);
        CIO* pIO = (CIO*)FindChannel(ioid);
        Q_ASSERT(pIO);
        Q_ASSERT(pIO->m_itype != MODULE_MOTOR);
        int iCore = kDefaultGugaoCoreId;
        uint32_t slaveValue1;
        uint32_t slaveValue2;
        switch(ioid) {
        case IO_INPUT:
        {
            GuGaoFUN(GT_GetGLinkDi(0, 0, (unsigned char*)&slaveValue1, 2));
            GuGaoFUN(GT_GetGLinkDi(1, 0, (unsigned char*)&slaveValue2, 2));
        }
        break;

        case IO_OUTPUT:
        {
            GuGaoFUN(GT_GetGLinkDo(0, 0, (unsigned char*)&slaveValue1, 2));
            GuGaoFUN(GT_GetGLinkDo(1, 0, (unsigned char*)&slaveValue2, 2));

        }
        break;

        }

        uint32_t iValue = slaveValue1 | (slaveValue2 << 16);
        for(int i = 0; i < kDefaultIoCount; i++) {
            int iIndex = pIO->m_io[i].ioIndex;
            if(idxstart <= iIndex && iIndex <= idxend) {

                pIO->m_io[i].fdata = (iValue >> iIndex) & 0x1 ? 0 : 1;
            }
        }

    } catch(QString& s) {
        MqDebug() << s;
        UNUSED(s);
    }
    return 0;
}

int32_t CMotorDriver_GUGAO::setio(int32_t ioid, int32_t ioidx,
                                  float64_t fdata)
{
    if(m_bEMG) return 0;
    if(ioidx < 0)return 0;
    CIO* pIO = (CIO*)FindChannel(ioid);
    Q_ASSERT(pIO);
    Q_ASSERT(pIO->m_itype == MODULE_IO_OUT
             || pIO->m_itype == MODULE_IO_OUTANALOG);


    int iCore = kDefaultGugaoCoreId;
    //模块编号
    int iSlaveNo = pIO->m_io[ioidx].ioIndex / 16;
    //IO 位
    int iIndex = pIO->m_io[ioidx].ioIndex % 16;
    switch(ioid) {
    case IO_OUTPUT:
    {
        bool bSet = int(fdata) ? 1 : 0;
        if(pIO->m_io[ioidx].bReversed) {
            bSet = !bSet;
        }
        short value = bSet ? 1 : 0;
        GuGaoFUN(GT_SetGLinkDoBit(iSlaveNo, iIndex, value));
        pIO->m_io[ioidx].fdata = fdata;
        break;
    }
    case IO_OUTPUT_ANALOG:
    {
        double dvalue = fdata > 10 ? 10 : fdata;
        short sValue = dvalue / 10.0 * 32767;
        GuGaoFUN(GTN_SetAuDac(iCore, iIndex, &sValue));
        pIO->m_io[ioidx].fdata = dvalue;
    }
    break;
    }
    return 0;
}

int32_t CMotorDriver_GUGAO::freshpos(int32_t motorid)
{
    try {
        CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
        Q_ASSERT(pMotor);

        int m_iMotorStyle = pMotor->m_Variants.get_VData(Var_Motor_Type_ID)->toInt();
        double dValue;
        double dRefValue;
        switch(m_iMotorStyle % 10) {
        case STEPMOTOR:
            dValue = freshFPos(motorid);
            dRefValue = freshAPos(motorid);
            break;
        case SERVOMOTOR:
            dValue = freshAPos(motorid);
            dRefValue = dValue;
            break;
        default:
            dValue = freshFPos(motorid);
            break;
        }

    } catch(QString& s) {
        UNUSED(s);
    }
    return 0;
}

int32_t CMotorDriver_GUGAO::absmove(int32_t motorid,
                                    float64_t fpos, bool_t bwaitfinished)
{
    if(m_bEMG) return 0;

	//limit_checked(motorid, fpos);
    CMotorDriver_Base::absmove(motorid, fpos, bwaitfinished);

    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);

    /*if(m_pfnMoveAvaliable) {
        m_pfnMoveAvaliable(motorid, true, 1, 0, false);
    }*/

    //pMotor->m_Variants.get_VData()->toDouble();

    //运动结束，并且需要wait
    if(is_move_finished(motorid) || bwaitfinished) {
		//wait_equal(motorid);
        //pMotor->m_dSpdCur = (pMotor->m_Variants.get_VData(Var_Motor_Default_speed_ID)->toDouble());
        //pMotor->m_dSpdAccCur = pMotor->m_Variants.get_VData(Var_Motor_Default_accspd_ID)->toDouble();
        //pMotor->m_dSpdDecCur = pMotor->m_Variants.get_VData(Var_Motor_Default_decspd_ID)->toDouble();

		double dTargetPos = fpos;
        if(DEQUAL0(pMotor->m_dSpdAccCur)) {
            MQT_THROW_STR(QObject::trUtf8("Acc Speed Equal 0 "));
        }
        if(DEQUAL0(pMotor->m_dSpdDecCur)) {
            MQT_THROW_STR(QObject::trUtf8("Dec Speed Equal 0 "));
        }
        if(DEQUAL0(pMotor->m_dSpdCur)) {
            MQT_THROW_STR(QObject::trUtf8("Speed Equal 0 "));
        }
        double dSpdPluse = pMotor->Spd_MMSToPlusMS(pMotor->m_dSpdCur);

        double dAccSpdPluse = pMotor->AccSpd_MMS2ToPlusMS2(pMotor->m_dSpdAccCur);

        double dDecSpdPluse = pMotor->AccSpd_MMS2ToPlusMS2(pMotor->m_dSpdDecCur);

		bool bReverse = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool();
		if (bReverse) {
			fpos = -fpos;
		}
        double dPosPluse = pMotor->MMToPlus(fpos);

        //如果轴反向

        int iCore, iAxisNum;
        TTrapPrm trap;
        pMotor->SerialToAxisID(iCore, iAxisNum);

        //检查是否在点位运动模式
        unsigned short uirvMode;
        GuGaoFUN(GTN_GetEcatAxisMode(iCore, iAxisNum, &uirvMode));
        if(motorid == MOTORY) {
            GuGaoFUN(GTN_SetHomingMode(iCore, iAxisNum, kMoveMode));
        } else {
            if(uirvMode != kMoveMode) {
                MQT_THROW_STR(QObject::trUtf8("Motor is not in move mode, Reset Axis"));
            }
        }

        //long prfMode;
        //GuGaoFUN(GTN_GetPrfMode(iCore, iAxisNum, &prfMode));
        //if(prfMode != 0) {
        //    MQT_THROW_STR(QObject::trUtf8("Motor is not in prfmode, Reset Axis"));
        //}

        GuGaoFUN(GTN_ClrSts(iCore, iAxisNum));

        GuGaoFUN(GTN_PrfTrap(iCore, iAxisNum));

        //检查状态
        is_move_finished(motorid);

        GuGaoFUN(GTN_GetTrapPrm(iCore, iAxisNum, &trap));

        trap.acc = dAccSpdPluse;
        trap.dec = dDecSpdPluse;
        trap.smoothTime = 25;

        GuGaoFUN(GTN_SetTrapPrm(iCore, iAxisNum, &trap));

        GuGaoFUN(GTN_SetPos(iCore, iAxisNum, dPosPluse));

        GuGaoFUN(GTN_SetVel(iCore, iAxisNum, dSpdPluse));

        GuGaoFUN(GTN_Update(iCore, 1 << (iAxisNum - 1)));

        if(bwaitfinished) {
            wait_move_finished(motorid);

			wait_move_arrived(motorid, dTargetPos);
        }
    } else {
        if(bwaitfinished) {
            wait_move_finished(motorid);
        }

        wait_move_arrived(motorid, fpos);
    }
    return 0;
}

int32_t CMotorDriver_GUGAO::relmove(int32_t motorid,
                                    float64_t fdis, bool_t bwaitfinished)
{
    if(m_bEMG) return 0;
    CMotor_GUGAO* pMotor =(CMotor_GUGAO*) FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore,  iAxisNum;
    pMotor->SerialToAxisID( iCore,  iAxisNum);
    double dPosCur=get_posdata(motorid,true);
    dPosCur+=fdis;
    absmove(  motorid,   dPosCur,   bwaitfinished);
    return 0;
}

#if 0
int32_t CMotorDriver_GUGAO::startmove(int32_t motorid, float64_t dspd)
{
    if(m_bEMG) return 0;

    //m_gSystem.IsMotorMoveAvailable(motorid,true,dspd>0?1:-1);
    if(m_pfnMoveAvaliable) {
        m_pfnMoveAvaliable(motorid, true, dspd > 0 ? 1 : -1, 0, false);
    }

    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    pMotor->SerialToAxisID(iCore, iAxisNum);
    if(DEQUAL0(pMotor->m_dSpdAccCur)) {
        MQT_THROW_STR(QObject::trUtf8("Acc Speed Equal 0 "));
    }
    if(DEQUAL0(pMotor->m_dSpdDecCur)) {
        MQT_THROW_STR(QObject::trUtf8("Dec Speed Equal 0 "));
    }
    if(DEQUAL0(dspd)) {
        MQT_THROW_STR(QObject::trUtf8("Jog Speed Equal 0 "));
    }
    double dSpdPluse = pMotor->Spd_MMSToPlusMS(dspd);

    double dAccSpdPluse = pMotor->AccSpd_MMS2ToPlusMS2(pMotor->m_dSpdAccCur);

    double dDecSpdPluse = pMotor->AccSpd_MMS2ToPlusMS2(pMotor->m_dSpdDecCur);


    GuGaoFUN(GTN_PrfJog(iCore, iAxisNum));

    TJogPrm jog;
    GuGaoFUN(GTN_GetJogPrm(iCore, iAxisNum, &jog));

    jog.acc = dAccSpdPluse;
    jog.dec = dDecSpdPluse;

    GuGaoFUN(GTN_SetJogPrm(iCore, iAxisNum, &jog));

    GuGaoFUN(GTN_SetVel(iCore, iAxisNum, dSpdPluse));

    GuGaoFUN(GTN_Update(iCore, 1 << (iAxisNum - 1)));
    return 0;
}
#endif
int32_t CMotorDriver_GUGAO::stopmove(int32_t motorid)
{
    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    pMotor->SerialToAxisID(iCore, iAxisNum);
    GuGaoFUN(GTN_Stop(iCore, 1 << (iAxisNum - 1), 0));
    return 0;
}

int32_t CMotorDriver_GUGAO::stopmove_all()
{
    GuGaoFUN(GTN_Stop(kDefaultGugaoCoreId, 0xFFFFFFFF, 0));
//    GuGaoFUN(GTN_Stop(2,0xFFFFFFFF,0));
    return 0;
}

int32_t CMotorDriver_GUGAO::preparehome(int32_t motorid)
{
    if(m_bEMG) return 0;
    setHomeStatus(motorid, false);

	return CMotorDriver_Base::preparehome(motorid);
}

int32_t CMotorDriver_GUGAO::home(int32_t motorid, bool_t bwait)
{
    if(m_bEMG) return 0;
    preparehome(motorid);
    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    pMotor->SerialToAxisID(iCore, iAxisNum);


    stopmove(motorid);
    enable_axis(motorid, false);

    //try{
    //	GuGaoFUN(GTN_StopEcatHoming(iCore, iAxisNum));
    //} catch (const QString& ex) {
    //	;
    //}

    MSleep(100);
    enable_axis(motorid, true);
    MSleep(100);

    GuGaoFUN(GTN_AlarmOff(iCore, iAxisNum));

    int iHomeType = pMotor->m_Variants.get_VData(Var_Motor_Home_mode_ID)
        ->toUInt();

    int iActualHomeMode = 0;
    switch(iHomeType) {
    case kHomePassitive:
        iActualHomeMode = 1;
        break;
    case kHomeNegative:
        iActualHomeMode = 2;
        break;
    case kHomePassitiveAndOrigin:
        iActualHomeMode = 26;   //向正限位找到原点，然后回到原点后，偏出原点再回到原点
        break;
    case kHomeNegativeAndOrigin:
        iActualHomeMode = 27;   //向负限位找到原点，然后回到原点后，偏出原点再回到原点
        break;
    default:
        break;
    }

    if(iActualHomeMode == 0) {
        MQT_THROW_CODESTR(ALARM_TRANS_SAFE_PROTECT, QObject::trUtf8("%1 Home Mode Not set").arg(pMotor->m_sName));
    }

    GuGaoFUN(GTN_ClrSts(iCore, iAxisNum));
    GuGaoFUN(GTN_ZeroPos(iCore, iAxisNum));

    GTN_SetHomingMode(iCore, iAxisNum, kHomeMode);

    double dHomeSpd = pMotor->m_Variants.get_VData(Var_Motor_Home_speed_ID)->toDouble();
    double dHomeSpdLow = pMotor->m_Variants.get_VData(Var_Motor_Home_creep_ID)->toDouble();
    double dHomeAccSpd = pMotor->m_Variants.get_VData(Var_Motor_Home_accspeed_ID)
        ->toDouble();
    double dHomeDecSpd = pMotor->m_Variants.get_VData(Var_Motor_Home_decspeed_ID)
        ->toDouble();

    if(DEQUAL0(dHomeSpd)) {
        MQT_THROW_STR(QObject::trUtf8("Home Speed Equal 0 "));
    }

    if(DEQUAL0(dHomeSpdLow)) {
        MQT_THROW_STR(QObject::trUtf8("Home Creep Speed Equal 0 "));
    }

    if(DEQUAL0(dHomeAccSpd)) {
        MQT_THROW_STR(QObject::trUtf8("Home Acc Speed Equal 0 "));
    }

    if(DEQUAL0(dHomeDecSpd)) {
        MQT_THROW_STR(QObject::trUtf8("Home Dec Speed Equal 0 "));
    }

    double dHomeSpdPluse = pMotor->MMToPlus(dHomeSpd);
    double dHomeSpdLowPluse = pMotor->MMToPlus(dHomeSpdLow);

    double dHomeAccSpdPluse = pMotor->MMToPlus(dHomeAccSpd);
    double dHomeDecSpdPluse = pMotor->MMToPlus(dHomeDecSpd);

    double dHomeOffset = pMotor->m_Variants.get_VData(Var_Motor_Home_orginaloffset_ID)
        ->toDouble();

    double dHomeOffsetPluse = pMotor->MMToPlus(dHomeOffset);

    //设置Smart Home回原点参数
    //bool bReverse = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool();
    //if(bReverse) {
    //    dHomeSpdPluse = -dHomeSpdPluse;
    //    dHomeSpdLowPluse = -dHomeSpdLowPluse;
    //    dHomeAccSpdPluse = -dHomeAccSpdPluse;
    //    dHomeOffset = -dHomeOffset;
    //    dHomeOffsetPluse = -dHomeOffsetPluse;
    //}


    GuGaoFUN(GTN_SetEcatHomingPrm(iCore, iAxisNum, iActualHomeMode, dHomeSpdPluse, dHomeSpdLowPluse, dHomeAccSpdPluse, dHomeDecSpdPluse, dHomeOffset));


    // 回原点模式，参考下面的回原点模式宏定义
    GuGaoFUN(GTN_StartEcatHoming(iCore, iAxisNum));

    if(bwait) {
        try {
            wait_home_finished(motorid);
        } catch(const QString&) {
            GuGaoFUN(GTN_SetHomingMode(iCore, iAxisNum, kMoveMode));
            throw;
        }
        //
        GuGaoFUN(GTN_SetHomingMode(iCore, iAxisNum, kMoveMode));
    }


    return 0;
}

int32_t CMotorDriver_GUGAO::wait_home_finished(int32_t motorid)
{
    if(m_bEMG) return 0;
    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    pMotor->SerialToAxisID(iCore, iAxisNum);
    unsigned short iHomeSts = 0;
    CPreciseTime timeSpan;
    timeSpan.StartTime();
	do {
		//MSleep(10);
		long sts;
		GuGaoFUN(GTN_GetSts(iCore, iAxisNum, &sts));
		if (sts & (kDriverAlarm)) {
			MQT_THROW_STR(QObject::trUtf8("%1 Motor Alarm, Restart Machine")
						  .arg(pMotor->m_sName));
		}

		GuGaoFUN(GTN_GetEcatHomingStatus(iCore, iAxisNum, &iHomeSts));//获取回原点状态
		if (m_bEMG) {
			GuGaoFUN(GTN_StopEcatHoming(iCore, iAxisNum));
			return 0;
		}

		if (iHomeSts & (kHomeError)) {
			//回原发生错误
			MQT_THROW_STR(QObject::trUtf8("%1 Motor Home Error, Restart Machine")
						  .arg(pMotor->m_sName));
		}

		//回原超时15S
		if (timeSpan.GetTime() > 20) {
			GuGaoFUN(GTN_StopEcatHoming(iCore, iAxisNum));
			MQT_THROW_STR(QObject::trUtf8("%1 Motor Home Timeout, Restart Machine")
						  .arg(pMotor->m_sName));
			return 0;
		}

		//} while (iHomeSts != 3);
	} while (!(iHomeSts & kHomeFinished) || (!(iHomeSts & kHomeSuccess))); // 等待搜索原点停止

    timeSpan.ClearTime();
    do {
        GuGaoFUN(GTN_GetEcatHomingStatus(iCore, iAxisNum, &iHomeSts));//获取回原点状态
        if (m_bEMG) {
            GuGaoFUN(GTN_StopEcatHoming(iCore, iAxisNum));
            return 0;
        }
        if (timeSpan.GetTime() > 2) {
            break;
        }
    } while (!(iHomeSts == 0));

    MSleep(200);

    GuGaoFUN(GTN_ZeroPos(iCore, iAxisNum));

    setHomeStatus(motorid, true);

    //int iLimitMode = 0;
    //double dSoftLimitNag = pMotor->m_Variants.get_VData(Var_Motor_Nag_softlimit_ID)
    //    ->toDouble();

    //double dSoftLimitPas = pMotor->m_Variants.get_VData(Var_Motor_Pas_softlimit_ID)
    //    ->toDouble();
    //if(!DEQUAL0(dSoftLimitNag) || !DEQUAL0(dSoftLimitPas)) {
    //    GuGaoFUN(GTN_SetSoftLimit(iCore, iAxisNum,
    //                              pMotor->MMToPlus(dSoftLimitPas),
    //                              pMotor->MMToPlus(dSoftLimitNag)));
    //    iLimitMode = 1;
    //}
    //GuGaoFUN(GTN_LmtsOnEx(iCore, iAxisNum, -1, iLimitMode));

    afterhome(motorid);
    return 0;
}

int32_t CMotorDriver_GUGAO::setdefaultspeed(int32_t motorid)
{

    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    double dAccSpd = pMotor->m_Variants.get_VData(Var_Motor_Default_accspd_ID)->toDouble();
    double dDecSpd = pMotor->m_Variants.get_VData(Var_Motor_Default_decspd_ID)->toDouble();
    double dSpd = pMotor->m_Variants.get_VData(Var_Motor_Default_speed_ID)->toDouble();
    if(DEQUAL0(dAccSpd)) {
        MQT_THROW_STR(QObject::trUtf8("Default Acc Speed Equal 0 "));
    }
    if(DEQUAL0(dDecSpd)) {
        MQT_THROW_STR(QObject::trUtf8("Default Dec Speed Equal 0 "));
    }
    if(DEQUAL0(dSpd)) {
        MQT_THROW_STR(QObject::trUtf8("Default Speed Equal 0 "));
    }
    setspeed(motorid, dSpd, dAccSpd, dDecSpd, 0, 0);
    return 0;
}

int32_t CMotorDriver_GUGAO::setspeed(int32_t motorid, float64_t fspd, float64_t faccspd, float64_t fdecspd, float64_t fjerk, float64_t fkdec)
{
    UNUSED(fjerk);
    UNUSED(fkdec);
    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    if(!DEQUAL0(fspd)) {
        pMotor->m_dSpdCur = fspd;
        //pMotor->m_Variants.get_VData(Var_Motor_Default_speed_ID)->setValue(fspd);
    }
    if(!DEQUAL0(faccspd)) {
        pMotor->m_dSpdAccCur = faccspd;
        //pMotor->m_Variants.get_VData(Var_Motor_Default_accspd_ID)->setValue(faccspd);
    }
    if(!DEQUAL0(fdecspd)) {
        pMotor->m_dSpdDecCur = fdecspd;
        //pMotor->m_Variants.get_VData(Var_Motor_Default_decspd_ID)->setValue(fdecspd);
    }

    return 0;
}

int32_t CMotorDriver_GUGAO::emg_on(bool bon)
{
    if(bon) {
        stopmove_all();
    }
    return CMotorDriver::emg_on(bon);
}

int32_t CMotorDriver_GUGAO::wait_move_finished(int32_t motorid, bool bcheckLimit)
{
    if(RTN_SUCCESS1 == CMotorDriver::wait_move_finished(motorid, bcheckLimit)) {
        //规划位置
        CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
        Q_ASSERT(pMotor);
        /*int iCore, iAxisNum;
        double dValue = 0;
        pMotor->SerialToAxisID(iCore, iAxisNum);
        GuGaoFUN(GTN_GetPrfPos(iCore, iAxisNum, &dValue));*/

		double dPrfPos = freshAPos(motorid);

		//编码器位置
		double dPos = freshFPos(motorid);
		//double dPosErr = pMotor->m_Variants.get_VData(Var_Motor_Pos_Err)->toDouble();
        double dPosErr = 0.1;
		//等待整定完成
		int iTimeMs = m_Variants.get_VData(Var_MotorDriver_Peg_Timeout)->toInt();
		if (motorid == MOTORB1_LOCK) {
			iTimeMs *= 2;
		}
		CPreciseTime time;
		time.StartTime();
		while (time.GetTime() * 1000 < iTimeMs) {
			//检查位置是否准确
			MSleep(50);

			dPrfPos = freshAPos(motorid);
			dPos = freshFPos(motorid);

			if (fabs(dPos - dPrfPos) < dPosErr) {
				break;
			}
		}
		if (fabs(dPos - dPrfPos) > dPosErr) {
			MQT_THROW_STR(QObject::tr("Motor Move Finished, But Position Not Equal Ref:%1 Enc:%2").arg(dPos).arg(dPrfPos));
		}
    }

    return RTN_SUCCESS1;
}


bool_t CMotorDriver_GUGAO::is_move_finished(int32_t motorid)
{
    if(m_bEMG) {
        stopmove(motorid); 
    }

    try {
        CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
        Q_ASSERT(pMotor);
        int iCore, iAxisNum;
        pMotor->SerialToAxisID(iCore, iAxisNum);
        long sts;
        GuGaoFUN(GTN_GetSts(iCore, iAxisNum, &sts));
        bool bErr = true;
        if(sts & kDriverAlarm) { //Alarm Flag
            setHomeStatus(motorid, false);
            MQT_THROW_STR(QObject::trUtf8("%1 Motor Alarm, Restart Machine")
                          .arg(pMotor->m_sName));
        } else if(sts & kFlowError) { //跟随误差
            setHomeStatus(motorid, false);
            MQT_THROW_STR(QObject::trUtf8("%1 Motor Alarm, Flow Error, Reset Axis")
                          .arg(pMotor->m_sName));
        } else if(sts & kPassitiveLimit) {
            setHomeStatus(motorid, false);
            MQT_THROW_STR(QObject::trUtf8("%1 Motor Alarm, PassitiveLimit, Reset Axis")
                          .arg(pMotor->m_sName));
        } else if(sts & kNegativeLimit) {
            setHomeStatus(motorid, false);
            MQT_THROW_STR(QObject::trUtf8("%1 Motor Alarm, NegativeLimit, Reset Axis")
                          .arg(pMotor->m_sName));
        } else if(sts & kEmgStop) {
            MQT_THROW_STR(QObject::trUtf8("%1 Motor Alarm, Emergency Stop")
                          .arg(pMotor->m_sName));
        } else {
            bErr = false;
        }

        //处理夹抓防撞
        /*if(motorid == MOTORA_CLAW) {
            AssertCollisionDetection();
        }*/
        
        std::string sExceptionInfo;
        if (m_pfnMovingDetected != nullptr && m_pfnMovingDetected(motorid, sExceptionInfo)) {
            stopmove(motorid);
            setdefaultspeed(motorid);
            MQT_THROW_CODESTR(ALARM_TRANS_SAFE_PROTECT, sExceptionInfo.c_str());
        }

        return sts & kPropMoveing ? false : true;

    } catch(QString& s) {
        throw(s);
    }
    return true;
}

bool_t CMotorDriver_GUGAO::is_motor_onpaslimit(int32_t motorid)
{
    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    long sts;
    pMotor->SerialToAxisID(iCore, iAxisNum);
    GuGaoFUN(GTN_GetSts(iCore, iAxisNum, &sts));

    return sts & kPassitiveLimit;
}

bool_t CMotorDriver_GUGAO::is_motor_onnaglimit(int32_t motorid)
{
    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    short Value;
    pMotor->SerialToAxisID(iCore, iAxisNum);
    long sts;
    GuGaoFUN(GTN_GetSts(iCore, iAxisNum, &sts));

    return sts & kNegativeLimit;
}

void CMotorDriver_GUGAO::offsetHomePos(int32_t motorid)
{
    UNUSED(motorid);
}

void CMotorDriver_GUGAO::setHomeStatus(int32_t motorid, bool bhasHomed)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    pMotor->m_Variants.set_VData(Var_Motor_Hashome_ID, bhasHomed);
}

void CMotorDriver_GUGAO::freshHomeStatus(int32_t motorid)
{

    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    pMotor->SerialToAxisID(iCore, iAxisNum);
    long sts;
    GuGaoFUN(GTN_GetSts(iCore, iAxisNum, &sts));

    bool bHasHome = (!(sts & (kDriverAlarm))) && (sts & (kEnablePower));
    setHomeStatus(motorid, bHasHome);
}

int32_t CMotorDriver_GUGAO::enable_axis(int32_t motorid, bool_t bEnable)
{

    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    pMotor->SerialToAxisID(iCore, iAxisNum);

    if(bEnable) {
		GuGaoFUN(GTN_ClrSts(iCore, iAxisNum));
        GuGaoFUN(GTN_AxisOn(iCore, iAxisNum));
    } else {
        GuGaoFUN(GTN_AxisOff(iCore, iAxisNum));
    }
    return CMotorDriver_Base::enable_axis(motorid, bEnable);

}

float64_t CMotorDriver_GUGAO::freshAPos(int32_t motorid)
{
    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    double dValue = 0;
    pMotor->SerialToAxisID(iCore, iAxisNum);
    GuGaoFUN(GTN_GetPrfPos(iCore, iAxisNum, &dValue));
    double dPrfPos = pMotor->PlusToMM(dValue);
    bool bReverse = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool();
    if(bReverse) {
        dPrfPos = -dPrfPos;
    }

    //pMotor->m_Variants.get_VData(Var_Motor_REF_POS)->setValue(dPrfPos);
    return dPrfPos;
}

float64_t CMotorDriver_GUGAO::freshFPos(int32_t motorid)
{
    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    double dValue = 0;
    pMotor->SerialToAxisID(iCore, iAxisNum);
    GuGaoFUN(GTN_GetAxisEncPos(iCore, iAxisNum, &dValue));

    double dPrfPos = pMotor->PlusToMM(dValue);
    
    bool bReverse = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool();
    if(bReverse) {
        dPrfPos = -dPrfPos;
    }
    pMotor->m_Variants.get_VData(Var_Motor_Pos_ID)->setValue(dPrfPos);

    long sts;
    GuGaoFUN(GTN_GetSts(iCore, iAxisNum, &sts));
	//pMotor->m_Variants.get_VData(Var_Motor_Stats)->setValue((int)sts);
    return dPrfPos;
}

int32_t CMotorDriver_GUGAO::afterhome(int32_t motorid)
{
    //
    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    int iCore, iAxisNum;
    double dValue = 0;
    pMotor->SerialToAxisID(iCore, iAxisNum);

    GuGaoFUN(GTN_ClrSts(iCore, iAxisNum));

    GuGaoFUN(GTN_SetHomingMode(iCore, iAxisNum, kMoveMode));

    return CMotorDriver_Base::afterhome(motorid);
}

void CMotorDriver_GUGAO::wait_move_arrived(int32_t motorid, double dTargetPos)
{

    CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
    //double dPosErr = pMotor->m_Variants.get_VData(Var_Motor_Pos_Err)->toDouble();
     double dPosErr = 0.1;
	//dPosErr = 0.01;
    int iTimeMs = m_Variants.get_VData(Var_MotorDriver_Peg_Timeout)->toInt();
	if (motorid == MOTORB1_LOCK) {
		iTimeMs *= 2;
	}
    CPreciseTime timeSpan;
    timeSpan.StartTime();
    while(true) {
        MSleep(10);

		double dPos = get_posdata(motorid, true);
        if(fabs(dPos - dTargetPos) > dPosErr) {
            continue;
        }

        if(timeSpan.GetTime() * 1000 > iTimeMs) {
            MQT_THROW_CODESTR(ALARM_MOTION_ERROR, QObject::tr("motor %1 not arrived to (%2) timeout in %4 ms.Cur Pos(%3)").arg(pMotor->m_sName)
                              .arg(dTargetPos).arg(dPos).arg(timeSpan.GetTime() * 1000));
        }

        double dRefPos = pMotor->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
        if(fabs(dPos - dRefPos) > dPosErr) {
            MQT_THROW_CODESTR(ALARM_MOTION_ERROR, QObject::tr("motor %1 move arrived to (%2) PosErr.Ref:(%3) Enc:(%4)").arg(pMotor->m_sName)
                              .arg(dTargetPos).arg(dPos).arg(dRefPos));
        }

        break;
    }

}

void CMotorDriver_GUGAO::wait_equal(int32_t motorid)
{
	if (motorid == MOTORB1_LOCK)return;
	CMotor_GUGAO* pMotor = (CMotor_GUGAO*)FindChannel(motorid);
	double dPosErr = 0.003;
	//等待整定完成
	int iTimeMs = m_Variants.get_VData(Var_MotorDriver_Peg_Timeout)->toInt();

	double dPrfPos = freshAPos(motorid);
	double dPos = freshFPos(motorid);
	CPreciseTime time;
	time.StartTime();
	while (time.GetTime() * 1000 < iTimeMs) {
		//检查位置是否准确

		dPrfPos = freshAPos(motorid);
		dPos = freshFPos(motorid);

		if (fabs(dPos - dPrfPos) < dPosErr) {
			break;
		}
		MSleep(10);
	}
}

//void CMotorDriver_GUGAO::AssertCollisionDetection()
//{
//
//    int clawCheck = get_iodata(IO_INPUT, IN_CLAW_Collision_Detection_Push, true);
//    if(clawCheck == 0) {
//        stopmove(MOTORA_CLAW);
//        setdefaultspeed(MOTORA_CLAW);
//        MQT_THROW_CODESTR(ALARM_TRANS_SAFE_PROTECT, QObject::trUtf8("Glaw strike sensor triggerd"));
//    }
//}
