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

int myround(double x);//cunix cannot use round in cmath

int p2(char *fname_edgeImage, int thr, char *fname_thresholdedEdgeImage, char *fname_houghImage)
{
	Image *edgeImage = new Image;//input edge image
	Image *thresholdedEdgeImage = new Image;//output thresholded edge image
	Image *houghImage = new Image;//output hough image
	readImage(edgeImage, fname_edgeImage);
	int NRows = getNRows(edgeImage);
	int NCols = getNCols(edgeImage);
	setSize(thresholdedEdgeImage,NRows,NCols);
	setColors(thresholdedEdgeImage,1);//output thresholded edge image is binary

	/*PART ONE: THRESHOLDING*/

	for (int i = 0; i<NRows; i++)
	{
		for (int j = 0; j<NCols; j++)
		{
			if (getPixel(edgeImage,i,j) < thr) setPixel(thresholdedEdgeImage,i,j,0);
			else setPixel(thresholdedEdgeImage,i,j,1);			
		}
	}


	/*ADDITIONAL PROCESS: EDGE THINNING*/
	//=============================================================
	Image *edgeOfEdge = new Image;
	setSize(edgeOfEdge,NRows,NCols);
	setColors(edgeOfEdge,1);

		/*USING ROBERTS (2*2)*/
	const int ROBERTS_X[4] = {0,1,-1,0};
	const int ROBERTS_Y[4] = {1,0,0,-1};

		/*EDGE MAGNITUDE*/
	int **edgeMagnitude = new int*[NRows];
	for (int i = 0; i < NRows; i++)
		edgeMagnitude[i] = new int[NCols];
	for (int i = 0; i < NRows; i++)
		for (int j = 0; j < NCols; j++)
			edgeMagnitude[i][j] = 0;

	int maxEdge = 0;
		/*COMPUTE EDGE MAGNITUDE*/
	for (int i = 1; i<NRows-1; i++)
	{
		for (int j = 1; j<NCols-1; j++)
		{
			int edgeX = getPixel(thresholdedEdgeImage,i-1,j-1)*ROBERTS_X[0]
				+ getPixel(thresholdedEdgeImage,i-1,j)*ROBERTS_X[1]
				+ getPixel(thresholdedEdgeImage,i,j-1)*ROBERTS_X[2]
				+ getPixel(thresholdedEdgeImage,i,j)*ROBERTS_X[3];

			int edgeY = getPixel(thresholdedEdgeImage,i-1,j-1)*ROBERTS_Y[0]
				+ getPixel(thresholdedEdgeImage,i-1,j)*ROBERTS_Y[1]
				+ getPixel(thresholdedEdgeImage,i,j-1)*ROBERTS_Y[2]
				+ getPixel(thresholdedEdgeImage,i,j)*ROBERTS_Y[3];


			edgeMagnitude[i][j] = myround(sqrt(double(edgeX*edgeX+edgeY*edgeY)));	
			if (edgeMagnitude[i][j] > maxEdge) maxEdge = edgeMagnitude[i][j];			
		}
	}

	for (int i = 1; i<NRows-1; i++)
	{
		for (int j = 1; j<NCols-1; j++)
		{
			setPixel(edgeOfEdge,i,j,edgeMagnitude[i][j]>0?1:0);
		}
	}

	//writeImage(edgeOfEdge, "EDGE_OF_EDGE.pgm");

	for (int i = 1; i<NRows-1; i++)
	{
		for (int j = 1; j<NCols-1; j++)
		{
			int temp = getPixel(thresholdedEdgeImage,i,j);
			setPixel(thresholdedEdgeImage,i,j,MAX(temp - getPixel(edgeOfEdge,i,j), 0) );
		}
	}

	writeImage(thresholdedEdgeImage,fname_thresholdedEdgeImage);//write output thresholded edge image

	//=============================================================	

	/*PART TWO: VOTING*/

	double rho_max = sqrt(NRows*NRows+NCols*NCols);
	int rho_level = (NCols>NRows)?NCols:NRows;//rho_level = max{NCols,NRows}
	int theta_level = 180;
	int **accArray = new int*[theta_level];
	for (int i = 0; i < theta_level; i++)
		accArray[i] = new int[rho_level];
	for (int i = 0; i < theta_level; i++)
		for (int j = 0; j < rho_level; j++)
			accArray[i][j] = 0;

	for (int i = 0; i < NRows; i++)
	{
		for (int j = 0; j < NCols; j++)
		{
			if (getPixel(thresholdedEdgeImage,i,j)==1)//is edge
			{			
				for (int t = 0; t < theta_level; t++)//we think that 181 degree is equal to 1 degree
				{
					double theta = double(t-theta_level/2)/theta_level*PI;//-PI/2 <= theta < PI/2
					double rho = (i*cos(theta)-j*sin(theta));
					accArray[t][myround((rho-(-rho_max))/(2*rho_max)*rho_level)]++;//rho can be negative (-rho_max to rho_max)
				}
			}
		}
	}

	/*PART THREE: NORMALIZING THE ACCARRAY*/
	int maxVote = 0;
	for (int i = 0; i < theta_level; i++)
		for (int j = 0; j < rho_level; j++)
			if (accArray[i][j] > maxVote) 
				maxVote = accArray[i][j];

	setSize(houghImage,theta_level,rho_level);
	setColors(houghImage,255);//output hough image has 255 colors
	for (int i = 0; i < theta_level; i++)
		for (int j = 0; j < rho_level; j++)
			setPixel(houghImage,i,j,myround(double(accArray[i][j])/maxVote*255));
	
	writeImage(houghImage,fname_houghImage);//write output hough image

	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 main(int argc, char **argv) 
{ 
	if (argc <= 4){ 
		printf("Usage: %s <input edge image> <threshold> <output thresholded edge image> <output hough image>\n", argv[0]); 
		return 0; 
	} 
	char *edgeImage = argv[1];
	int thr = atoi(argv[2]);
	char *thresholdedEdgeImage = argv[3];
	char *houghImage = argv[4];
	p2(edgeImage,thr,thresholdedEdgeImage,houghImage);
	return 0; 
}
