/* #########################################
 * Traffic monitoring using Raspberry PI
 * Author: Michal Zacpal
 * MAIN WINDOW FILE
 * 2015
 * ######################################### */
#include "mainwindow.h"
#include "ui_mainwindow.h"

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

/* Variables + Init ===================================================================== */
    iWidth = 1280;       // Main window width
    iHeight = 720;      // Main window height
    bSidePanel = false;
    bPopUpWindow = false;
    bSettingsWin = false;
    bLDWWin = false;
    bCamWin = false;
    bLDWinPic = false;
    bGrabbing = false;
    bFrontCamActive = false;
    bRearCamActive = false;
    iRearCam = 0;
    Path = "/home/pi/TrafficMonitoring_Gallery";        // For saving images
    iPic = 0;
    iGrabPeriod = 1000;      // Grabbing period
    bGrabFront = false;
    bGrabRear = false;
    iROIFramePos = 220;
    iprevROIFramePos = iROIFramePos;
    bLDWROISet = false;
    iRoiH = 100;
    bLDWSActive = false;
    f = 122;
    H = 170;
    theta = 130;        // Uhel mezi noramlou kamery a zemi
    rearf = 192;
    rearH =205;
    rearTheta = 144 ;        // Uhel mezi noramlou kamery a zemi
    iLaneOffset = 30;
    iMaxLaneWidth = MAX_LANE_WIDTH;
    iMinLaneWidth = MIN_LANE_WIDTH;
    iSDCount = 60;
    bRearTVTM = false;
    TripStart = QTime::currentTime();
    bParkView = false;
    bLDWSwasAct = false;
    bPrintTemp = false;
    bKey = true;
/* Variables + Init ===================================================================== */
/* Set up MainWindow, style etc.========================================================= */
    this->setFixedWidth(iWidth);        // Width
    this->setFixedHeight(iHeight);      // Height
    ui->frame_screen_front->setVisible(false);
    // Main window style
    this->setStyleSheet(
            "background-color: rgb(54, 64, 72);"
            // "color: rgb(0, 170, 255);"
            "color: rgb(255, 255,255);"
           // "background: qradialgradient(cx:0.5, cy:0.5, radius: 0.65,fx:0.5, fy:0.5, stop:0 gray, stop:1 rgb(54, 64, 72));"
                );
    // Time style
    ui->btn_time->setStyleSheet("background-color: rgba(255, 255, 255, 0);"
                                "border: 2px solid #555;"
                                "border-radius: 4px;"
                                "padding: 5px;"
                                );
    // Setting visibility
    AllFramesInvisible();
    ui->btnCar->setVisible(true);
    ui->btn_ShowHideSideFrame->setIcon(QIcon(QString(":/icons/icons/arrow_down.png")));
    ui->lbl_Title->setStyleSheet("background-color: rgba(255, 255, 255, 0);");
    ui->btnRightLine->setVisible(false);
    ui->btnLeftLine->setVisible(false);
/*^Set up MainWindow, style etc.========================================================= */
/* Set timers =========================================================================== */
    // start timer - for displaying animation and picture from camera
    tmrTimer = new QTimer(this);
    connect(tmrTimer, SIGNAL(timeout()),this, SLOT(processFrameAndupdateGUI()));
    tmrTimer->setInterval(50);
    tmrTimer->start();
    // CrateShutDown timer
    tmrShutDown = new QTimer(this);
    connect(tmrShutDown,SIGNAL(timeout()),this,SLOT(ShutDownCounting()));
    tmrShutDown->setInterval(1000);
    // Temperature print timer
    tmrTemperature = new QTimer(this);
    connect(tmrTemperature,SIGNAL(timeout()),this,SLOT(PrintTmpRoutine()));
    tmrTemperature->setInterval(300000);
    tmrTemperature->start();
/*^Set timers =========================================================================== */
    // Initilalize GPIO pins
    GPIOInit();
    // Set Power pin
    digitalWrite(POWER_PIN,HIGH);
    // Initialize TVTM
    TVTMInit();
    RearTVTMInit();
    // Things which could be in loop or thread
    InfoAlarm(0);
}
/* Timer routine ======================================================================================================================== */
void MainWindow::processFrameAndupdateGUI()
{
    ActTime = QTime::currentTime();
    // Display front camera ******************************************************************
    if (bFrontCamActive && !bParkView)
    {
        if (VidFrontCam.isOpened()){
            VidFrontCam.grab();
            VidFrontCam.retrieve(matFrontImg);
            if (bLDWSActive){
              // Display lines ----------------------------------------------------------
                DispLDWs = LDWRoutine(matFrontImg);
                // Right line
                if (!RightLane.NoLane){
                    ui->btnRightLine->setVisible(true);
                    switch (DetermineLane(RightLaneArr)){
                    case 0: ui->btnRightLine->setVisible(false); break;
                    case 1: ui->btnRightLine->setIcon((QIcon(QString(":/icons/icons/SolidLine.png")))); break;
                    case 2: ui->btnRightLine->setIcon((QIcon(QString(":/icons/icons/DashedLine.png"))));
                    }
                }
                else{
                    ui->btnRightLine->setVisible(false);
                }
                // Left line
                if (!LeftLane.NoLane){
                    ui->btnLeftLine->setVisible(true);
                    ui->btnLeftLine->setVisible(true);
                    switch (DetermineLane(LeftLaneArr)){
                    case 0: ui->btnLeftLine->setVisible(false); break;
                    case 1: ui->btnLeftLine->setIcon((QIcon(QString(":/icons/icons/SolidLine.png")))); break;
                    case 2: ui->btnLeftLine->setIcon((QIcon(QString(":/icons/icons/DashedLine.png"))));
                    }
                }
                else{
                    ui->btnLeftLine->setVisible(false);
                }
            }
            else{
                ui->btnRightLine->setVisible(false);
                ui->btnLeftLine->setVisible(false);
            }
            if (!bPopUpWindow)
                ui->frame_screen_front->setPixmap(QPixmap::fromImage(Mat2QImage(matFrontImg,ui->frame_video_front->width(),ui->frame_video_front->height())));                                   // Display on screen object
            if (bPopUpWindow)
            {
                if (bLDWROISet) // If ROI setting draw a rectangle
//                    rectangle(matFrontImg,Point(0,iROIFramePos),Point(iROIFramePos+iRoiH, ui->frame_video_2->width()), Scalar () -1,8);
                rectangle(matFrontImg,Point(0,iROIFramePos),Point(matFrontImg.cols,iROIFramePos+iRoiH),Scalar(255,236,0),4,8);
                ui->frame_video_2->setPixmap(QPixmap::fromImage(Mat2QImage(matFrontImg,ui->frame_video_2->width(),ui->frame_video_2->height())));                                   // Display on screen object

                if(bLDWSActive)
                {
                    if (!DispLDWs.empty())
                        ui->frame_video_LDW->setPixmap(QPixmap::fromImage(Mat2QImage(DispLDWs,ui->frame_video_LDW->width(),ui->frame_video_LDW->height())));                                   // Display on screen object
                }
            }
        }
    }
    // Display rear camera ******************************************************************
    if (bRearCamActive)
    {
            VidRearCam >> matRearImg;
            if (!bPopUpWindow)
                ui->frame_screen_rear->setPixmap(QPixmap::fromImage(Mat2QImage(matRearImg,ui->frame_video_rear->width(),ui->frame_video_rear->height())));                                   // Display on screen object
            if (bPopUpWindow && !bParkView)
                ui->frame_video_3->setPixmap(QPixmap::fromImage(Mat2QImage(matRearImg,ui->frame_video_3->width(),ui->frame_video_3->height())));                                   // Display on screen object
    }
    // Display parking camera ***************************************************************
    if (bPopUpWindow && bParkView && bRearCamActive){
        _ParkView = ParkView(matRearImg);
        ui->frame_video_2->setPixmap(QPixmap::fromImage(Mat2QImage(matRearImg,ui->frame_video_2->width(),ui->frame_video_2->height())));                                   // Display on screen object
        ui->frame_video_3->setPixmap(QPixmap::fromImage(Mat2QImage(_ParkView,ui->frame_video_3->width(),ui->frame_video_3->height())));                                   // Display on screen object
    }
    // Display LDW **************************************************************************
    if (bPopUpWindow && bLDWSActive){
        if (!DispLDWs.empty())
            ui->frame_video_LDW->setPixmap(QPixmap::fromImage(Mat2QImage(DispLDWs,ui->frame_video_LDW->width(),ui->frame_video_LDW->height())));                                   // Display on screen object
    }
    // Display time *************************************************************************
    ui->btn_time->setText(GetSystemTime(2));
    // Check if key is in ignition
    CheckKeyIngnition();
}
/* Timer routine ======================================================================================================================== */
void MainWindow::SaveImgTimerRoutine()
{
    // Save image from front cam
    if (ui->cbFrontCamSave->isChecked())
    {

        if(!save_pic(Path,matFrontImg,0))
        {
            InfoMessage("Snimek nebyl ulozen.",true);
        }
        if(!save_pic(Path,DispLDWs,2)){
            {
                InfoMessage("Snimek nebyl ulozen.",true);
            }
        }
    }
    // Save image from rear cam
    if (ui->cbRearCamSave->isChecked())
    {
        if(!save_pic(Path,matRearImg,1))
        {
            InfoMessage("Snimek nebyl ulozen.",true);
        }
    }

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


/*
 * Hide or show side frame with controls items (maximalized, minimalized, disable/enable sound)
 */
void MainWindow::on_btn_ShowHideSideFrame_clicked()
{
    // Hide panel
    if (bSidePanel){
        ui->frame_side->setVisible(false);
        ui->btn_ShowHideSideFrame->setIcon(QIcon(QString(":/icons/icons/arrow_down.png")));
        bSidePanel = false;
    }
    // Show panel
   else{
        ui->frame_side->setVisible(true);
        ui->btn_ShowHideSideFrame->setIcon(QIcon(QString(":/icons/icons/arrow_up.png")));
        bSidePanel = true;
    }
}
/*
 *  Button - minimalizing screen
 */
void MainWindow::on_btn_minim_clicked()
{
    if (!MainWindow::isFullScreen()){
        // ui->statusBar->showMessage("Screen is already minimalized.");
        // ui->statusBar->showMessage("Okno je ji? minimalizovan.");
        InfoMessage("Okno je jiz minimalizovan.",true);
        return;
    }
    MainWindow::showNormal();

}
/*
 * Button - maximalizing screen
 */
void MainWindow::on_btn_maxim_clicked()
{
    if (MainWindow::isFullScreen()){
        // ui->statusBar->showMessage("Screen is already at full screen.");
        // ui->statusBar->showMessage("Okno je ji maximalizovan.");
        InfoMessage("Okno je jiz maximalizovan.",true);
        return;
    }
    MainWindow::showFullScreen();
}
/*
 * Button - Enable/disable sound
 */
void MainWindow::on_btn_sound_clicked()
{
    // disable sound
    if (bSound)
    {
        ui->btn_sound->setIcon(QIcon(QString(":/icons/icons/sound_on.png")));
        // ui->statusBar->showMessage("Zvuk je vypnut.");
        InfoMessage("Zvuk je vypnut.",true);
        // TODO: Disable sound - Only Buzzer (with radio also radio)
        bSound = false;

    }
    // enable sound
    else
    {
        ui->btn_sound->setIcon(QIcon(QString(":/icons/icons/sound_off.png")));
        // ui->statusBar->showMessage("Zvuk je zapnut");
        InfoMessage("Zvuk je zapnut.",true);
        // TODO: Enable sound - Only Buzzer (with radio also radio)
        bSound =  true;
    }
}
/*
 * Button - Settings
 */
void MainWindow::on_btnSettings_clicked()
{
    if (bPopUpWindow)
    {
        //ui->statusBar->showMessage("Nastaven je ji? otev?eno. Nebo okno vy?uv jin funkce.");
        InfoMessage("Nastaveni je jiz otevreno. Nebo okno vyuziva jina funkce.",true);
        return;
    }
    if(bLDWSActive)
    {
        on_btnLDW_clicked();
    }
    bPopUpWindow = true;                    // Pop up window is open
    bSettingsWin = true;
    ui->frame_popup->setVisible(true);      // Show pop up window
    ui->frame_settings->setVisible(true);   // Show Settings things
    FillPopUpWin(QString("Nastaveni"));     // Set Title
}
void MainWindow::FillPopUpWin(QString qsTitle)
{
    ui->lbl_Title->setText(qsTitle);
}
/*
 * Button - Close pop up win
 */
void MainWindow::on_btn_ClosePUWin_clicked()
{
    ui->btn_SavePPWin->setEnabled(true);    // Enable save btn if it is disabled
    AllFramesInvisible();
    bLDWROISet = false;
    bParkView = false;
}
/*
 * Button - Save settings
 */
void MainWindow::on_btn_SavePPWin_clicked()
{
    AllFramesInvisible();
    // Zkontroluj checkboxy a uloz z jakejch kamer se ma ukladat
    bGrabFront = ui->cbFrontCamSave->isChecked();
    bGrabRear = ui->cbRearCamSave->isChecked();
    bLDWinPic = ui->cbLDW->isChecked();
    bLDWROISet = false;
    iprevROIFramePos = iROIFramePos;

}
/*
 * Alaram
 */
void MainWindow::InfoAlarm(int iAlarm)
{
    switch (iAlarm){
    case 0: {   ui->btn_Alarm->setIcon(QIcon(QString(":/icons/icons/OK.png")));
                ui->lbl_Warning->setText("OK");
                ui->lbl_Warning->setStyleSheet("color: rgb(0, 169, 236);""background-color: rgb(255,255,255,0);");
                break;}
    case 1: {   ui->btn_Alarm->setIcon(QIcon(QString(":/icons/icons/frost.png")));
                //ui->lbl_Warning->setText("Varovn nmrazy!");
                ui->lbl_Warning->setText("Namraza");
                ui->lbl_Warning->setStyleSheet("color: rgb(205, 33, 10);""background-color: rgb(0,0,0,0);");
                break;}
    case 2: {   ui->btn_Alarm->setIcon(QIcon(QString(":/icons/icons/coffe.png")));
                //ui->lbl_Warning->setText("as na pestvku!");
                ui->lbl_Warning->setText("Prestavka");
                ui->lbl_Warning->setStyleSheet("color: rgb(205, 33, 10);""background-color: rgb(255,255,255,0);");}
    }
}
/*
 * Show Information message and if sound enable then beep (error or info beep)
 */
void MainWindow::InfoMessage(QString sMessage, bool bBeep)
{
    ui->statusBar->showMessage(sMessage);
    ui->LogBox->addItem( QString(GetSystemTime(5) + " |" + sMessage));
    if (bBeep){
        PlaySound();
    }

}
/*
 * Button - Camera grabbing
 */
void MainWindow::on_btnCam_clicked()
{
    if(bGrabbing)
    {
        bGrabbing = false;
        tmrGrab->stop();
        // Resit pres zmenu barvy tlacitka
        InfoMessage("Zaznam ukoncen.",false);
        ui->btnCam->setStyleSheet(
           "border: 4px solid #555;"
        );

    }
    else
    {
        if(!bFrontCamActive && bGrabFront)
        {
            InfoMessage("Predni kamera neni aktivni",true);
            return;
        }
        if(!bRearCamActive && bGrabRear)
        {
            InfoMessage("Zadni kamera neni aktivni",true);
            return;
        }
        if (!bGrabFront && !bGrabRear)
        {
            InfoMessage("Neni zvolena kamera pro zaznam. Nastaveni -> zaznam",true);
            return;
        }
        // start Grabbing timer - for saving image
        tmrGrab = new QTimer(this);
        connect(tmrGrab, SIGNAL(timeout()),this, SLOT(SaveImgTimerRoutine()));
        tmrGrab->setInterval(iGrabPeriod);
        tmrGrab->start();
        bGrabbing = true;
        InfoMessage("Zaznam zahajen.",false);
        ui->btnCam->setStyleSheet(
            "border: 2px solid;"
            "border-color: rgb(0, 172, 217);"
        );
    }

}
/*
 * Button - camera picture resize show
 */
void MainWindow::on_btnResizeFront_clicked()
{
    if (bPopUpWindow)
    {
        InfoMessage("Okno je jiz otevreno. Nebo jej vyuvziva jina funkce.",true);
        return;
    }
    bPopUpWindow = true;                        // Pop up window is open
    bCamWin =  true;
    ui->frame_popup->setVisible(true);          // Show pop up window
    ui->frame_video_2->setVisible(true);        // Show video front cam
    ui->frame_video_3->setVisible(true);        // Show video rear cam
    FillPopUpWin(QString("Obraz z kamer"));     // Set Title
    ui->btn_SavePPWin->setEnabled(false);       // Disable save button
}
/*
 *Button Front Cam
 */
void MainWindow::on_btnFrontCam_clicked()
{
    if (!bFrontCamActive)
    {
        StartFrontCam();

    }
    // Stop Front cam
    else
    {
           StopFrontCam();

    }

}
void MainWindow::on_btnRearCam_clicked()
{
    // Start Rear CAM
    if (!bRearCamActive)
        StartRearCam();
    // Stop Rear CAM
    else
        StopRearCam();

}
void MainWindow::on_btnResizeRear_clicked()
{
    if (bPopUpWindow)
    {
        InfoMessage("Okno je jiz otevreno. Nebo jej vyuvziva jina funkce.",true);
        return;
    }
    if (!bRearTVTM){
        RearTVTMInit();
        InfoMessage("Provadi se vypocet LUT pro zadni kameru", false);
    }
    bPopUpWindow = true;                        // Pop up window is open
    bCamWin =  true;
    bParkView = true;
    ui->frame_popup->setVisible(true);          // Show pop up window
    ui->frame_video_2->setVisible(true);        // Show video rear cam
    ui->frame_video_3->setVisible(true);        // Show Bird eye view
    FillPopUpWin(QString("Parkovaci kamera"));     // Set Title
    ui->btn_SavePPWin->setEnabled(false);       // Disable save button
    if (bLDWSActive){
        bLDWSActive = false;
        bLDWSwasAct = true;
    }
}
void MainWindow::on_btnLDW_clicked()
{
    // Deactivate LDWS
    if (bLDWSActive)
    {
        bLDWSActive  = false;
        if (bPopUpWindow) on_btn_ClosePUWin_clicked();
        ui->btnLDW->setStyleSheet(
           "border: 4px solid #555;");
        InfoMessage("LDS deaktivovan.",false);
        return;
    }
    if (bPopUpWindow)
    {
        //ui->statusBar->showMessage("Nastaven je jiz otevreno. Nebo okno vy?uv jin funkce.");
        InfoMessage("LDS je jiz otevren. Nebo okno vyuziva jina funkce.",true);
        return;
    }
    if (bLDWinPic)
    {
        bPopUpWindow = true;                    // Pop up window is open
        bLDWWin = true;
        ui->frame_popup->setVisible(true);      // Show pop up window
        ui->frame_LDW->setVisible(true);   // Show LDW things
        FillPopUpWin(QString("LDS - Lane Detection system"));     // Set Title
        ui->btn_SavePPWin->setEnabled(false);   // Disable save button
        ui->frame_video_2->setVisible(true);
        ui->frame_video_LDW->setVisible(true);
        if(!bFrontCamActive) StartFrontCam();
    }
    bLDWSActive = true;
    InfoMessage("LDS aktivovan.",false);
    ui->btnLDW->setStyleSheet(
        "border: 2px solid;"
        "border-color: rgb(0, 172, 217);"
            );
}

void MainWindow::AllFramesInvisible()
{
    ui->frame_side->setVisible(false);
    ui->frame_popup->setVisible(false);
    ui->frame_settings->setVisible(false);
    ui->frame_video_2->setVisible(false);
    ui->frame_video_3->setVisible(false);
    ui->frame_LDW->setVisible(false);
    bPopUpWindow = false;
    bSettingsWin = false;
    bLDWWin = false;
    if (bLDWSwasAct){
        bLDWSwasAct = false;
        bLDWSActive = true;}
}
/*
 * ShutDown button
 */
void MainWindow::on_btnShutDown_clicked()
{
    QMessageBox::StandardButton reply;
    tmrShutDown->start();
    reply = QMessageBox::question(this,"Ukoncen aplikace", "Opravdu chcete zarizeni vypnout?",QMessageBox::Yes | QMessageBox::No);
    if (reply == QMessageBox::Yes)
    {
        system("sudo shutdown -f \"now\"");
    }
     if (reply == QMessageBox::No){
        tmrShutDown->stop();
        iSDCount = 60;
        InfoMessage("",false);
        bKey = false;
    }
}
void MainWindow::ShutDownCounting(){
    iSDCount--;
    InfoMessage("Zarizeni se vypne za: "+ QString::number(iSDCount) + " s.",false);
    if (iSDCount <= 0){
        system("sudo shutdown -f \"now\"");
    }
}
void MainWindow::MoveCar(int position)
{
    int iwidth = ui->btnCar->width();
    int iheight = ui->btnCar->height();
    switch (position)
    {
        case CAR_CENTER : ui->btnCar->setGeometry(120,60,iwidth,iheight); break;
        case CAR_LEFT : ui->btnCar->setGeometry(80,80,iwidth,iheight); break;
        case CAR_RIGHT : ui->btnCar->setGeometry(180,80,iwidth,iheight); break;
    }
}
void MainWindow::on_btnLDWSetROI_clicked()
{
    if(!bFrontCamActive)
    {
        StartFrontCam();
    }
    bLDWROISet = true;
    iROIFramePos = iprevROIFramePos;
    ui->frame_video_2->setVisible(true);
}
void MainWindow::on_btnROIUP_clicked()
{
    if (iROIFramePos - 10 > 0)
    {iROIFramePos = iROIFramePos - 10;}
}
void MainWindow::on_btnROIDown_clicked()
{
    if (iROIFramePos + 10 < matFrontImg.rows)
    {iROIFramePos = iROIFramePos + 10;}
}
bool MainWindow::StartFrontCam()
{
    // Start Front CAM
    if (!bFrontCamActive){
        // Set camera settings
        VidFrontCam.set(CV_CAP_PROP_FRAME_WIDTH, 640);
        VidFrontCam.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
        VidFrontCam.set(CV_CAP_PROP_FORMAT, CV_8UC3);
        VidFrontCam.open();                             // Open cam
        if(!VidFrontCam.isOpened())
           return false;
        ui->frame_screen_front->setVisible(true);       // Enable imager
        ui->btnResizeFront->setEnabled(true);           // Enable button
        bFrontCamActive = true;
        // beep
        ui->btnFrontCam->setStyleSheet(
            "border: 2px solid;"
            "border-color: rgb(0, 172, 217);"
                );
        return true;
    }
    return false;
}
bool MainWindow::StartRearCam(){
    if (!bRearCamActive){
        // Set camera settings
        VidRearCam.set(CV_CAP_PROP_FRAME_WIDTH, 640);
        VidRearCam.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
        //VidRearCam.set(CV_CAP_PROP_FORMAT, CV_8UC1);
        VidRearCam.open(iRearCam);                             // Open cam             - Nebude kamera pob?? po?d!!!
        if(!VidRearCam.isOpened())
            return false;
        ui->frame_screen_rear->setVisible(true);       // Enable imager
        ui->btnResizeRear->setEnabled(true);           // Enable button
        bRearCamActive = true;
        // beep
        ui->btnRearCam->setStyleSheet(
            "border: 2px solid;"
            "border-color: rgb(0, 172, 217);"
                );
        return true;
    }
    return false;
}
void MainWindow::StopRearCam(){
    if (bRearCamActive)
    {
        VidRearCam.release();                      // Close Cam                - Nebude kamera pob?? po?d!!!
        bRearCamActive = false;
        ui->frame_screen_rear->setVisible(false);  // Disable imager
        // TODO: Set defaul image
        ui->btnResizeRear->setEnabled(false);      // Disable button
        // beep
        ui->btnRearCam->setStyleSheet(
           "border: 4px solid #555;");
    }
}
void MainWindow::StopFrontCam()
{
    if (bFrontCamActive)
    {
        VidFrontCam.release();                      // Close Cam
        bFrontCamActive = false;
        ui->frame_screen_front->setVisible(false);  // Disable imager
        // TODO: Set defaul image
        ui->btnResizeFront->setEnabled(false);      // Disable button
        // beep
        ui->btnFrontCam->setStyleSheet(
           "border: 4px solid #555;");
    }
}
bool MainWindow::TVTMInit()
{
    Mat inImg;
    if (StartFrontCam()){
        VidFrontCam.grab();
        VidFrontCam.retrieve(inImg);
        StopFrontCam();
    }
    else
        InfoMessage("Front TVTM init - Front CAM disconected." ,false);
    if (inImg.empty()){
        inImg = imread("TVTMINIT.png",0);
    }
    else{
        cvtColor(inImg,inImg,CV_BGR2GRAY);
        inImg.convertTo(inImg,CV_8UC1,1,50);
    }
    Mat RoiImg1;
    Mat RoiImg = Mat::zeros(220,inImg.cols,inImg.type());
    Rect RoiRect(0,220,inImg.cols,iRoiH);
    Rect RoiRect1(0,0,inImg.cols,iRoiH);
    inImg(RoiRect).copyTo(RoiImg1);
    RoiImg1.copyTo(RoiImg(RoiRect1));
    TVTModel =  OptimizeTVTM(RoiImg, H, f,theta);
    CenterX = TVTModel.lut[RoiImg.cols/2].y - TVTModel.a;
    InfoMessage("Front TVTM init - DONE" ,false);
    return true;
}
bool MainWindow::RearTVTMInit(){
    Mat inImg;
    if (!bRearCamActive){
        if(StartRearCam()){
           VidRearCam >> inImg;
           StopRearCam();
        }
        else
            InfoMessage("Rear TVTM init - Rear CAM disconected." ,false);
    }
    if (inImg.empty())
        inImg = imread("TVTMINIT.png",0);
    else
        cvtColor(inImg,inImg,CV_BGR2GRAY);
    Rect _roi(0,inImg.rows/2,inImg.cols,inImg.rows/2);
    Mat _RoiImg;
    inImg(_roi).copyTo(_RoiImg);
    rearTV = OptimizeTVTM(_RoiImg, rearH, rearf,rearTheta);
    bRearTVTM = true;
    InfoMessage("Rear TVTM init - DONE",false);
    return true;
}

Mat MainWindow::LDWRoutine(Mat InImg)
{
    Mat Out;
    // Display TVTM etc.
    Mat RoiImg;
    Rect ROI(0,iROIFramePos,matFrontImg.cols,iRoiH);
    InImg(ROI).copyTo(RoiImg);
    cvtColor(RoiImg,RoiImg,CV_BGR2GRAY);
    Mat LDWSImg = RetTVImg(RoiImg,TVTModel,true);
    Mat InterpolImg = InterpolTVTM(LDWSImg);
    GaussianBlur(InterpolImg, InterpolImg, Size(9,3), 3, 0,BORDER_REFLECT);
    // Lane departure warning system *********
    // Edged
    Mat Edge = ImEdge(InterpolImg);
    Edge.convertTo(Edge,CV_8UC1,10,5);
    vector<int> hh = HorizHistU(Edge);
    // Find local max on left
    int iMax = (Edge.rows/4);
    int iRight = -1;
    int iLeft = -1;
    for (int i = 0; i < CenterX; i++)
    {
        if (hh[i] > iMax)
        {
            iMax = hh[i];
            iLeft = i;
        }
    }

    // Find local max on right
    iMax = (Edge.rows/4);
    for (int i = CenterX; i < (int)hh.size(); i++)
    {
        if (hh[i] > iMax)
        {
            iMax = hh[i];
            iRight = i;
        }
    }
    // Convert img to RGB due to coloring lanes
    Mat OutImg(InterpolImg.size(), CV_8UC3);
    cvtColor(InterpolImg, OutImg, CV_GRAY2RGB);
    // Recognize road lane
    RightLane = ReturnLane(Edge,iRight,iLaneOffset,iMinLaneWidth,iMaxLaneWidth);
    LeftLane = ReturnLane(Edge,iLeft,iLaneOffset,iMinLaneWidth,iMaxLaneWidth);
    // Draw a center line
    for (int i = InImg.rows/2; i < InImg.rows; i = i +20){
        line(OutImg,Point(CenterX,i),Point(CenterX,i+10),Scalar(255,255,0),2,8);
    }
    // Draw a line
    if (!RightLane.NoLane)
        OutImg = DrawLane(RightLane,OutImg);
    if (!LeftLane.NoLane)
        OutImg = DrawLane(LeftLane,OutImg);

    // Fill right lane buffer
    if(RightLaneArr.size() == MAX_LANE_BUFFER_SIZE){
        RightLaneArr.erase(RightLaneArr.begin());
    }
    RightLaneArr.push_back(RightLane.Type);
    // Fill left lane buffer
    if(LeftLaneArr.size() == MAX_LANE_BUFFER_SIZE){
        LeftLaneArr.erase(LeftLaneArr.begin());
    }
    LeftLaneArr.push_back(LeftLane.Type);
    return OutImg;

}
RoadLane MainWindow::ReturnLane(Mat Lane, int iPos, int iLaneOffset, int iMinLaneWidth, int iMaxLaneWidth){
    RoadLane _RoadLane;
    _RoadLane = RoadLane();
    if (iPos ==-1){
        return _RoadLane;
    }
    // Crop lane
    Mat _Lane = CropLane(Lane,iPos,iLaneOffset);
    // Thresh croped lanes - otsu method

    if (_Lane.empty())
        return _RoadLane;
    Mat _Thresh;
    threshold(_Lane, _Thresh, 0, 255, CV_THRESH_BINARY | CV_THRESH_OTSU);
    if (_Thresh.empty())
        return _RoadLane;
    //Get Coordinates of lanes
    LaneCoordinates LaneCoor = LaneBorder(_Thresh,iMinLaneWidth,iMaxLaneWidth,iPos,iLaneOffset);
    // Separate a recognize lane
    RoadLane _Out = RoadLane(LaneCoor,Lane.rows);
    return _Out;
}
Mat MainWindow::DrawLane(RoadLane Lines, Mat InImg)
{
    if (!Lines.NoLane)
    {
        for (int i = 0; i < Lines.NumberLane; i++)
        {
            for (int j = 0; j < (int)Lines.CenterOfLine[i].size()-1; j++)
            {
                Point pt1 = Point(Lines.CenterOfLine[i][j].x,Lines.CenterOfLine[i][j].y);
                Point pt2 = Point(Lines.CenterOfLine[i][j+1].x,Lines.CenterOfLine[i][j+1].y);
                switch (Lines.Type){
                case 0: line(InImg,pt1,pt2,Scalar(0,0,255),2,8);break;
                case 1: line(InImg,pt1,pt2,Scalar(0,255,0),2,8);break;
                case 2: line(InImg,pt1,pt2,Scalar(0,102,255),2,8);break;
                case-1: line(InImg,pt1,pt2,Scalar(0,255,255),2,8);break;
                }
            }
        }
    }
    return InImg;
}

void MainWindow::on_btn_time_clicked()
{
    system("scrot");
}
void MainWindow::PrintTmpRoutine(){

    // Set Output
    float TmpOut = ReturnTemperatureF(SensTempOut);
    if (TmpOut != 999) // Otherwise let previous value
        ui->btn_TmpOut->setText(QString(QString::number(TmpOut) +" C"));
    if (TmpOut <= 0)
        InfoAlarm(1);
    // Set Input
    /*float TmpIn = ReturnTemperatureF(SensTempIn);
    if (TmpIn != 999) // Otherwise let previous value
        ui->btn_TmpIn->setText(QString(QString::number(TmpIn) +" C"));*/
}
Mat MainWindow::ParkView(Mat inImg){
    Mat Out;
    // Set ROI to half of image it is OK
     Rect _roi(0,inImg.rows/2,inImg.cols,inImg.rows/2);
    Mat _RoiImg;
    inImg(_roi).copyTo(_RoiImg);
    // Compute TVTM for ecah channel
    Mat rearTopViewImg[3];
    split(_RoiImg,rearTopViewImg);
    rearTopViewImg[0] = RetTVImg(rearTopViewImg[0],rearTV,false);
    rearTopViewImg[1] = RetTVImg(rearTopViewImg[1],rearTV,false);
    rearTopViewImg[2] = RetTVImg(rearTopViewImg[2],rearTV,false);
    rearTopViewImg[0] = InterpolTVTM(rearTopViewImg[0]);
    rearTopViewImg[1] = InterpolTVTM(rearTopViewImg[1]);
    rearTopViewImg[2] = InterpolTVTM(rearTopViewImg[2]);
    merge(rearTopViewImg,3,Out);
    GaussianBlur(Out, Out, Size(7,7), 1.5, 1.5,BORDER_REFLECT);
    // Draw a triangles
        Point Triangle1[1][3];
        Triangle1[0][0] = Point(0,0);
        Triangle1[0][1] = Point(0,Out.rows);
        Triangle1[0][2] = Point(rearTV.a,Out.rows);
        const Point* ppt[1] = { Triangle1[0] };
        int npt[] = { 3 };
        fillPoly( Out,
                  ppt,
                  npt,
                  1,
                  Scalar( 0, 0, 0 ),8);
        Point Triangle2[1][3];
        Triangle2[0][0] = Point(Out.cols,0);
        Triangle2[0][1] = Point(Out.cols-1,Out.rows-1);
        Triangle2[0][2] = Point(rearTV.a + rearTV.c,Out.rows);
        const Point* ppt2[1] = { Triangle2[0] };
        int npt2[] = { 3 };
        fillPoly( Out,
                  ppt2,
                  npt2,
                  1,
                  Scalar( 0, 0, 0 ),8);

    return Out;
}
/*
 *
 */
void MainWindow::on_btn_RearCamInit_clicked()
{
    InfoMessage("Prepocet pro zadni kameru zahajen.", false);
    RearTVTMInit();
    InfoMessage("Prepocet pro zadni kameru ukoncen.", false);

}
/*
 *
 */
void MainWindow::GPIOInit(){

/*
 *  KEY_PIN 0
 *  POWER_PIN 26
 *  SOUND_PIN 2
 */
// GPIO init
    wiringPiSetup();
    pinMode(POWER_PIN, OUTPUT);
    pinMode(KEY_PIN,INPUT);
    pinMode(SOUND_PIN,OUTPUT);
}
/*
 *
 */
void MainWindow::CheckKeyIngnition(){
    if (digitalRead(KEY_PIN) == LOW && bKey){
        MainWindow::on_btnShutDown_clicked();
    }
}
/*
 * Function play the sound (only error sound)
 */
void MainWindow::PlaySound(){

    if(bSound){
        for(int i = 0; i < 10; i++)
        {
            digitalWrite(SOUND_PIN,HIGH);
            delayMicroseconds(100);
            digitalWrite(SOUND_PIN,LOW);
            delayMicroseconds(100);
        }
        delay(100);
        for(int i = 0; i < 10; i++)
        {
            digitalWrite(SOUND_PIN,HIGH);
            delayMicroseconds(100);
            digitalWrite(SOUND_PIN,LOW);
            delayMicroseconds(100);
        }
    }
}
/*
 *  Determine the type of road lane
 */
int MainWindow::DetermineLane(vector<int> LaneBuffer){
    int iPrev = LaneBuffer[0];
    vector<int> LaneType(3);
    for (int i = 0; i < LaneType.size(); i++){
        LaneType[i] = 0;
    }
    for (int i = 1; i < LaneBuffer.size(); i++){
        switch(LaneBuffer[i]){
        case 0: if (iPrev == 0){LaneType[0]++;} else{LaneType[0] = 0;} break;
        case 1: if (iPrev == 1){LaneType[1]++;} else{LaneType[1] = 0;} break;
        case 2: if (iPrev == 2){LaneType[2]++;} else{LaneType[2] = 0;} break;
        }
        iPrev = LaneBuffer[i];
    }
    int _Max = 0;
    int _Pos = -1;
    for (int i = 0; i < LaneType.size();i++){
        if (LaneType[i] > _Max){
            _Max = LaneType[i];
            _Pos = i;
        }
    }
    return _Pos;
}
