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


MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
//        car_control_module.set_lat_lgt_ctrl_req(false);
//        car_control_module.set_target_gear( GearPrkgAssistReq::kNoRequest);
//        car_control_module.set_target_pinion_ag_in_deg(0);
//        car_control_module.set_target_acc_mps2(0);
//        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
    //    car_control_module.sm_task_20ms();
//    QTimer * timerTask = new QTimer();
//    connect(timerTask,SIGNAL(timeout()),this,SLOT(onTimerTask()));
//    timerTask->setTimerType(Qt::PreciseTimer);
//    timerTask->start(20);

    QTimer * timer = new QTimer();
    connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
    timer->setTimerType(Qt::PreciseTimer);
    timer->start(20);

    mpthread = new std::thread(&MainWindow::threadctrl,this);
}

MainWindow::~MainWindow()
{
    car_control_module.set_lat_lgt_ctrl_req(false);
    car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
    delete ui;
}

void MainWindow::on_cBox_Handshake_clicked(bool checked)
{
    bctrlAct = checked;
    ui->cBoxGearD->setEnabled(checked);
    ui->cBoxGearR->setEnabled(checked);
    ui->cBoxGearP->setEnabled(checked);
    ui->cBoxLampR->setEnabled(checked);
    ui->cBoxLampL->setEnabled(checked);
    ui->cBoxLampH->setEnabled(checked);
    ui->sBoxEpsSet->setEnabled(checked);
    ui->sBoxSpeedSet->setEnabled(checked);
    ui->vSliderEpsSet->setEnabled(checked);
    ui->vSliderSpeedSet->setEnabled(checked);
}

void MainWindow::on_cBoxGearD_clicked(bool checked)
{
    if(checked)
    {
        gearSetVal = GearPrkgAssistReq::kTargetGearD;
        ui->cBoxGearP->setChecked(false);
        ui->cBoxGearR->setChecked(false);
    }
    else
    {
        gearSetVal = GearPrkgAssistReq::kTargetGearP;
        ui->cBoxGearD->setChecked(false);
        ui->cBoxGearR->setChecked(false);
        ui->cBoxGearP->setChecked(true);
    }
}

void MainWindow::on_cBoxGearR_clicked(bool checked)
{
    if(checked)
    {
        gearSetVal = GearPrkgAssistReq::kTargetGearR;
        ui->cBoxGearP->setChecked(false);
        ui->cBoxGearD->setChecked(false);
    }
    else
    {
        gearSetVal =GearPrkgAssistReq::kTargetGearP;
        ui->cBoxGearD->setChecked(false);
        ui->cBoxGearR->setChecked(false);
        ui->cBoxGearP->setChecked(true);
    }
}

void MainWindow::on_cBoxGearP_clicked(bool checked)
{
    gearSetVal =GearPrkgAssistReq::kTargetGearP;
    ui->cBoxGearP->setChecked(true);
    if(checked)
    {
        ui->cBoxGearD->setChecked(false);
        ui->cBoxGearR->setChecked(false);
    }
}

void MainWindow::on_cBoxLampL_clicked(bool checked)
{
    if(checked)
    {
        turnLightSetVal = TurnLightIndicReq::kLeft;
        ui->cBoxLampR->setChecked(false);
        ui->cBoxLampH->setChecked(false);
    }
    else
    {
        turnLightSetVal = TurnLightIndicReq::kOff;
        ui->cBoxLampL->setChecked(false);
        ui->cBoxLampR->setChecked(false);
        ui->cBoxLampH->setChecked(true);
    }
}

void MainWindow::on_cBoxLampR_clicked(bool checked)
{
    if(checked)
    {
        turnLightSetVal = TurnLightIndicReq::kRight;
        ui->cBoxLampL->setChecked(false);
        ui->cBoxLampH->setChecked(false);
    }
    else
    {
        turnLightSetVal = TurnLightIndicReq::kOff;
        ui->cBoxLampL->setChecked(false);
        ui->cBoxLampR->setChecked(false);
        ui->cBoxLampH->setChecked(true);
    }
}

void MainWindow::on_cBoxLampH_clicked(bool checked)
{
    turnLightSetVal = TurnLightIndicReq::kOff;
    ui->cBoxLampH->setChecked(true);
    if(checked)
    {
        ui->cBoxLampL->setChecked(false);
        ui->cBoxLampR->setChecked(false);
    }
}

void MainWindow::on_sBoxSpeedSet_valueChanged(double arg1)
{
    speedSetVal = arg1;
    ui->vSliderSpeedSet->setValue(int(arg1));
}

void MainWindow::on_vSliderSpeedSet_valueChanged(int value)
{
    speedSetVal = double(value);
    ui->sBoxSpeedSet->setValue(double(value));
}

void MainWindow::on_sBoxEpsSet_valueChanged(double arg1)
{
    EpsSetVal = arg1;
    ui->vSliderEpsSet->setValue(int(arg1));
}

void MainWindow::on_vSliderEpsSet_valueChanged(int value)
{
    EpsSetVal = double(value);
    ui->sBoxEpsSet->setValue(double(value));
}


void MainWindow::onTimerTask()
{
    if(!car_control_module.is_lat_lgt_ctrl_active() && bctrlAct){
        car_control_module.set_lat_lgt_ctrl_req(true);
//        car_control_module.set_target_gear( GearPrkgAssistReq::kTargetGearP);
//        car_control_module.set_target_pinion_ag_in_deg(car_control_module.get_current_steer_ang_in_deg());
//        car_control_module.set_target_acc_mps2(0);
//        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
//        qDebug()<<1;
    }
//    else if(car_control_module.is_lat_lgt_ctrl_active() && !bctrlAct){
//        car_control_module.set_lat_lgt_ctrl_req(false);
//        car_control_module.set_target_gear( GearPrkgAssistReq::kNoRequest);
//        car_control_module.set_target_pinion_ag_in_deg(car_control_module.get_current_steer_ang_in_deg());
//        car_control_module.set_target_acc_mps2(0);
//        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);

////        qDebug()<<2;
//    }
//    else if(!car_control_module.is_lat_lgt_ctrl_active() && !bctrlAct){
////        qDebug()<<3;
//        car_control_module.set_lat_lgt_ctrl_req(false);
//        car_control_module.set_target_gear( GearPrkgAssistReq::kNoRequest);
//        car_control_module.set_target_pinion_ag_in_deg(car_control_module.get_current_steer_ang_in_deg());
//        car_control_module.set_target_acc_mps2(0);
//        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
//    }
    else if(car_control_module.is_lat_lgt_ctrl_active() && bctrlAct){
//        qDebug()<<4;
        car_control_module.set_target_gear( GearPrkgAssistReq::kTargetGearP);
        car_control_module.set_target_pinion_ag_in_deg(90);
        car_control_module.set_target_acc_mps2(0);
        car_control_module.set_turn_light_status(TurnLightIndicReq::kRight);
    }
    car_control_module.sm_task_20ms();
}

void MainWindow::onTimer()
{
//    std::this_thread::sleep_for(std::chrono::milliseconds(10));
//    std::cout<<"time: "<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<std::endl;
//    return;
    bool status;
    GearPrkgAssistReq gearSetVal;
    GearLevelIndicate gearRealVal;
    ChassisErrCode chassErr;
    StsMach stsMach;
    float steerDeg, speed;

    status = car_control_module.is_lat_lgt_ctrl_active();
    if (status)
        ui->lineEHandSake->setText("true");
    else
        ui->lineEHandSake->setText("false");

    gearSetVal = car_control_module.get_setted_tar_gear();
    switch (gearSetVal) {
    case GearPrkgAssistReq::kNoRequest :
        ui->lineESetGear->setText("N");
        break;
    case GearPrkgAssistReq::kTargetGearD :
        ui->lineESetGear->setText("D");
        break;
    case GearPrkgAssistReq::kTargetGearP :
        ui->lineESetGear->setText("P");
        break;
    case GearPrkgAssistReq::kTargetGearR :
        ui->lineESetGear->setText("R");
        break;
    default:
        ui->lineESetGear->setText("-");
        break;
    }

    gearRealVal = car_control_module.get_cur_disp_gear();
    switch (gearRealVal) {
    case GearLevelIndicate::kManMode :
        ui->lineECurGear->setText("Man");
        break;
    case GearLevelIndicate::kDrive :
        ui->lineECurGear->setText("Auto");
        break;
    case GearLevelIndicate::kPark :
        ui->lineECurGear->setText("Park");
        break;
    case GearLevelIndicate::kNeutro :
        ui->lineECurGear->setText("Neutro");
        break;
    default:
        ui->lineECurGear->setText("Reserved");
        break;
    }
    int tmp1[10] = {0,1,2,3,4,5,6,7,8,9};
    chassErr = car_control_module.get_chassis_err_state();
    ui->lineEChaissErr->setText(QString::number(tmp1[static_cast<int>(chassErr)]));
    stsMach = car_control_module.get_chassis_statemachine_sts();
    ui->lineEMachErr->setText(QString::number(tmp1[static_cast<int>(stsMach)]));
    qDebug("chassisErr %d  state machine %d", chassErr,stsMach);
    speed = car_control_module.get_current_vehicle_spd_in_ms();
    ui->lineESpeed->setText(QString::number(speed));
    steerDeg = car_control_module.get_current_steer_ang_in_deg();
    ui->lineEEPS->setText(QString::number(steerDeg));
}

void MainWindow::threadctrl()
{
    if(car_control_module.is_lat_lgt_ctrl_active())
    {
        car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
        car_control_module.set_turn_light_status(TurnLightIndicReq::kRight);
        car_control_module.set_target_acc_mps2(0.5);
        car_control_module.set_target_pinion_ag_in_deg(-40);
    }
    else
    {
        car_control_module.set_lat_lgt_ctrl_req(true);
    }
}