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

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

    connect(ui->InPut, SIGNAL(triggered()), this, SLOT(onInputTriggered()));
    connect(ui->OutPut, SIGNAL(triggered()), this, SLOT(onOutputTriggered()));

    initPLC();
    onInputTriggered();
}

MainWindow::~MainWindow()
{
    onOutputTriggered();

    delete ui;
}

bool MainWindow::init(const std::string &filename) {
    if (!proto_tool::read_proto_param(filename, velodyne_parameters)) {
        std::cout << "失败" << std::endl;
        return false;
    }

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

    for (int i = 0; i < velodyne_parameters.region_size(); ++i) {
        ui->EntranceComboBox->addItem(QString::number(velodyne_parameters.region(i).region_id()));
    }

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

    return true;
}

bool MainWindow::initPLC() {
    m_snap7_client = &Snap7Clamp::get_instance_references();
    m_snap7_client->communication_init();
    Snap7Clamp::Snap7_communication_statu status = m_snap7_client->get_status();
    while (status == Snap7Clamp::SNAP7_COMMUNICATION_READY || status == Snap7Clamp::SNAP7_COMMUNICATION_RECEIVE) {
        sleep(1);
        status = m_snap7_client->get_status();
    }
    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::saveRecord(const std::string &fileName) {
    /**************** 作缓存记录 ****************/
    proto_tool::write_proto_param(fileName, velodyne_parameters);
}

int MainWindow::findRegionIndex(const int &region_id) {
    for (int i = 0; i < velodyne_parameters.region().size(); i++) {
        if (velodyne_parameters.region(i).has_region_id() && velodyne_parameters.region(i).region_id() == region_id) {
            return i;
        }
    }

    return -1;
}

void MainWindow::on_ComputeButton_clicked()
{
    WheelDistance wd;
    wd.currentText = ui->EntranceComboBox->currentText();
    wd.lf = ui->LeftFrontDIstance->value();
    wd.lr = ui->LeftRearDistance->value();
    wd.rf = ui->RightFrontDistance->value();
    wd.rr = ui->RightRearDistance->value();
    wd.wheel_base = ui->WheelBase->value();

    if (wd.lf == 0 && wd.lr == 0 && wd.rf == 0 && wd.rr == 0) {
        ui->textEdit->append("*?* 大哥，四个0懒得算了");
        return;
    }


    if (ui->Forward->isChecked()) {
        wd.forward = true;
    } else {
        wd.forward = false;
    }

    m_distance_map.insert(std::pair<int, WheelDistance>(record_id, wd));
//    auto iter = m_ground_region_map.find(ui->EntranceComboBox->currentText().toInt());
    int region_index = findRegionIndex(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++;
    for (int i = record_id; i < record_id_max; i++) {
        m_distance_map.erase(i);
    }
    record_id_max = record_id;

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

        velodyne_parameters.mutable_region(region_index)->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(velodyne_parameters.region(region_index).plc_reverse_offset());
        offset.set_plc_offsetx(offset.plc_offsetx() + offset_x);
        offset.set_plc_offset_degree(offset.plc_offset_degree() + offset_angle);

        velodyne_parameters.mutable_region(region_index)->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->plc_offsetx->setText(QString::number(offset.plc_offsetx(), 'f', 4));
    ui->plc_offsety->setText(QString::number(offset.plc_offsety(), 'f', 4));
    ui->plc_offset_degree->setText(QString::number(offset.plc_offset_degree(), 'f', 4));
    ui->plc_offset_wheel_base->setText(QString::number(offset.plc_offset_wheel_base(), 'f', 4));

    /**************** 计算参数清零，防止多次误计算 ****************/
    ui->LeftFrontDIstance->setValue(0);
    ui->LeftRearDistance->setValue(0);
    ui->RightFrontDistance->setValue(0);
    ui->RightRearDistance->setValue(0);

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

    saveRecord(ETC_PATH"/record/record_" + std::to_string(record_id) + ".prototxt");

    showStatu();
}


void MainWindow::on_EntranceComboBox_currentTextChanged(const QString &arg1)
{
    int region_index = findRegionIndex(ui->EntranceComboBox->currentText().toInt());
    if (region_index == -1) {
        ui->textEdit->append("find region id error");
        return;
    } else {
//        ui->textEdit->append("on_EntranceComboBox_currentTextChanged "+ arg1);
    }

    auto iter_dis = m_distance_map.find(record_id);
    if (iter_dis != m_distance_map.end()) {
        ui->LeftFrontDIstance->setValue(iter_dis->second.lf);
        ui->LeftRearDistance->setValue(iter_dis->second.lr);
        ui->RightFrontDistance->setValue(iter_dis->second.rf);
        ui->RightRearDistance->setValue(iter_dis->second.rr);
        ui->WheelBase->setValue(iter_dis->second.wheel_base);
    } else {
        ui->LeftFrontDIstance->setValue(0);
        ui->LeftRearDistance->setValue(0);
        ui->RightFrontDistance->setValue(0);
        ui->RightRearDistance->setValue(0);
    }

    if (ui->Forward->isChecked()) {
        ui->LF_Label->setText("Y3(mm)");
        ui->RF_Label->setText("Y4(mm)");
        ui->LR_Label->setText("Y5(mm)");
        ui->RR_Label->setText("Y6(mm)");

        ui->plc_offsetx->setText(QString::number(velodyne_parameters.region(region_index).plc_forward_offset().plc_offsetx(), 'f', 4));
        ui->plc_offsety->setText(QString::number(velodyne_parameters.region(region_index).plc_forward_offset().plc_offsety(), 'f', 4));
        ui->plc_offset_degree->setText(QString::number(velodyne_parameters.region(region_index).plc_forward_offset().plc_offset_degree(), 'f', 4));
        ui->plc_offset_wheel_base->setText(QString::number(velodyne_parameters.region(region_index).plc_forward_offset().plc_offset_wheel_base(), 'f', 4));
    }
    else if (ui->Reverse->isChecked()) {
        ui->LF_Label->setText("Y6(mm)");
        ui->RF_Label->setText("Y5(mm)");
        ui->LR_Label->setText("Y4(mm)");
        ui->RR_Label->setText("Y3(mm)");
        ui->plc_offsetx->setText(QString::number(velodyne_parameters.region(region_index).plc_reverse_offset().plc_offsetx(), 'f', 4));
        ui->plc_offsety->setText(QString::number(velodyne_parameters.region(region_index).plc_reverse_offset().plc_offsety(), 'f', 4));
        ui->plc_offset_degree->setText(QString::number(velodyne_parameters.region(region_index).plc_reverse_offset().plc_offset_degree(), 'f', 4));
        ui->plc_offset_wheel_base->setText(QString::number(velodyne_parameters.region(region_index).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) {
        if (!proto_tool::read_proto_param(ETC_PATH"/record/record_" + std::to_string(record_id) + ".prototxt", velodyne_parameters)) {
            std::cout << "失败" << std::endl;
        }

        auto iter = m_distance_map.find(record_id);
        if (iter != m_distance_map.end()) {
            if (iter->second.forward) {
                ui->Forward->setChecked(true);
                ui->Reverse->setChecked(false);
            } else {
                ui->Forward->setChecked(false);
                ui->Reverse->setChecked(true);
            }
            ui->EntranceComboBox->setCurrentText(iter->second.currentText);
        }

        on_EntranceComboBox_currentTextChanged(ui->EntranceComboBox->currentText());
        ui->textEdit->append("往后进行一次重做");
    } else {
        ui->textEdit->append("*?* 大哥，没动西重做了！！！！别点了！！！");
        record_id = record_id_max;
    }
}


void MainWindow::on_QuashButton_clicked()
{
    record_id--;
    if (record_id <= record_id_max && record_id >= 0) {
        if (!proto_tool::read_proto_param(ETC_PATH"/record/record_" + std::to_string(record_id) + ".prototxt", velodyne_parameters)) {
            std::cout << "失败" << std::endl;
        }

        auto iter = m_distance_map.find(record_id);
        if (iter != m_distance_map.end()) {
            if (iter->second.forward) {
                ui->Forward->setChecked(1);
            } else {
                ui->Reverse->setChecked(1);
            }
            ui->EntranceComboBox->setCurrentText(iter->second.currentText);
        }

        on_EntranceComboBox_currentTextChanged(ui->EntranceComboBox->currentText());
        ui->textEdit->append("撤销到上一次计算");
    } else {
        ui->textEdit->append("*?* 大哥，没动西撤销了！！！！别点了！！！");
        record_id = 0;
    }
}

void MainWindow::onInputTriggered() {
    QString fileName = QFileDialog::getOpenFileName(
        this,
        tr("Open File"),
        "~/",
        tr("protobuf文件 (*.prototxt *.txt)"));

    if (fileName.isEmpty()) {
        return;
    }

    init(fileName.toStdString());
}

void MainWindow::onOutputTriggered() {
    QString fileName = QFileDialog::getSaveFileName(
        this,
        tr("Save File"),
        "~/",
        tr("protobuf文件 (*.prototxt *.txt)"));

    if (fileName.isEmpty()) {
        return;
    }

    saveRecord(fileName.toStdString());
}


void MainWindow::on_AutoGetDistance_clicked()
{
    if (m_snap7_client == nullptr) {
        return;
    }
    std::unique_lock<std::mutex> t_lock1(m_snap7_client->m_data_lock);
    Snap7Clamp::PLCData plc_value = m_snap7_client->plcData;
    std::cout << plc_value.wheels[0].gap << std::endl;
    std::cout << plc_value.wheels[1].gap << std::endl;
    std::cout << plc_value.wheels[2].gap << std::endl;
    std::cout << plc_value.wheels[3].gap << std::endl;
}