#include <opencv2/core/utils/logger.hpp>
#include <opencv2/opencv.hpp>

#include <iostream>

#include <input.h>
#include <processing.h>
#include <control_module.h>
#include <interpreter.h>
#include <intersection_handler.h>

void sweep_em_all(int thresholdBinary, int videoHeight, int videoWidth, int gaussKernelSize, int thresholdCanny1, int thresholdCanny2, int apertureSizeCanny)
{
    Input input(videoHeight, videoWidth);
    Processing processing;

	namedWindow("Display window");
    while(true)
    {
        Mat image = input.readFile("C:\\Users\\User\\Desktop\\Studium\\02_Master_MSY\\2. Semester  Winter 22 23\\Projekt\\Line-Following-Robot\\Test_data");
        Mat processedImage = image;
        processing.processImage(processedImage, thresholdBinary, gaussKernelSize, thresholdCanny1, thresholdCanny2 ,apertureSizeCanny);
        std::vector<Vec4i> lines = processing.calculateLineSegments(processedImage);
        for( size_t i = 0; i < lines.size(); i++ )
        {
            line( image, Point(lines[i][0], lines[i][1]),
            Point( lines[i][2], lines[i][3]), (0,0,255), 1, 8 );
        }  
        imshow("Display window", image);
        char c = (char)waitKey(1);
    }
    destroyWindow("Display window");
}

void in_depth_processing_chain(int thresholdBinary, int videoHeight, int videoWidth, int gaussKernelSize, int thresholdCanny1, int thresholdCanny2, int apertureSizeCanny)
{
    std::string outputFolder = "C:\\Users\\User\\Desktop\\temp";
    Input input(videoHeight, videoWidth);

    Mat image = input.readFile("C:\\Users\\User\\Desktop\\Studium\\02_Master_MSY\\2. Semester  Winter 22 23\\Projekt\\Line-Following-Robot\\Test_data");
    imwrite(outputFolder + "\\01_input.jpg", image);
    cvtColor(image, image, COLOR_BGR2GRAY);
    imwrite(outputFolder + "\\02_color_convert.jpg", image);
    GaussianBlur(image, image, Size(gaussKernelSize, gaussKernelSize), 0); 
    imwrite(outputFolder + "\\03_gauss.jpg", image);
    threshold(image, image, thresholdBinary, 255, THRESH_BINARY);
    imwrite(outputFolder + "\\04_threshold.jpg", image);
    Canny(image, image, thresholdCanny1, thresholdCanny2, apertureSizeCanny);
    imwrite(outputFolder + "\\05_canny.jpg", image);

}


int main(void)
{
    //Disable opencv logging messages
    cv::utils::logging::setLogLevel(cv::utils::logging::LOG_LEVEL_WARNING);

    const int thresholdBinary = 140;
    const int videoHeight = 720;
    const int videoWidth = 960;
    const int gaussKernelSize = 11;
    const int thresholdCanny1 = 50;
    const int thresholdCanny2 = 100;
    const int apertureSizeCanny = 3;

    //sweep_em_all(thresholdBinary, videoHeight, videoWidth, gaussKernelSize, thresholdCanny1, thresholdCanny2, apertureSizeCanny);
    in_depth_processing_chain(thresholdBinary, videoHeight, videoWidth, gaussKernelSize, thresholdCanny1, thresholdCanny2, apertureSizeCanny);
}