#include "mainwindow.h"
#include "./ui_mainwindow.h"

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    this->init(ETC_PATH"/velodyne_manager.prototxt");
}

MainWindow::~MainWindow()
{
    /**************** 作缓存记录 ****************/

    velodyne::velodyneManagerParams velodyne_parameters;

    if (!proto_tool::read_proto_param(ETC_PATH"/velodyne_manager.prototxt", velodyne_parameters)) {
        std::cout << "失败" << std::endl;
            return;
    }

    velodyne_parameters.clear_region();
    for (auto &region: m_ground_region_map) {
        velodyne_parameters.add_region()->CopyFrom(region.second);
    }

    proto_tool::write_proto_param(ETC_PATH"/velodyne_manager.prototxt", velodyne_parameters);

    delete ui;
}

bool MainWindow::init(const std::string &filename) {
    velodyne::velodyneManagerParams velodyne_parameters;

    if (!proto_tool::read_proto_param(ETC_PATH"/velodyne_manager.prototxt", velodyne_parameters)) {
        std::cout << "失败" << std::endl;
        return false;
    }

    ui->Forward->setChecked(true);
    ui->EntranceComboBox->clear();

    m_ground_region_map.clear();
    for (int i = 0; i < velodyne_parameters.region_size(); ++i) {
//        std::cout << velodyne_parameters.region(i).region_id() << std::endl;
        m_ground_region_map.insert(std::pair<int, velodyne::Region>(velodyne_parameters.region(i).region_id(), velodyne_parameters.region(i)));
        ui->EntranceComboBox->addItem(QString::number(velodyne_parameters.region(i).region_id()));
    }

    showStatu();
    return true;
}

void MainWindow::showStatu() {
    QString entrance_txt = "入口id: " + ui->EntranceComboBox->currentText();

    QString direction;
    if (ui->Forward->isChecked()) {
        direction = "正向";
    }
    else if (ui->Reverse->isChecked()) {
        direction = "反向";
    }

    QString record_txt = "当前记录id: " + QString::number(record_id) + ", 最大记录id: " + QString::number(record_id_max);

    ui->statusbar->showMessage(entrance_txt + ", " +
                               direction + ", " +
                               record_txt);
}


void MainWindow::on_ComputeButton_clicked()
{
    auto iter = m_ground_region_map.find(ui->EntranceComboBox->currentText().toInt());

    /**************** 作计算处理 ****************/
    double left_dis = ui->LeftFrontDIstance->value() - ui->LeftRearDistance->value();
    double right_dis = ui->RightFrontDistance->value() - ui->RightRearDistance->value();
    double angle_M = std::asin((left_dis - right_dis) * 0.5 / ui->WheelBase->value());
    double offset_angle = -(angle_M * 180.0 / M_PI);

    double front_dis = ui->LeftFrontDIstance->value() - ui->RightFrontDistance->value();
//    double rear_dis = ui->LeftRearDistance->value() - ui->RightRearDistance->value();
    double offset_x = -((front_dis) * 0.5 - (left_dis - right_dis) * 0.25) / 1000;

    record_id++;
    record_id_max = record_id;

    velodyne::PlcOffset offset;
    if (ui->Forward->isChecked()) {
        offset.CopyFrom(iter->second.plc_forward_offset());
        offset.set_plc_offsetx(iter->second.plc_forward_offset().plc_offsetx() + offset_x);
        offset.set_plc_offset_degree(iter->second.plc_forward_offset().plc_offset_degree() + offset_angle);

        iter->second.mutable_plc_forward_offset()->CopyFrom(offset);
        ui->textEdit->append("*" + QString::number(record_id) + "* " + ui->EntranceComboBox->currentText() + "正向 计算结果： x轴需要调整" + QString::number(offset_x, 'f', 4)  + ", 角度需要调整" + QString::number(offset_angle, 'f', 4) + "度。");
    }
    else if (ui->Reverse->isChecked()) {
        offset.CopyFrom(iter->second.plc_reverse_offset());
        offset.set_plc_offsetx(iter->second.plc_reverse_offset().plc_offsetx() + offset_x);
        offset.set_plc_offset_degree(iter->second.plc_reverse_offset().plc_offset_degree() + offset_angle);

        iter->second.mutable_plc_reverse_offset()->CopyFrom(offset);
        ui->textEdit->append("*" + QString::number(record_id) + "* " + ui->EntranceComboBox->currentText() + "反向 计算结果： x轴需要调整" + QString::number(offset_x, 'f', 4) + ", 角度需要调整" + QString::number(offset_angle, 'f', 4) + "度。");
    }
    /**************** 计算参数清零，防止多次误计算 ****************/
    ui->LeftFrontDIstance->setValue(0);
    ui->LeftRearDistance->setValue(0);
    ui->RightFrontDistance->setValue(0);
    ui->RightRearDistance->setValue(0);

    /**************** 作缓存记录 ****************/

    velodyne::velodyneManagerParams velodyne_parameters;

    if (!proto_tool::read_proto_param(ETC_PATH"/velodyne_manager.prototxt", velodyne_parameters)) {
        std::cout << "失败" << std::endl;
            return;
    }

    velodyne_parameters.clear_region();
    for (auto &region: m_ground_region_map) {
            velodyne_parameters.add_region()->CopyFrom(region.second);
    }

    proto_tool::write_proto_param(ETC_PATH"/record/record_" + std::to_string(record_id) + ".prototxt", velodyne_parameters);

    showStatu();
}


void MainWindow::on_EntranceComboBox_currentTextChanged(const QString &arg1)
{
    auto iter = m_ground_region_map.find(arg1.toInt());

    ui->LeftFrontDIstance->setValue(0);
    ui->LeftRearDistance->setValue(0);
    ui->RightFrontDistance->setValue(0);
    ui->RightRearDistance->setValue(0);

    if (ui->Forward->isChecked()) {
        ui->plc_offsetx->setText(QString::number(iter->second.plc_forward_offset().plc_offsetx(), 'f', 4));
        ui->plc_offsety->setText(QString::number(iter->second.plc_forward_offset().plc_offsety(), 'f', 4));
        ui->plc_offset_degree->setText(QString::number(iter->second.plc_forward_offset().plc_offset_degree(), 'f', 4));
        ui->plc_offset_wheel_base->setText(QString::number(iter->second.plc_forward_offset().plc_offset_wheel_base(), 'f', 4));
    }
    else if (ui->Reverse->isChecked()) {
        ui->plc_offsetx->setText(QString::number(iter->second.plc_reverse_offset().plc_offsetx(), 'f', 4));
        ui->plc_offsety->setText(QString::number(iter->second.plc_reverse_offset().plc_offsety(), 'f', 4));
        ui->plc_offset_degree->setText(QString::number(iter->second.plc_reverse_offset().plc_offset_degree(), 'f', 4));
        ui->plc_offset_wheel_base->setText(QString::number(iter->second.plc_reverse_offset().plc_offset_wheel_base(), 'f', 4));
    }

    showStatu();
}


void MainWindow::on_Forward_clicked()
{
    on_EntranceComboBox_currentTextChanged(ui->EntranceComboBox->currentText());
}


void MainWindow::on_Reverse_clicked()
{
    on_EntranceComboBox_currentTextChanged(ui->EntranceComboBox->currentText());
}

void MainWindow::on_RedoButton_clicked()
{
    record_id++;
    if (record_id <= record_id_max && record_id >= 0) {
        init(ETC_PATH"/record/record_" + std::to_string(record_id) + ".prototxt");
    } else {
        record_id = record_id_max;
    }
}


void MainWindow::on_QuashButton_clicked()
{
    record_id--;
    if (record_id <= record_id_max && record_id >= 0) {
        init(ETC_PATH"/record/record_" + std::to_string(record_id) + ".prototxt");
    } else {
        record_id = 0;
    }
}