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

#include <iostream>
#include <direct.h>

#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)
{
    Input input(videoHeight, videoWidth);

    cv::String outputFolder = "C:\\Users\\tim-z\\Desktop\\temp";
    cv::String inputFolder = "C:\\Users\\tim-z\\Desktop\\Studium\\02_Master MSY\\2. Semester Winter 2022 2023\\Projekt\\Repo\\Line-Following-Robot\\AutonomousMode\\Test_data";

    std::vector<cv::String> filenames;
    cv::glob(inputFolder, filenames);

    //filenames.begin(), filenames.end()
    int i = 0;
    for(std::vector<cv::String>::iterator it = filenames.begin(); it != filenames.end(); it++)
    {
        std::string current_output = outputFolder + "\\" + to_string(i);
        std::cout << current_output << std::endl;
        const char* current_output_char = current_output.c_str();
        _mkdir(current_output_char);

        std::string inputFile = inputFolder + "\\image" + to_string(i+1) + ".jpeg";
        Mat original_image = input.readFile(inputFile);
        imwrite(current_output + "\\00_input.jpg", original_image);

        Point roiOrigin(0, original_image.rows*(7.5/12.0));
        Rect roi = Rect(roiOrigin.x, roiOrigin.y, original_image.cols, original_image.rows/12);

        Mat image = original_image(roi);

        imwrite(current_output + "\\01_roi.jpg", image);
        cvtColor(image, image, COLOR_BGR2GRAY);
        imwrite(current_output + "\\02_color_convert.jpg", image);
        GaussianBlur(image, image, Size(gaussKernelSize, gaussKernelSize), 0); 
        imwrite(current_output + "\\03_gauss.jpg", image);
        threshold(image, image, thresholdBinary, 255, THRESH_BINARY);
        imwrite(current_output + "\\04_threshold.jpg", image);

        // Opening (reduces noise)
        Mat kernel(5,5, CV_8UC1,1);
        morphologyEx(image, image, 2, kernel);
        imwrite(current_output + "\\05_opening.jpg", image);

        //Canny(image, image, thresholdCanny1, thresholdCanny2, apertureSizeCanny);
        //imwrite(outputFolder + "\\06_canny.jpg", image);

        vector<vector<Point> > contours;
        vector<Vec4i> hierarchy;
        vector<Rect> vectorOfRects;
        vector<Point> vectorOfLeftEdges;

        findContours(image,contours, hierarchy, RETR_LIST, CHAIN_APPROX_SIMPLE);
        int amountOfValidRects = 0;
        for( int i = 0; i< contours.size(); i++ ) // iterate through each contour. 
        {
                double a = contourArea( contours[i],false);  //  Find the area of contour
                if(a > 3500)
                {
                    drawContours(original_image, contours, i, Scalar(0,255,255), 1, 8, hierarchy, 0, roiOrigin);
                    
                    Rect currentBoundingRect = boundingRect(contours[i]);
                    //Handle roi offset:
                    currentBoundingRect.x += roiOrigin.x;
                    currentBoundingRect.y += roiOrigin.y;
                    vectorOfRects.push_back(currentBoundingRect);

                    rectangle(original_image, currentBoundingRect, Scalar(0,255,0));
                 
                    // get mid-point of rect
                    Point midRect = Point(currentBoundingRect.x+currentBoundingRect.width/2, currentBoundingRect.y+currentBoundingRect.height/2);

                    // Draw middle as small rect instead of circle because for whatever reasons drawing a circle doesnt work.
                    Rect testRect(Point(midRect.x-2, midRect.y-2), Point(midRect.x+2, midRect.y+2));
                    rectangle(original_image, testRect, Scalar(0,0,255));
                    // get the left edge of rect
                    // used as offset as raspicam is not
                    // mounted on mid of regbot
                    Point leftEdge(currentBoundingRect.x, currentBoundingRect.y+currentBoundingRect.height/2);
                    vectorOfLeftEdges.push_back(leftEdge);

                    testRect = Rect(Point(leftEdge.x-2, leftEdge.y-2), Point(leftEdge.x+2, leftEdge.y+2));
                    rectangle(original_image, testRect, Scalar(0,0,255));
                    amountOfValidRects++;
                }
        }
        imwrite(current_output + "\\06_contours.jpg", original_image);
        i++;
    }
}


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