#include "RobotCommunication.h"

QByteArray RobotCommunication::intToHex(int intNum)
{
    QByteArray result;
    result.append((char)(intNum & 0xFF));        // 低八位
    result.append((char)((intNum >> 8) & 0xFF)); // 高八位
    return result;
}


RobotCommunication::RobotCommunication(const QString& portName, int baudRate):portName(portName),baudRate(baudRate)
{
    serial = new QSerialPort(portName,this);
    serial->setBaudRate(baudRate);
    if (!serial->open(QIODevice::ReadWrite)) {
        qWarning() << "Failed to open port!";
    }
    else {
        qDebug() << "Port opened successfully!";
    }
}

RobotCommunication::~RobotCommunication()
{
    if (serial) {
        serial->close();
        delete serial;  // 释放内存
    }
}

QString RobotCommunication::getPortName()
{
    return portName;
}

void RobotCommunication::setPortName(QString portName)
{
    portName = portName;
}

int RobotCommunication::getBaudRate()
{
    return baudRate;
}

void RobotCommunication::setBaudRate(int baudRate)
{
    baudRate = baudRate;
}

QList<QSerialPortInfo> RobotCommunication::queryCurrentAllSerialPort()
{
    QList<QSerialPortInfo> ports = QSerialPortInfo::availablePorts();
    if (ports.size() == 0) {
        qDebug() << "No Port";
    }
    for (const QSerialPortInfo& port : ports) {
        qDebug() << "availablePorts name: " << port.portName();
    }
    return ports;
}

void RobotCommunication::send(const QByteArray& data)
{
    if (serial->isOpen()) {
        serial->write(data);
        if (serial->waitForBytesWritten(1000)) {
            qDebug() << "上位机数据发送成功" << data.toHex();
        }
        else {
            qWarning() << "上位机数据发送失败";
        }
    }
}

void RobotCommunication::cmd_contrilMultiSteeringEngine(const QList<unsigned int>& idList, const QList<unsigned int>& angleList, int time)
{
    QByteArray sendData;
    sendData.append(0x55).append(0x55);//帧头
    sendData.append((char)(idList.size() * 3 + 5));//数据长度
    sendData.append(0x03);//指令
    /*
    参数 1：要控制舵机的个数
    参数 2：时间低八位
    参数 3：时间高八位
    参数 4：舵机 ID 号
    参数 5：角度位置低八位
    参数 6：角度位置高八位
    */
    sendData.append((char)idList.size());
    QByteArray timeData = intToHex(time);
    sendData.append(timeData);
    for (int i = 0; i < idList.size(); ++i) {
        sendData.append(idList[i]);
        QByteArray angleData = intToHex(angleList[i]);
        sendData.append(angleData);
    }
    send(sendData);
}

void RobotCommunication::cmd_stopAction()
{
    QByteArray sendData;
    sendData.append(0x55).append(0x55).append(0x02).append(0x07);
    send(sendData);
}

void RobotCommunication::cmd_recover()
{
    QList<unsigned int> idList = { 1, 2, 3, 4, 5, 6 };
    QList<unsigned int> angleList = {1500,1500,1500,1500,1500,1500};
    cmd_contrilMultiSteeringEngine(idList, angleList);
}