#include "MotoController.h"


MotoController::MotoController(QObject *parent) : QObject(parent)
{
    this->portName = "COM2";
    this->baudRate = "9600";
}


bool MotoController::initSerialMCU()
{
    QList<QSerialPortInfo> list = QSerialPortInfo::availablePorts();
    if (list.isEmpty() == true)
        return false;

    this->portName = SettingConfig::getInstance().PORT_NAME;

    serial.setPortName(this->portName);
    serial.setBaudRate(this->baudRate.toInt());
    serial.setDataBits(QSerialPort::Data8);
    serial.setParity(QSerialPort::NoParity);
    serial.setStopBits(QSerialPort::OneStop);
    serial.setFlowControl(QSerialPort::NoFlowControl);

    bool succ = serial.open(QIODevice::ReadWrite);

    //连接信号和槽
//    LOG(DEBUG) << QString("[MotoController][initSerialMCU]打开电机串口[%1][%2][%3]").arg(portName).arg(baudRate).arg(succ).toStdString();

    return succ;
}

void MotoController::sendDataAutoEnding(QByteArray data)
{
//    LOG(INFO) << QString("向串口发送数据[send] %1").arg(data.data()).toStdString();

    // 加上0x0D 0x0A换行结束
    data.append(0x0D).append(0x0A);
    serial.write(data);
}


/**
 * @brief MotoController::encodeProtocol
 * @param content
 * @return
 * 格式  +0000 00   长度=9
 * bit1 = +/- +表示向上运动 -表示向下运动
 * bit2-bit4 表示调整的量
 * bit5 开关灯控制量
 * bit6 空格
 * bit7-bit8 数值的累加和
 * bit9 空格
 */
QByteArray MotoController::encodeProtocol(QString content)
{
    if (content.toInt() > 0)
    {
        content = "+" + content;
    }

    // 调整数值的绝对值 三位数字
    int abs = content.mid(1, content.length()).toInt();

    // 补齐3位数值
    if (abs < 10)
    {
        content.insert(1, "00");
    } else if (abs < 100)
    {
        content.insert(1, "0");
    }

    // 计算累加和
    int sum = 0;
    for (int i = 0; i < content.mid(1, content.length()).length(); i++)
    {
        sum += content.mid(i + 1, 1).toInt();
    }

    // 格式化累加和字符串
    QString sumStr = QString::number(sum);
    if (sumStr.length() == 1)
    {
        sumStr.prepend("0");
    } else if (sumStr.length() > 2)
    {
        sumStr = QString::number(sum).mid(0, 2);
    }

    // 开关灯的控制量 = 0
    content.append("0");
    content = content.append(0x20).append(sumStr).append(0x20);

    return content.toUtf8();
}

void MotoController::autoFacePosition(CasicFaceInfo * faceInfo)
{
    if (faceInfo->hasFace == false)
    {
        return;
    }

    const int centerLine = 300;
    int faceTop = faceInfo->faceRecTL[1];
    int faceCenterLine = faceTop + faceInfo->face.pos.height / 2;

    int diff = (faceCenterLine - centerLine) * -0.8;
    QByteArray data = this->encodeProtocol(QString::number(diff));
    this->sendDataAutoEnding(data);
}

void MotoController::autoPosition(int eyeCenter)
{
    int center = 300;
    int dir = eyeCenter > center ? -1 : 1;
    int step = abs(eyeCenter - center) / 20;
    for (int i = 0; i <= step; i++)
    {
        QByteArray data = this->encodeProtocol(QString::number(20 * dir));
        this->sendDataAutoEnding(data);
        QThread::msleep(100);
    }
}