#include "processing.h"

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

Processing::~Processing()
{
}

static double angle( Point pt1, Point pt2, Point pt0 )
{
    double dx1 = pt1.x - pt0.x;
    double dy1 = pt1.y - pt0.y;
    double dx2 = pt2.x - pt0.x;
    double dy2 = pt2.y - pt0.y;
    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

void Processing::processImage(Mat& inputPicture, int thresholdBinary, int gaussKernelSize, int thresholdCanny1, int thresholdCanny2, int apertureSizeCanny)
{
    //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 a opening
    Mat kernel(5,5, CV_8UC1,1);
    morphologyEx(inputPicture, inputPicture, 2, kernel);
}

FrameData Processing::calculateLineSegments(const Mat& inputPicture, const cv::Rect& roi)
{
    FrameData data;
    cv::findContours(inputPicture, data.contours, RETR_LIST, CHAIN_APPROX_SIMPLE);

    //Delete the areas that are too small
    auto iterator = data.contours.begin();
    while(iterator != data.contours.end())
    {
        if (contourArea(*iterator) < 3500)
        {
            iterator = data.contours.erase(iterator);
        }
        else
        {
            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;
}