
#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 myline(Image *im, Image *thresholedEdgeImage, int y0, int x0, int y1, int x1, int color, int NRows, int NCols);//modified function line

int p4(char *fname_sceneImage, char *fname_houghImage, char *fname_thresholdedEdgeImage, int houghThr, char *fname_croppedLinedImage)
{

	Image *sceneImage = new Image;//input scene image
	Image *houghImage = new Image;//input hough image
	Image *thresholdedEdgeImage = new Image;//input thresholded edge image
	Image *croppedLinedImage = new Image;//output cropped line detected image

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

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

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

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

	/*COPY THE SCENE IMAGE*/
	for (int i = 0; i<NRows; i++)
	{
		for (int j = 0; j<NCols; j++)
		{
			setPixel(croppedLinedImage,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*/
	
		myline(croppedLinedImage,thresholdedEdgeImage,y[0],x[0],y[1],x[1],255,NRows,NCols);

		iter++;
	}

	writeImage(croppedLinedImage,fname_croppedLinedImage);
		
	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 myline(Image *im, Image *thresholedEdgeImage, int y0, int x0, int y1, int x1, int color, int NRows, int NCols) {  
  int xmin,xmax; /* line coordinates */
  int ymin,ymax;
  int dir;       /* scan direction */
  int dx;        /* distance along X */
  int dy;        /* distance along Y */

  /* increments: East, North-East, South, South-East, North */
  int incrE,
      incrNE,
      incrSE;
  int d;         /* the D */
  int x,y;       /* running coordinates */
  int mpCase;    /* midpoint algorithm's case */
  int done;      /* set to 1 when done */

  /* TODO note that the x and y are switched! */
  xmin=y0;
  xmax=y1;
  ymin=x0;
  ymax=x1;
  
  dx=xmax-xmin;
  dy=ymax-ymin;

  if (dx*dx>dy*dy) /* horizontal scan */
  {
    dir=DIR_X;
    if (xmax<xmin)
    {
      SWAP(xmin,xmax);
      SWAP(ymin,ymax);
    }
    dx=xmax-xmin;
    dy=ymax-ymin;

    if (dy>=0)
    {
      mpCase=1;
      d=2*dy-dx;      
    }
    else
    {
      mpCase=2;
      d=2*dy+dx;      
    }

    incrNE=2*(dy-dx);
    incrE=2*dy;
    incrSE=2*(dy+dx);
  }
  else /* vertical scan */
  {
    dir=DIR_Y;
    if (ymax<ymin)
    {
      SWAP(xmin,xmax);
      SWAP(ymin,ymax);
    }
    dx=xmax-xmin;
    dy=ymax-ymin;    

    if (dx>=0)
    {
      mpCase=1;
      d=2*dx-dy;      
    }
    else
    {
      mpCase=2;
      d=2*dx+dy;      
    }

    incrNE=2*(dx-dy);
    incrE=2*dx;
    incrSE=2*(dx+dy);
  }
  
  /* start the scan */
  x=xmin;
  y=ymin;
  done=0;

  while(!done)
  {
    
	/*CROP THE LINES*/

	/*Compare the lined image with the thresholded edge image
	If there is no thresholded edge points in a pixel's 3*3 neighbour, it will not be displayed as a white pixel*/
	if (getPixel(thresholedEdgeImage,CLAMP(x-1,0,NRows),CLAMP(y-1,0,NCols))
		+ getPixel(thresholedEdgeImage,CLAMP(x-1,0,NRows),CLAMP(y,0,NCols))
		+ getPixel(thresholedEdgeImage,CLAMP(x-1,0,NRows),CLAMP(y+1,0,NCols))
		+ getPixel(thresholedEdgeImage,CLAMP(x,0,NRows),CLAMP(y-1,0,NCols))
		+ getPixel(thresholedEdgeImage,CLAMP(x,0,NRows),CLAMP(y,0,NCols))
		+ getPixel(thresholedEdgeImage,CLAMP(x,0,NRows),CLAMP(y+1,0,NCols))
		+ getPixel(thresholedEdgeImage,CLAMP(x+1,0,NRows),CLAMP(y-1,0,NCols))
		+ getPixel(thresholedEdgeImage,CLAMP(x+1,0,NRows),CLAMP(y,0,NCols))
		+ getPixel(thresholedEdgeImage,CLAMP(x+1,0,NRows),CLAMP(y+1,0,NCols))
		> 2) setPixel(im,x,y,color);
  
    /* move to the next point */
    switch(dir)
    {
      case DIR_X: /* horizontal */
      {
        if (x<xmax)
        {
	      switch(mpCase)
          {
            case 1:
	          if (d<=0)
	          {
	            d+=incrE;  x++;
	          }
              else
              {
                d+=incrNE; x++; y++;
              }
            break;
  
            case 2:
              if (d<=0)
              {
                d+=incrSE; x++; y--;
              }
              else
              {
                d+=incrE;  x++;
              }
            break;
	      } /* mpCase */
        } /* x<xmax */
        else
	      done=1;
      }  
      break;

      case DIR_Y: /* vertical */
      {
        if (y<ymax)
        {
          switch(mpCase)
          {
            case 1:
              if (d<=0)
              {
                d+=incrE;  y++;
              }
              else
              {
                d+=incrNE; y++; x++;
              }
            break;
  
            case 2:
              if (d<=0)
              {
                d+=incrSE; y++; x--;
              }
              else
              {
                d+=incrE;  y++;
              }
            break;
	      } /* mpCase */
        } /* y<ymin */
        else
	      done=1;
      }
      break;    
    }
  }

  return 0; /* no error */
}

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