#include "cmotordriver_acs.h"
#include "logic_compdef.h"
#include "cmotor.h"
#include "def_param_driver.h"
#include "macro_def.h"
#include "logic_io_define.h"
#include "csystem_external.h"
#include <QDateTime>

#define COMMLENGTH 256
CMotorDriver_ACS::CMotorDriver_ACS(const QString& name, int32_t id)
    :CMotorDriver_Base(name, id)
{
}

CMotorDriver_ACS::~CMotorDriver_ACS()
{

}
void CMotorDriver_ACS::SendCommand(const char* pCommand, char* pResponse)
{
    QMutexLocker locker(&m_CardMutex);
    char  comm[256] = { 0 };
    strcpy(comm, pCommand);
    strcat(comm, "\r");
    int Received;
    if (!acsc_Transaction(m_AcsHandle, comm,
                          strlen(comm),
                          pResponse, RESPONSELENGTH - 1,
                          &Received, NULL)) {
        char  str2[256] = { 0 };
        // retrieve last error
        int iErrCode = acsc_GetLastError();
        acsc_GetErrorString(m_AcsHandle, iErrCode, str2, 256, &Received);
        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Electric;Code:%1;Command:%2;Message:%3")
                          .arg(iErrCode)
                          .arg(pCommand)
                          .arg(str2)
        );
    }
    //  QString str=pResponse;
    //  str.remove('\r');
    //  str.remove(':');
    //  str.remove(' ');
    //  memset(pResponse,0,RESPONSELENGTH);
    //  strcpy(pResponse,str.toLocal8Bit().data());
    char ch[RESPONSELENGTH] = { 0 };
    int iR = 0, ich = 0;
    while (pResponse[iR]) {
        if (pResponse[iR] != '\r' && pResponse[iR] != ':' &&
            pResponse[iR] != ' ') {
            ch[ich] = pResponse[iR];
            ich++;
        }
        iR++;
    }
    strcpy(pResponse, ch);
}

int32_t CMotorDriver_ACS::init_driver()
{
    QString sSerialID = m_Variants.get_VData(Var_Base_SerialID_ID)->toString();
    MqDebug() << sSerialID;
    m_AcsHandle = acsc_OpenCommEthernet(sSerialID.toLatin1().data(), ACSC_SOCKET_STREAM_PORT);
    if (m_AcsHandle == ACSC_INVALID) {
        acsc_CloseComm(m_AcsHandle);
        MQT_THROW_CODESTR(ALARM_MOTION_INIT, QObject::trUtf8
        ("ACS Communication link has not been established"));

    }
    acsc_OpenHistoryBuffer(m_AcsHandle, RESPONSELENGTH);
    //char ch_response[RESPONSELENGTH]={0};
    //TODO这里的作用不是很清楚
    //SendCommand("?IN0.0",ch_response);
    CHANNELMAP::iterator ii = mchannels.begin();
    for (; ii != mchannels.end(); ii++) {
        if ((*ii)->m_itype != MODULE_MOTOR)continue;
        setdefaultspeed(ii.key());
        freshHomeStatus(ii.key());
        is_motor_enabled(ii.key());
    }
    freshio_multi(IO_OUTPUT, 0, 63);
    freshio_multi(IO_INPUT, 0, 63);
    return CMotorDriver_Base::init_driver();
}

int32_t CMotorDriver_ACS::exit_driver()
{
    if (m_bInited) {
        acsc_CloseHistoryBuffer(m_AcsHandle);
        acsc_CloseComm(m_AcsHandle);
        CloseHandle(m_AcsHandle);
        m_AcsHandle = NULL;
    }
    return CMotorDriver::exit_driver();
}

int32_t CMotorDriver_ACS::freshio(int32_t ioid, int32_t ioidx)
{
    try {
        if (ioidx < 0)return RTN_SUCCESS1;
        CIO* pIO = (CIO*)FindChannel(ioid);
        Q_ASSERT(pIO);
        Q_ASSERT(pIO->m_itype != MODULE_MOTOR);

        int32_t ioActualIndex = pIO->m_io[ioidx].ioIndex;

        char comm[COMMLENGTH] = { 0 };
        int iwordIndex = ioActualIndex / 32;
        int ibitIndex = ioActualIndex % 32;
        switch (ioid) {
            case IO_INPUT:
                sprintf(comm, "?IN(%d).%d", iwordIndex, ibitIndex);
                break;
            case IO_OUTPUT:
                sprintf(comm, "?OUT(%d).%d", iwordIndex, ibitIndex);
                break;
            case IO_INPUT_ANALOG:
                sprintf(comm, "?AIN%d", ioActualIndex);
                break;
            case IO_OUTPUT_ANALOG:
                sprintf(comm, "?AOUT%d", ioActualIndex);
                break;
        }
        char ch_response[RESPONSELENGTH] = { 0 };
        SendCommand(comm, ch_response);
        if (ioid != IO_INPUT && ioid != IO_OUTPUT) {
            float fValue = atof(ch_response);
            fValue -= pIO->m_io[ioidx].fanalogOffset;
            fValue *= pIO->m_io[ioidx].fanalogUnit;

            pIO->m_io[ioidx].fdata = fValue;
        } else {
            //输入输出IO
            uint16_t iValue = atoi(ch_response);
            //pIO->m_io[ioidx].fdata=(value>>ibitIndex)&0x1;
            if (pIO->m_io[ioidx].bReversed) {
                //IO 反转
                pIO->m_io[ioidx].fdata = iValue > 0 ? 0 : 1;
            } else {
                pIO->m_io[ioidx].fdata = iValue;
            }

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

int32_t CMotorDriver_ACS::freshio_multi(int32_t ioid,
                                        int32_t idxstart,
                                        int32_t idxend)
{

    return CMotorDriver_Base::freshio_multi(ioid,
                                            idxstart,
                                            idxend);
}

int32_t CMotorDriver_ACS::setio(int32_t ioid, int32_t ioidx, float64_t fdata)
{
    if (m_bEMG) {
        //争停时如果是关闭激光则不处理急停
        //急停的情况下，需要能关掉激光
        if (m_pfnSetLaserOff != nullptr && m_pfnSetLaserOff(ioidx, fdata)) {
        } else {
            return 0;
        }
    }

    //m_gSystem.IsMotorIOSetAvailable(ioidx,DEQUAL0(fdata)?false:true);
    if (m_pfnIOAvaliable) {
        m_pfnIOAvaliable(ioidx, DEQUAL0(fdata) ? false : true);
    }

    if (ioidx < 0) {
        return RTN_SUCCESS1;
    }

    CIO* pIO = (CIO*)FindChannel(ioid);
    Q_ASSERT(pIO);
    Q_ASSERT(pIO->m_itype != MODULE_MOTOR);
    int32_t ioActualIndex = pIO->m_io[ioidx].ioIndex;
    char comm[COMMLENGTH] = { 0 };
    //IO 从IN(1)开始
    int iwordIndex = ioActualIndex / 32;
    int ibitIndex = ioActualIndex % 32;
    bool bset = (int)fdata ? true : false;
    switch (ioid) {
        case IO_OUTPUT:
        {
            //IO是否需要取反，默认为0
            if (pIO->m_io[ioidx].bReversed)
            {
                bset = !bset;
            }

            char ch_response[RESPONSELENGTH] = { 0 };
            sprintf(comm, "OUT(%d).%d=%d", iwordIndex, ibitIndex, bset ? 1 : 0);
            if (OUT_LASERGATE == ioidx) {
                //LOG_DEBUG(fmt::format("{} {}", "IO Set Laser Gate:", comm));
                _SysExt_Instance.AddProductInfo(PROCESS_CUT, fmt::format("{} {}", "IO Set Laser Gate:", comm).c_str());
            }

            //HACK:55也是激光
            if (ioActualIndex == 55 && OUT_LASERGATE != ioidx)
            {
                _SysExt_Instance.AddProductInfo(PROCESS_CUT, fmt::format("{} {}", "IO Set Laser Gate by other:", comm).c_str());
            }

            if (OUT_Shutter_All_Open == ioidx)
            {
                _SysExt_Instance.AddProductInfo(PROCESS_CUT, fmt::format("{} {}", "IO Set Shutter:", comm).c_str());
            }

            SendCommand(comm, ch_response);
        }
        break;
        case IO_OUTPUT_ANALOG:
            break;
        default:
            MQT_THROW_CODESTR(ALARM_RESERVED, "set io not support this type");
    }

    //更新IO值
    pIO->m_io[ioidx].fdata = fdata;

    //完全重载此函数
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_ACS::freshpos(int32_t motorid)
{
    double dPos = 0;
    try {
        CMotor* pMotor = (CMotor*)FindChannel(motorid);
        Q_ASSERT(pMotor);
        QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

        //    qDebug()<<pMotor->m_sName;
        int m_iMotorStyle = pMotor->m_Variants.get_VData(Var_Motor_Type_ID)->toInt();
        char comm[COMMLENGTH] = { 0 };
        switch (m_iMotorStyle % 10) {
            case STEPMOTOR://步进电机
                sprintf(comm, "?FPOS%s", sSerialID.toLatin1().data());
                break;
            case SERVOMOTOR://伺服电机
                sprintf(comm, "?APOS%s", sSerialID.toLatin1().data());
                break;
            case RLMOTOR:
                Q_ASSERT(0);
                break;
            default:
                Q_ASSERT(0);
                break;
        }

        if (STEPMOTOR == m_iMotorStyle % 10
            && acsc_GetFPosition(m_AcsHandle, sSerialID.toInt(), &dPos, NULL)) {
            //?FPOS精度不够，使用API精度更高
#ifdef __IO_214LOWK_CUT__
            if (/*MOTORG2_POWER == motorid || */MOTORG3_WIDEINDEX == motorid
                || MOTORG4_WIDE5050 == motorid || MOTORG5_NARROWINDEX == motorid
                || MOTORG6_NARROW5050 == motorid || MOTORG6_NARROW5050 == motorid) {
                //脉冲转角度

                dPos = dPos * 360 / 20000;
            }
#endif
        } else {
            char ch_response[RESPONSELENGTH] = { 0 };
            SendCommand(comm, ch_response);
            dPos = atof(ch_response);
        }

        //判断是否反向
        dPos = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool() ? -dPos : dPos;

        pMotor->m_Variants.get_VData(Var_Motor_Pos_ID)->setValue(dPos);
    } catch (QString& s) {
        UNUSED(s);
        //LOG(ERROR) << motorid << "fresh pos failed" <<endl;
    }
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_ACS::freshPos_multi(int32_t maxaxiss)
{
    UNUSED(maxaxiss);
    return CMotorDriver::freshPos_multi(maxaxiss);
}

float64_t CMotorDriver_ACS::freshAPos(int32_t motorid)
{
    double dPos = 0;
    try {
        CMotor* pMotor = (CMotor*)FindChannel(motorid);
        Q_ASSERT(pMotor);
        QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

        char comm[COMMLENGTH] = { 0 };
        sprintf(comm, "?APOS%s", sSerialID.toLatin1().data());
        char ch_response[RESPONSELENGTH] = { 0 };
        SendCommand(comm, ch_response);
        dPos = atof(ch_response);

        dPos = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool() ? dPos : -dPos;
    } catch (QString& s) {
        UNUSED(s);
    }
    return dPos;
}

float64_t CMotorDriver_ACS::freshFPos(int32_t motorid)
{
    double dPos = 0;
    try {
        CMotor* pMotor = (CMotor*)FindChannel(motorid);
        Q_ASSERT(pMotor);
        QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

        char comm[COMMLENGTH] = { 0 };
        sprintf(comm, "?FPOS%s", sSerialID.toLatin1().data());
        char ch_response[RESPONSELENGTH] = { 0 };
        SendCommand(comm, ch_response);
        dPos = atof(ch_response);

        //增加方向控制
        dPos = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool() ? dPos : -dPos;
    } catch (QString& s) {
        UNUSED(s);
    }
    return dPos;
}

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

    if (m_pfnMoveAvaliable) {
        m_pfnMoveAvaliable(motorid, true,
                           fpos > get_posdata(motorid, true) ? 1 : -1, fpos, false);
    }

    /*m_gSystem.IsMotorMoveAvailable(motorid,true,
                                   fpos>get_posdata(motorid,true)?1:-1, fpos);*/

    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    //    wait_move_finished(motorid,false);
    stopmove(motorid);
    QString sSerialID = pMotor->m_Variants.
        get_VData(Var_Base_SerialID_ID)->toString();

    /*int bufferId = sSerialID.toInt();
    if (MOTORX == motorid || MOTORY == motorid) {
        bufferId += 1;
    }*/
    uint bufferId = pMotor->m_Variants.get_VData(Var_Motor_Home_mode_ID)->toUInt();
    if (!is_prg_finished(bufferId)) {
        MQT_THROW_CODESTR(ALARM_MOTOR_MOVE_SAFER_PROTECT, QObject::trUtf8("motor card err.\n"
                                                                          "wait motor program:%1 finished before start another abs move")
                          .arg(sSerialID.toInt()));
    }

    char comm[COMMLENGTH] = { 0 };

    //增加方向控制
    fpos = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool() ? -fpos : fpos;

    //if (MOTORG2_POWER == motorid || MOTORG3_WIDEINDEX == motorid
    //    || MOTORG4_WIDE5050 == motorid || MOTORG5_NARROWINDEX == motorid
    //    || MOTORG6_NARROW5050 == motorid || MOTORG6_NARROW5050 == motorid) {
    //    //角度转脉冲
    //    fpos = (int)(fpos * 20000 / 360);
    //}

    sprintf(comm, "PTP %s,%f", sSerialID.toLatin1().data(), fpos);

    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    if (bwaitfinished) {
        wait_move_finished(motorid);
    }
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_ACS::relmove(int32_t motorid, float64_t fdis, bool_t bwaitfinished)
{
    if (m_bEMG) return 0;

    limit_checked(motorid, fdis, true);

	double dCurPos = get_posdata(motorid, true);
	dCurPos += fdis;
    //m_gSystem.IsMotorMoveAvailable(motorid,true, fdis>0?1:-1);
    if (m_pfnMoveAvaliable) {
        m_pfnMoveAvaliable(motorid, true, fdis > 0 ? 1 : -1, dCurPos, false);
    }

    stopmove(motorid);
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    wait_move_finished(motorid, false);
    QString sSerialID = pMotor->m_Variants.
        get_VData(Var_Base_SerialID_ID)->toString();

    if (!is_prg_finished(sSerialID.toInt())) {
        MQT_THROW_CODESTR(ALARM_MOTOR_MOVE_SAFER_PROTECT, QObject::trUtf8("motor card err.\n"
                                                                          "wait motor program:%1 finished before start another abs move")
                          .arg(sSerialID.toInt()));
    }

    //处理轴反向
    fdis = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool() ? -fdis : fdis;

    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "PTP/r %s,%f", sSerialID.toLatin1().data(), fdis);

    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    if (bwaitfinished) {
        wait_move_finished(motorid);
    }
    return RTN_SUCCESS1;

    //    if(m_bEMG) return 0;
    //    double dPos=get_posdata(motorid,true)+fdis;
    //    absmove(motorid,dPos,bwaitfinished);
    return 0;
}

int32_t CMotorDriver_ACS::absmove_direct(int32_t motorid, float64_t fpos, bool bwaitfinished/*=true*/)
{
    if (m_bEMG) return 0;
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(motorid == MOTORX || motorid == MOTORY);
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    if (IsSimulator()) {
        MSleep(10);
        pMotor->m_Variants.get_VData(Var_Motor_Pos_ID)->setValue(fpos);
        return RTN_SUCCESS1;
    }

    //处理轴反向
    fpos = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool() ? -fpos : fpos;

    bool ok = false;
    int axisId = sSerialID.toInt(&ok);
    Q_ASSERT(ok);
    if (!acsc_ToPoint(m_AcsHandle, 0, axisId, fpos, NULL)) {
        int Received;
        char  str2[256] = { 0 };
        // retrieve last error
        int iErrCode = acsc_GetLastError();
        acsc_GetErrorString(m_AcsHandle, iErrCode, str2, 256, &Received);
        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_ERROR, QObject::trUtf8("Electric;Code:%1;Command:%2;Message:%3")
                          .arg(iErrCode)
                          .arg("absmove_direct")
                          .arg(str2)
        );
    }
    if (true == bwaitfinished) {
        wait_move_finished_block(motorid, 30000);
    }

    return RTN_SUCCESS1;
}

//启动持续运动
int32_t CMotorDriver_ACS::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* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    if (DEQUAL0(dspd)) {
        //MQT_THROW_STR(QObject::trUtf8("%1 Jog move speed equal 0").arg(pMotor->m_sName));
    }
    float64_t dAccSpeed = pMotor->m_Variants.get_VData(Var_Motor_Default_accspd_ID)->toDouble();
    float64_t dDecSpeed = pMotor->m_Variants.get_VData(Var_Motor_Default_decspd_ID)->toDouble();

    if (MOTORG2_POWER == motorid || MOTORG3_WIDEINDEX == motorid
        || MOTORG4_WIDE5050 == motorid || MOTORG5_NARROWINDEX == motorid
        || MOTORG6_NARROW5050 == motorid || MOTORG6_NARROW5050 == motorid) {
        //角度转脉冲
        MQT_THROW_STR(QObject::tr("Not support start move with galss motor"));
    }

    char comm[COMMLENGTH] = { 0 };
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    sprintf(comm, "ACC(%s)=%f;DEC(%s)=%f;JOG/v (%s),%f",
            sSerialID.toLatin1().data(), dAccSpeed,
            sSerialID.toLatin1().data(), dDecSpeed,
            sSerialID.toLatin1().data(), dspd);
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_ACS::stopmove(int32_t motorid)
{
    try {
        CMotor* pMotor = (CMotor*)FindChannel(motorid);
        Q_ASSERT(pMotor);
        Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
        char comm[COMMLENGTH] = { 0 };
        QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

        /*sprintf(comm,"HALT %s",sSerialID.toLatin1().data());
        char ch_response[RESPONSELENGTH]={0};
        SendCommand(comm,ch_response);*/

        //使用指令直接停止
        bool ok = false;
        int motorId = sSerialID.toInt(&ok);
        Q_ASSERT(ok);
        int ret = acsc_Halt(m_AcsHandle, motorId, NULL);
        if (!ret) {
            MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Stop move failed with code:%1").arg(ret));
        }
    } catch (QString& s) {
        MqDebug() << s;
        return 0;
    }
    //   setdefaultspeed(motorid);
    return 0;
}

int32_t CMotorDriver_ACS::stopmove_all()
{
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand("HALT ALL", ch_response);
    setdefaultspeed_all();
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_ACS::abortprm(int32_t buffid)
{
    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "STOP %d", buffid);
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_ACS::abortprm_all()
{
    for (int i = 0; i < MAX_BUFF; i++) {
        if (i != RESERVE_BUFF) {
            abortprm(i);
        }
    }
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_ACS::preparehome(int32_t motorid)
{
    if (m_bEMG) return 0;
    UNUSED(motorid);
    //m_gSystem.PrepareHome(motorid);
    //m_gSystem.IsMotorMoveAvailable(motorid, false, 1, 0, true);
    if (m_pfnPrepareHome)
        m_pfnPrepareHome(motorid);
    return 0;
}

int32_t CMotorDriver_ACS::home(int32_t motorid, bool_t bwait)
{
    LOG_INFO("home motor {0}", motorid);

    if (m_bEMG) return 0;
    //m_gSystem.SafeDoorCheck();
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);

    setHomeStatus(motorid, false);
    preparehome(motorid);

    char comm[COMMLENGTH] = { 0 };
    //QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();
    //int bufferId = sSerialID.toInt();
    /*if (MOTORX == motorid || MOTORY == motorid)
    {
        bufferId += 1;
    }*/
    uint bufferId = pMotor->m_Variants.get_VData(Var_Motor_Home_mode_ID)->toUInt();
    LOG_INFO("home motor {0} ID:{1} Buffer:{2}", pMotor->m_sName.toLocal8Bit().data(), motorid, bufferId);
    Q_ASSERT(bufferId > 0 && bufferId < 32 && "home buffer id not config");
  
    sprintf(comm, "START %d,1", bufferId);
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);

    //LOG(INFO)<<pMotor->m_sName.toStdString().c_str()<<" Home";
    if (bwait) {
        wait_home_finished(motorid);
    }
    LOG_INFO("{0}  Home finished", pMotor->m_sName.toLocal8Bit().data());
    return 0;
}

int32_t CMotorDriver_ACS::wait_home_finished(int32_t motorid)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    QString sSerialID = pMotor->m_Variants.
        get_VData(Var_Base_SerialID_ID)->toString();

    /*int bufferId = sSerialID.toInt();
    if (MOTORX == motorid || MOTORY == motorid) {
        bufferId += 1;
    }*/
    uint bufferId = pMotor->m_Variants.get_VData(Var_Motor_Home_mode_ID)->toUInt();
    wait_prg_finished(bufferId);
    //   wait_move_finished(motorid);
    afterhome(motorid);
    return 0;
}

int32_t CMotorDriver_ACS::afterhome(int32_t motorid)
{
    setdefaultspeed(motorid);
    is_motor_enabled(motorid);
    //m_gSystem.AfterHome(motorid);
    if (m_pfnAfterHome) {
        m_pfnAfterHome(motorid);
    }

    //setHomeStatus(motorid,true);
    freshHomeStatus(motorid);
    return 0;
}

int32_t CMotorDriver_ACS::home_all(int32_t style)
{
    UNUSED(style);
    return MSG_FINISHED;
}

int32_t CMotorDriver_ACS::setdefaultspeed(int32_t motorid)
{
    /*设置默认速度无效return 0;*/
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);

    setspeed(motorid, pMotor->m_Variants.get_VData(Var_Motor_Default_speed_ID)->toDouble(),
             pMotor->m_Variants.get_VData(Var_Motor_Default_accspd_ID)->toDouble(),
             pMotor->m_Variants.get_VData(Var_Motor_Default_decspd_ID)->toDouble(),
             pMotor->m_Variants.get_VData(Var_Motor_Default_jerk_ID)->toDouble(),
             pMotor->m_Variants.get_VData(Var_Motor_Default_kdec_ID)->toDouble());

    return RTN_SUCCESS1;
}

int32_t CMotorDriver_ACS::setspeed(int32_t motorid, float64_t fspd, float64_t faccspd, float64_t fdecspd,
                                   float64_t fjerk, float64_t fkdec)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();
    QByteArray sArray = sSerialID.toLatin1();
    const char* sn = sArray.data();
    if (DEQUAL0(fspd)) {
        MQT_THROW_CODESTR(ALARM_MOTOR_MOVE_SAFER_PROTECT, QObject::trUtf8("%1 set speed   zero").arg(pMotor->m_sName));
    }
    if (DEQUAL0(faccspd)) {
        faccspd = pMotor->m_Variants.get_VData(Var_Motor_Default_accspd_ID)
            ->toDouble();
    }
    if (DEQUAL0(fdecspd)) {
        fdecspd = pMotor->m_Variants.get_VData(Var_Motor_Default_decspd_ID)
            ->toDouble();
    }
    if (DEQUAL0(fjerk)) {
        fjerk = pMotor->m_Variants.get_VData(Var_Motor_Default_jerk_ID)
            ->toDouble();
    }
    if (DEQUAL0(fkdec)) {
        fkdec = pMotor->m_Variants.get_VData(Var_Motor_Default_kdec_ID)
            ->toDouble();
    }
#ifdef __IO_214LOWK_CUT__
    if (/*MOTORG2_POWER == motorid || */MOTORG3_WIDEINDEX == motorid
        || MOTORG4_WIDE5050 == motorid || MOTORG5_NARROWINDEX == motorid
        || MOTORG6_NARROW5050 == motorid || MOTORG6_NARROW5050 == motorid) {
        //角度转脉冲
        fspd = (int)(fspd * 20000 / 360);
        faccspd = (int)(faccspd * 20000 / 360);;
        fdecspd = (int)(fdecspd * 20000 / 360);;
        fjerk = (int)(fjerk * 20000 / 360);;
        fkdec = (int)(fkdec * 20000 / 360);;
    }
#endif

    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "IMM VEL(%s)=%f;"
            "IMM ACC(%s)=%f;"
            "IMM DEC(%s)=%f;"
            "IMM JERK(%s)=%f;"
            "IMM KDEC(%s)=%f;",
            sn, fspd,
            sn, faccspd,
            sn, fdecspd,
            sn, fjerk,
            sn, fkdec);
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_ACS::emg_on(bool bon)
{
    if (!m_bInited) return 0;
    stopmove_all();
    abortprm_all();
    PegStop(MOTORX);
    return CMotorDriver::emg_on(bon);
}

int32_t CMotorDriver_ACS::wait_move_finished(int32_t motorid, bool bcheckLimit)
{
    CMotorDriver_Base::wait_move_finished(motorid, bcheckLimit);

    return 0;
}

int32_t CMotorDriver_ACS::wait_move_finished_block(int32_t motorid, int iMS)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);

    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();
    bool ok = false;
    int axisId = sSerialID.toInt(&ok);
    Q_ASSERT(ok);
    //if(!acsc_WaitLogicalMotionEnd(m_AcsHandle, axisId, iMS)){
    //	MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Wait motor %1 move end timeout").arg(pMotor->m_sName));
    //}

    CPreciseTime tm;
    tm.StartTime();
    do {
        int errCode;
        acsc_GetMotorError(m_AcsHandle, axisId, &errCode, ACSC_SYNCHRONOUS);
        if (errCode > 0) {
            char strErr[256];
            int receivedLen;
            if (acsc_GetErrorString(m_AcsHandle, errCode, strErr, 255, &receivedLen)) {
                strErr[receivedLen] = '\0';
                MQT_THROW_CODESTR(ALARM_MOTION_ERROR, (QString("Motor error: %1 (%2)").arg(errCode).arg(strErr)));
            }
        }

        if (tm.GetTime() * 1000 > iMS) {
            MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Wait motor %1 move end timeout").arg(pMotor->m_sName));
        }
    } while (!acsc_WaitLogicalMotionEnd(m_AcsHandle, axisId, 1000));

    return 0;
}

int32_t CMotorDriver_ACS::wait_prg_finished(int32_t buffid)
{
    DWORD dwTimeOut = m_Variants.get_VData(Var_MotorDriver_Peg_Timeout)->toInt();
    CPreciseTime tm;
    tm.StartTime();
    do {
        MSleep(10);

        if (tm.GetTime() > dwTimeOut) {
            MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Wait PRG Finish Time Out"));
        }
    } while (!is_prg_finished(buffid));

    return 0;
}

int32_t CMotorDriver_ACS::wait_prg_finished_block(int32_t bufferid)
{
    DWORD dwTimeOut = m_Variants.get_VData(Var_MotorDriver_Peg_Timeout)->toInt();
    if (!acsc_WaitProgramEnd(m_AcsHandle, bufferid, dwTimeOut * 1000)) {
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Wait PRG Finish Time Out. Buffer is %1").arg(bufferid));
    }

    return 0;
}

int32_t CMotorDriver_ACS::wait_prg_finished_all()
{
    for (int i = 0; i < MAX_BUFF; i++) {
        if (i != RESERVE_BUFF) {
            wait_prg_finished(i);
        }
    }
    return RTN_SUCCESS1;
}


bool_t CMotorDriver_ACS::is_prg_finished(int32_t buffid)
{
    if (m_bEMG) {
        StopBuff(buffid); return true;
    }
    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "?PST%d.1", buffid);
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    //   qDebug()<<comm<<ch_response;
    int iSet = atoi(ch_response);
    is_prg_err(buffid);

    return !iSet ? true : false;
}

bool_t CMotorDriver_ACS::is_move_finished(int32_t motorid)
{
    if (m_bEMG) {
        stopmove(motorid); return true;
    }
    //m_gSystem.IsMotorMoveAvailable(motorid);
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    int iSet;
    char comm[COMMLENGTH] = { 0 };
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    sprintf(comm, "?MST%s.5", sSerialID.toLatin1().data());
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    iSet = atoi(ch_response);

    //解析轴异常
    int errCode;
    int axisId = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toInt();
    acsc_GetMotorError(m_AcsHandle, axisId, &errCode, ACSC_SYNCHRONOUS);
    if (errCode != 0) {
        //LOG(ERROR)<< "MOTOR Err:"<<pMotor->m_sName.toStdString().c_str() << "Code:" << errCode;
        char strErr[256] = { 0 };
        int receivedLen;
        if (acsc_GetErrorString(m_AcsHandle, errCode, strErr, 255, &receivedLen)) {
            strErr[receivedLen] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_CANTMOVE_AXISERROR, QObject::trUtf8("%1 checking finished status failed.because motor error:%2 %3.")
                          .arg(pMotor->m_sName)
                          .arg(errCode)
                          .arg(strErr));
    }
    //处理防撞检测或其他检测
    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 !iSet ? true : false;
}

bool_t CMotorDriver_ACS::is_motor_enabled(int32_t motorid)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    int iSet;
    char comm[COMMLENGTH] = { 0 };
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    sprintf(comm, "?MST%s.0", sSerialID.toLatin1().data());
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    iSet = atoi(ch_response);
    bool b = iSet ? true : false;
    pMotor->m_Variants.get_VData(Var_Motor_Axis_Enabled)->setValue(b);
    return b;
}

bool_t CMotorDriver_ACS::is_motor_onpaslimit(int32_t motorid)
{
    if (motorid == MOTORE2_COAT_COAT_ARM)
        return false;

    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    char comm[COMMLENGTH] = { 0 };
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    sprintf(comm, "?FAULT%s.0", sSerialID.toLatin1().data());
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    int iSet = atoi(ch_response);
    return iSet ? true : false;
}

bool_t CMotorDriver_ACS::is_motor_onnaglimit(int32_t motorid)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    char comm[COMMLENGTH] = { 0 };
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    sprintf(comm, "?FAULT%s.1", sSerialID.toLatin1().data());
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    int iSet = atoi(ch_response);
    return iSet ? true : false;
}

int32_t CMotorDriver_ACS::get_prg_remain_size(int32_t buffid)
{
    UNUSED(buffid);
    return 0;
}

int32_t CMotorDriver_ACS::pause_prg(int32_t buffid, int iMode)
{

    UNUSED(buffid);
    UNUSED(iMode);
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_ACS::resume_prg(int32_t buffid)
{
    UNUSED(buffid);
    return RTN_SUCCESS1;

}

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

void CMotorDriver_ACS::setHomeStatus(int32_t motorid, bool bhasHomed)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    char comm[COMMLENGTH] = { 0 };
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();
    sprintf(comm, "AXIS_HOMEOK(%s)=%d",
            sSerialID.toLatin1().data(),
            bhasHomed);
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    pMotor->m_Variants.get_VData(Var_Motor_Hashome_ID)->setValue(bhasHomed);
}

void CMotorDriver_ACS::freshHomeStatus(int32_t motorid)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    char comm[COMMLENGTH] = { 0 };
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    sprintf(comm, "?AXIS_HOMEOK(%s)",
            sSerialID.toLatin1().data());
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    pMotor->m_Variants.get_VData(Var_Motor_Hashome_ID)->setValue(atoi(ch_response) ? true : false);
}

int32_t CMotorDriver_ACS::enable_axis(int32_t motorid, bool_t bEnable)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    char comm[COMMLENGTH] = { 0 };
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    if (bEnable) {
        sprintf(comm, "ENABLE %s", sSerialID.toLatin1().data());
    } else {
        sprintf(comm, "DISABLE %s", sSerialID.toLatin1().data());
    }
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    return CMotorDriver::enable_axis(motorid, bEnable);
}


int CMotorDriver_ACS::PEGMove(int32_t motorid, double dStartPos, double dEndPos, double* pegpos, int* state, int isize)
{
    if (m_bEMG) return 0;
    Q_ASSERT(pegpos);
    Q_ASSERT(state);
    if (isize > 1024) {
        isize = 1024;
    }
    Q_ASSERT(isize > 1);

    Q_ASSERT(motorid == MOTORX);
    absmove(motorid, dStartPos, TRUE);
    QMutexLocker locker(&m_CardMutex);

    int Received;
    if (!acsc_WriteReal(m_AcsHandle, ACSC_NONE, "Position",
                        0, isize - 1, ACSC_NONE, ACSC_NONE, pegpos, NULL)) {
        char  str2[256] = { 0 };
        acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Peg move err: %1 ")
                          .arg(str2));
    }
    if (!acsc_WriteInteger(m_AcsHandle, ACSC_NONE, "STAT",
                           0, isize - 1, ACSC_NONE, ACSC_NONE, state, NULL)) {
        char  str2[256] = { 0 };
        acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Peg move err: %1 ")
                          .arg(str2));
    }
    locker.unlock();
    PegStart(motorid, isize);
    WaitPegReady(motorid);
    absmove(motorid, dEndPos, FALSE);
    wait_move_finished(motorid);
    //		WaitForStatus(MOTORY,kill_deceleration);
    PegStop(motorid);
    //		acsc_StopPeg(m_AcsHandle,iMotorID, 	NULL );
    return RTN_SUCCESS1;
}

BOOL CMotorDriver_ACS::IsPEGREADY(int32_t motorid)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();
    //AST(AXIS0).#PEGREADY 查询轴的PEG状态是否为Ready

    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "?AST%s.4", sSerialID.toLatin1().data());
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    int iSet = atoi(ch_response);
    return iSet ? TRUE : FALSE;
}

int CMotorDriver_ACS::WaitPegReady(int32_t motorid)
{
    DWORD dwTimeOut = m_Variants.get_VData(Var_MotorDriver_Peg_Timeout)->toInt();
    CPreciseTime tm;
    tm.StartTime();

    while (!IsPEGREADY(motorid)) {
        MSleep(10);
        if (DWORD(tm.GetTime()) > dwTimeOut) {
            PegStop(motorid);
            MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Wait PEG Ready Time Out"));
        }
    }
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::PegStart(int32_t motorid, int isize)
{
    if (m_bEMG) return 0;
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);

    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "PEG_R %s,0.1,0x4444,0,%d-1,Position,STAT",
            sSerialID.toLatin1().data(), isize);
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::PegStop(int32_t motorid)
{
    QMutexLocker locker(&m_CardMutex);
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    int iMotorID = sSerialID.toInt();
    int Received;
    if (!acsc_StopPeg(m_AcsHandle, iMotorID, NULL)) {
        char  str2[256] = { 0 };
        acsc_GetErrorString(m_AcsHandle,
                            acsc_GetLastError(),
                            str2, 256, &Received);

        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Peg stop err: %1 ")
                          .arg(str2));
    }
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::PEGMove_BuffGate(int32_t motorid, int32_t gateIoIdx, double dStartPos, double dEndPos, double* pegpos, int isize, bool bEnableLaser)
{
    if (m_bEMG) {
        //LOG(ERROR)<<__FUNCTION__<<"EMG CANCED";
        return 0;
    }
    Q_ASSERT(pegpos);
    if (isize > 1024) {
        isize = 1024;
    }
    Q_ASSERT(isize == 2);

    CPreciseTime timeElpase;
    timeElpase.StartTime();
    CPreciseTime timeElpaseAll;
    timeElpaseAll.StartTime();
    Q_ASSERT(motorid == MOTORX || motorid == MOTORY);
    //如果轴不在起点位置，则移动到起点后再启动Gate，不然会乱开光
    float curPos = get_posdata(motorid, true);
    if (!INRANGE(dStartPos, curPos - 0.01, curPos + 0.01)) {
        //直接移动，不检查安全性
		if (motorid == MOTORY) {
			absmove_direct(motorid, dStartPos, false);
			wait_move_finished_block(motorid, 30000);
		}
		else {
			if (dStartPos > dEndPos) {
				if (curPos < dStartPos) {
					MqDebug() << "Logic buffer start pos error" << "Start:" << dStartPos << "End" << dEndPos << "CurPos" << curPos;
					std::stringstream stream;
					stream << "Logic buffer start pos error" << "Start:" << dStartPos << "End" << dEndPos << "CurPos" << curPos;
					LOG_ERR(stream.str());
					MQT_THROW_CODESTR(ALARM_MOTION_MOVE, "Logic buffer start pos error");
				}
			}
			else if(dStartPos < dEndPos){
				if (curPos > dStartPos) {
					MqDebug() << "Logic buffer start pos error" << "Start:" << dStartPos << "End" << dEndPos << "CurPos" << curPos;
					std::stringstream stream;
					stream << "Logic buffer start pos error" << "Start:" << dStartPos << "End" << dEndPos << "CurPos" << curPos;
					LOG_ERR(stream.str());
					MQT_THROW_CODESTR(ALARM_MOTION_MOVE, "Logic buffer start pos error");
				}
			}
		}
    }
    //double dKePos = get_posdata(MOTOR_SHUTTER, true);
    //LOG(INFO)<<"Before cut"<< " Shutter Pos:"<<dKePos;
    int bufferIdx = m_Variants.get_VData(Var_Motor_GateBuffer_ID)->toInt();

    //记录划线起始
    _SysExt_Instance.AddProductInfo(PROCESS_CUT, QString("\t\tStart Buffer Gate: Start:%1 End:%2 Cur:%3 Length:%4 at %5")
        .arg(dStartPos).arg(dEndPos).arg(curPos).arg(dStartPos-dEndPos).arg(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss zzz")));
        //LOG(INFO)<<"PEG::Move to Start Pos "<< dStartPos<<" CurPos:"<<curPos<<" TimeSpan:"<< timeElpase.GetTime()<<"S" << " Distance:"<<fabs(curPos-dStartPos);
    timeElpase.ClearTime();
    timeElpase.StartTime();
    //int ret = LoadBufferGate(motorid, bufferIdx, OUT_LASERGATE, pegpos[0], pegpos[1]);
    //int ret = LoadBufferGate(motorid, bufferIdx, OUT_LASERGATE, pegpos[0], pegpos[1], bEnableLaser);
    int ret = LoadBufferGate(motorid, bufferIdx, gateIoIdx, pegpos[0], pegpos[1], bEnableLaser);
    if (ret != RTN_SUCCESS1) {
        //LOG(ERROR)<<"LoadBufferGate Failed";
        MQT_THROW_CODESTR(ALARM_FALTAL_LASER_CUTPRG_LOADFAILED, QObject::tr("Laser cut program load failed"));
        return RTN_FAILED1;
    }
    //LOG(INFO)<<"LoadBufferGate Time:"<<timeElpase.GetTime()<<"S";
    timeElpase.ClearTime();
    timeElpase.StartTime();

    StartBuff(bufferIdx);
    is_prg_err(bufferIdx);

    //如果一开始就结束了说明buffer数据有异常，导致激光未出光
    if (TRUE == is_prg_finished(bufferIdx)) {
        //LOG(ERROR)<<"PEG Buffer early termination";
        MQT_THROW_CODESTR(ALARM_FALTAL_LASER_CUTPRG_EARLY_TERMINATION, QObject::tr("Laser cut program early termination"));
    }

    //LOG(INFO)<<"StartBuff Time:"<<timeElpase.GetTime()<<"S";
    timeElpase.ClearTime();
    timeElpase.StartTime();

    //运动到结束位置
    //absmove(motorid, dEndPos, FALSE);
    //检查光闸是否打开
	bool bShutterDown = get_iodata(IO_INPUT, IN_LASER_Shutter_All_Down, true);
	double dZPos = get_posdata(MOTORZ, true);
	double dG2Pos = get_posdata(MOTORG2_POWER, true);
	_SysExt_Instance.AddProductInfo(PROCESS_CUT, fmt::format("Before Cut:ZPos:{} PowerPos:{} ShutterDown:{}", dZPos, dG2Pos, bShutterDown).c_str());
	if (false == bShutterDown)
	{
		MQT_THROW_CODESTR(ALARM_FALTAL_LASER_ABNORMAL_SHUTDOWN, QObject::tr("Laser shutter not opened"));
	}

    absmove_direct(motorid, dEndPos, false);
    wait_move_finished_block(motorid, 30000);

    double dCurPos = get_posdata(motorid, true);
    //LOG(INFO)<<"absmove Time:"<<timeElpase.GetTime()<<"S" <<" TargetPos:"<<dEndPos<<" CurPos:"<<dCurPos;
    //dKePos = get_posdata(MOTOR_SHUTTER, true);
    //LOG(INFO)<<"After cut"<< " Shutter Pos:"<<dKePos;
    timeElpase.ClearTime();
    timeElpase.StartTime();

    //StopBuff(bufferIdx);

    wait_prg_finished_block(bufferIdx);

    double dRealStartPos = 0;
    double dRealEndPos = 0;
    double dCutLength = 0;
    int closeStatus = 1;
    ReadGlobalReal("RealTimePosStart", dRealStartPos);
    ReadGlobalReal("RealTimePosEnd", dRealEndPos);
    ReadGlobalReal("RealCutLength", dCutLength);
    ReadGlobalInt("G_LASERGATE_CloseStatus", closeStatus);
    if (bEnableLaser) {
        //检查激光触发是否正常
        int openStatus = 0;
        int openAfterStatus = 0;
        ReadGlobalInt("G_LASERGATE_OpenStatus", openStatus);
        ReadGlobalInt("G_LASERGATE_Status", openAfterStatus);

        //如果状态没有更新说明激光未出光
        if (openStatus == 0 || openAfterStatus == 0 || closeStatus == 1) {
            std::ostringstream stream;
            stream << "PEG Buffer Running Error." << "OpenStatus:"<<openStatus<<"CloseStatus:"<<closeStatus<<"OpenAfter:"<<openAfterStatus;
            LOG_ERR(stream.str());

            MQT_THROW_CODESTR(ALARM_FALTAL_LASER_ABNORMAL_SHUTDOWN, QObject::tr("Laser abnormal shutdown"));
        }
    }
    else
    {
        if (closeStatus == 1)
        {
            std::ostringstream stream;
            stream << "PEG Buffer Running Error." << "CloseStatus:" << closeStatus;
            LOG_ERR(stream.str());

            MQT_THROW_CODESTR(ALARM_FALTAL_LASER_ABNORMAL_SHUTDOWN, QObject::tr("Laser abnormal shutdown"));
        }
    }

	_SysExt_Instance.AddProductInfo(PROCESS_CUT, fmt::format("TargetGate:Start:{} End:{} Length:{}", pegpos[0], pegpos[0], pegpos[1]-pegpos[0]).c_str());
    _SysExt_Instance.AddProductInfo(PROCESS_CUT, fmt::format("RealCutPos:Start:{} End:{} Length:{}", dRealStartPos, dRealEndPos, dCutLength).c_str());

    if (false == get_iodata(IO_INPUT, IN_LASER_Shutter_All_Down, true))
    {
        MQT_THROW_CODESTR(ALARM_FALTAL_LASER_ABNORMAL_SHUTDOWN, QObject::tr("Laser shutter status abnormal"));
    }

    //LOG(INFO)<<"wait_prg_finished_block Time:"<<timeElpase.GetTime()<<"S" << " EnableLaser:"<< bEnableLaser;
    //LOG(INFO)<<"PEGMove_BuffGate Time:"<<timeElpaseAll.GetTime()<<"S";
    _SysExt_Instance.AddProductInfo(PROCESS_CUT, QString("\t\tEnd Buffer Gate: Start:%1 End:%2 Cur:%3 at %4 length:%5")
        .arg(dStartPos).arg(dEndPos).arg(curPos).arg(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss zzz")).arg(dEndPos - dStartPos));
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::LoadBufferGate(int32_t motorId, int32_t bufferIdx, int32_t gateIoIdx, double dStartPos, double dEndPos, bool bEnableLaser)
{
	
	CIO* pIO = (CIO*)FindChannel(IO_OUTPUT);
	Q_ASSERT(pIO);
	int32_t ioActualIndex = pIO->m_io[gateIoIdx].ioIndex;

    CPreciseTime timeElpase;
    timeElpase.StartTime();
    //X轴从左到右，原点在最左方 向右为正方向
    bool isLeftToRight = (dEndPos > dStartPos) ? true : false;//运动方向(true 是从左到右) 可以从左到右，也可以从右到左

    CMotor* pMotor = (CMotor*)FindChannel(motorId);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    double openPos = dStartPos;//打开光闸的点
    double closePos = dEndPos;//关闭光闸的点     

    //处理划片轴反向处理
    bool bMotorReverse = pMotor->m_Variants.get_VData(Var_Motor_Direction_Reverse_ID)->toBool();
    if (bMotorReverse)
    {
        isLeftToRight = !isLeftToRight;
        openPos *= -1;
        closePos *= -1;
    }


    int iMotorID = sSerialID.toInt();

    char buf[1024];
    strcpy(buf, "AUTOEXEC:\n");//buf 自动执行
    strcat(buf, "LOCAL int AXIS;\n");//变量定义
    strcat(buf, "LOCAL real OpenPOS,ClosePOS;\n");//变量定义
    strcat(buf, QString::asprintf("AXIS=%ld;\n", iMotorID).toLatin1().data());
    strcat(buf, QString::asprintf("OpenPOS=%.3f;\n", openPos).toLatin1().data());//范围起始坐标
    strcat(buf, QString::asprintf("ClosePOS=%.3f;\n", closePos).toLatin1().data());//范围结束坐标

    int iwordIndex = ioActualIndex / 32;
    int ibitIndex = ioActualIndex % 32;
    QString changeIOStr = QString::asprintf("OUT(%d).%d=%1\n", iwordIndex, ibitIndex);

    //用于反馈激光状态
    QString ioOpenStatus = QString::asprintf("G_LASERGATE_OpenStatus=OUT(%d).%d\n", iwordIndex, ibitIndex);
    QString ioCloseStatus = QString::asprintf("G_LASERGATE_CloseStatus=OUT(%d).%d\n", iwordIndex, ibitIndex);
    QString ioCurStatus = QString::asprintf("G_LASERGATE_Status=OUT(%d).%d\n", iwordIndex, ibitIndex);

    //激光是反向的
    int iCloseState = LASER_OFF;
    int iOpenState = LASER_ON;
	if (pIO->m_io[gateIoIdx].bReversed) {
		iCloseState = LASER_ON;
		iOpenState = LASER_OFF;
	}
    int iSet = iCloseState;
    if (bEnableLaser) {
        iSet = iOpenState;
    }
    //G_LASERGATE_Status 检查设置IO是否设置成功，先是0，后面变为1
    //G_LASERGATE_OpenStatus 检查激光过程中是否一直保持打开状态，先为0，后面变为1
    // 即初始状态都为关闭状态，执行完后变为打开状态
    //G_LASERGATE_CloseStatus 检查激光是否运行了关闭状态
    strcat(buf, "G_LASERGATE_Status=0\n");
    strcat(buf, "G_LASERGATE_OpenStatus=0\n");
    strcat(buf, "RealTimePosStart=0\n");
    strcat(buf, "RealTimePosEnd=0\n");//变量End
    strcat(buf, "RealCutLength=0\n");//变量Length
    if (isLeftToRight) {
        strcat(buf, "TILL FPOS(AXIS)>OpenPOS;\n");
        strcat(buf, "RealTimePosStart=FPOS(AXIS);\n");
        strcat(buf, changeIOStr.arg(iSet).toLatin1().data());
        strcat(buf, ioCurStatus.toLatin1().data());
        strcat(buf, "TILL FPOS(AXIS)>ClosePOS;\n");
        strcat(buf, "RealTimePosEnd=FPOS(AXIS);\n");
        strcat(buf, "RealCutLength=RealTimePosEnd-RealTimePosStart;\n");
        strcat(buf, ioOpenStatus.toLatin1().data());
    } else {
        strcat(buf, "TILL FPOS(AXIS)<OpenPOS;\n");
        strcat(buf, "RealTimePosStart=FPOS(AXIS);\n");
        strcat(buf, changeIOStr.arg(iSet).toLatin1().data());
        strcat(buf, ioCurStatus.toLatin1().data());
        strcat(buf, "TILL FPOS(AXIS)<ClosePOS;\n");
        strcat(buf, "RealTimePosEnd=FPOS(AXIS);\n");
        strcat(buf, "RealCutLength=RealTimePosEnd-RealTimePosStart;\n");
        strcat(buf, ioOpenStatus.toLatin1().data());
    }

    //结束时关闭激光，先为打开状态，后变为关闭状态
    strcat(buf, "G_LASERGATE_CloseStatus=1\n");
    strcat(buf, changeIOStr.arg(iCloseState).toLatin1().data());
    strcat(buf, ioCloseStatus.toLatin1().data());

    strcat(buf, "STOP\n");

#ifdef __DETAILED_LOG__
    LOG(INFO) << buf;
#endif // __DETAILED_LOG__
    //LOG(INFO)<<"Ready Load Buffer" << timeElpase.GetTime() << " S";
    timeElpase.ClearTime();
    timeElpase.StartTime();
    QMutexLocker lck(&m_CardMutex);
    if (!acsc_LoadBuffer(m_AcsHandle, bufferIdx, buf, strlen(buf), ACSC_SYNCHRONOUS)) {
        int Received;
        char  str2[256] = { 0 };
        // retrieve last error
        int iErrCode = acsc_GetLastError();
        acsc_GetErrorString(m_AcsHandle, iErrCode, str2, 256, &Received);
        if (Received) {
            str2[Received - 1] = '\0';
        }

        //LOG(ERROR) << "acsc_LoadBuffer Error:" << str2;
        return RTN_FAILED1;
    }
    //LOG(INFO)<<"acsc_LoadBuffer Time: " << timeElpase.GetTime() << " S";
    timeElpase.ClearTime();
    timeElpase.StartTime();
    if (!acsc_CompileBuffer(m_AcsHandle, bufferIdx, ACSC_SYNCHRONOUS)) {
        int Received;
        char  str2[256] = { 0 };
        // retrieve last error
        int iErrCode = acsc_GetLastError();
        acsc_GetErrorString(m_AcsHandle, iErrCode, str2, 256, &Received);
        if (Received) {
            str2[Received - 1] = '\0';
        }

        //LOG(ERROR) << "acsc_CompileBuffer Error:" << str2;
        return RTN_FAILED1;
    }
    //LOG(INFO)<<"acsc_CompileBuffer Time: " << timeElpase.GetTime() << " S";
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::CircleMove()
{
    int axes[] = { 2, 0, -1 };
    //上电
    //acsc_EnableM(m_AcsHandle, axes, NULL);

    double startPoint[2], center[2];//, FinalPoint[2], FinalPointArc2[2]; 
    //起点
    startPoint[0] = 441;
    startPoint[1] = 290;

    acsc_ToPointM(m_AcsHandle, 0, axes, startPoint, NULL);

    wait_move_finished(MOTORY);
    wait_move_finished(MOTORX);

    //没有到起点不能运动
    if (!acsc_ExtendedSegmentedMotionV2(m_AcsHandle, ACSC_AMF_VELOCITY, axes, startPoint,
                                        5000, ACSC_NONE, ACSC_NONE, ACSC_NONE, ACSC_NONE, ACSC_NONE, ACSC_NONE,
                                        ACSC_NONE, ACSC_NONE, NULL, ACSC_NONE, ACSC_NONE, ACSC_NONE, ACSC_NONE,
                                        ACSC_NONE, ACSC_NONE, ACSC_NONE, ACSC_SYNCHRONOUS)) {
        printf("transaction error: %d\n", acsc_GetLastError());
    }

    //移动到起点
    if (!acsc_SegmentLineV2(m_AcsHandle, // Communication handle 
                            ACSC_AMF_VELOCITY, // Velocity is specified
                            axes, // Axes 0 and 1 
                            startPoint, // Final point
                            200, // Vector velocity
                            ACSC_NONE, // End velocity is not specified
                            ACSC_NONE, // Time is not specified
                            NULL, // Values array is not specified
                            NULL, // Variables array is not specified
                            ACSC_NONE, // Index is not specified
                            NULL, // Masks array is not specified
                            ACSC_NONE, // external_loop_type
                            ACSC_NONE, // minimal_segment_length
                            ACSC_NONE, // maximum_allowed_deviation
                            ACSC_NONE, // Lci State
                            NULL // Waiting call
    )) {
        printf("transaction error: %d\n", acsc_GetLastError());
    }

    center[0] = 446;
    center[1] = 141;
    //NOTE:如果当前不在起始位置，这条语句会下电
    if (!acsc_SegmentArc2V2(m_AcsHandle,
                            ACSC_AMF_VELOCITY | ACSC_AMF_ENDVELOCITY,// Flags
                            axes,// Axes 
                            center,//Center
                            -3.141529 * 2,// Angle PI=180度 顺时针为负，以起点开始绘制圆
                            NULL,// FinalPoints 
                            100,// Velocity
                            100,// EndVelocity 
                            ACSC_NONE,// Time 
                            NULL,// Values 
                            NULL,// Variables
                            ACSC_NONE,// Index 
                            NULL,// Masks
                            ACSC_NONE, //ExtLoopType
                            ACSC_NONE, //MinSegemntlegth
                            ACSC_NONE, // maximum_allowed_deviation
                            ACSC_NONE, // LciState
                            NULL))// Wait
    {
        printf("transaction error: %d\n", acsc_GetLastError());
    }

    acsc_EndSequenceM(m_AcsHandle, axes, NULL);

    WaitSegmentFinshed(MOTORX);

    return 0;
}

int CMotorDriver_ACS::WaitSegmentFinshed(int32_t motorid)
{
    DWORD dwTimeOut = m_Variants.get_VData(Var_Motor_Prg_Timeout)->toInt();
    CPreciseTime tm;
    tm.StartTime();
    MSleep(10);
    while (!IsSegFinished(motorid)) {
        MSleep(10);
        if (DWORD(tm.GetTime()) > dwTimeOut * 1000) {
            MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Wait Segment Ready Time Out"));
        }
    }
    return RTN_SUCCESS1;
}

BOOL CMotorDriver_ACS::IsSegFinished(int32_t motorId)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorId);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();
    //GSEG(AXIS0) 查询轴的SEG状态是否为-1
    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "?GSEG(%s)", sSerialID.toLatin1().data());
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    int iSet = atoi(ch_response);
    return iSet == -1 ? TRUE : FALSE;
}

int CMotorDriver_ACS::MultAxisMultiMove(bool bCyclic)
{
    if (m_bEMG) return 0;

    if (m_pfnMoveAvaliable) {
        m_pfnMoveAvaliable(MOTORX, false, 1, 0, false);
        m_pfnMoveAvaliable(MOTORY, false, 1, 0, false);
        /*m_gSystem.IsMotorMoveAvailable(MOTORX);
        m_gSystem.IsMotorMoveAvailable(MOTORY);*/
    }

    QMutexLocker locker(&m_CardMutex);

    int Received;
    int iFlags = 0;
    if (bCyclic) {
        iFlags = ACSC_AMF_CYCLIC;
    }

    int axes[] = { 2,0,-1 };

    if (!acsc_MultiPointM(m_AcsHandle, iFlags, axes, 0, NULL)) {
        char  str2[256] = { 0 };
        acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Mutimove MultiPoint err: %1 ")
                          .arg(str2));
    }
    double ptX[4] = { 400, 400, 480, 480 };
    double ptY[4] = { 140, 180, 180, 140 };

    for (int i = 0; i < 4; i++) {

        double ptAxis[2] = { ptX[i], ptY[i] };
        if (!acsc_AddPointM(m_AcsHandle, axes, ptAxis, NULL)) {
            char  str2[256] = { 0 };
            acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

            if (Received) {
                str2[Received - 1] = '\0';
            }
            MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Mutimove AddPoint err: %1 ")
                              .arg(str2));
        }


    }

    if (!acsc_EndSequenceM(m_AcsHandle, axes, NULL)) {
        char  str2[256] = { 0 };
        acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Mutimove EndSequence err: %1 ")
                          .arg(str2));
    }

    wait_move_finished(MOTORY);
    wait_move_finished(MOTORX);
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::SegmentCircleMove(double startPosX, double startPosY, double endPosX, double endPosY, double centerX, double centerY, double angle, double speed)
{
    //从外面接近，绘制
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::WaitForStatus(int32_t motorid, MoveStatus status)
{
    while (!is_move_finished(motorid)) {
        MSleep(10);
        int status1 = GetMotorStatus(motorid);
        if (status1 >= status) {
            MqDebug() << "Motor Status" << status1;
            break;
        }
    }
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::GetMotorStatus(int32_t motorid)
{
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    Q_ASSERT(pMotor->m_itype == MODULE_MOTOR);
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();

    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "?GPHASE%s", sSerialID.toLatin1().data());
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    int iSet = atoi(ch_response);
    return iSet;
}

int CMotorDriver_ACS::WriteDouble(LPCTSTR lpArrayName, double* pArray, int iSize, int iBuff)
{
    QMutexLocker locker(&m_CardMutex);
    int Received;
    if (!acsc_WriteReal(m_AcsHandle, iBuff, (LPSTR)lpArrayName,
                        0, iSize - 1, ACSC_NONE, ACSC_NONE, pArray, NULL)) {
        char  str2[256] = { 0 };
        acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Peg write double  err: %1 ")
                          .arg(str2));
    }
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::WriteInt(LPCTSTR lpArrayName, int* pArray, int iSize, int iBuff)
{
    QMutexLocker locker(&m_CardMutex);
    int Received;
    if (!acsc_WriteInteger(m_AcsHandle, iBuff, (LPSTR)lpArrayName,
                           0, iSize - 1, ACSC_NONE, ACSC_NONE, pArray, NULL)) {
        char  str2[256] = { 0 };
        acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Peg write int  err: %1 ")
                          .arg(str2));
    }
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::StartBuff(int iBuffID)
{
    if (m_bEMG) return 0;
    if (is_prg_finished(iBuffID)) {
        char comm[COMMLENGTH] = { 0 };
        sprintf(comm, "START %d,1", iBuffID);
        char ch_response[RESPONSELENGTH] = { 0 };
        SendCommand(comm, ch_response);

        //LOG(INFO)<<"Start Buffer "<<iBuffID;
    } else {
        //LOG(INFO)<<"Start Buffer Ignore:" << iBuffID;
    }
    return RTN_SUCCESS1;
}

int CMotorDriver_ACS::StopBuff(int iBuffID)
{
    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "STOP %d", iBuffID);
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    return RTN_SUCCESS1;
}

/*!
 * \brief CMotorDriver_ACS::MultiPointMove
 * \param motorid 轴
 * \param pPoint 位置数据
 * \param iPointSize 点位数量
 * \param bCyclic 是否循环
 * \param pVel
 * \return
 */
int CMotorDriver_ACS::MultiPointMove(int32_t motorid, double* pPoint,
                                     int iPointSize, BOOL bCyclic,
                                     double* pVel)
{
    if (m_bEMG) return 0;

    if (IsSimulator()) {
        MSleep(100);
        return RTN_SUCCESS1;
    }

    //TODO:
    //if(motorid == MOTORE2_COAT_COAT_ARM){
    //	for (int i=0;i<iPointSize;i++)
    //	{
    //		m_gSystem.IsMotorMoveAvailable(MOTORE2_COAT_COAT_ARM, false,1,pPoint[i]);
    //	}
    //}
    //else
    //	m_gSystem.IsMotorMoveAvailable(motorid);

    QMutexLocker locker(&m_CardMutex);
    CMotor* pMotor = (CMotor*)FindChannel(motorid);
    Q_ASSERT(pMotor);
    QString sSerialID = pMotor->m_Variants.get_VData(Var_Base_SerialID_ID)->toString();
    int iMotorID = sSerialID.toInt();
    int Received;
    int iFlags = 0;
    if (bCyclic) {
        iFlags = ACSC_AMF_CYCLIC;
    }
    if (pVel) {
        iFlags |= ACSC_AMF_VELOCITY;
        for (int i = 0; i < iPointSize; i++) {
            if (DEQUAL0(pVel[i])) {
                MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Mutimove Speed is Zero. "));
            }
        }
    }
    if (!acsc_MultiPoint(m_AcsHandle, iFlags, iMotorID, 0, NULL)) {
        char  str2[256] = { 0 };
        acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Mutimove MultiPoint err: %1 ")
                          .arg(str2));
    }
    for (int i = 0; i < iPointSize; i++) {

        if (pVel) {
            if (!acsc_ExtAddPoint(m_AcsHandle, iMotorID, pPoint[i], pVel[i], NULL)) {
                char  str2[256] = { 0 };
                acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

                if (Received) {
                    str2[Received - 1] = '\0';
                }
                MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Mutimove ExtAddPoint err: %1 ")
                                  .arg(str2));
            }
        } else {
            if (!acsc_AddPoint(m_AcsHandle, iMotorID, pPoint[i], NULL)) {
                char  str2[256] = { 0 };
                acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

                if (Received) {
                    str2[Received - 1] = '\0';
                }
                MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Mutimove AddPoint err: %1 ")
                                  .arg(str2));
            }

        }
    }

    if (!acsc_EndSequence(m_AcsHandle, iMotorID, NULL)) {
        char  str2[256] = { 0 };
        acsc_GetErrorString(m_AcsHandle, acsc_GetLastError(), str2, 256, &Received);

        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Mutimove EndSequence err: %1 ")
                          .arg(str2));
    }
    return RTN_SUCCESS1;
}

bool CMotorDriver_ACS::is_prg_err(int32_t buffid)
{
    QMutexLocker locker(&m_CardMutex);
    int iErr;
    if (!acsc_GetProgramError(m_AcsHandle, buffid, &iErr, 0)) {

        MQT_THROW_CODESTR(ALARM_MOTION_STATE, QObject::trUtf8("acsc_GetProgramError failed,buff ID:%1  ")
                          .arg(buffid));
    }
    if (iErr) {
        int Received = 0;
        char  str2[256] = { 0 };
        acsc_GetErrorString(m_AcsHandle, iErr, str2, 256, &Received);
        if (Received) {
            str2[Received - 1] = '\0';
        }
        MQT_THROW_CODESTR(ALARM_PROGRAM_ERROR, QObject::trUtf8("Electric buff err;Code:%1;Message:%2")
                          .arg(iErr)
                          .arg(str2)
        );
    }
    return false;
}

void CMotorDriver_ACS::ReadGlobalInt(const char* varName, int& iValue)
{
    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "?(%s)", varName);
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    iValue = atoi(ch_response);
}

void CMotorDriver_ACS::ReadGlobalReal(const char* varName, double& fValue)
{
    char comm[COMMLENGTH] = { 0 };
    sprintf(comm, "?(%s)", varName);
    char ch_response[RESPONSELENGTH] = { 0 };
    SendCommand(comm, ch_response);
    fValue = atof(ch_response);
}

//加载光闸控制buf (并且执行)(非退火过程中控制)
int CMotorDriver_ACS::LoadMachineGateCtrlBuf(bool bOn)
{
    int bufferId = 1;
    int axisId = 2; //X轴
    char buf[1024] = { 0 };

    strcpy(buf, "INT iAxis\n");
    strcat(buf, "REAL Move_velocity,Delay,Coord\n");
    strcat(buf, QString::asprintf("iAxis =%ld\n", axisId).toLatin1().data());
    int motorSpeed = 100;
    strcat(buf, QString::asprintf("Move_velocity = %ld\n", motorSpeed).toLatin1().data());//速度

    int Coord = 100;
    strcat(buf, QString::asprintf("Coord = %ld\n", Coord).toLatin1().data());
    strcat(buf, "PTP/ev(iAxis),Coord, Move_velocity\n");//移动到绝对坐标
    strcat(buf, "WAIT 1000\n");//延时ms
    strcat(buf, "STOP\n");

    if (!acsc_LoadBuffer(m_AcsHandle, bufferId, buf, strlen(buf), ACSC_SYNCHRONOUS)) {
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Buffer load err: %1  "));
    }
    if (!acsc_RunBuffer(m_AcsHandle, bufferId, NULL, ACSC_SYNCHRONOUS)) {
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Buffer run err: %1 "));
    }
    //等待buf执行
    Sleep(1500);
    return RTN_SUCCESS1;
}

//加载运动中IO 控制的buf (StartPOS < EndPOS)(StopPOS是Axis 运动结束坐标)
int CMotorDriver_ACS::LoadIOGateCtrlBuf_TILL_cmd(int BufIdx, int IoPortBit, double StartPOS, double EndPOS, double StopPOS)
{
    bool ScanDir = (StopPOS > EndPOS) ? true : false;//运动方向(true 是从左到右)
    double OpenPOS = (ScanDir) ? StartPOS : EndPOS;//打开光闸的点
    double ClosePOS = (ScanDir) ? EndPOS : StartPOS;//关闭光闸的点     
    int AsixIdx = 2;
    int WAsixIdx = 1; //光闸轴

    char buf[1024];
    strcpy(buf, "AUTOEXEC:\n");//buf 自动执行
    strcat(buf, "LOCAL int AXIS;\n");//变量定义
    strcat(buf, "LOCAL real OpenPOS,ClosePOS;\n");//变量定义
    strcat(buf, QString::asprintf("AXIS=%ld;\n", AsixIdx).toLatin1().data());
    strcat(buf, QString::asprintf("OpenPOS=%.3f;\n", OpenPOS).toLatin1().data());//范围起始坐标
    strcat(buf, QString::asprintf("ClosePOS=%.3f;\n", ClosePOS).toLatin1().data());//范围结束坐标
    int MachineGateMoveVal = 100;
    int MotorWSpeed = 100;

    strcat(buf, "TILL RPOS(AXIS)<OpenPOS;\n");
    strcat(buf, QString::asprintf("PTP/ev(%d),%d, %d\n", WAsixIdx, 0, MotorWSpeed).toLatin1().data());
    strcat(buf, "TILL RPOS(AXIS)<ClosePOS;\n");
    strcat(buf, QString::asprintf("PTP/ev(%d),%d, %d\n", WAsixIdx, MachineGateMoveVal, MotorWSpeed).toLatin1().data());

    strcat(buf, "STOP\n");

    if (!acsc_LoadBuffer(m_AcsHandle, BufIdx, buf, strlen(buf), ACSC_SYNCHRONOUS)) {
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Buffer load err: %1  "));
    }

    if (!acsc_RunBuffer(m_AcsHandle, BufIdx, NULL, ACSC_SYNCHRONOUS)) {
        MQT_THROW_CODESTR(ALARM_MOTION_MOVE, QObject::trUtf8("Buffer load err: %1  "));
    }
    return RTN_SUCCESS1;
}
