#include "Mycontrol.h"


Mycontrol::Mycontrol()
{
}

// 获取单例实例（C++11 线程安全）
Mycontrol& Mycontrol::getInstance() {
    static Mycontrol instance;
    return instance;
}

void Mycontrol::init() {
    volatile int result = 0;
    // Init vars
    int ioin_count = 0, motor_count = 0, connect_rebuild = 1;

    // Load config file
    wchar_t path[] = L"D:\\Program Files\\demo\\glass.csv";
    result = XT_Controler_Extend::Profile_Load(path);
    if (result == 1) {
        qDebug() << "XTSystem - Init: Profile_Load succeed!";
    }
    else {
        qDebug() << "XTSystem - Init: Profile_Load failed!";
        return;
    }

    // Init controller
    //XT_Controler_Extend::Profile_DeInit_Controller();
    //qDebug() << "Do deinit controller";
    result = XT_Controler_Extend::Profile_Init_Controller(1);
    qDebug() << "XTSystem - Init: Do init controller";
    if (result == 1) {
        qDebug() << "XTSystem - Init: Profile_Init_Controller succeed!";

        ioin_count = XT_Controler_Extend::Profile_Get_IoIn_Count();
        motor_count = XT_Controler_Extend::Profile_Get_Axis_Count();
        connect_rebuild = 0;

        qDebug() << "XTSystem - Init:ioin_count: " << ioin_count << ", "
            << "motor_count: " << motor_count << ", "
            << "connect_rebuild: " << connect_rebuild;
    }
    else {
        qDebug() << "XTSystem - Init: Profile_Init_Controller failed!";
        return;
    }

    // Init 3 axis motors
    VCM_Resource_struct xtMotorsResources[3];
    xtMotorsResources[0].CanID = 1;
    xtMotorsResources[0].iAxis = motor_count + 1;
    xtMotorsResources[0].Connet_Rebuild = connect_rebuild;
    xtMotorsResources[0].IO_ID = ioin_count + 3 * 0;
    xtMotorsResources[0].iThread = 2;
    xtMotorsResources[0].iThread_Curve = 3;
    xtMotorsResources[0].Z_Index_ID = -1;
    xtMotorsResources[0].IO_CAN_ID = 4;
    xtMotorsResources[0].Extren_IO_Index = 0;

    xtMotorsResources[1].CanID = 2;
    xtMotorsResources[1].iAxis = motor_count + 2;
    xtMotorsResources[1].Connet_Rebuild = connect_rebuild;
    xtMotorsResources[1].IO_ID = ioin_count + 3 * 1;
    xtMotorsResources[1].iThread = 5;
    xtMotorsResources[1].iThread_Curve = 6;
    xtMotorsResources[1].Z_Index_ID = -1;
    xtMotorsResources[1].IO_CAN_ID = 4;
    xtMotorsResources[1].Extren_IO_Index = 1;

    xtMotorsResources[2].CanID = 3;
    xtMotorsResources[2].iAxis = motor_count + 3;
    xtMotorsResources[2].Connet_Rebuild = connect_rebuild;
    xtMotorsResources[2].IO_ID = ioin_count + 3 * 2;
    xtMotorsResources[2].iThread = 8;
    xtMotorsResources[2].iThread_Curve = 9;
    xtMotorsResources[2].Z_Index_ID = -1;
    xtMotorsResources[2].IO_CAN_ID = 4;
    xtMotorsResources[2].Extren_IO_Index = 2;

    VCMT_resource_alloc(xtMotorsResources, 3);

    // 初始化音圈控制端
    result = Soft_landing_dll_init(3);
    if (result == 1) {
        qDebug() << "XTSystem - Init:Soft_landing_dll_init succeed!";
    }
    else {
        qDebug() << "XTSystem - Init:Soft_landing_dll_init failed!";
    }

    // 获取电机状态
    result = Get_Init_Ready();
    if (result == -1) {
        qDebug() << "XTSystem - Init:Motors not exist!";
    }
    else if (result == 0) {
        qDebug() << "XTSystem - Init:Motors busy!";
    }
    else {
        qDebug() << "XTSystem - Init:Motors init succeed!";
    }

    // 电机使能
    SetServoOnOff(1, true);
    SetServoOnOff(2, true);
    SetServoOnOff(3, true);
    QThread::msleep(1000);
    qDebug() << "XTSystem - Init:Servos enable succeed!";

    // 配置三轴参数，motor_id =3, 1, 2 对应 z, y, x
    SetPosModejerk(3, 500000);
    SetPosModeAcc(3, 50);
    SetPosModeSpeed(3, 15);
    SetGoZeroSpeed(3, 10);
    SetGoZeroDistance(3, 20);
    SetRunDirect(3, 0, 20000);

    if (Init_Go_Zero(3, 1)) {
        qDebug() << "XTsystem - init: Z axis go zero succeed!";
    }

    SetPosModejerk(1, 500000);
    SetPosModeAcc(1, 50);
    SetPosModeSpeed(1, 15);
    SetGoZeroSpeed(1, 10);
    SetGoZeroDistance(1, 500);
    SetRunDirect(1, 0, 10000);

    if (Init_Go_Zero(1, 1)) {
        qDebug() << "XTsystem - init: Y axis go zero succeed!";
    }

    SetPosModejerk(2, 500000);
    SetPosModeAcc(2, 5);
    SetPosModeSpeed(2, 15);
    SetGoZeroSpeed(2, 10);
    SetGoZeroDistance(2, 500);
    SetRunDirect(2, 1, 10000);

    if (Init_Go_Zero(2, 1)) {
        qDebug() << "XTsystem - init: x axis go zero succeed!";
    }


    qDebug() << QString("XTSystem: init() called");
    
}

void Mycontrol::moveBy(int motorID, double distance, int block) {
    qDebug() << QString("Mycontrol: moveBy(motorID=%1, distance=%2, block=%3)")
        .arg(motorID).arg(distance).arg(block);
    double nowPosition = 0.0;
    GetNowPos(motorID, nowPosition);
    SetPosModePos(motorID, nowPosition + distance);
    TillPosModePos(motorID, block);
}

void Mycontrol::moveTo(int motorID, double targetPosition, int block) {
    qDebug() << QString("Mycontrol: moveTo(motorID=%1, targetPosition=%2, block=%3)")
        .arg(motorID).arg(targetPosition).arg(block);
    SetPosModePos(motorID, targetPosition);
    TillPosModePos(motorID, block);
}

void Mycontrol::moveByX(double xDis) {
    moveBy(2, xDis);
}

void Mycontrol::moveByY(double yDis) {
    moveBy(1, yDis);
} 

void Mycontrol::moveByZ(double zDis) {
    moveBy(3, zDis);
}




double Mycontrol::getXPos() const {
    double xPos;
    GetNowPos(2, xPos);
    return xPos;
}

double Mycontrol::getYPos() const {
    double yPos;
    GetNowPos(1, yPos);
    return yPos;
}

double Mycontrol::getZPos() const {
    double zPos;
    GetNowPos(3, zPos);
    return zPos;
}