#include "cmotordriver_base.h"
#include "logic_compdef.h"
#include "def_param_motor.h"
#include "def_param_driver.h"

CMotorDriver_Base::CMotorDriver_Base(const QString& name, int32_t id)
    :CMotorDriver(name, id)
{
    m_Variants.add_Variant(Var_Motor_Prg_Timeout,
                           QObject::trUtf8("PrgTimeout"), int(10), false,
                           Variant::TOXML);

    m_Variants.add_Variant(Var_Motor_GateBuffer_ID,
                           QObject::trUtf8("GateBufferId"), int(6), false,
                           Variant::TOXML);


}

int32_t CMotorDriver_Base::absmove(int32_t motorid, float64_t fpos, bool_t bwaitfinished/*=true*/)
{
    limit_checked(motorid, fpos);

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

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

    if (IsSimulator()) {
        CChannel* pMotor = FindChannel(motorid);
        Q_ASSERT(pMotor);

        pMotor->m_Variants.get_VData(Var_Motor_Pos_ID)->setValue(fpos);
    }
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_Base::relmove(int32_t motorid,float64_t fdis,bool_t bwaitfinished/*=true*/)
{
    double dTargetPos = fdis + get_posdata(motorid, true);
    return absmove(motorid, dTargetPos, bwaitfinished);
}

int32_t CMotorDriver_Base::freshpos(int32_t motorid)
{
    return RTN_SUCCESS1;
}

float64_t CMotorDriver_Base::freshAPos(int32_t motorid)
{
    CChannel* pMotor = FindChannel(motorid);
    Q_ASSERT(pMotor);

    double curPos = pMotor->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
    return curPos;
}

float64_t CMotorDriver_Base::freshFPos(int32_t motorid)
{
    return freshAPos(motorid);
}

float64_t CMotorDriver_Base::get_iodata_analog(int32_t cid, int32_t ioid, bool bfresh)
{
	if (bfresh) {
		freshio(cid, ioid);
	}

	CIO* cio = (CIO*)mchannels.value(cid);
	Q_ASSERT(cio);
	if (ioid < 0)return true;

	Q_ASSERT(ioid >= 0 && ioid < cio->m_iIOcnt);
	return cio->m_io[ioid].fdata;
}

int32_t CMotorDriver_Base::wait_move_finished_block(int32_t motorid, int iMS)
{
    //MSleep(10);
    return 0;
}

int32_t CMotorDriver_Base::wait_prg_finished_block(int32_t bufferid)
{
    //MSleep(10);
    return 0;
}

void CMotorDriver_Base::setHomeStatus(int32_t motorid, bool bhasHomed)
{
    CChannel* pMotor = FindChannel(motorid);
    Q_ASSERT(pMotor);
    pMotor->m_Variants.get_VData(Var_Motor_Hashome_ID)->setValue(bhasHomed);
}

int32_t CMotorDriver_Base::preparehome(int32_t motorid)
{
    //m_gSystem.PrepareHome(motorid);
    if (m_pfnPrepareHome) {
        m_pfnPrepareHome(motorid);
    }
    return 0;
}

int32_t CMotorDriver_Base::home(int32_t motorid, bool_t bwait/*=false*/)
{
    setHomeStatus(motorid, false);
    preparehome(motorid);
    MSleep(10);
    afterhome(motorid);
    return 0;
}

int32_t CMotorDriver_Base::afterhome(int32_t motorid)
{
    setHomeStatus(motorid, true);

    if (m_pfnAfterHome) {
        m_pfnAfterHome(motorid);
    }
    return 0;
}

int32_t CMotorDriver_Base::setio(int32_t cid, int32_t ioid, float64_t fdata)
{
    if (m_pfnIOAvaliable) {
        m_pfnIOAvaliable(ioid, fdata);
    }
    return CMotorDriver::setio(cid, ioid, fdata);
}


int CMotorDriver_Base::PEGMove_BuffGate(int32_t motorid, int32_t ioIdx, double dStartPos, double dEndPos, double* pegpos, int isize, bool bEnableLaser)
{
    return RTN_SUCCESS1;
}

int CMotorDriver_Base::LoadBufferGate(int32_t motorId, int32_t bufferIdx, int32_t gateIoIdx, double dStartPos, double dEndPos, bool bEnableLaser)
{
    return RTN_SUCCESS1;
}

int CMotorDriver_Base::MultiPointMove(int32_t motorid, double* pPoint, int iPointSize, BOOL bCyclic /*= TRUE*/, double* pVel /*= NULL*/)
{
    return RTN_SUCCESS1;
}

/*!
 * \brief set_cylinder
 * 设置气缸开并检查输入状态，检查iTimeOut ms
 * \param IOOutput
 * \param IOInput 检查输出是否为true
 * \param bset 设置当前值
 * \param iTimeOut
 * \return
 */
int32_t CMotorDriver_Base::set_cylinder(int32_t IOOutput, int32_t IOInput,
                                        bool_t bset, int iTimeOut)
{
    if (IsSimulator()) {
        setio(IO_OUTPUT, IOOutput, bset);
        MSleep(100);
        return RTN_SUCCESS1;
    }

    /*if (!get_iodata(IO_INPUT,IN_MAIN_PRESSURE,true))
    {
        MQT_THROW_STR(QObject::trUtf8("Main Press Light Off:%1")
                      .arg(IN_MAIN_PRESSURE));
    }*/

    if (m_pfnMainPresureDetected) {
        m_pfnMainPresureDetected();
    }

    if (m_pfnIOAvaliable) {
        m_pfnIOAvaliable(IOOutput, bset);
    }

    setio(IO_OUTPUT, IOOutput, bset);
    if (IOInput == -1) {
        return 0;
    }

    BOOL bTimeOut = TRUE;
    if (!iTimeOut) {
        iTimeOut = m_Variants.get_VData(Var_MotorDriver_TimeLimit_Cylinder_ID)
            ->toInt();
    }
    CPreciseTime Time;
    Time.StartTime();
    do {
        if (get_iodata(IO_INPUT, IOInput, true)) {
            bTimeOut = FALSE;
            break;
        }
        MSleep(10);
    } while ((Time.GetTime() * 1000) < iTimeOut);

    if (bTimeOut) {
        MQT_THROW_STR(QObject::trUtf8("Cylinder Time Out,"
                                      "Output(%1):%2,Input(%3):%4")
                      .arg(IOOutput)
                      .arg(getioname(IO_OUTPUT, IOOutput))
                      .arg(IOInput)
                      .arg(getioname(IO_INPUT, IOInput)));
    }
    return RTN_SUCCESS1;
}

int32_t CMotorDriver_Base::check_cylinder(int32_t IOOutput, int32_t IOInput, bool bset/*=true*/, int iTimeOut/*=0*/)
{
    BOOL bTimeOut = TRUE;
    if (!iTimeOut) {
        iTimeOut = m_Variants.get_VData(Var_MotorDriver_TimeLimit_Cylinder_ID)
            ->toInt();
    }
    CPreciseTime Time;
    Time.StartTime();
    do {
        if (get_iodata(IO_INPUT, IOInput, true)) {
            bTimeOut = FALSE;
            break;
        }
        MSleep(10);
    } while ((Time.GetTime() * 1000) < iTimeOut);

    if (bTimeOut) {
        MQT_THROW_STR(QObject::trUtf8("Cylinder Time Out,"
                                      "Output(%1):%2,Input(%3):%4")
                      .arg(IOOutput)
                      .arg(getioname(IO_OUTPUT, IOOutput))
                      .arg(IOInput)
                      .arg(getioname(IO_INPUT, IOInput)));
    }
    return 0;
}

int CMotorDriver_Base::cylindermove(int32_t IOOUTStart, int32_t IOINEnd,
                                    int32_t IOOutCtrl1, int32_t IOOutCtrl2,
                                    bool_t bCtrl1Set, bool_t bCtrl2Set,
                                    int32_t IOINAlm, int iTimeOut)
{
    DWORD dwLevelSleep = m_Variants.get_VData(Var_MotorDriver_Timewait_Cylinder_motor_ID)
        ->toInt();
    if (!iTimeOut) {
        iTimeOut = m_Variants.get_VData(Var_MotorDriver_Timelimit_Cylinder_motor_ID)
            ->toInt();
    }
    UNUSED(IOOutCtrl2); UNUSED(bCtrl2Set);
    setio(IO_OUTPUT, IOOutCtrl1, bCtrl1Set);
    //   setio(IO_OUTPUT,IOOutCtrl2,bCtrl2Set);
    setio(IO_OUTPUT, IOOUTStart, true);
    Sleep(dwLevelSleep);
    setio(IO_OUTPUT, IOOUTStart, false);
    //    setio(IO_OUTPUT,IOOutCtrl1,false);
    //    setio(IO_OUTPUT,IOOutCtrl2,false);

    CPreciseTime Time;
    Time.StartTime();
    BOOL bTimeOut = TRUE;
    do {
        if (get_iodata(IO_INPUT, IOINEnd, true)) {
            bTimeOut = FALSE;
            break;
        }
        if (!get_iodata(IO_INPUT, IOINAlm, true)) {
            MQT_THROW_STR(QObject::trUtf8("Cylinder Motor Move err: %1 Alarm")
                          .arg(getioname(IO_INPUT, IOINAlm)));
        }
        MSleep(10);
    } while ((Time.GetTime() * 1000) < iTimeOut);
    if (bTimeOut) {
        MQT_THROW_STR(QObject::trUtf8("Cylinder Motor Move err: Time out"));
    }
    Sleep(dwLevelSleep);
    return 0;
}

int CMotorDriver_Base::cylinderhome(int32_t IOOUTHome, int32_t IOINHome,
                                    int32_t IOINAlm, int iTimeOut)
{
    DWORD dwLevelSleep = m_Variants.get_VData(Var_MotorDriver_Timewait_Cylinder_motor_ID)
        ->toInt();
    if (!iTimeOut) {
        iTimeOut = m_Variants.get_VData(Var_MotorDriver_Timelimit_Cylinder_motor_ID)
            ->toInt();
    }
    setio(IO_OUTPUT, IOOUTHome, true);
    Sleep(dwLevelSleep);
    setio(IO_OUTPUT, IOOUTHome, FALSE);

    CPreciseTime Time;
    Time.StartTime();
    BOOL bTimeOut = TRUE;
    do {
        if (!get_iodata(IO_INPUT, IOINAlm, true)) {
            MQT_THROW_STR(QObject::trUtf8("Cylinder Motor Home err: %1 Alarm")
                          .arg(getioname(IO_INPUT, IOINAlm)));
        }
        if (get_iodata(IO_INPUT, IOINHome, true)) {

        } else {
            bTimeOut = FALSE;
            break;
        }
        MSleep(10);
    } while ((Time.GetTime() * 1000) < iTimeOut);
    if (bTimeOut) {
        MQT_THROW_STR(QObject::trUtf8("Cylinder Motor Home err: Time out"));

    }
    return 0;
}

void CMotorDriver_Base::limit_checked(int32_t motorId, double targetPos, bool bRealMove)
{
    CChannel* pMotor = FindChannel(motorId);
    Q_ASSERT(pMotor);

    if (bRealMove) {
        freshpos(motorId);
        targetPos += pMotor->m_Variants.get_VData(Var_Motor_Pos_ID)->toDouble();
    }

    double positivePos = pMotor->m_Variants.get_VData(Var_Motor_Pas_softlimit_ID)->toDouble();
    double negativePos = pMotor->m_Variants.get_VData(Var_Motor_Nag_softlimit_ID)->toDouble();
    if (!DEQUAL0(positivePos) && targetPos > positivePos) {
        MQT_THROW_STR(QObject::trUtf8("%1 Move to %2. Target pos exceed the positive limit.").arg(pMotor->m_sName).arg(targetPos));
    }

    if (!DEQUAL0(negativePos) && targetPos < negativePos) {
        MQT_THROW_STR(QObject::trUtf8("%1 Move to %2. Target pos exceed the negative limit.").arg(pMotor->m_sName).arg(targetPos));
    }

    //外部判断限位
    if (m_pfnLimitCheck) {
        m_pfnLimitCheck(motorId, targetPos);
    }
}
