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

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

    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(50);
}

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);
    }
    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(0);
        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){
        car_control_module.set_target_gear( gearSetVal);
        car_control_module.set_target_pinion_ag_in_deg(EpsSetVal);

        double speed = car_control_module.get_current_vehicle_spd_in_ms();
        if(speed < 10)
            car_control_module.set_target_acc_mps2(speedSetVal);
        else
            car_control_module.set_target_acc_mps2(0);
        car_control_module.set_turn_light_status(turnLightSetVal);
    }
    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)]));
    QString strLog;
    strLog.sprintf("%s %d %s %d","chassisErr ", chassErr,  "state machine ",stsMach);
    ui->textBrLog->append(strLog);
    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::keyPressEvent(QKeyEvent* event){
    qDebug()<<"keypress!";
    //(event->modifiers() == (Qt::ControlModifier|Qt::ShiftModifier)) &&
    if((event->key() == Qt::Key_W))
    {
        qDebug()<<"acc up";
        speedSetVal+=0.1;
        ui->sBoxSpeedSet->setValue(speedSetVal);
    }
    else if((event->key() == Qt::Key_S))
    {
        qDebug()<<"acc Down";
        speedSetVal-=0.1;
        ui->sBoxSpeedSet->setValue(speedSetVal);
    }
    //(event->modifiers() == (Qt::ControlModifier|Qt::ShiftModifier)) &&
    else if( (event->key() == Qt::Key_A))
    {
        qDebug()<<"turn left"<<EpsSetVal;
        ui->sBoxEpsSet->setValue(EpsSetVal);
        EpsSetVal+=5;
    }
    else if((event->key() == Qt::Key_D))
    {
        qDebug()<<"turn right"<<EpsSetVal;
        ui->sBoxEpsSet->setValue(EpsSetVal);
        EpsSetVal-=5;
    }
};