#include "rscompcode.h"

RSCompCode::RSCompCode()
{

}


void RSCompCode::setXStartPos(double value)
{
    xStartPos = value;
}

void RSCompCode::setYStartPos(double value)
{
    yStartPos = value;
}

void RSCompCode::setXInterval(double value)
{
    xInterval = value;
}
void RSCompCode::setYInterval(double value)
{
    yInterval = value;
}

void RSCompCode::setXWorkRange(double value)
{
    xWorkRange = value;
}

void RSCompCode::setYWorkRange(double value)
{
    yWorkRange = value;
}

int RSCompCode::getLookupRows()
{
    // 比如范围为3，间隔为1，Y方向会有4条线段
    return int(xWorkRange/xInterval)+1;
}

int RSCompCode::getLookupCols()
{
    return int(yWorkRange/yInterval)+1;
}


QString RSCompCode::getCode()
{

    QString code = QString("global real LookupTable(%1)(%2)\n").arg(getLookupRows()).arg(getLookupCols());
    code +=
    // 设置APOS和RPOS非默认连接
    "MFLAGS(Z).#DEFCON = 0\n"
    "CONNECT RPOS(Z) = RPOS(Z)\n"
    // 执行connect命令后控制其将电机依赖重置为默认值
    // 如果连接公式实际上导致电机依赖于另一轴，则需要重新指定依赖
    "DEPENDS Z, Z\n"+
    // 防止开启补偿的瞬间Z1轴跳动
    //QString("SET APOS(Z) = RPOS(Z) - MAP2(FPOS(Y), FPOS(X), LookupTable, %1, %2, %3, %4)\n").arg(yStartPos-yInterval).arg(yInterval).arg(xStartPos-xInterval).arg(xInterval)+
    //QString("CONNECT RPOS(Z) = APOS(Z) + MAP2(FPOS(Y), FPOS(X), LookupTable,%1, %2, %3, %4)\n").arg(yStartPos-yInterval).arg(yInterval).arg(xStartPos-xInterval).arg(xInterval)+
    QString("SET APOS(Z) = RPOS(Z) - MAP2(FPOS(Y), FPOS(X), LookupTable, %1, %2, %3, %4)\n").arg(yStartPos).arg(yInterval).arg(xStartPos).arg(xInterval)+
    QString("CONNECT RPOS(Z) = APOS(Z) + MAP2(FPOS(Y), FPOS(X), LookupTable,%1, %2, %3, %4)\n").arg(yStartPos).arg(yInterval).arg(xStartPos).arg(xInterval)+
    "DEPENDS Z, Z\n"
    "runF = 1\n"
    "STOP";

    return code;
}