#include "acscontroller.h"
#include <QString>
#include "myexception.h"
#include <QDebug>


ACSController::ACSController()
{
}

ACSController::~ACSController()
{

}

void ACSController::connect(char* Address, int Port)
{
    // 关闭掉非ACS.Framework.exe应用的连接
//    terminatePrevConnect();
    disconnect();
    m_hComm = ACSC_INVALID;
    m_hComm = acsc_OpenCommEthernetTCP(Address,Port);
    // m_hComm 为void* 指针，无效时放置的地址值为-1
    if(m_hComm == ACSC_INVALID)
    {
        throw MyException("网络连接失败。",acsc_GetLastError());
    }
}

void ACSController::disconnect()
{
    if(m_hComm != ACSC_INVALID)
    {
        if(!acsc_CloseComm(m_hComm))
        {
            throw MyException("关闭网络连接失败。",acsc_GetLastError());
        }
        m_hComm = ACSC_INVALID;
    }
}


bool ACSController::getConnectState()
{
    ACSC_CONNECTION_INFO ConnectionInfo;
    if(!acsc_GetConnectionInfo(m_hComm,&ConnectionInfo))
        return false;
    else
        return true;
}

bool ACSController::getBufferRunState(int AxisNum)
{
    int programState;
    if(!acsc_GetProgramState(m_hComm, AxisNum, &programState, ACSC_SYNCHRONOUS))
    {
        throw MyException(QString("读取buffer%1运行状态失败").arg(AxisNum),acsc_GetLastError());
    }
    return bool(programState&ACSC_PST_RUN);
}

bool ACSController::getMotorState(int AxisNum)
{
    int motorState;
    if(!acsc_GetMotorState(m_hComm, AxisNum, &motorState, ACSC_SYNCHRONOUS))
    {
        throw MyException(QString("读取电机状态失败"),acsc_GetLastError());
    }
    return bool(motorState&ACSC_MST_INPOS);
}

void ACSController::terminatePrevConnect()
{
    // 转义字符“\”
    char pSeperator[] = "\\";
    char *pToken = nullptr;
    char pApplicationName[256] = { 0x00, };
    int nConnections;
    // 定义一个已连接应用的列表，最大数量为10
    ACSC_CONNECTION_DESC ConnectionList[MAX_CONNECT_NUM];
    //10为最大连接数
    // 第三个参数为实际连接数的指针
    if(!acsc_GetConnectionsList(ConnectionList, MAX_CONNECT_NUM, &nConnections))
    {
        throw MyException("获取连接列表失败。",acsc_GetLastError());
    }
    for (int i = 0; i < nConnections; i++)
    {
        //原型 char *strtok(char s[], const char *delim);
        //第一个参数为待分割的字符串，第二个参数为分割符
        //功能在原字符串第一个分割符处打断，返回值为打断处之前的部分的指针
        pToken = strtok(ConnectionList[i].Application, pSeperator);
        //发送格式化输出到PApplicationName
        //把pToKen指向的文本发送到pApplicationName指向的地址
        sprintf(pApplicationName, pToken);
        //只留下应用名，去掉多余的路径信息
        while (pToken)
        {
            // 空指针可以转换为任意类型的指针
            pToken = strtok(nullptr, pSeperator);	// Find next
            if(pToken != nullptr)
            {
                sprintf(pApplicationName, pToken);
            }
        }
        // 关闭掉非ACS.Framework.exe的句柄
        if((strcmp(pApplicationName, "ACS.Framework.exe")) != 0)
        {
            if(!acsc_CloseComm(ConnectionList[i].Handle))
            {
                throw MyException("关闭已连接应用失败。",acsc_GetLastError());
            }
        }
    }
}

QVector<double> ACSController::getVELParams(int axisNum)
{
    QVector<double> velParams;
    velParams.append(getVEL(axisNum));
    velParams.append(getACC(axisNum));
    velParams.append(getDEC(axisNum));
    velParams.append(getJERK(axisNum));
    velParams.append(getKDEC(axisNum));
    return velParams;
}

double ACSController::getVEL(int axisNum)
{
    double Vel;
    if(!acsc_GetVelocity(m_hComm, axisNum, &Vel, ACSC_SYNCHRONOUS))
    {
        throw MyException("获取轴速度失败。",acsc_GetLastError());
    }
    return Vel;
}


double ACSController::getACC(int axisNum)
{
    double Acc;
    if(!acsc_GetAcceleration(m_hComm, axisNum, &Acc, ACSC_SYNCHRONOUS))
    {
        throw MyException("获取轴加速度失败。",acsc_GetLastError());
    }
    return Acc;
}


double ACSController::getDEC(int axisNum)
{
    double Dec;
    if(!acsc_GetDeceleration(m_hComm, axisNum, &Dec, ACSC_SYNCHRONOUS))
    {
        throw MyException("获取减速速度失败。",acsc_GetLastError());
    }
    return Dec;
}


double ACSController::getJERK(int axisNum)
{
    double Jerk;
    if(!acsc_GetJerk(m_hComm, axisNum, &Jerk,ACSC_SYNCHRONOUS))
    {
        throw MyException("获取加加速度失败。",acsc_GetLastError());
    }
    return Jerk;
}


double ACSController::getKDEC(int axisNum)
{
    double KDec;
    if(!acsc_GetKillDeceleration(m_hComm, axisNum, &KDec, ACSC_SYNCHRONOUS))
    {
        throw MyException("获取减减速度失败。",acsc_GetLastError());
    }
    return KDec;
}

void ACSController::setVEL(int axisNum, double Vel)
{
    if(!acsc_SetVelocity(m_hComm, axisNum, Vel,ACSC_SYNCHRONOUS))
    {
        throw MyException("设置速度失败。",acsc_GetLastError());
    }
}

void ACSController::setACC(int axisNum,double Acc)
{
    if(!acsc_SetAcceleration(m_hComm, axisNum, Acc,ACSC_SYNCHRONOUS))
    {
        throw MyException("设置加速度失败。",acsc_GetLastError());
    }
}



void ACSController::setDEC(int axisNum,double Dec)
{
    if(!acsc_SetDeceleration(m_hComm, axisNum, Dec,ACSC_SYNCHRONOUS))
    {
        throw MyException("设置减速度失败。",acsc_GetLastError());
    }
}



void ACSController::setJERK(int axisNum, double Jerk)
{
    if(!acsc_SetJerk(m_hComm, axisNum, Jerk,ACSC_SYNCHRONOUS))
    {
        throw MyException("设置加加速度失败。",acsc_GetLastError());
    }
}



void ACSController::setKDEC(int axisNum,double KDec)
{
    if(!acsc_SetKillDeceleration(m_hComm, axisNum, KDec,ACSC_SYNCHRONOUS))
    {
        throw MyException("设置减减速度失败。",acsc_GetLastError());
    }
}

void ACSController::setVELParams(int axisNum, double Vel)
{
    setVEL(axisNum,Vel);
    setACC(axisNum,Vel*10);
    setDEC(axisNum,Vel*10);
    setJERK(axisNum,Vel*100);
    setKDEC(axisNum,Vel*100);
}


void ACSController::enable(int axisNum)
{
    if(!acsc_Enable(m_hComm, axisNum, ACSC_SYNCHRONOUS))
    {
        throw MyException("使能失败。",acsc_GetLastError());
    }
}

void ACSController::disable(int axisNum)
{
    if(!acsc_Disable(m_hComm, axisNum, ACSC_SYNCHRONOUS))
    {
        throw MyException("取消使能失败。",acsc_GetLastError());
    }

    if(!acsc_WaitMotorEnabled(m_hComm, axisNum, 0, TIMEOUT_MOTOR_ENABLED))
    {
        throw MyException("等待取消使能失败。",acsc_GetLastError());
    }

}

void ACSController::halt(int axisNum)
{
    // 减速停止
     if(!acsc_Halt(m_hComm, axisNum, ACSC_SYNCHRONOUS))
     {
        throw MyException("停止轴运动失败。",acsc_GetLastError());
     }
}


void ACSController::jogNeg(int axisNum)
{
    // Jog command without velocity
    if(!acsc_Jog(m_hComm, 0, axisNum, ACSC_NEGATIVE_DIRECTION, ACSC_SYNCHRONOUS))
    {
       throw MyException("负方向点动失败。",acsc_GetLastError());
    }
}


void ACSController::jogNeg(int axisNum, double vel)
{
     // Jog command with velocity
     if(!acsc_Jog(m_hComm, ACSC_AMF_VELOCITY, axisNum, vel * (-1), ACSC_SYNCHRONOUS))
     {
         throw MyException("负方向点动失败。",acsc_GetLastError());
     }
}



void ACSController::jogPos(int axisNum)
{
     if(!acsc_Jog(m_hComm, 0, axisNum, ACSC_POSITIVE_DIRECTION, ACSC_SYNCHRONOUS))
     {
        throw MyException("正方向点动失败。",acsc_GetLastError());
     }
}


void ACSController::jogPos(int axisNum,double vel)
{
     if(!acsc_Jog(m_hComm, ACSC_AMF_VELOCITY, axisNum, vel, ACSC_SYNCHRONOUS))
     {
        throw MyException("正方向点动失败。",acsc_GetLastError());
     }
}


void ACSController::ABSToPoint(int axisNum,double point)
{

    //第二个参数‘0’，绝对；立即执行，2 为相对的
    if(!acsc_ToPoint(m_hComm,0,axisNum, point,ACSC_SYNCHRONOUS))
    {
        throw MyException("点到点绝对运动失败。",acsc_GetLastError());
    }
}

void ACSController::ABSToPoint(int axisNum1,double point1,int axisNum2,double point2)
{
    //第二个参数‘0’，绝对；立即执行，2 为相对的
    int Axes[] = {axisNum1,axisNum2,-1};
    double Points[] = {point1,point2};
    if(!acsc_ToPointM(m_hComm,0,Axes, Points,ACSC_SYNCHRONOUS))
    {
        throw MyException("点到点绝对运动失败。",acsc_GetLastError());
    }
}

void ACSController::RToPointNeg(int axisNum,double point)
{
    // 移动相对位置（从当前位置）
    if(!acsc_ToPoint(m_hComm, ACSC_AMF_RELATIVE,axisNum, point*(-1),ACSC_SYNCHRONOUS))
    {
        throw MyException("点到点相对运动失败。",acsc_GetLastError());
    }
}


void ACSController::RToPointPos(int axisNum,double point)
{
    // Move relative position (from current position)
    if(!acsc_ToPoint(m_hComm, ACSC_AMF_RELATIVE, axisNum, point, ACSC_SYNCHRONOUS))
    {
       throw MyException("点到点相对运动失败。",acsc_GetLastError());
    }
}

void ACSController::RToPoint(int axisNum1,double point1,int axisNum2,double point2)
{
    //第二个参数‘0’，绝对；立即执行，2 为相对的
    int Axes[] = {axisNum1,axisNum2,-1};
    double Points[] = {point1,point2};
    if(!acsc_ToPointM(m_hComm,ACSC_AMF_RELATIVE,Axes, Points,ACSC_SYNCHRONOUS))
    {
       throw MyException("点到点相对运动失败。",acsc_GetLastError());
    }
}

double ACSController::getFPOS(int AxisNum)
{
    double FPos;
    char vName[] = "FPOS";
    if(!acsc_ReadReal(m_hComm, -1, vName, AxisNum, AxisNum, -1, -1, &FPos, ACSC_SYNCHRONOUS))
    {
       throw MyException("获取轴当前位置失败。",acsc_GetLastError());
    }
    return FPos;
}
void ACSController::SetFPOS(int AxisNum,double fPos)
{
    if(!acsc_SetFPosition(m_hComm, AxisNum, fPos, ACSC_SYNCHRONOUS))
    {
       throw MyException("设置轴当前位置失败。",acsc_GetLastError());
    }

}

bool ACSController::getEnable(int AxisNum)
{
    double mst = getAxesMST(AxisNum);
    return bool(int(mst)&ACSC_MST_ENABLE);
}

LIMIT_STATE ACSController::getAxesLimitState(int AxisNum)
{
    int m_FaultMask;
    int m_MotorFault;
    LIMIT_STATE temp = NORMAL;
    if(!acsc_GetFaultMask(m_hComm, AxisNum, &m_FaultMask, ACSC_SYNCHRONOUS))
    {
       throw MyException("获取FaultMask失败。",acsc_GetLastError());
    }
    if(!acsc_GetFault(m_hComm, AxisNum, &m_MotorFault, ACSC_SYNCHRONOUS))
    {
       throw MyException("获取Fault失败。",acsc_GetLastError());
    }

    if(m_MotorFault & ACSC_SAFETY_SRL)
        temp = RIGHT_SLIMIT;
    if(m_MotorFault& ACSC_SAFETY_SLL)
        temp = LEFT_SLIMIT;
    if(m_MotorFault & ACSC_SAFETY_LL)
        temp = LEFT_LIMIT;
    if(m_MotorFault& ACSC_SAFETY_RL)
        temp = RIGHT_LIMIT;
    return temp;
}


bool ACSController::getEmergencyStopState()
{
    int m_EmergencyMask;
    int m_EmergencyFault;
    if(!acsc_GetFaultMask(m_hComm, ACSC_NONE, &m_EmergencyMask, ACSC_SYNCHRONOUS))
    {
        throw MyException("获取FaultMask失败。",acsc_GetLastError());
    }
    if(!acsc_GetFault(m_hComm, ACSC_NONE, &m_EmergencyFault, ACSC_SYNCHRONOUS))
    {
        throw MyException("获取Fault失败。",acsc_GetLastError());
    }
    if(m_EmergencyMask & ACSC_SAFETY_ES)
        return m_EmergencyFault & ACSC_SAFETY_ES;
    else
        return true;
}


double ACSController::getAxesMST(int AxisNum)
{
    double reValue;
    char vName[] = "MST";
    if(!acsc_ReadReal(m_hComm, -1, vName, AxisNum, AxisNum, -1, -1, &reValue,ACSC_SYNCHRONOUS))
    {
       throw MyException("获取轴MST失败。",acsc_GetLastError());
    }
    return reValue;
}


void ACSController::runBuffer(int bufferNum)
{
    // Run buffer program from first line 从第一行开始运行buffer程序
    // 第三个参数为程序label标签
    if(!acsc_RunBuffer(m_hComm, bufferNum, nullptr,  ACSC_SYNCHRONOUS))
    {
        throw MyException("获取buffer运行状态失败。",acsc_GetLastError());
    }

}



void ACSController::stopBuffer(int bufferNum)
{

    if(!acsc_StopBuffer(m_hComm, bufferNum, ACSC_SYNCHRONOUS))
    {
        throw MyException("停止buffer失败。",acsc_GetLastError());
    }

}

void ACSController::SuspendBuffer(int bufferNum)
{

    if(!acsc_SuspendBuffer(m_hComm, bufferNum, ACSC_SYNCHRONOUS))
    {
        throw MyException("暂停buffer失败。",acsc_GetLastError());
    }

}

void ACSController::appendBuffer(char* codeText,int bufferNum)
{
    // 无符号整型和有符号整型转换时，考虑最高位是否为1，如果为1则转换值会变
    if(!acsc_AppendBuffer(m_hComm,bufferNum,codeText,int(strlen(codeText)),ACSC_SYNCHRONOUS))
    {
       throw MyException("追加buffer代码失败。",acsc_GetLastError());
    }
}



void ACSController::clearBuffer(int bufferNum)
{
    if(!acsc_ClearBuffer(m_hComm,bufferNum,1,100000,ACSC_SYNCHRONOUS))
    {
        throw MyException("清除buffer代码失败。",acsc_GetLastError());
    }
}



void ACSController::compileBuffer(int bufferNum)
{
    if(!acsc_CompileBuffer(m_hComm,bufferNum, ACSC_SYNCHRONOUS))
    {
        throw MyException("编译buffer代码失败。",acsc_GetLastError());
    }
}



void ACSController::loadBuffer(int bufferNum,char* program)
{
    if(!acsc_LoadBuffer(m_hComm,bufferNum,program,int(strlen(program)),ACSC_SYNCHRONOUS))
    {
       throw MyException("下载buffer代码失败。",acsc_GetLastError());
    }
}



void ACSController::loadBuffersFromFile(char* fileName)
{
    if(!acsc_LoadBuffersFromFile(m_hComm,fileName,ACSC_SYNCHRONOUS))//fileName
    {

       throw MyException("把文件中代码下载到buffer失败。",acsc_GetLastError());

    }
}

void ACSController::suspendBuffer(int bufferNum)
{
    if(!acsc_SuspendBuffer(m_hComm,bufferNum,ACSC_SYNCHRONOUS))
    {
        throw MyException("暂停buffer运行失败。",acsc_GetLastError());
    }
}


QByteArray ACSController::uploadBuffer(int bufferNum)
{
    char bufferData[10000];
    int Received;
    if(!acsc_UploadBuffer(m_hComm,bufferNum,0,bufferData,10000,&Received,ACSC_SYNCHRONOUS))
    {
        throw MyException("上传buffer代码失败。",acsc_GetLastError());
    }
    return QByteArray(bufferData);

}



void ACSController::setDO(int port,int bit,bool value)
{
    if(!acsc_SetOutput(m_hComm,port,bit,int(value),ACSC_SYNCHRONOUS))
    {
        throw MyException("设置DO失败。",acsc_GetLastError());

    }
}

bool ACSController::getDO(int port,int bit)
{
    int value=0;
    if(!acsc_GetOutput(m_hComm,port,bit,&value,ACSC_SYNCHRONOUS))
    {
        throw MyException("获取DO失败。",acsc_GetLastError());

    }

    return bool(value);
}


bool ACSController::getDI(int port,int bit)
{
    int state;
    // 第2参数为port
    // 第3个参数为bit
    // 第4个参数为指向返回值的指针
    if(!acsc_GetInput(m_hComm,port,bit,&state,ACSC_SYNCHRONOUS))
    {
        throw MyException("获取DI失败。",acsc_GetLastError());
    }
    return bool(state);
}


double ACSController::getGlobalReal(char* realName)
{
    double reValue;
    if(!acsc_ReadReal(m_hComm,ACSC_NONE,realName,0,0,-1,-1,&reValue,ACSC_SYNCHRONOUS))
    {
        throw MyException("读取变量失败。",acsc_GetLastError());
    }
    return reValue;
}

QVector<double> ACSController::getGlobalReal(char* realName,int bufNum,int from1,int end1)
{
    QVector<double> reValue(end1-from1+1);
    double *reValue_ptr = reValue.data();
    if(!acsc_ReadReal(m_hComm,bufNum,realName,from1,end1,-1,-1,reValue_ptr,ACSC_SYNCHRONOUS))
    {
       throw MyException("读取变量失败。",acsc_GetLastError());
    }
    return reValue;
}

void ACSController::setGlobalReal(char* realName,double value)
{
    if(!acsc_WriteReal(m_hComm,ACSC_NONE,realName,0,0,-1,-1,&value,ACSC_SYNCHRONOUS))
    {
       throw MyException("写变量失败。",acsc_GetLastError());
    }
}

void ACSController::setGlobalReal(char* realName,int from1,int end1,QVector<double> value)
{
    double *value_ptr = value.data();
    if(!acsc_WriteReal(m_hComm,ACSC_NONE,realName,from1,end1,-1,-1,value_ptr,ACSC_SYNCHRONOUS))
    {
       throw MyException("写变量失败。",acsc_GetLastError());
    }
}

void ACSController::setGlobalReal(char* realName,int from1,int end1,int from2,int end2,double* value)
{
    if(!acsc_WriteReal(m_hComm,ACSC_NONE,realName,from1,end1,from2,end2,value,ACSC_SYNCHRONOUS))
    {
        int errorCode =acsc_GetLastError();
        throw MyException(QString("写全局浮点变量%1失败。").arg(QString(realName)),errorCode);
    }
}


int ACSController::getGlobalInt(char *name)
{
    int temp;
    if(!acsc_ReadInteger(m_hComm,ACSC_NONE,name,0,0,-1,-1,&temp,ACSC_SYNCHRONOUS))
    {
        throw MyException("读变量值失败。",acsc_GetLastError());
    }
    return temp;
}

// 读取多个全局整型
QVector<int> ACSController::getGlobalInt(char* name,int from1,int end1)
{
    QVector<int> reValue(end1-from1+1);
    int *reValue_ptr = reValue.data();
    if(!acsc_ReadInteger(m_hComm,ACSC_NONE,name,from1,end1,-1,-1,reValue_ptr,ACSC_SYNCHRONOUS))
    {
        throw MyException("读变量值失败。",acsc_GetLastError());
    }
    return reValue;
}

void ACSController::setGlobalInt(char* intName,int value)
{
    if(!acsc_WriteInteger(m_hComm,ACSC_NONE,intName,0,0,-1,-1,&value,ACSC_SYNCHRONOUS))
    {
       throw MyException("写变量失败。",acsc_GetLastError());
    }
}

void ACSController::setGlobalInt(char* intName,int from1,int to1,QVector<int> value)
{
    int *value_ptr = value.data();
    if(!acsc_WriteInteger(m_hComm,ACSC_NONE,intName,from1,to1,-1,-1,value_ptr,ACSC_SYNCHRONOUS))
    {
       throw MyException("写变量失败。",acsc_GetLastError());
    }
}


int ACSController::getBufferLines(int bufferNum)
{
    int temp[10];
    char vName[] = "PLINES";
    if(!acsc_ReadInteger(m_hComm,ACSC_NONE,vName,0,9,-1,-1,temp,ACSC_SYNCHRONOUS))
    {
        throw MyException("获取buffer代码行数失败。",acsc_GetLastError());
    }
    return temp[bufferNum];
}



void ACSController::mapEtherCatInput(int offset,QString vbName)
{
    QByteArray qbVbName = vbName.toLatin1();
    char* cVBName = qbVbName.data();
    if(! acsc_MapEtherCATInput(m_hComm,0,offset,cVBName,ACSC_SYNCHRONOUS))
    {
        throw MyException("EtherCat地址映射失败。",acsc_GetLastError());
    }
}

void ACSController::mapEtherCatOutput(int offset,QString vbName)
{
    QByteArray qbVbName = vbName.toLatin1();
    char* cVBName = qbVbName.data();
    if(!acsc_MapEtherCATOutput(m_hComm,0,offset,cVBName,ACSC_SYNCHRONOUS))
    {
       throw MyException("EtherCat地址映射失败。",acsc_GetLastError());
    }
}

void ACSController::resetMapEtherCatInputOutput()
{
    if(!acsc_UnmapEtherCATInputsOutputs(m_hComm,ACSC_SYNCHRONOUS))
    {
        throw MyException("重置EtherCat地址映射失败。",acsc_GetLastError());
    }
}

void ACSController::disableComp(int axisNum)
{
    int temp;
    const int DISABLE_CONNECT_MASK = 131072;
    char vName[] = "MFLAGS";
    if(!acsc_ReadInteger(m_hComm,ACSC_NONE,vName,axisNum,axisNum,-1,-1,&temp,ACSC_SYNCHRONOUS))
    {
       throw MyException("禁用补偿失败。",acsc_GetLastError());
    }
    temp = (temp|DISABLE_CONNECT_MASK);
    if(!acsc_WriteInteger(m_hComm,ACSC_NONE,vName,axisNum,axisNum,-1,-1,&temp,ACSC_SYNCHRONOUS))
    {
        throw MyException("禁用补偿失败。",acsc_GetLastError());
    }

    if(!acsc_ReadInteger(m_hComm,ACSC_NONE,vName,axisNum,axisNum,-1,-1,&temp,ACSC_SYNCHRONOUS))
    {
        throw MyException("禁用补偿失败。",acsc_GetLastError());
    }

}

int ACSController::getProgramError(int bufferNum)
{
    int errorCode;
    if(!acsc_GetProgramError(m_hComm,bufferNum,&errorCode,ACSC_SYNCHRONOUS))
    {
         throw MyException("禁用补偿失败。",acsc_GetLastError());
    }

    return errorCode;
}

int ACSController::getMotorError(int AxisNum)
{
    int errorCode;
    if(!acsc_GetMotorError(m_hComm,AxisNum,&errorCode,ACSC_SYNCHRONOUS))
    {
         throw MyException("获取电机状态失败。",acsc_GetLastError());
    }

    return errorCode;
}


bool ACSController::getMoveState(int AxisNum)
{
    double mst = getAxesMST(AxisNum);//
    return bool(int(mst)&ACSC_MST_MOVE);
}