#include "forcesensorchart.h"
#include "ui_forcesensorchart.h"
#include "qcustomplot.h"

forceSensorChart::forceSensorChart(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::forceSensorChart)
    , serial(new QSerialPort)
    , isPortOpen(false)
    , isDataNull(true)
    , isPauseData(false)
    , drawMode(FOLLOW)
    , forceSensorPtr(new ForceSensor)
    , m_clock(new QTimer(this))
    , m_timer(new QElapsedTimer)
{
    ui->setupUi(this);
    chartInit();
    graphInit();
    serialInit();
    connectInit();


}

forceSensorChart::~forceSensorChart()
{
    delete ui;
}


void forceSensorChart::chartInit()
{
    //实例化
    chart = new QChart();
    axisX = new QValueAxis();
    axisYForce = new QValueAxis();
    axisYTorque = new QValueAxis();
    for (int i = 0; i < 6; ++i) {
        seriesForce[i] = new QSplineSeries();
    }


    //图表设置
    ui->chartView->setChart(chart);
    ui->chartView->setRenderHint(QPainter::Antialiasing);//抗锯齿
    chart->setTitle("六维力传感器数据曲线");
    chart->addAxis(axisX, Qt::AlignBottom);//chart 添加axis在series依附在axis之前
    chart->addAxis(axisYForce, Qt::AlignLeft);
    chart->addAxis(axisYTorque, Qt::AlignRight);
    for (int i = 0; i < 6; ++i) {
        chart->addSeries(seriesForce[i]);
    }

    //坐标轴设置
    axisX->setTitleText("时间(s)");
    axisX->setRange(0, timeRange);
    axisX->setLabelFormat("%.1f");
    axisX->setTickCount(timeRange + 1);
    axisX->setMinorTickCount(10);
    axisX->setGridLineVisible(true);
    axisYForce->setTitleText("力(N)");
    axisYForce->setRange(forceRangeMin, forceRangeMax);
    axisYForce->setLabelFormat("%.2f");
    axisYForce->setTickCount((forceRangeMax - forceRangeMin)/2 + 1);
    axisYForce->setMinorTickCount(2);
    axisYForce->setGridLineVisible(false);
    axisYForce->setMinorGridLineVisible(false);
    axisYTorque->setTitleText("力矩(N·M)");
    axisYTorque->setRange(torqueRangeMin, torqueRangeMax);
    axisYTorque->setLabelFormat("%.2f");
    axisYTorque->setTickCount((torqueRangeMax - torqueRangeMin)*10 + 1);
    axisYTorque->setMinorTickCount(0.1);
    axisYTorque->setGridLineVisible(false);
    axisYTorque->setMinorGridLineVisible(false);

    //折线序列设置
    for (int i = 0; i < 6; ++i) {
        seriesForce[i]->attachAxis(axisX);
        if (i < 3) {
            seriesForce[i]->attachAxis(axisYForce);
        }
        else {
            seriesForce[i]->attachAxis(axisYTorque);
        }

    }
    seriesForce[0]->setName("Fx");
    seriesForce[1]->setName("Fy");
    seriesForce[2]->setName("Fz");
    seriesForce[3]->setName("Mx");
    seriesForce[4]->setName("My");
    seriesForce[5]->setName("Mz");

    //笔刷设置
//    QPen pen;
//    pen.setStyle(Qt::SolidLine);
//    pen.setWidth(2);


}

void forceSensorChart::graphInit()
{
    customPlotForce = ui->forceWidget;
    customPlotTorque = ui->torqueWidget;
    // 力曲线数据图
    customPlotForce->addGraph();
    customPlotForce->graph(0)->setPen(QPen(Qt::red));
    customPlotForce->addGraph();
    customPlotForce->graph(1)->setPen(QPen(Qt::green));
    customPlotForce->addGraph();
    customPlotForce->graph(2)->setPen(QPen(Qt::blue));
    QSharedPointer<QCPAxisTickerTime> timeTicker{new QCPAxisTickerTime};
    timeTicker->setTimeFormat("%m:%s:%z");
    customPlotForce->xAxis->setTicker(timeTicker);
    customPlotForce->axisRect()->setupFullAxesBox();
    customPlotForce->yAxis->setRange(forceRangeMin, forceRangeMax);
    connect(customPlotForce->xAxis, SIGNAL(rangeChanged(QCPRange)), customPlotForce->xAxis2, SLOT(setRange(QCPRange)));
    connect(customPlotForce->yAxis, SIGNAL(rangeChanged(QCPRange)), customPlotForce->yAxis2, SLOT(setRange(QCPRange)));
    customPlotForce->setInteractions(QCP::iRangeDrag | QCP::iRangeZoom | QCP::iSelectPlottables);

    // 力矩曲线数据图
    customPlotTorque->addGraph();
    customPlotTorque->graph(0)->setPen(QPen(Qt::red));
    customPlotTorque->addGraph();
    customPlotTorque->graph(1)->setPen(QPen(Qt::green));
    customPlotTorque->addGraph();
    customPlotTorque->graph(2)->setPen(QPen(Qt::blue));
    customPlotTorque->xAxis->setTicker(timeTicker);
    customPlotTorque->axisRect()->setupFullAxesBox();
    customPlotTorque->yAxis->setRange(-0.5, 0.5); //bug:无法直接使用 torqueRangeMin、torqueRangeMax
    connect(customPlotTorque->xAxis, SIGNAL(rangeChanged(QCPRange)), customPlotTorque->xAxis2, SLOT(setRange(QCPRange)));
    connect(customPlotTorque->yAxis, SIGNAL(rangeChanged(QCPRange)), customPlotTorque->yAxis2, SLOT(setRange(QCPRange)));
    customPlotTorque->setInteractions(QCP::iRangeDrag | QCP::iRangeZoom | QCP::iSelectPlottables);

    // 获取显示器支持帧率
    QScreen* screen = QGuiApplication::primaryScreen();
    int refreshRate = screen->refreshRate();
    qDebug() << "当前显示器支持帧率：" << refreshRate << "hz";
    connect(m_clock, &QTimer::timeout, this, &forceSensorChart::realtimeDataPlot);
    m_clock->start(refreshRate);//开始定时
    m_timer->start();//开始计时
}

void forceSensorChart::serialInit()
{
    //更新端口
    on_btnRefreshPort_clicked();

    //初始化串口
//    serial->setPortName(ui->comboPortBox->currentText());//设置串口号
    serial->setBaudRate(QSerialPort::Baud115200);//设置波特率为115200
    serial->setDataBits(QSerialPort::Data8);//设置数据位为8
    serial->setParity(QSerialPort::NoParity);//设置奇偶校验位为无
    serial->setStopBits(QSerialPort::OneStop);//设置停止位为1
//    serial->open(QSerialPort::ReadWrite);//设置读写

}

void forceSensorChart::connectInit()
{
    //接收数据
//    connect(serial, &QSerialPort::readyRead, this, &forceSensorChart::on_serial_received);
    connect(serial, &QSerialPort::readyRead, this, &forceSensorChart::on_serial_received2);
    //绘图模式连接
    connect(ui->chartMode1, &QRadioButton::clicked, this,  [=](){
        drawMode = FOLLOW;
    });
    connect(ui->chartMode2, &QRadioButton::clicked, this, [=](){
        drawMode = SQUEEZE;
    });
    //是否暂停接收数据
    void(QCheckBox:: *f)(int) = &QCheckBox::stateChanged;
    connect(ui->chkPause, f, this, [=](int state){
        if (state == Qt::Checked) {
            isPauseData = true;
            ui->stateBar->setText("【INFO】：已暂停数据接收");
        }
        else {
            isPauseData = false;
            ui->stateBar->setText("【INFO】：已开启数据接收");
        }
        qDebug() << isPauseData;
    });

    //指定绘图频率
//    QTimer* timer = new QTimer;
//    connect(timer, &QTimer::timeout, this, &forceSensorChart::updateData);
//    timer->start(timeInterval * 1000);
}

void forceSensorChart::forceSensorDataProcess(unsigned char* buf)
{
    unsigned short forceDataRaw[6];
    forceDataRaw[0] = (((unsigned short)buf[1] << 8) | (unsigned short)buf[2]) >> 4;
    forceDataRaw[1] = (((unsigned short)buf[2] << 8) | (unsigned short)buf[3]) & 0xFFF;
    forceDataRaw[2] = (((unsigned short)buf[4] << 8) | (unsigned short)buf[5]) >> 4;
    forceDataRaw[3] = (((unsigned short)buf[5] << 8) | (unsigned short)buf[6]) & 0xFFF;
    forceDataRaw[4] = (((unsigned short)buf[7] << 8) | (unsigned short)buf[8]) >> 4;
    forceDataRaw[5] = (((unsigned short)buf[8] << 8) | (unsigned short)buf[9]) & 0xFFF;

    m_forceData[0] = (forceDataRaw[0] & 0x800) ?
                         -((~forceDataRaw[0] + (unsigned short)1) & 0x7FF) * forceCoeff : forceDataRaw[0] * forceCoeff;
    m_forceData[1] = (forceDataRaw[1] & 0x800) ?
                         -((~forceDataRaw[1] + (unsigned short)1) & 0x7FF) * forceCoeff : forceDataRaw[1] * forceCoeff;
    m_forceData[2] = (forceDataRaw[2] & 0x800) ?
                         -((~forceDataRaw[2] + (unsigned short)1) & 0x7FF) * forceCoeff : forceDataRaw[2] * forceCoeff;
    m_forceData[3] = (forceDataRaw[3] & 0x800) ?
                         -((~forceDataRaw[3] + (unsigned short)1) & 0x7FF) * torqueCoeff : forceDataRaw[3] * torqueCoeff;
    m_forceData[4] = (forceDataRaw[4] & 0x800) ?
                         -((~forceDataRaw[4] + (unsigned short)1) & 0x7FF) * torqueCoeff : forceDataRaw[4] * torqueCoeff;
    m_forceData[5] = (forceDataRaw[5] & 0x800) ?
                         -((~forceDataRaw[5] + (unsigned short)1) & 0x7FF) * torqueCoeff : forceDataRaw[5] * torqueCoeff;

    // 打印
    qDebug() << "Fx = " << m_forceData[0];
    qDebug() << "Fy = " << m_forceData[1];
    qDebug() << "Fz = " << m_forceData[2];
    qDebug() << "Mx = " << m_forceData[3];
    qDebug() << "My = " << m_forceData[4];
    qDebug() << "Mz = " << m_forceData[5];

    // 立即绘图
    drawForce();
}

void forceSensorChart::decode(QByteArray buf)
{
    quint16 forceDataRaw[6];
    forceDataRaw[0] = (((quint16)buf.at(1) << 8) | (quint16)buf.at(2)) >> 4;
    forceDataRaw[1] = (((quint16)buf.at(2) << 8) | (quint16)buf.at(3)) & 0xFFF;
    forceDataRaw[2] = (((quint16)buf.at(4) << 8) | (quint16)buf.at(5)) >> 4;
    forceDataRaw[3] = (((quint16)buf.at(5) << 8) | (quint16)buf.at(6)) & 0xFFF;
    forceDataRaw[4] = (((quint16)buf.at(7) << 8) | (quint16)buf.at(8)) >> 4;
    forceDataRaw[5] = (((quint16)buf.at(8) << 8) | (quint16)buf.at(9)) & 0xFFF;

    m_forceData[0] = (forceDataRaw[0] & 0x800) ?
                         -((~forceDataRaw[0] + (quint16)1) & 0x7FF) * forceCoeff : forceDataRaw[0] * forceCoeff;
    m_forceData[1] = (forceDataRaw[1] & 0x800) ?
                         -((~forceDataRaw[1] + (quint16)1) & 0x7FF) * forceCoeff : forceDataRaw[1] * forceCoeff;
    m_forceData[2] = (forceDataRaw[2] & 0x800) ?
                         -((~forceDataRaw[2] + (quint16)1) & 0x7FF) * forceCoeff : forceDataRaw[2] * forceCoeff;
    m_forceData[3] = (forceDataRaw[3] & 0x800) ?
                         -((~forceDataRaw[3] + (quint16)1) & 0x7FF) * torqueCoeff : forceDataRaw[3] * torqueCoeff;
    m_forceData[4] = (forceDataRaw[4] & 0x800) ?
                         -((~forceDataRaw[4] + (quint16)1) & 0x7FF) * torqueCoeff : forceDataRaw[4] * torqueCoeff;
    m_forceData[5] = (forceDataRaw[5] & 0x800) ?
                         -((~forceDataRaw[5] + (quint16)1) & 0x7FF) * torqueCoeff : forceDataRaw[5] * torqueCoeff;

    forceSensorPtr->forceX.push_back(m_forceData[0]);
    forceSensorPtr->forceY.push_back(m_forceData[1]);
    forceSensorPtr->forceZ.push_back(m_forceData[2]);
    forceSensorPtr->torqueX.push_back(m_forceData[3]);
    forceSensorPtr->torqueY.push_back(m_forceData[4]);
    forceSensorPtr->torqueZ.push_back(m_forceData[5]);

}

void forceSensorChart::updateCanvas(float currentTime)
{
    // 时间轴范围
    if(currentTime > timeRange) {
        switch (drawMode) {
        case FOLLOW://跟随
//            chart->scroll(timeRange, 0);//使用sroll方法
            axisX->setRange(currentTime - timeRange, currentTime);
            break;
        case SQUEEZE://全部显示
            axisX->setRange(0, currentTime);
            break;
        default:
            break;
        }
    }

}

void forceSensorChart::updateData()
{
    static float t = 0.0f;

#ifdef DATATEST
    //测试数据样本
    for (int i = 0; i < 6; ++i) {
        qreal y = 0;
        double A = QRandomGenerator::global()->bounded(3.0);
        double w = QRandomGenerator::global()->bounded(3.0);
        int offset = QRandomGenerator::global()->bounded(-3, 4);
        y = A * qSin(t + w) + offset;
        seriesForce[i]->append(t, y);
    }
    updateCanvas(t);
    t += timeInterval;
#else
    //串口打开且有数据再画图           BUG：没发送数据也画图,原因是readyRead()信号没被触发，槽函数没被执行，bytesAvailable()自然为false
    qDebug() << (serial->bytesAvailable() > 0);
    if (isPortOpen && (!isDataNull)) {
        qDebug() << "isPortOpen = " << isPortOpen;
        qDebug() << "isDataNull = " << isDataNull;
        //判断是否停止接收数据
        if(isPauseData == true) {
            t += timeInterval;
            return;
        }
        for (int i = 0; i < 6; ++i) {
            seriesForce[i]->append(t, m_forceData[i]);
        }
        updateCanvas(t);
        t += timeInterval;
    }
#endif
}

void forceSensorChart::drawForce()
{
    static float t = 0.0f;
    for (int i = 0; i < 6; ++i) {
        seriesForce[i]->append(t, m_forceData[i]);
    }
    updateCanvas(t);
    t += 1;
}


void forceSensorChart::on_btnOpenPort_toggled(bool checked)
{
    //开关端口按钮样式设置
    QPalette btnTextColor = ui->btnOpenPort->palette();
    if(checked) {
        ui->btnOpenPort->setText("关闭端口");
        btnTextColor.setColor(QPalette::ButtonText, Qt::darkGreen);
        //样式表
//        ui->btnOpenPort->setStyleSheet("background-color: green;"
//                                       "color: white");

        //打开串口
        serial->setPortName(ui->comboPortBox->currentText());
        serial->open(QSerialPort::ReadWrite);//设置读写
        if(serial->isOpen()) {
            qDebug() << "串口打开成功！";
            isPortOpen = true;
            ui->stateBar->setText("【INFO】：串口打开成功！");
            ui->comboPortBox->setDisabled(checked);//失能串口下拉框
        }
        else {
            qDebug() << "【串口打开失败！";
            ui->stateBar->setText("【WARNING】：串口打开失败！");
            QApplication::beep();//提示音
            QMessageBox::critical(this, "串口打开失败！", "该串口可能被占用，请检查串口可用性！");
            ui->btnOpenPort->setChecked(false);//弹起按键
            btnTextColor.setColor(QPalette::ButtonText, Qt::black);

        }

    }
    else {
        isPortOpen = false;
        ui->btnOpenPort->setText("打开端口");
        btnTextColor.setColor(QPalette::ButtonText, Qt::black);
        ui->btnOpenPort->setPalette(btnTextColor);

        //关闭串口
        serial->close();
        ui->stateBar->setText("【INFO】：串口已关闭！");
        ui->comboPortBox->setDisabled(checked);//使能串口下拉框
    }

    ui->btnOpenPort->setPalette(btnTextColor);


}


void forceSensorChart::on_btnRefreshPort_clicked()
{
    ui->comboPortBox->clear();
    //这样刷新太慢了
//    QStringList availablePortList;
//    foreach (const QSerialPortInfo& port, QSerialPortInfo::availablePorts()) {
//        QSerialPort serialPort;
//        serialPort.setPort(port);
//        if(serialPort.open(QIODevice::ReadWrite)) {
//            availablePortList.append(serialPort.portName());
//            serialPort.close();
//        }
//    }
//    ui->comboPortBox->addItems(availablePortList);
    //不检查能否打开
    foreach (const QSerialPortInfo& port, QSerialPortInfo::availablePorts()) {
        ui->comboPortBox->addItem(port.portName());
    }
}

void forceSensorChart::on_serial_received()
{
    if (serial->bytesAvailable() <= 0) {
        isDataNull = true;
        memset(m_forceData, 0, sizeof(m_forceData));
        return;
    }
    isDataNull = false;
    auto data = serial->readAll();
    unsigned char* recvData = reinterpret_cast<unsigned char*>(data.data());
//    for (int i = 0; i < data.length(); ++i) {
//        printf("%x", recvData[i]);
//    }
//    std::cout << std::endl;


    // 数据接收和拼接



    //判断帧头帧尾
    if ((recvData[0] == 0x48)
        && (recvData[10] == 0x0D)
        && (recvData[11] == 0x0A))
    {
#ifndef DATATEST
        forceSensorDataProcess(recvData);
//        updateData();
#endif
    }
    // 添加到显示
    ui->comRecord->append(data.toHex());
}

void forceSensorChart::on_serial_received2()
{
    if (serial->bytesAvailable() != 12) {
        qDebug() << "该次读取数据帧是" << serial->bytesAvailable() << "字节";
        return;
    }
    QByteArray dataFrame = serial->readAll();
    if ((dataFrame.at(0) == 0x48)
        && (dataFrame.at(10) == 0x0D)
        && (dataFrame.at(11) == 0x0A)) {
        decode(dataFrame);
    }
}


void forceSensorChart::on_btnClear_clicked()
{
    ui->comRecord->clear();
}


void forceSensorChart::on_btnSend_clicked()
{
    auto dataStr = ui->plainTextEdit->toPlainText();
    serial->write(dataStr.toLocal8Bit());
}

void forceSensorChart::realtimeDataPlot()
{
    if (forceSensorPtr->forceX.isEmpty()) {
        qDebug() << "没数据";
        return;
    }
    double timeNow = m_timer->elapsed()/1000.0;
    QVector<qreal> timeAxis(forceSensorPtr->forceX.size(), timeNow);
    customPlotForce->graph(0)->addData(timeAxis, forceSensorPtr->forceX);
    customPlotForce->graph(1)->addData(timeAxis, forceSensorPtr->forceY);
    customPlotForce->graph(2)->addData(timeAxis, forceSensorPtr->forceZ);
    customPlotTorque->graph(0)->addData(timeAxis, forceSensorPtr->torqueX);
    customPlotTorque->graph(1)->addData(timeAxis, forceSensorPtr->torqueY);
    customPlotTorque->graph(2)->addData(timeAxis, forceSensorPtr->torqueZ);
    // 清空数据
    forceSensorPtr->forceX.clear();
    forceSensorPtr->forceY.clear();
    forceSensorPtr->forceZ.clear();
    forceSensorPtr->torqueX.clear();
    forceSensorPtr->torqueY.clear();
    forceSensorPtr->torqueZ.clear();
    //时间轴范围
    customPlotForce->xAxis->setRange(timeNow, 8, Qt::AlignRight);
    // 绘图
    customPlotForce->replot();
    customPlotTorque->replot();

}