#include "cmotor_gugao.h"

CMotor_GUGAO::CMotor_GUGAO(const QString &name, int32_t id)
    :CMotor(name,id)
{
    m_dSpdCur=0;
    m_dSpdAccCur=0;
    m_dSpdDecCur=0;
}

void CMotor_GUGAO::SerialToAxisID(int &iCore, int &iAxisNum)
{
    int iSerialID=m_Variants.get_VData(Var_Base_SerialID_ID)->toInt();
    iCore=iSerialID/100+1;
    iAxisNum=iSerialID%100;
}

double CMotor_GUGAO::PlusToMM(double dPlus)
{
    double dPulseEquivalent= m_Variants.
            get_VData(Var_Motor_PulseEquivalent_ID)->toDouble();
    if(DEQUAL0(dPulseEquivalent))
    {
        MQT_THROW_STR(QObject::trUtf8("Motor PulseEquivalent Equal 0"));
    }
    return dPlus/dPulseEquivalent;
}

double CMotor_GUGAO::MMToPlus(double dMM)
{
    double dPulseEquivalent= m_Variants.
            get_VData(Var_Motor_PulseEquivalent_ID)->toDouble();
    if(DEQUAL0(dPulseEquivalent))
    {
        MQT_THROW_STR(QObject::trUtf8("Motor PulseEquivalent Equal 0"));
    }

    return dMM*dPulseEquivalent;
}

double CMotor_GUGAO::Spd_PlusMSToMMS(double dPlusMS)
{
    return PlusToMM(dPlusMS)*1000.0;
}

double CMotor_GUGAO::Spd_MMSToPlusMS(double dMMS)
{
    return MMToPlus(dMMS)/1000.0;
}

double CMotor_GUGAO::AccSpd_PlusMS2ToMMS2(double dPlusMS2)
{
    return PlusToMM(dPlusMS2)*1000.0*1000.0;
}

double CMotor_GUGAO::AccSpd_MMS2ToPlusMS2(double dMMS2)
{
    return MMToPlus(dMMS2)/(1000.0*1000.0);
}

int32_t CMotor_GUGAO::init_channel()
{
    int iCore,  iAxisNum;
    SerialToAxisID( iCore,  iAxisNum);

    int iMotorType=m_Variants.get_VData(Var_Motor_Type_ID)
            ->toUInt();
    switch(iMotorType)
    {
    case  STEPMOTOR:
        //GuGaoFUN(GTN_CtrlMode(iCore, iAxisNum,1));
        break;
    case  SERVOMOTOR:
    {
         TPid Pid;
        //GuGaoFUN(GTN_CtrlMode(iCore, iAxisNum,0));
        GuGaoFUN(GTN_GetPid(iCore, iAxisNum,1,&Pid));
        Pid.kp=1;
        GuGaoFUN(GTN_SetPid(iCore, iAxisNum,1,&Pid));
    }
        break;
    }
    long sts;
    GuGaoFUN(GTN_GetSts(iCore,  iAxisNum, &sts));

    bool bHasHome=(!(sts&(1<<1)))&&(sts&(1<<9));
    m_Variants.set_VData(Var_Motor_Hashome_ID,bHasHome);

//    GuGaoFUN(GTN_AxisOn(iCore,iAxisNum));
//    m_Variants.set_VData(Var_Motor_AxisEnabled,true);
//    GuGaoFUN(GTN_AlarmOn(iCore,iAxisNum));
//    int iLimitMode=0;
//    double dSoftLimitNag=m_Variants.get_VData(Var_Motor_Nag_softlimit_ID)
//            ->toDouble();

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

    return CMotor::init_channel();
}

int32_t CMotor_GUGAO::exit_channel()
{
    int iCore,  iAxisNum;
    SerialToAxisID( iCore,  iAxisNum);
    GuGaoFUN(GTN_Stop(iCore,1<<(iAxisNum-1),0));
//    GuGaoFUN(GTN_AxisOff(iCore,iAxisNum));
    return CMotor::exit_channel();
}

