#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 index;
        // get the most left/right contour
        
        int leftmostEdge = imageColums;
        int rightmostEdge = 0;

        for (int i = 0; i < data.leftEdges.size(); i++)
        {
            int edge = data.leftEdges[i].x;
                                    
            if (edge <= leftmostEdge && turnLeft)
            {
                leftmostEdge = edge;
                index = i;
            }
            else if(edge >= rightmostEdge && !turnLeft)
            {
                rightmostEdge = edge;
                index = i;
            }
        }
  
        // Find the Top-Left and Bottom-Left Point of the Contour
            
        std::vector<cv::Point> leftMostContour = data.contours[index];

        /*
        Not going to dive into that part to fix it.
        Trying to use centroid instead.
        int xleftBottom = imageColums; 
        int xleftTop = imageColums;
        int yBottom = 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 < xleftBottom)
            {
                xleftBottom = leftMostContour[i].x;
            }
            else if(leftMostContour[i].y > yTop -4 && leftMostContour[i].x < xleftTop)
            {
                xleftTop = leftMostContour[i].x;
            }
        }

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

        //Get the contours center using moments:
        cv::Moments moments=cv::moments(leftMostContour);
        LFRPoint contourCenter(moments.m10/moments.m00, moments.m01/moments.m00);
        LFRPoint imageCenter(double(imageColums)/2.0, double(imageRows)/2.0);
        LFRPoint focusPoint(imageCenter.x, imageCenter.y+1000.0);

        LFRVector a(imageCenter-focusPoint);
        LFRVector b(contourCenter-focusPoint);

        double angle = a.angle(b);
        if (b.x < 0.0)
        {
            angle *= -1.0;
        }

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