#include "processing.h"




Processing::Processing(/* args */)
{
}

Processing::~Processing()
{
}

void Processing::filterReflections(FrameData& frameData)
{
    //Try to filter reflections from the frame data.

    std::vector<int> indicesToDelete;
    for(int i = 0; i < frameData.contours.size(); i++)
    {

        // First approach: correct contours nearly fill their bounding box.
        // So delete all contours with an area smaller than 75% of their bounding box
        double contourArea = cv::contourArea(frameData.contours[i], false);
        double boundingBoxArea = double(frameData.boundingBoxes[i].width*frameData.boundingBoxes[i].height);
        double minRatio = 0.75;
        if(boundingBoxArea/contourArea < minRatio)
        {
            indicesToDelete.push_back(i);
            continue;
        }

        // Second approach: The contour should be nearly convex
        // So delete all contours with an area smaller than 95% of their convex hull.
        std::vector<Point> hull;
        cv::convexHull(frameData.contours[i], hull);
        double hullArea = cv::contourArea(hull, false);
        double minRatioHull = 0.95;
        if(contourArea/boundingBoxArea < minRatio)
        {
            indicesToDelete.push_back(i);
            continue;
        }
        
    }

    //reverse the vector with the indices so the order isn't messed up when deleting:
    std::reverse(indicesToDelete.begin(), indicesToDelete.end());
    for(int index : indicesToDelete)
    {
        frameData.boundingBoxes.erase(frameData.boundingBoxes.begin() + index);
        frameData.contours.erase(frameData.contours.begin() + index);
        frameData.leftEdges.erase(frameData.leftEdges.begin() + index);
        frameData.middlePoints.erase(frameData.middlePoints.begin() + index);
    }
    return;
}

bool Processing::checkIntersection(const std::vector<std::vector<Point>>& lines, const std::vector<Point>& outline)
{

    /* TODO:
    * Was ich mir hier vorstelle:
    * Wir haben die konturen und die hough linien
    * Konturen haben schwäche mit Reflektionen, hough linien sind unvollständig.
    * Irgendwie sollten die beiden Datensätze gemerged werden, um das "gute" aus beiden zu vereinen.
    * Evtl in richtung "Schau den Bereich der Contour (+ Toleranz?) im hough bild an. Wenn keine Linie im Bereich ist, ist die Contour eine Reflektion"
    * 
    * Aktuell nehmen wir die abkürzung, die segmente rauszuwerfen, sobald keine Linie im Bild ist.
    * Funktioniert für unsere Daten auch ziemlich gut
    */

    if(lines.size() == 0)
    {
        return false;
    }
    return true;
}

void Processing::calcAngles(FrameData &data, int imageColums, int imageRows, bool turnLeft)
{
    
    if(data.contours.size() > 0)
    { 
    int angle = 0;
    int index;    
            if(turnLeft){
                // get the most left/right contour
                int leftmostEdge = imageColums;
                
                for (int i = 0; i < data.leftEdges.size(); i++)
                {
                    int edge = data.leftEdges[i].x;
                                            
                    if (edge <= leftmostEdge)
                    {
                        leftmostEdge = edge;
                        index = i;
                    }
                }
            }else
            {
                int rightmostEdge = 0;
                
                for (int i = 0; i < data.leftEdges.size(); i++)
                {
                    int edge = data.leftEdges[i].x;
                    
                    if (edge >= rightmostEdge)
                    {
                        rightmostEdge = edge;
                        index = i;
                    }
                }
            }    
            // Find the Top-Left and Buttom-Left Point of the Contour
            
            std::vector<cv::Point> leftMostContour = data.contours[index];
            int xleftButtom = imageColums; 
            int xleftTop = imageColums;
            int yButtom = data.boundingBoxes[index].y;
            int yTop = data.boundingBoxes[index].height;
            
            for (int i = 0; i < leftMostContour.size(); i++){
                if(leftMostContour[i].y == 0 && leftMostContour[i].x < xleftButtom)
                {
                    xleftButtom = leftMostContour[i].x;
                }else if(leftMostContour[i].y > yTop -4 && leftMostContour[i].x < xleftTop){
                    xleftTop = leftMostContour[i].x;
                }
            }

        // calculate angle
        int deltaX = abs(xleftButtom - xleftTop);
        int deltaY = yTop;                
        angle = Calcs::calcAngle(deltaX, deltaY);

        //Write to Data
        data.angle = angle;
        data.index = index;
    }
}


void Processing::processImage(Mat& inputPicture, int thresholdBinary, int gaussKernelSize)
{
    //Idea here is: Processing module consists of two methods:
    // One (this) to do all kinds of stuff to the picture (grayscale conversion, threshold, gauss etc etc)
    // And one (the other one) to segment the lines.
    // No return value here as the input is passed by reference -> directly modified.
    cvtColor(inputPicture, inputPicture, COLOR_BGR2GRAY);
    GaussianBlur(inputPicture, inputPicture, Size(gaussKernelSize, gaussKernelSize), 0); 
    threshold(inputPicture, inputPicture, thresholdBinary, 255, THRESH_BINARY);

    //Perform an opening
    Mat kernel(5,5, CV_8UC1,1);
    morphologyEx(inputPicture, inputPicture, 2, kernel);
}

FrameData Processing::calculateLineSegments(Mat& inputPicture, const cv::Rect& roi)
{

    FrameData data;
    Mat inputPictureRoi = inputPicture(roi);
    cv::findContours(inputPictureRoi, data.contours, RETR_LIST, CHAIN_APPROX_SIMPLE);

    vector<Vec4i> lines;
    std::vector<std::vector<Point>> houghLines;
    Canny(inputPicture, inputPicture, 50, 100, 3);
    HoughLinesP(inputPicture, lines, 1, CV_PI/180, 100, 30, 150);  
    //Draw lines  
    inputPicture = Mat::zeros(inputPicture.size().height, inputPicture.size().width, CV_8UC1);

    for( size_t i = 0; i < lines.size(); i++ )
    {
        line( inputPicture, Point(lines[i][0], lines[i][1]), Point(lines[i][2], lines[i][3]), Scalar(255,255,255), 3, 8 );
        std::vector<Point> line;
        line.push_back(Point(lines[i][0], lines[i][1]));
        line.push_back(Point(lines[i][2], lines[i][3]));
        houghLines.push_back(line);
    }


    //Delete the areas that are too small
    auto iterator = data.contours.begin();
    while(iterator != data.contours.end())
    {
        if (contourArea(*iterator) < 7000)
        {
            iterator = data.contours.erase(iterator);
        }
        else if (!checkIntersection(houghLines, *iterator))
        {
            iterator = data.contours.erase(iterator);
        }
        else
        {
            //Check for intersection with lines:
            Rect boundingBox = boundingRect(*iterator);
            boundingBox.x += roi.x;
            boundingBox.y += roi.y;
            data.boundingBoxes.push_back(boundingBox);
            data.middlePoints.push_back(Point(boundingBox.x+boundingBox.width/2, boundingBox.y+boundingBox.height/2));
            data.leftEdges.push_back(Point(boundingBox.x, boundingBox.y+boundingBox.height/2));
            ++iterator;
        }
    }
    return data;
}