#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "find_edges.h"
#include "Algorithms.h"

#define M_PI 3.1416;

void lum2RGB(unsigned char* lum_in, unsigned char* rgb_out, int width, int height)
{
  int i, j, rgb_index, lum_index;

  for(i=0; i<width; i++)
  for(j=0; j<height; j++)
  {
	lum_index = j*width + i;
	rgb_index = lum_index*3;

	rgb_out[rgb_index  ] = lum_in[lum_index];
	rgb_out[rgb_index+1] = lum_in[lum_index];
	rgb_out[rgb_index+2] = lum_in[lum_index];
  }
}

void gaussiano1(unsigned char* rgb_in, unsigned char* rgb_out, int width, int height)
{
  int i, j, k, index, gauss;

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  for(k=0; k<3; k++)
  {
	index = (j*width + i)*3 + k;

	gauss = 4*rgb_in[index];
	gauss += 2*(rgb_in[index+3] + rgb_in[index-3] + rgb_in[index+width*3] + rgb_in[index-width*3]);
	gauss += 1*(rgb_in[index+width*3+3] + rgb_in[index+width*3-3] + rgb_in[index-width*3+3] + rgb_in[index-width*3-3]);

	gauss = gauss / 16;
	if(gauss > 255) gauss = 255;

    rgb_out[index] = gauss;
  }
}

void gaussiano2(unsigned char* rgb_in, unsigned char* rgb_out, int width, int height)
{
  int i, j, k, index, gauss;

  for(i=2; i<width-2; i++)
  for(j=2; j<height-2; j++)
  for(k=0; k<3; k++)
  {
	index = (j*width + i)*3 + k;

	gauss = 41*rgb_in[index];
	gauss += 26*(rgb_in[index+3] + rgb_in[index-3] + rgb_in[index+width*3] + rgb_in[index-width*3]);
	gauss += 16*(rgb_in[index+width*3+3] + rgb_in[index+width*3-3] + rgb_in[index-width*3+3] + rgb_in[index-width*3-3]);
	gauss += 7*(rgb_in[index+6] + rgb_in[index-6] + rgb_in[index+width*6] + rgb_in[index-width*6]);
	gauss += 4*(rgb_in[index+width*6+3] + rgb_in[index+width*6-3] + rgb_in[index-width*6+3] + rgb_in[index-width*6-3] + rgb_in[index+width*3+6] + rgb_in[index+width*3-6] + rgb_in[index-width*3+6] + rgb_in[index-width*3-6]);
	gauss += 1*(rgb_in[index+width*6+6] + rgb_in[index+width*6-6] + rgb_in[index-width*6+6] + rgb_in[index-width*6-6]);

	gauss = gauss / 273;
	if(gauss > 255) gauss = 255;

    rgb_out[index] = gauss;
  }
}

void gaussiano3(unsigned char* rgb_in, unsigned char* rgb_out, int width, int height)
{
  int i, j, k, index, gauss;

  for(i=3; i<width-3; i++)
  for(j=3; j<height-3; j++)
  for(k=0; k<3; k++)
  {
	index = (j*width + i)*3 + k;

	gauss = 91*rgb_in[index];
	gauss += 71*(rgb_in[index+3] + rgb_in[index-3] + rgb_in[index+width*3] + rgb_in[index-width*3]);
	gauss += 55*(rgb_in[index+width*3+3] + rgb_in[index+width*3-3] + rgb_in[index-width*3+3] + rgb_in[index-width*3-3]);
	gauss += 33*(rgb_in[index+6] + rgb_in[index-6] + rgb_in[index+width*6] + rgb_in[index-width*6]);
	gauss += 26*(rgb_in[index+width*6+3] + rgb_in[index+width*6-3] + rgb_in[index-width*6+3] + rgb_in[index-width*6-3] + rgb_in[index+width*3+6] + rgb_in[index+width*3-6] + rgb_in[index-width*3+6] + rgb_in[index-width*3-6]);
	gauss += 12*(rgb_in[index+width*6+6] + rgb_in[index+width*6-6] + rgb_in[index-width*6+6] + rgb_in[index-width*6-6]);
	gauss += 10*(rgb_in[index+9] + rgb_in[index-9] + rgb_in[index+width*9] + rgb_in[index-width*9]);
	gauss += 7*(rgb_in[index+width*9+3] + rgb_in[index+width*9-3] + rgb_in[index-width*9+3] + rgb_in[index-width*9-3] + rgb_in[index+width*3+9] + rgb_in[index+width*3-9] + rgb_in[index-width*3+9] + rgb_in[index-width*3-9]);
	gauss += 4*(rgb_in[index+width*9+6] + rgb_in[index+width*9-6] + rgb_in[index-width*9+6] + rgb_in[index-width*9-6] + rgb_in[index+width*6+9] + rgb_in[index+width*6-9] + rgb_in[index-width*6+9] + rgb_in[index-width*6-9]);
	gauss += 1*(rgb_in[index+width*9+9] + rgb_in[index+width*9-9] + rgb_in[index-width*9+9] + rgb_in[index-width*9-9]);

	gauss = gauss / 1115;
	if(gauss > 255) gauss = 255;

    rgb_out[index] = gauss;
  }
}
/*
void blackAndWhite(unsigned char* img_in, unsigned char* img_out, int width, int height)
{
  int i, j, index;
  double bw;
  unsigned char baw;

  for(i=0; i<width; i++)
  for(j=0; j<height; j++)
  {
	index = (j*width + i)*3;

	bw = 0.30*(double)img_in[index] + 0.59*(double)img_in[index+1] + 0.11*(double)img_in[index+2];
	baw = (unsigned char)bw;

    img_out[index  ] = baw;
    img_out[index+1] = baw;
    img_out[index+2] = baw;
  }
}

void gradiente(unsigned char* img_in, unsigned char* img_out, int width, int height)
{
  int i, j, k, index, grad;

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  for(k=0; k<3; k++)
  {
	index = (j*width + i)*3 + k;

	grad = abs(img_in[index+3] - img_in[index-3]);
	grad += abs(img_in[index+width*3] - img_in[index-width*3]);

	if(grad > 255) grad = 255;

    img_out[index] = grad;
  }
}

void prewitt(unsigned char* img_in, unsigned char* img_out, int width, int height)
{
  int i, j, k, index, grad, gradX, gradY;

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  for(k=0; k<3; k++)
  {
	index = (j*width + i)*3 + k;

	gradX = abs(img_in[index+3] - img_in[index-3] + img_in[index+width*3+3] + img_in[index-width*3+3] - img_in[index+width*3-3] - img_in[index-width*3-3]);
	gradY = abs(img_in[index+width*3] - img_in[index-width*3] + img_in[index+3+width*3] + img_in[index-3+width*3] - img_in[index+3-width*3] - img_in[index-3-width*3]);
	grad = gradX + gradY;

	if(grad > 255) grad = 255;

    img_out[index] = grad;
  }
}

void sobel(unsigned char* img_in, unsigned char* img_out, int width, int height)
{
  int i, j, k, index, grad, gradX, gradY;

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  for(k=0; k<3; k++)
  {
	index = (j*width + i)*3 + k;

	gradX = abs(2*(img_in[index+3] - img_in[index-3]) + img_in[index+width*3+3] + img_in[index-width*3+3] - img_in[index+width*3-3] - img_in[index-width*3-3]);
	gradY = abs(2*(img_in[index+width*3] - img_in[index-width*3]) + img_in[index+3+width*3] + img_in[index-3+width*3] - img_in[index+3-width*3] - img_in[index-3-width*3]);
	grad = gradX + gradY;

	if(grad > 255) grad = 255;

    img_out[index] = grad;
  }
}

void laplaciano(unsigned char* img_in, unsigned char* img_out, int width, int height)
{
  int i, j, k, index, laplace;

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  for(k=0; k<3; k++)
  {
	index = (j*width + i)*3 + k;

	laplace = 4*img_in[index] - img_in[index+3] - img_in[index-3];
	laplace = laplace - img_in[index+width*3] - img_in[index-width*3];

	if(laplace < 0) laplace = -1*laplace;
	laplace = 2*laplace;
	if(laplace > 255) laplace = 255;

    img_out[index] = (unsigned char)laplace;
  }
}

void makeMask(unsigned char* img_in, unsigned char* img_out, int threshold, int width, int height)
{
  int i, j, index, sum;
  unsigned char res;

  for(i=0; i<width; i++)
  for(j=0; j<height; j++)
  {
	index = (j*width + i)*3;

	sum = img_in[index] + img_in[index+1] + img_in[index+2];
	if(sum > threshold) res = 255;
	else res = 0;

    img_out[index  ] = res;
    img_out[index+1] = res;
    img_out[index+2] = res;
  }
}
*/
void dilatation(unsigned char* lum_in, unsigned char* lum_out, int width, int height)
{
  int i, j, index, count;
  unsigned char res;

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  {
	index = j*width + i;
	if(lum_in[index] > 0) 
	{
	  lum_out[index] = 255;
	  continue;
	}

	count = 0;

	if(lum_in[index+1] > 0) count++;
	if(lum_in[index-1] > 0) count++;
	if(lum_in[index+width+1] > 0) count++;
	if(lum_in[index+width] > 0) count++;
	if(lum_in[index+width-1] > 0) count++;
	if(lum_in[index-width+1] > 0) count++;
	if(lum_in[index-width] > 0) count++;
	if(lum_in[index-width-1] > 0) count++;

	if(count>0) res = 255;
	else res = 0;

    lum_out[index] = res;
  }
}

void erosion(unsigned char* lum_in, unsigned char* lum_out, int width, int height)
{
  int i, j, index, count;
  unsigned char res;

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  {
	index = j*width + i;
	if(lum_in[index] == 0) 
	{
	  lum_out[index] = 0;
	  continue;
	}

	count = 0;

	if(lum_in[index+1] > 0) count++;
	if(lum_in[index-1] > 0) count++;
	if(lum_in[index+width+1] > 0) count++;
	if(lum_in[index+width] > 0) count++;
	if(lum_in[index+width-1] > 0) count++;
	if(lum_in[index-width+1] > 0) count++;
	if(lum_in[index-width] > 0) count++;
	if(lum_in[index-width-1] > 0) count++;

	if(count>7) res = 255;
	else res = 0;

    lum_out[index] = res;
  }
}

void gradienteCanny(unsigned char* rgb_in, unsigned char* lum_out, int width, int height)
{
  int i, j, rgb_index, lum_index, grad, gradX, gradY;

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  {
	lum_index = j*width + i;
	rgb_index = lum_index*3;

	gradX = rgb_in[rgb_index+width*3+3] - rgb_in[rgb_index+width*3-3];
	gradX += rgb_in[rgb_index-width*3+3] - rgb_in[rgb_index-width*3-3];
	gradX += 2*(rgb_in[rgb_index+3] - rgb_in[rgb_index-3]);

	gradY = rgb_in[rgb_index-width*3-3] - rgb_in[rgb_index+width*3-3];
	gradY += rgb_in[rgb_index-width*3+3] - rgb_in[rgb_index+width*3+3];
	gradY += 2*(rgb_in[rgb_index-width*3] - rgb_in[rgb_index+width*3]);

	grad = abs(gradX) + abs(gradY);
	if(grad > 255) grad = 255;

    lum_out[lum_index] = grad;
  }
}

void canny(unsigned char* rgb_in, unsigned char* lum_out, unsigned char* lum_aux, int width, int height)
{
  int i, j, rgb_index, lum_index, grad, gradX, gradY;
  unsigned char direction, left, right, res;
  unsigned char highThreshold = 120;
  unsigned char lowThreshold = 40;
  unsigned char P1, P2, P3, P4, P5, P6, P7, P8;

  double orientation;

  gradienteCanny(rgb_in, lum_aux, width, height);

  for(i=0; i<width; i++)
  for(j=0; j<height; j++)
  {
	lum_index = j*width + i;
	rgb_index = lum_index*3;

	if((i<=2)||(j<=2)||(i>=width-3)||(j>=height-3))
	{
	  lum_out[lum_index] = 0;
//	  rgb_out[index+1] = 0;
//	  rgb_out[index+2] = 0;
	  continue;
	}

	gradX = rgb_in[rgb_index+width*3+3] - rgb_in[rgb_index+width*3-3];
	gradX += rgb_in[rgb_index-width*3+3] - rgb_in[rgb_index-width*3-3];
	gradX += 2*(rgb_in[rgb_index+3] - rgb_in[rgb_index-3]);

	gradY = rgb_in[rgb_index-width*3-3] - rgb_in[rgb_index+width*3-3];
	gradY += rgb_in[rgb_index-width*3+3] - rgb_in[rgb_index+width*3+3];
	gradY += 2*(rgb_in[rgb_index-width*3] - rgb_in[rgb_index+width*3]);

	grad = abs(gradX) + abs(gradY);
	if(grad>255) grad = 255;

	if(gradX == 0)
	{
	  if(gradY==0) orientation = 0.0;
	  else if(gradY<0)
	  {
		gradY = -gradY;
		orientation = 90.0;
	  } else orientation = 90.0;
	}

	/* Can't take invtan of angle in 2nd Quad */
	else if(gradX<0 && gradY>0)
	{
	  gradX = -gradX;
	  orientation = 180.0 - ((atan((double)(gradY)/(double)(gradX))) * (180.0/3.1416));
	}

	/* Can't take invtan of angle in 4th Quad */
	else if(gradX>0 && gradY<0)
	{
	  gradY = -gradY;
	  orientation = 180.0 - ((atan((double)(gradY)/(double)(gradX))) * (180.0/3.1416));
	}

	/* else angle is in 1st or 3rd Quad */
	else orientation = (atan((double)(gradY)/(double)(gradX))) * (180.0/3.1416);

	if(orientation < 22.5) direction = 0;
	else if(orientation < 67.5) direction = 45;
	else if(orientation < 112.5) direction = 90;
	else if(orientation < 157.5) direction = 135;
	else direction = 0;

	if(direction == 0)
	{
	  left = lum_aux[lum_index-1];
	  right = lum_aux[lum_index+1];
	}

	else if(direction == 45)
	{
	  left = lum_aux[lum_index+width-1];
	  right = lum_aux[lum_index-width+1];
	}

	else if(direction == 90)
	{
	  left = lum_aux[lum_index-width];
	  right = lum_aux[lum_index+width];
	}

	else   
	{
	  left = lum_aux[lum_index-width-1];
	  right = lum_aux[lum_index+width+1];
	}

	if((grad<left)||(grad<right)) res = 0;
	else
	{
	  // Hysteresis
	  highThreshold = 120;
	  lowThreshold = 40;

	  if(grad >= highThreshold) res = 255; /* edge */
	  else if(grad < lowThreshold) res = 0;  /* nonedge */

	  /* SUM is between T1 & T2 */
	  else
	  {
		/* Determine values of neighboring pixels */
		P1 = rgb_in[rgb_index-width*3-3];
		P2 = rgb_in[rgb_index-width*3  ];
		P3 = rgb_in[rgb_index-width*3+3];
		P4 = rgb_in[rgb_index-3];
		P5 = rgb_in[rgb_index+3];
		P6 = rgb_in[rgb_index+width*3-3];
		P7 = rgb_in[rgb_index+width*3  ];
		P8 = rgb_in[rgb_index+width*3+3];

		/* Check to see if neighboring pixel values are edges */
		if((P1>highThreshold) || (P2>highThreshold) || (P3>highThreshold) || (P4>highThreshold) || 
			(P5>highThreshold) || (P6>highThreshold) || (P7>highThreshold) || (P8>highThreshold))
			 res = 255; /* make edge */

		else res = 0; /* make nonedge */
	  }
	}

    lum_out[lum_index] = res;
//    rgb_out[index+1] = res;
//    rgb_out[index+2] = res;
  }
}

float calculateArea(int *xs, int *ys, int size)
{
  int i, x0, y0, x1, y1, iaux;

  iaux = 0;

  for(i=0; i<size; i++)
  {
	x0 = xs[i];
	y0 = ys[i];

	if(i<(size-1))
	{
	  x1 = xs[i+1];
	  y1 = ys[i+1];
	} else
	{
	  x1 = xs[0];
	  y1 = ys[0];
	}
	iaux += x0*y1 - x1*y0; 
  }

  return (float)(fabs((double)iaux) / 2.0);
}

/*
void findMaskCorners(unsigned char* img_in, unsigned char* img_mask, unsigned char* img_out, unsigned char* img_aux1, unsigned char* img_aux2, int width, int height)
{
  unsigned char* gradX = img_aux1;
  unsigned char* gradY = img_aux2;

  int i, j, index, grad_x, grad_y;
  int k = 2; //Tamanho do quadrado
  int ki, kj, kindex; 
  double a, b, c, d, av1, av2, delta;

  int count = 0;
  double max1 = 0;
  double min1 = 999999999;
  double avg1 = 0;
  double max2 = 0;
  double min2 = 999999999;
  double avg2 = 0;

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  {
	index = (j*width + i)*3;

	grad_x = img_in[index+width*3+3] - img_in[index+width*3-3];
	grad_x += img_in[index-width*3+3] - img_in[index-width*3-3];
	grad_x += 2*(img_in[index+3] - img_in[index-3]);

	grad_y = img_in[index-width*3-3] - img_in[index+width*3-3];
	grad_y += img_in[index-width*3+3] - img_in[index+width*3+3];
	grad_y += 2*(img_in[index-width*3] - img_in[index+width*3]);

	if(grad_x > 255) grad_x = 255;
	if(grad_y > 255) grad_y = 255;

    gradX[index  ] = grad_x;
    gradX[index+1] = grad_x;
    gradX[index+2] = grad_x;

    gradY[index  ] = grad_y;
    gradY[index+1] = grad_y;
    gradY[index+2] = grad_y;
  }

  for(i=1+k; i<width-1-k; i++)
  for(j=1+k; j<height-1-k; j++)
  {
	index = (j*width + i)*3;
	if(img_mask[index] == 0)
	{
	  img_out[index  ] = 0;
	  img_out[index+1] = 0;
	  img_out[index+2] = 0;
	  continue;
	}

	a=0; b=0; c=0; d=0;

	for(ki=-k; ki<=k; ki++)
	for(kj=-k; kj<=k; kj++)
	{
	  kindex = index + ki*3 + kj*width*3;
	  a += gradX[kindex]*gradX[kindex];
	  b += gradX[kindex]*gradY[kindex];
	  d += gradY[kindex]*gradY[kindex];
	}
	c=b;

	delta = sqrt((a-d)*(a-d) + 4*b*c);
	av1 = (a+d) + delta;
	av2 = (a+d) - delta;

//	count++;
//	if(av1>max1) max1 = av1;
//	if(av1<min1) min1 = av1;
//	avg1 += av1;
//	if(av2>max2) max2 = av2;
//	if(av2<min2) min2 = av2;
//	avg2 += av2;

	if(av2 < 300000)
	{
      img_out[index  ] = 0;
      img_out[index+1] = 0;
      img_out[index+2] = 0;
	} else
	{
      img_out[index  ] = 255;
      img_out[index+1] = 255;
      img_out[index+2] = 255;
	}
  }
//  avg1 = avg1 / count;
//  avg2 = avg2 / count;
}
*/

/*
 *  1  8  7
 *  2  x  6
 *  3  4  5
 */
__inline void visitPixel(unsigned char* lum, int width, int height, int x, int y, int stackCount)
{
  int dx[9] = {-1, -1, -1, 0, 1, 1, 1, 0, -1};
  int dy[9] = {-1, 0, 1, 1, 1, 0, -1, -1, -1};
  int i;
  int numW2B;
  int index = y*width + x;

  if((stackCount > 200)||(x==0)||(y==0)||(x==width-1)||(y==height-1)) return;

  numW2B = 0;

  for(i=0; i<8; i++)
    if( (lum[index + dx[i] + dy[i]*width] != 0) &&
		(lum[index + dx[i+1] + dy[i+1]*width] == 0)) numW2B++;

  if(numW2B < 2)
  {
	lum[index] = 0;
	for(i=0; i<8; i++)
      if(lum[index + dx[i] + dy[i]*width] != 0)
	  {
	    visitPixel(lum, width, height, x + dx[i], y + dy[i], stackCount+1);
	  }
  }
}

void detectCycles(unsigned char* lum, int width, int height)
{
  int i, j, index;

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  {
	index = j*width + i;
	if(lum[index] != 0) 
	{
	  visitPixel(lum, width, height, i, j, 0);
	  break;
	}
  }
}

/*
 *  1  8  7
 *  2  x  6
 *  3  4  5
 */
int detectCycle(unsigned char* lum, int width, int height, int x, int y, int startIndex, int x_dest, int y_dest, int first, int stackCount)
{
  int dx[16] = {-1, -1, -1, 0, 1, 1, 1, 0, -1, -1, -1, 0, 1, 1, 1, 0};
  int dy[16] = {-1, 0, 1, 1, 1, 0, -1, -1, -1, 0, 1, 1, 1, 0, -1, -1};
  int i;
  int firstB2W;
  int index = y*width + x;

  if((stackCount > 400)||(x==0)||(y==0)||(x==width-1)||(y==height-1)) return 0;
  if((!first)&&(x==x_dest)&&(y==y_dest)) 
  {
	return 1;
  }

  firstB2W = -1;

  for(i=startIndex; i<startIndex+8; i++)
    if( (lum[index + dx[i] + dy[i]*width] == 0) &&
		(lum[index + dx[i+1] + dy[i+1]*width] != 0)) 
	{
	  firstB2W = (i+1) % 8;
	  break;
	}

  if(firstB2W >= 0)
  {
	if(!first) lum[index] = 0;
	veAddPoint((float)x, (float)y);
	return detectCycle(lum, width, height, x + dx[firstB2W], y + dy[firstB2W], (firstB2W+6) % 8, x_dest, y_dest, 0, stackCount+1);
  }

  return 0;
}

int detectCorners(unsigned char* lum, int width, int height, int xs[4], int ys[4])
{
  int i, j, k, index, size, iscycle;
  float* vertices;
  int x1s[20];
  int y1s[20];

  for(i=1; i<width-1; i++)
  for(j=1; j<height-1; j++)
  {
	veClear();
	veSetError(2.0);
	iscycle = 0;

	index = j*width + i;
	if(lum[index] != 0) 
	{
	  iscycle = detectCycle(lum, width, height, i, j, 0, i, j, 1, 0);
	}

	veNoMorePoints();
	veGetVertices(&vertices, &size);

	if((iscycle)&&(size>=4)&&(size<=20))
	{
	  for(k=0; k<size; k++)
	  {
		x1s[k] = (int)vertices[2*k];
		y1s[k] = (int)vertices[2*k+1];
	  }

	  veClear();
	  veSetError(4.0);

	  for(k=0; k<size; k++)
	  {
		veAddPoint((float)x1s[k], (float)y1s[k]);
	  }

	  veNoMorePoints();
	  veGetVertices(&vertices, &size);

	  if((size == 5)||(size == 6))
	  {
		for(k=1; k<5; k++)
		{
		  xs[k-1] = (int)vertices[2*k];
		  ys[k-1] = (int)vertices[2*k+1];
		}

		if(calculateArea(xs, ys, 4) > 150.0) return 1;
	  }
//	  index = (((int)vertices[2*k+1])*width + (int)vertices[2*k]);
//	  lum[index] = 50;
//	  drawPoint(lum, width, height, (int)vertices[2*k], (int)vertices[2*k+1], 100);
	}
  }

  return 0;
}