
#include <stdlib.h> 
#include <stdio.h> 
#include <cmath>//for use of PI, abs, sin...
#include <vector>
#include "vision_utilities.h"

using namespace std;

typedef struct _theta_rho
{
	double theta;
	double rho;
} Theta_Rho;
	

int myround(double x);//cunix cannot use round in cmath
int min(int a, int b);

int p3(char *fname_sceneImage, char *fname_houghImage, int houghThr, char *fname_linedImage)
{

	Image *sceneImage = new Image;//input scene image
	Image *houghImage = new Image;//input hough image
	Image *linedImage = new Image;//output line detected image

	readImage(sceneImage, fname_sceneImage);
	readImage(houghImage, fname_houghImage);

	int NRows = getNRows(sceneImage);
	int NCols = getNCols(sceneImage);

	double rho_max = sqrt(NRows*NRows+NCols*NCols);

	setSize(linedImage,NRows,NCols);
	setColors(linedImage,255);

	/*COPY THE SCENE IMAGE*/
	for (int i = 0; i<NRows; i++)
	{
		for (int j = 0; j<NCols; j++)
		{
			setPixel(linedImage,i,j,getPixel(sceneImage,i,j));
		}
	}

	int theta_level = getNRows(houghImage);
	//printf("theta_level is %d\n",theta_level);
	int rho_level = getNCols(houghImage);
	//printf("rho_level is %d\n",rho_level);


	vector<Theta_Rho> theta_rho_arr;//each element is a theta_rho pair in theta_rho space and also a line in scene image

	/*FIND THE STRONG EDGE*/
	for (int ii = 0; ii < (theta_level+2)/3; ii++)
	{
		for (int jj = 0; jj < (rho_level+2)/3; jj++)
		{
			int patch_max = getPixel(houghImage,ii*3,jj*3);//max value in 3*3 patch
			int i_max = ii*3;
			int j_max = jj*3;// 0
			int i,j;
			if (getPixel(houghImage,i=ii*3,j=min(jj*3+1,rho_level)) > patch_max)// 1
			{
				patch_max = getPixel(houghImage,i,j);
				j_max = j;
			}
			if (getPixel(houghImage,i=ii*3,j=min(jj*3+2,rho_level)) > patch_max)// 2
			{
				patch_max = getPixel(houghImage,i,j); 
				j_max = j;
			}
			if (getPixel(houghImage,i=min(ii*3+1,theta_level),j=jj*3) > patch_max)// 3
			{
				patch_max = getPixel(houghImage,i,j);
				i_max = i;
				j_max = j;
			}
			if (getPixel(houghImage,i=min(ii*3+1,theta_level),j=min(jj*3+1,rho_level)) > patch_max)// 4
			{
				patch_max = getPixel(houghImage,i,j); 
				i_max = i;
				j_max = j;
			}
			if (getPixel(houghImage,i=min(ii*3+1,theta_level),j=min(jj*3+2,rho_level)) > patch_max)// 5
			{
				patch_max = getPixel(houghImage,i,j); 
				i_max = i;
				j_max = j;
			}
			if (getPixel(houghImage,i=min(ii*3+2,theta_level),j=jj*3) > patch_max)// 6
			{
				patch_max = getPixel(houghImage,i,j); 
				i_max = i;
				j_max = j;
			}
			if (getPixel(houghImage,i=min(ii*3+2,theta_level),j=min(jj*3+1,rho_level)) > patch_max)// 7
			{
				patch_max = getPixel(houghImage,i,j); 
				i_max = i;
				j_max = j;
			}
			if (getPixel(houghImage,i=min(ii*3+2,theta_level),j=min(jj*3+2,rho_level)) > patch_max)// 8
			{
				patch_max = getPixel(houghImage,i,j); 
				i_max = i;
				j_max = j;
			}

			if (patch_max > houghThr) 
			{
				double theta = double(i_max-theta_level/2)/theta_level*PI;
				double rho = 2*j_max*rho_max/rho_level-rho_max;
				//myround((rho-(rho_max))/(2*rho_max)*rho_level) = j; so that rho = 2*j*rho_max/rho_level+rho_max (with some approximation)
				Theta_Rho theta_rho_pair;
				theta_rho_pair.theta = theta;
				theta_rho_pair.rho = rho;
				theta_rho_arr.push_back(theta_rho_pair);
			}
		}
	}

	vector<Theta_Rho>::iterator iter = theta_rho_arr.begin();
/*	int numPairs = 0;
	while (iter!=theta_rho_arr.end())
	{
		printf("Pair %d: theta = %f, rho = %f\n", ++numPairs, (*iter).theta, (*iter).rho);
		iter++;
	}
*/
	iter = theta_rho_arr.begin();
	while (iter!=theta_rho_arr.end())
	{
		/*FIND THE END POINTS OF THE LINE*/
		
		double tempTheta = (*iter).theta;
		double tempRho = (*iter).rho;
		double xmin = 0;
		double xmax = NCols;
		double ymin = 0;
		double ymax = NRows;
		int x[4];
		int y[4];
		int index = 0;
		if (((xmin*sin(tempTheta)+tempRho)/cos(tempTheta) >= ymin)
			&&((xmin*sin(tempTheta)+tempRho)/cos(tempTheta) <= ymax))//if when x = xmin, ymin <= y <= ymax
		{
			x[index] = myround(xmin);
			y[index] = myround((xmin*sin(tempTheta)+tempRho)/cos(tempTheta));
			index++;
		}
		if (((xmax*sin(tempTheta)+tempRho)/cos(tempTheta) >= ymin)
			&&((xmax*sin(tempTheta)+tempRho)/cos(tempTheta) <= ymax))//if when x = xmax, ymin <= y <= ymax
		{
			x[index] = myround(xmax);
			y[index] = myround((xmax*sin(tempTheta)+tempRho)/cos(tempTheta));
			index++;
		}
		if (((ymin*cos(tempTheta)-tempRho)/sin(tempTheta) >= xmin)
			&&((ymin*cos(tempTheta)-tempRho)/sin(tempTheta) <= xmax))//if when y = ymin, xmin <= x <= xmax
		{
			x[index] = myround((ymin*cos(tempTheta)-tempRho)/sin(tempTheta));
			y[index] = myround(ymin);
			index++;
		}
		if (((ymax*cos(tempTheta)-tempRho)/sin(tempTheta) >= xmin)
			&&((ymax*cos(tempTheta)-tempRho)/sin(tempTheta) <= xmax))//if when y = ymax, xmin <= x <= xmax
		{
			x[index] = myround((ymax*cos(tempTheta)-tempRho)/sin(tempTheta));
			y[index] = myround(ymax);
			index++;
		}

		/*DRAW THE LINE*/
	
		line(linedImage,y[0],x[0],y[1],x[1],255);

		iter++;
	}

	writeImage(linedImage,fname_linedImage);
		
	printf("U got it\n");
	return 0;
}

int myround(double x)
{
	if (x<floor(x)+0.50) return floor(x);
	else return (floor(x)+1);
}

int min(int a, int b)
{
	return a<b?a:b;
}

int main(int argc, char **argv) 
{ 
	if (argc <= 4){ 
		printf("Usage: %s <scene image> <hough image> <hough threshold> <line detected image>\n", argv[0]); 
		return 0; 
	} 
	char *fname_sceneImage = argv[1];
	char *fname_houghImage = argv[2];
	int houghThr = atoi(argv[3]);
	char *fname_linedImage = argv[4];
	p3(fname_sceneImage,fname_houghImage,houghThr,fname_linedImage);
	return 0; 
}
