#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <cmath>

#define PI 3.14159265

#define uchar unsigned char

using namespace std;

struct HoughLine
{
    double theta;
    double r;
};



int main( int argc, char** argv )
{

    const int neighbourhoodSize = 4;
    int maxTheta = 180;
    double thetaStep = PI / maxTheta;
    int ** houghArray;
    float centerX, centerY;
    int houghHeight;
    int doubleHeight;
    int numPoints;
    double * sinCache;
    double * cosCache;

    if(argc < 3)
    {
        cout << "Usage: " << argv[0] <<" imageName threashold" << endl;
        return -1;
    }

    cv::Mat img = cv::imread(argv[1], 0); //imagem original
    cv::Mat linesImg;  // imagem linhas
    cv::Mat houghImg;  // imagem espaço de hough

    int width = img.cols;
    int height = img.rows;

    linesImg.create(height, width, CV_8UC1);
    linesImg = 255;


    houghHeight = (int)  (  sqrt(2.0) * std::max<int>(img.rows, img.cols));

    doubleHeight = 2 * houghHeight;

    houghArray = new int*[maxTheta];

    for(int i = 0 ; i<maxTheta; i++)
    {
        houghArray[i] = new int[doubleHeight];
        memset(houghArray[i], 0, doubleHeight*sizeof(int));
    }

    centerX = width / 2;
    centerY = height / 2;

    numPoints = 0;

    sinCache = new double[maxTheta];
    cosCache =  new double[maxTheta];


    for (int t = 0; t < maxTheta; t++) {
        double realTheta = t * thetaStep;
        sinCache[t] = sin(realTheta);
        cosCache[t] = cos(realTheta);
    }


    for(int i=0; i< img.rows; i++)
    {
        uchar * pixel = img.ptr<uchar>(i);

        for( int j=0; j<img.cols; j++)
        {
            if(pixel[j] < 100)
            {
                int x = j;
                int y = img.rows - i;

                for (int t = 0; t < maxTheta; t++) {
                    int r = (int) (((x - centerX) * cosCache[t]) + ((y - centerY) * sinCache[t]));
                    r += houghHeight;
                    if (r < 0 || r >= doubleHeight) continue;

                    houghArray[t][r]++;

                }
                numPoints++;
            }
        }
    }


    vector<HoughLine> lines;

    float threshold = 0;
    sscanf(argv[2], "%f", &threshold);
    int newr;
    int r;
    bool onLoop = false;


    if (numPoints != 0)
    {
        for (int t = 0; t < maxTheta; t++)
        {
loop:
            if(onLoop)
            {
                newr = ++r;
                onLoop = false;
            }
            else newr = neighbourhoodSize;

            for (r = newr; r < doubleHeight - neighbourhoodSize; r++)
            {
                if (houghArray[t][r] > threshold)
                {
                    int peak = houghArray[t][r];

                    for (int dx = -neighbourhoodSize; dx <= neighbourhoodSize; dx++)
                    {
                        for (int dy = -neighbourhoodSize; dy <= neighbourhoodSize; dy++)
                        {
                            int dt = t + dx;
                            int dr = r + dy;
                            if (dt < 0) dt = dt + maxTheta;
                            else if (dt >= maxTheta) dt = dt - maxTheta;
                            if (houghArray[dt][dr] > peak) {
                                onLoop = true;
                                goto loop;
                            }
                        }
                    }

                    double theta = t * thetaStep;

                    // add the line to the vector
                    HoughLine l;
                    l.r = r;
                    l.theta = theta;
                    lines.push_back(l);
                }
            }
        }
    }

    cout << "Numero de linhas encontradas utilizando o threshold " << threshold << " : " <<lines.size() <<endl;
    for(int i=0; i< lines.size(); i++)
    {
        cout<<"Linha n"<<i<<": Theta " << lines[i].theta/thetaStep << " distancia do centro " << lines[i].r - houghHeight << endl;
    }

    int max = 0;
    for (int t = 0; t < maxTheta; t++) {
        for (int r = 0; r < doubleHeight; r++) {
            if (houghArray[t][r] > max) {
                max = houghArray[t][r];
            }
        }
    }


    houghImg.create(doubleHeight, maxTheta, CV_8UC1);

    // desenha espaço de hough
    for (int t = 0; t < maxTheta; t++) {
        for (int r = 0; r < doubleHeight; r++) {
            double value = 255.0 * ((double) houghArray[t][r]) / (double)max;
            int v = 255 - (int) value;
            houghImg.at<uchar>(r, t) = v;
        }
    }


    // desenha linhas
    for(int i=0; i< lines.size(); i++)
    {
        for( int j=0; j<img.cols; j++)
        {
            double realTheta = lines[i].theta;
            double dcos = cos(realTheta);
            double dsin = sin(realTheta);
            double rr = lines[i].r - houghHeight;
            double first = rr - dcos * (j - img.cols/2);
            int y = first / dsin;
            y = img.rows - y;
            y-= img.rows/2;
            if(y >= 0 && y < img.rows)
                linesImg.at<uchar>(y, j) = 0;
        }
    }


    cv::imshow( "Original Image", img );
    cv::imshow( "Hough Image", houghImg );
    cv::imshow( "Lines Image", linesImg );

    uchar c = 0;

    while(c != 27)
    {
        c = cv::waitKey(0);
    }

    return 0;
}