#include "RobotArmControl.h"

RobotArmControl::RobotArmControl(QWidget* parent): QDialog(parent)
{
	ui.setupUi(this);
    robotController = new RobotController(this);    

	connect(ui.get_task_list_btn, SIGNAL(clicked()), this, SLOT(getTaskList()));
    connect(ui.get_pose_btn, SIGNAL(clicked()), this, SLOT(getPose()));
	connect(ui.connect2arm_btn, SIGNAL(clicked()), this, SLOT(connectRobotArm()));

	connect(ui.execute_task_btn, SIGNAL(clicked()), this, SLOT(executeTaskWithoutHyperImager()));
	connect(ui.pause_task_btn, SIGNAL(clicked()), this, SLOT(pauseTask()));
    connect(ui.continue_task_btn, SIGNAL(clicked()), this, SLOT(continueTask()));

    robotMonitor = new EC8056;
    connect(robotMonitor, &EC8056::dataReceived, this, &RobotArmControl::monitorRobotArm);
    

    m_pModel = new QStringListModel(ui.taskList_listView);

    ui.taskList_listView->setModel(m_pModel);
}

RobotArmControl::~RobotArmControl()
{

}

void RobotArmControl::monitorRobotArm(const ECData& data)
{
    double x = data.machinePose[0];
    double y = data.machinePose[1];
    double z = data.machinePose[2];

    ui.pose_x_label->setText(QString::number(x));
    ui.pose_y_label->setText(QString::number(y));
    ui.pose_z_label->setText(QString::number(z));
}

void RobotArmControl::onCommandResponse(QString str, const QJsonObject& response)
{
    //qDebug() << "response:" << response;

    QString re;
    if (response.contains("result"))
    {
        re = response["result"].toVariant().toString();
        ui.textEdit->append(str + " Result: " + re);
    }
    else if (response.contains("error"))
    {
        //auto delete11 = response["error"].toObject();
        //qDebug() << "response[\"error\"]:" << delete11;

        auto errorStr = response["error"].toObject()["message"].toString();
    
        ui.textEdit->append(str + " Error: " + errorStr);
    }
}

void RobotArmControl::getTaskList()
{
    FileOperation* fileOperation = new FileOperation();
    string directory = fileOperation->getDirectoryOfExe();

    QString pythonScript = QString::fromStdString(directory) + "\\get_jbi_filename.py";

    QProcess process;

    process.start("python.exe", QStringList() << pythonScript);
    process.waitForFinished();
    QString output = process.readAllStandardOutput();

    QStringList files;
    //files.append("tc20250324down.jbi");
    //files.append("tc20250324circle.jbi");
    //files.append("tc20250324side.jbi");
    QStringList lines = output.split('\n', QString::SkipEmptyParts);
    for (const QString& line : lines)
    {
        files.append(line.trimmed());
    }

    ////先清除数据，然后添加数据
    //for (size_t i = 0; i < files.length(); i++)
    //{
    //    int row = m_pModel->rowCount();
    //    m_pModel->insertRow(row);
    //    QModelIndex index = m_pModel->index(row);
    //    m_pModel->setData(index, files[i]);
    //}

    m_pModel->setStringList(files);
}

void RobotArmControl::getPose()
{
    QJsonObject response;
    bool x = robotController->getRobotPose(response);
    onCommandResponse("getPose", response);
}

void RobotArmControl::connectRobotArm()
{
    bool re = robotController->connectToRobot("192.168.1.100");
    robotMonitor->connectToHost("192.168.1.100");
}

void RobotArmControl::executeTaskWithHyperImager()
{
    QModelIndex index = ui.taskList_listView->currentIndex();
    if (-1 == index.row())
    {
        //弹窗提示用户选择文件
        ui.textEdit->append("Please select file on the left!");
        return;
    }
    QString fileName = index.data(Qt::DisplayRole).toString();

    QJsonObject response;
    bool x;

    x = robotController->checkJbiExist(fileName, response);
    onCommandResponse("checkJbiExist", response);
    if (!x)
    {
        return;
    }

    x = robotController->setServoStatus(1, response);
    onCommandResponse("setServoStatus", response);
    if (!x)
    {
        return;
    }

    x = robotController->runJbi(fileName, response, true);
    onCommandResponse("runJbi", response);
    if (!x)
    {
        return;
    }
}

void RobotArmControl::executeTaskWithoutHyperImager()
{
    QModelIndex index = ui.taskList_listView->currentIndex();
    if (-1 == index.row())
    {
        //弹窗提示用户选择文件
        ui.textEdit->append("Please select file on the left!");
        return;
    }
    QString fileName = index.data(Qt::DisplayRole).toString();

    QJsonObject response;
    bool x;

    x = robotController->checkJbiExist(fileName, response);
    onCommandResponse("checkJbiExist", response);
    if (!x)
    {
        return;
    }

    x = robotController->setServoStatus(1, response);
    onCommandResponse("setServoStatus", response);
    if (!x)
    {
        return;
    }

    x = robotController->runJbi(fileName, response, false);
    onCommandResponse("runJbi", response);
    if (!x)
    {
        return;
    }
}

void RobotArmControl::pauseTask()
{
    QJsonObject response;
    bool x;

    x = robotController->pauseTask(response);
    onCommandResponse("pauseTask", response);
}

void RobotArmControl::continueTask()
{
    QJsonObject response;
    bool x;

    x = robotController->continueTask(response);
    onCommandResponse("continueTask", response);
}






RobotController::RobotController(QObject* parent) : QObject(parent), socket(new QTcpSocket(this))
{
    //connect(socket, &QTcpSocket::readyRead, this, &RobotController::onReadyRead);

    m_timer = new QTimer(this);
    connect(m_timer, SIGNAL(timeout()), this, SLOT(getPoint()));
}

RobotController::~RobotController()
{
    disconnectFromRobot();
}

bool RobotController::connectToRobot(const QString& ip, quint16 port)
{
    socket->setProxy(QNetworkProxy::NoProxy);

    socket->connectToHost(ip, port);

    if (!socket->waitForConnected(3000)) {
        qDebug() << "Connection failed:" << socket->errorString();
        return false;
    }
    qDebug() << "Connected successfully!";

    return true;
}

void RobotController::disconnectFromRobot()
{
    if (socket->isOpen())
    {
        socket->close();
    }
}

bool RobotController::processResponse(QJsonObject response, QString& result)
{
    //qDebug() << "response:" << response;

    if (response.contains("result"))
    {
        result = response["result"].toVariant().toString();

        //qDebug() << "result1:" << result;

        return true;
    }
    else if (response.contains("error"))
    {
        result = response["error"].toObject()["message"].toString();
        //qDebug() << "result2:" << result;

        return false;
    }
}

bool RobotController::processResponse_getJbiState(QJsonObject response, QString& result)
{
    //qDebug() << "response:" << response;

    if (response.contains("result"))
    {
        QString resultStr = response["result"].toString();

        QJsonDocument resultDoc = QJsonDocument::fromJson(resultStr.toUtf8());
        QJsonObject resultObj = resultDoc.object();
        //qDebug() << "resultObj:" << resultObj;

        int runState = resultObj["runState"].toInt();
        //qDebug() << "runState:" << runState;

        result = QString::number(runState);
        //qDebug() << "result:" << result;

        return true;
    }
    else if (response.contains("error"))
    {
        result = response["error"].toObject()["message"].toString();

        return false;
    }
}

void RobotController::getPoint()
{
    QJsonObject response;
    getJbiState(response);//0 停止状态,1 暂停状态,2 急停状态,3 运行状态,4 错误状态
    QString result;
    bool x = processResponse_getJbiState(response, result);
    //qDebug() << "getJbiState:" << result;

    if (result.toInt() != 3)
    {
        m_timer->stop();

        m_iCurrentJbiJobLine = 0;
        m_iFileCounter = 0;

        //发射信号，相机停止采集
        emit hsiRecordSignal(-1);

        return;
    }

    QJsonObject response2;
    getCurrentJobLine(response2);
    QString result2;
    bool x2 = processResponse(response2, result2);

    int m_iCurrentJbiJobLine_tmp = result2.toInt();
    

    if (m_iCurrentJbiJobLine_tmp != m_iCurrentJbiJobLine)
    {
        m_iFileCounter++;

        qDebug() << "Changed! CurrentJobLine:" << m_iCurrentJbiJobLine_tmp;
        //发射信号，相机停止采集
        emit hsiRecordSignal(-1);

        m_iCurrentJbiJobLine = m_iCurrentJbiJobLine_tmp;

        //发射信号，相机开始采集
        emit hsiRecordSignal(m_iFileCounter);
    }
}

void RobotController::sendCommand(const QString& cmd, const QJsonValue& params, int id)
{
    QJsonObject request;
    request["method"] = cmd;
    request["params"] = params;
    request["jsonrpc"] = "2.0";
    request["id"] = id;

    QJsonDocument doc(request);
    QByteArray data = doc.toJson(QJsonDocument::Compact) + "\n";
    //qDebug() << "send command:" << data.constData();

    socket->write(data);
    socket->waitForBytesWritten();
}

bool RobotController::onReadyRead(QJsonObject& re)
{
    QByteArray data = socket->readAll();
    QJsonDocument doc = QJsonDocument::fromJson(data);
    if (!doc.isNull() && doc.isObject())
    {
        re = doc.object();
        //qDebug() << "Received all:" << re;

        if (re.contains("result"))
        {
            //qDebug() << "Received result:" << re["result"].toVariant();
            return true;
        }
        else if (re.contains("error"))
        {
            //qDebug() << "Received error:" << re["error"];
            return false;
        }

        //emit commandResponse(true, doc.object());
    }
    else
    {
        //emit commandResponse(false, QJsonObject());

        re = QJsonObject();
        return false;
    }
}

bool RobotController::getRobotPose(QJsonObject& re)
{
    sendCommand("getRobotPose");
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}

bool RobotController::getRobotState(QJsonObject& re)
{
    sendCommand("getRobotState");
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}

bool RobotController::getJbiState(QJsonObject& re)
{
    sendCommand("getJbiState");
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}

bool RobotController::getCurrentJobLine(QJsonObject& re)
{
    sendCommand("getCurrentJobLine");
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}

bool RobotController::getRobotMode(QJsonObject& re)
{
    sendCommand("getRobotMode");
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}

bool RobotController::checkJbiExist(const QString& jbiFilename, QJsonObject& re)
{
    QJsonObject paramsRunJbi;
    paramsRunJbi["filename"] = jbiFilename;//使用对象结构

    sendCommand("checkJbiExist", paramsRunJbi);
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}

bool RobotController::setServoStatus(int status, QJsonObject& re)
{
    QJsonObject params_set_servo_status;
    params_set_servo_status["status"] = status;//使用对象结构

    sendCommand("set_servo_status", params_set_servo_status);
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}

bool RobotController::runJbi(const QString& jbiFilename, QJsonObject& re, bool isRecordHsi)
{
    QJsonObject paramsRunJbi;
    paramsRunJbi["filename"] = jbiFilename;//使用对象结构

    sendCommand("runJbi", paramsRunJbi);
    socket->waitForReadyRead(m_iTimeout);

    if (isRecordHsi)
    {
        m_timer->start(1000);
    }

    return onReadyRead(re);
}

bool RobotController::pauseTask(QJsonObject& re)
{
    sendCommand("pause");
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}

bool RobotController::run(QJsonObject& re)
{
    sendCommand("run");
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}

bool RobotController::continueTask(QJsonObject& re)
{
    sendCommand("run");
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}

bool RobotController::setRobotPowerStatus(int status, QJsonObject& re)
{
    sendCommand("set_robot_power_status", QJsonArray{ status });
    socket->waitForReadyRead(m_iTimeout);

    return onReadyRead(re);
}








EC8056::EC8056(QObject* parent) : QObject(parent), socket(new QTcpSocket(this))
{
    connect(socket, &QTcpSocket::readyRead, this, &EC8056::onReadyRead);
    /*connect(socket, QOverload<QAbstractSocket::SocketError>::of(&QTcpSocket::errorOccurred),
        this, &EC8056::onSocketError);*/
    //connect(socket, SIGNAL(error(QAbstractSocket::SocketError)),
    //    this, SLOT(onSocketError(QAbstractSocket::SocketError)));
}

void EC8056::connectToHost(const QString& host, quint16 port)
{
    socket->setProxy(QNetworkProxy::NoProxy);

    socket->connectToHost(QHostAddress(host), port);

    if (!socket->waitForConnected(3000)) {
        qDebug() << "Connection failed:" << socket->errorString();
        return;
    }
    qDebug() << "Connected successfully!";
}

quint8 EC8056::readUInt8(const QByteArray& buf, int& offset)
{
    return static_cast<quint8>(buf[offset++]);
}

quint32 EC8056::readUInt32(const QByteArray& buf, int& offset)
{
    quint32 val;
    memcpy(&val, buf.constData() + offset, sizeof(val));
    offset += sizeof(val);
    return qFromBigEndian(val);
}

qint32 EC8056::readInt32(const QByteArray& buf, int& offset)
{
    qint32 val;
    memcpy(&val, buf.constData() + offset, sizeof(val));
    offset += sizeof(val);
    return qFromBigEndian(val);
}

quint64 EC8056::readUInt64(const QByteArray& buf, int& offset)
{
    quint64 val;
    memcpy(&val, buf.constData() + offset, sizeof(val));
    offset += sizeof(val);
    return qFromBigEndian(val);
}

double EC8056::readDouble(const QByteArray& buf, int& offset)
{
    QByteArray b = buf.mid(offset, sizeof(double));
    offset += sizeof(double);
    if (QSysInfo::ByteOrder == QSysInfo::LittleEndian)
        std::reverse(b.begin(), b.end());

    double val;
    memcpy(&val, b.constData(), sizeof(double));
    return val;
}

void EC8056::onReadyRead()
{
    buffer.append(socket->readAll());

    if (buffer.size() < 1024)
        return;

    int offset = 0;
    ECData data;

    data.msgSize = readUInt32(buffer, offset);
    offset = 1020;
    data.matchingWord = readUInt32(buffer, offset);

    if ((data.msgSize == 1024)/* && (data.matchingWord == 3967833836)*/)
    {
        offset = 0;

        data.msgSize = readUInt32(buffer, offset);
        data.timeStamp = readUInt64(buffer, offset);
        data.auto_cycle = readUInt8(buffer, offset);

        for (int i = 0; i < 8; ++i) data.machinePos[i] = readDouble(buffer, offset);
        for (int i = 0; i < 6; ++i) data.machinePose[i] = readDouble(buffer, offset);
        for (int i = 0; i < 6; ++i) data.machineUserPose[i] = readDouble(buffer, offset);
        for (int i = 0; i < 8; ++i) data.torque[i] = readDouble(buffer, offset);

        data.robotState = readUInt32(buffer, offset);
        data.servoReady = readUInt32(buffer, offset);
        data.can_motor_run = readUInt32(buffer, offset);

        for (int i = 0; i < 8; ++i) data.motor_speed[i] = readInt32(buffer, offset);

        data.robotMode = readUInt32(buffer, offset);

        for (int i = 0; i < 3; ++i) data.analog_ioInput[i] = readDouble(buffer, offset);
        for (int i = 0; i < 5; ++i) data.analog_ioOutput[i] = readDouble(buffer, offset);

        data.digital_ioInput = readUInt64(buffer, offset);
        data.digital_ioOutput = readUInt64(buffer, offset);

        data.collision = readUInt8(buffer, offset);

        for (int i = 0; i < 6; ++i) data.machineFlangePose[i] = readDouble(buffer, offset);
        for (int i = 0; i < 6; ++i) data.machineUserFlangePose[i] = readDouble(buffer, offset);

        data.emergencyStopState = readUInt8(buffer, offset);
        data.tcpSpeed = readDouble(buffer, offset);
        for (int i = 0; i < 8; ++i) data.joIntSpeed[i] = readDouble(buffer, offset);
        data.tcpAcc = readDouble(buffer, offset);
        for (int i = 0; i < 8; ++i) data.joIntAcc[i] = readDouble(buffer, offset);
        for (int i = 0; i < 6; ++i) data.joIntTemperature[i] = readDouble(buffer, offset);
        for (int i = 0; i < 6; ++i) data.joIntTorque[i] = readDouble(buffer, offset);
        for (int i = 0; i < 6; ++i) data.extJoIntTorques[i] = readDouble(buffer, offset);
        for (int i = 0; i < 6; ++i) data.exTcpForceIntool[i] = readDouble(buffer, offset);

        data.dragState = readUInt8(buffer, offset);
        data.sensor_connected_state = readUInt8(buffer, offset);
        data.reserved = readUInt8(buffer, offset);
        data.matchingWord = readUInt32(buffer, offset);

        /*if (data.msgSize != 1024 || data.matchingWord != 0xec8056ec) {
            buffer.clear();
            emit errorOccurred("Invalid packet received");
            return;
        }*/

        emit dataReceived(data);
    }

    
    buffer.clear();
}

void EC8056::onSocketError(QAbstractSocket::SocketError socketError)
{
    Q_UNUSED(socketError)
        emit errorOccurred(socket->errorString());
}