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

#include "vectorization.h"

void drawPixel(int x, int y, RGB& color, unsigned char* mask, int width, int height)
{
  int index = (y*width + x)*3;

  mask[index  ] = color.b;
  mask[index+1] = color.g;
  mask[index+2] = color.r;
}

void drawPoint(int x, int y, RGB& color, unsigned char* mask, int width, int height)
{
  drawPixel(x, y, color, mask, width, height);
  if(x>0) drawPixel(x-1, y, color, mask, width, height);
  if(x<width-1) drawPixel(x+1, y, color, mask, width, height);
  if(y>0) drawPixel(x, y-1, color, mask, width, height);
  if(y<height-1) drawPixel(x, y+1, color, mask, width, height);
}

void drawLine(int x0, int y0, int x1, int y1, RGB& vertColor, RGB& segColor, unsigned char* mask, int width, int height)
{
  int dx, dy, i, j, iaux;

  dx = x1 - x0;
  dy = y1 - y0;

  if(abs(dx) >= abs(dy))
  {
	if(dx<0)
	{
	  iaux=x0; x0=x1; x1=iaux;
	  iaux=y0; y0=y1; y1=iaux;
	}
	for(i=x0; i<=x1; i++)
	{
	  if(dx==0) j = y0;
	  else j = y0 + ((i-x0)*(y1-y0))/abs(dx);
	  iaux = (j*width + i)*3;
	  mask[iaux  ] = segColor.b;
	  mask[iaux+1] = segColor.g;
	  mask[iaux+2] = segColor.r;
	}
  } else
  {
	if(dy<0)
	{
	  iaux=x0; x0=x1; x1=iaux;
	  iaux=y0; y0=y1; y1=iaux;
	}
	for(j=y0; j<=y1; j++)
	{
	  if(dy==0) i = x0;
	  else i = x0 + ((j-y0)*(x1-x0))/abs(dy);
	  iaux = (j*width + i)*3;
	  mask[iaux  ] = segColor.b;
	  mask[iaux+1] = segColor.g;
	  mask[iaux+2] = segColor.r;
	}
  }

  drawPoint(x0, y0, vertColor, mask, width, height);
  drawPoint(x1, y1, vertColor, mask, width, height);
}

void drawCycle(pointSet& cycle, RGB& vertColor, RGB& segColor, unsigned char* mask, int width, int height)
{
  int i, size;

  RGB firstColor;
  firstColor.r = 255;
  firstColor.g = 255;
  firstColor.b = 0;

  RGB secondColor;
  secondColor.r = 0;
  secondColor.g = 255;
  secondColor.b = 0;

  size = cycle.size();
  if(size < 3) return;

  for(i=0; i<(size-1); i++)
  {
	drawPoint(cycle[i].x, cycle[i].y, vertColor, mask, width, height);
	drawLine(cycle[i].x, cycle[i].y, cycle[i+1].x, cycle[i+1].y, vertColor, segColor, mask, width, height);
  }

  drawPoint(cycle[size-1].x, cycle[size-1].y, vertColor, mask, width, height);
  drawLine(cycle[size-1].x, cycle[size-1].y, cycle[0].x, cycle[0].y, vertColor, segColor, mask, width, height);

  drawPoint(cycle[0].x, cycle[0].y, firstColor, mask, width, height);
  drawPoint(cycle[1].x, cycle[1].y, secondColor, mask, width, height);
}

void drawObjects(objectSet& objects, unsigned char* img, int width, int height)
{
  int i;
  RGB vertColor;
  RGB segColor;
  RGB starColor;

  vertColor.r = 255;
  vertColor.g = 0;
  vertColor.b = 0;

  segColor.r = 0;
  segColor.g = 255;
  segColor.b = 0;

  starColor.r = 255;
  starColor.g = 128;
  starColor.b = 0;

  for(i=0; i<objects.size(); i++)
  {
	if(objects[i].isPattern) 
		drawCycle(objects[i].points, vertColor, starColor, img, width, height);
	else 
		drawCycle(objects[i].points, vertColor, segColor, img, width, height);
  }
}

///////////////////////
//
//    Algoritmo 2
//
///////////////////////

enum traceDirection {any, up, down, left, right, up_left, up_right, down_left, down_right};

bool checkDirection(traceDirection direction, int i, int j, traceDirection& nextDirection)
{
  switch(direction)
  {
	case any:
	  if((i==-1)&&(j==-1)) nextDirection = down_left;
	  if((i==-1)&&(j==0)) nextDirection = left;
	  if((i==-1)&&(j==1)) nextDirection = up_left;
	  if((i==0)&&(j==-1)) nextDirection = down;
	  if((i==0)&&(j==1)) nextDirection = up;
	  if((i==1)&&(j==-1)) nextDirection = down_right;
	  if((i==1)&&(j==0)) nextDirection = right;
	  if((i==1)&&(j==1)) nextDirection = up_right;
	  return true;
	  break;
	case up:
	  if(j>=0) 
	  {
		if(i==-1) nextDirection = up_left;
		if(i==0) nextDirection = up;
		if(i==1) nextDirection = up_right;
		return true;
	  }
	  break;
	case down:
	  if(j<=0) 
	  {
		if(i==-1) nextDirection = down_left;
		if(i==0) nextDirection = down;
		if(i==1) nextDirection = down_right;
		return true;
	  }
	  break;
	case left:
	  if(i<=0) 
	  {
		if(j==-1) nextDirection = down_left;
		if(j==0) nextDirection = left;
		if(j==1) nextDirection = up_left;
		return true;
	  }
	  break;
	case right:
	  if(i>=0) 
	  {
		if(j==-1) nextDirection = down_right;
		if(j==0) nextDirection = right;
		if(j==1) nextDirection = up_right;
		return true;
	  }
	  break;
	case up_left:
	  nextDirection = up_left;
	  if(((j==1)&&((i==0)||(i==-1)))||((j==0)&&(i==-1))) return true;
	  break;
	case up_right:
	  nextDirection = up_right;
	  if(((j==1)&&((i==0)||(i==1)))||((j==0)&&(i==1))) return true;
	  break;
	case down_left:
	  nextDirection = down_left;
	  if(((j==-1)&&((i==0)||(i==-1)))||((j==0)&&(i==-1))) return true;
	  break;
	case down_right:
	  nextDirection = down_right;
	  if(((j==-1)&&((i==0)||(i==1)))||((j==0)&&(i==1))) return true;
	  break;
  }
  return false;
}

bool isQuad(object& obj)
{
  int i, n, step;
  double perimeterMarks[3] = {0.37, 0.62, 0.87};
  double angleMarks[3] = {90, 180, 270};
  double perimeter = 0;
  double angTotal = 0;
  double x0, y0, x1, y1, x2, y2, ang;
  double angTol = 45;
  double direction = 1;

  step = 0;
  n = obj.points.size();
  for(i=0; i<n-1; i++)
  {
	x0 = obj.points[i-1].x;
	y0 = obj.points[i-1].y;
	x1 = obj.points[i].x;
	y1 = obj.points[i].y;
	x2 = obj.points[i+1].x;
	y2 = obj.points[i+1].y;

	perimeter += sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)) / obj.perimeter;
	if(i==0) continue;

	ang = 3.1416 - acos(((x0-x1)*(x2-x1) + (y0-y1)*(y2-y1))/(sqrt((x0-x1)*(x0-x1) + (y0-y1)*(y0-y1))*sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1))));
	if((y2-y0)*(x1-x0) < (y1-y0)*(x2-x0)) ang = -1*ang;
	ang = (ang*180)/3.1416;
	angTotal += ang;

	if(perimeter >= perimeterMarks[step])
	{
	  if((step==0)&&(ang<0)) direction = -1;
	  if(fabs(angTotal - direction*angleMarks[step]) > angTol) return false;
	  step++;
	}
  }

  obj.name = "Quadrilatero";

  if(direction == -1) obj.clockWise = false;
  else obj.clockWise = true;

  return true;
}

void nVertices(object& obj, int numVertices)
{
  int i, j, n, maxIndex;
  double x0, y0, x1, y1, x2, y2, ang, maxAng;
  vector<double> angs;
  vector<bool> isVertice;
  vector<point> vertices;

  n = obj.points.size();

  if(numVertices >= n) return;

  for(i=0; i<n; i++)
  {
	x0 = obj.points[i-1].x;
	y0 = obj.points[i-1].y;
	x1 = obj.points[i].x;
	y1 = obj.points[i].y;
	x2 = obj.points[i+1].x;
	y2 = obj.points[i+1].y;

	if(i==0)
	{
	  x0 = obj.points[n-1].x;
	  y0 = obj.points[n-1].y;
	}

	if(i==(n-1))
	{
	  x2 = obj.points[0].x;
	  y2 = obj.points[0].y;
	}

	ang = 3.1416 - acos(((x0-x1)*(x2-x1) + (y0-y1)*(y2-y1))/(sqrt((x0-x1)*(x0-x1) + (y0-y1)*(y0-y1))*sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1))));
	ang = fabs((ang*180)/3.1416);

	angs.push_back(ang);
	isVertice.push_back(false);
  }

  for(i=0; i<numVertices; i++)
  {
	maxAng = 0.0;
	maxIndex = -1;
	for(j=0; j<n; j++)
	{
	  if((!isVertice[j])&&(angs[j] > maxAng))
	  {
		maxAng = angs[j];
		maxIndex = j;
	  }
	}
	if(maxIndex >= 0)
	{
	  isVertice[maxIndex] = true;
	}
  }

  for(i=0; i<n; i++)
  {
	if(isVertice[i])
	{
	  vertices.push_back(obj.points[i]);
	}
  }

  obj.points = vertices;
}

bool validateObject(object& obj)
{
  vector<double> angVec;
  double perimeter = 0;
  double area = 0;
  int i, x0, y0, x1, y1, dx, dy, xmin, xmax, ymin, ymax, iaux, size;

  iaux = 0;
  size = obj.points.size();
  if(size < 3) return false;

  xmin = obj.points[0].x;
  xmax = obj.points[0].x;
  ymin = obj.points[0].y;
  ymax = obj.points[0].y;

  for(i=0; i<size; i++)
  {
	x0 = obj.points[i].x;
	y0 = obj.points[i].y;

	if(x0>xmax) xmax = x0;
	if(x0<xmin) xmin = x0;
	if(y0>ymax) ymax = y0;
	if(y0<ymin) ymin = y0;

	if(i<(size-1))
	{
	  x1 = obj.points[i+1].x;
	  y1 = obj.points[i+1].y;
	} else
	{
	  x1 = obj.points[0].x;
	  y1 = obj.points[0].y;
	}
	dx = x1-x0;
	dy = y1-y0;
	perimeter += sqrt(dx*dx + dy*dy);
	iaux += x0*y1 - x1*y0; 
  }

  area = fabs((double)iaux) / 2.0;
  obj.perimeter = perimeter;
  obj.area = area;
  obj.center.x = (xmax+xmin) / 2;
  obj.center.y = (ymax+ymin) / 2;
  obj.name = "Objeto";
  if(perimeter < 20.0) return false; 
  if(area < 500.0) return false;
//  obj.isPattern = isStar(obj);
  obj.isPattern = isQuad(obj);

  if(obj.isPattern) 
  {
	nVertices(obj, 4);
	if(!obj.clockWise)
	{
	  point paux = obj.points[1];
	  obj.points[1] = obj.points[3];
	  obj.points[3] = paux;
	}
  }

  return obj.isPattern;
}

void traceLines2(point origin, 
				 point pt, 
				 traceDirection direction, 
				 double ang_min, 
				 double ang_max, 
				 int num_blacks, 
				 pointSet& nextCandidates, 
				 pointSet& cycleCandidates, 
				 unsigned char* mask, 
				 int width, 
				 int height)
{
  int index, nextIndex, nextNumBlacks, i, j, dx, dy;
  double dist, ang, ang1, ang2, alfa;
  double line_width_tol = 2.0;
  point new_pt;
  traceDirection nextDirection;
  bool isInDirection;
  unsigned char nextValue;

  index = (pt.y*width + pt.x)*3;
  mask[index  ] = 1;
  mask[index+1] = 0;
  mask[index+2] = 0;

  for(i=-1; i<=1; i++)
  for(j=-1; j<=1; j++)
  {
	if((i==0)&&(j==0)) continue;

	new_pt.x = pt.x+i;
	new_pt.y = pt.y+j;
//	if((new_pt.x < 0)||(new_pt.x >= width)||(new_pt.y < 0)||(new_pt.y >= height)) continue;
	nextIndex = index+i*3+j*width*3;
	nextValue = mask[nextIndex];
	isInDirection = checkDirection(direction, i, j, nextDirection);
	dx = (new_pt.x)-(origin.x);
	dy = (new_pt.y)-(origin.y);
	dist = sqrt(dx*dx + dy*dy);

	if(nextValue == 0) continue;
//	nextNumBlacks = num_blacks;
//	if(nextValue == 0) nextNumBlacks++;
//	if(nextNumBlacks > dist / 20) continue;

	if((nextValue == 2)&&(dist > 3.00)) 
	{
	  cycleCandidates.push_back(new_pt);
	}

	if(isInDirection)
	{
	  if(dx==0) ang = 3.1416 / 2.00;
	  else ang = atan((double)dy / (double)dx);

	  if(nextDirection == up) ang = 3.1416 / 2.0;
	  if(nextDirection == down) ang = -3.1416 / 2.0;
	  if(nextDirection == left) ang = 3.1416;
	  if(nextDirection == right) ang = 0.0;
	  if(nextDirection == up_left) ang += 3.1416;
	  if(nextDirection == down_left) 
	  {
		if(direction == left)
		{
		  ang_min -= 2.0*3.1416;
		  ang_max -= 2.0*3.1416;
		}
		ang -= 3.1416;
	  }

	  if((ang<ang_min)||(ang>ang_max)) isInDirection = false;
	}

	if(!isInDirection)
	{
	  if(nextValue == 255) 
	  {
		nextCandidates.push_back(new_pt);
	  }
	} else
	{
	  if(dist>line_width_tol)
	  {
		alfa = asin(line_width_tol / dist);
		ang1 = ang + alfa;
		ang2 = ang - alfa;
	  } else
	  {
		ang1=ang_max; 
		ang2=ang_min;
	  }
	  if(ang1>ang_max) ang1=ang_max; 
	  if(ang2<ang_min) ang2=ang_min;
	  traceLines2(origin, new_pt, nextDirection, ang2, ang1, nextNumBlacks, nextCandidates, cycleCandidates, mask, width, height);
	}
  }

  mask[index  ] = 0;
}

struct guardianPoint {
  point location;
  pointSet previousVertices;
};

typedef vector<guardianPoint> guardianTable;

void findCycles2(point pt, 
				 unsigned char* mask, 
				 objectSet& objects, 
				 int width, 
				 int height)
{
  guardianTable verticeTable;
  pointSet allCandidates;
  pointSet nextCandidates;
  pointSet cycleCandidates;
  object cycle;
  point currPoint;
  guardianPoint currEntry;
  int i, j, k, common_pt_index, index;

  currEntry.location = pt;
  currEntry.previousVertices.clear();
  verticeTable.push_back(currEntry);

  while(verticeTable.size() > 0)
  {
	currEntry = verticeTable[verticeTable.size()-1];
	verticeTable.pop_back();
	currPoint = currEntry.location;
	nextCandidates.clear();
	allCandidates.clear();
	cycleCandidates.clear();
	traceLines2(currPoint, currPoint, any, -2.0*3.1416, 2.0*3.1416, -1, allCandidates, cycleCandidates, mask, width, height);
	for(i=0; i<allCandidates.size(); i++)
	{
	  index = allCandidates[i].x*3 + allCandidates[i].y*width*3;
	  if(mask[index]>0)
	  {
		nextCandidates.push_back(allCandidates[i]);
	  }
	}
	currEntry.previousVertices.push_back(currPoint);
	for(i=0; i<cycleCandidates.size(); i++)
	{
	  for(j=0; j<verticeTable.size(); j++)
	  {
		if(verticeTable[j].location == cycleCandidates[i])
		{
		  cycle.points.clear();
		  common_pt_index = -1;
		  for(k=0; (k<currEntry.previousVertices.size())&&(k<verticeTable[j].previousVertices.size()); k++)
		  {
			if(currEntry.previousVertices[k] == verticeTable[j].previousVertices[k]) common_pt_index++;
		  }
		  for(k=common_pt_index; k<currEntry.previousVertices.size(); k++)
		  {
			cycle.points.push_back(currEntry.previousVertices[k]);
		  }
		  cycle.points.push_back(cycleCandidates[i]);
		  for(k=verticeTable[j].previousVertices.size()-1; k>common_pt_index; k--)
		  {
			cycle.points.push_back(verticeTable[j].previousVertices[k]);
		  }
		  if(validateObject(cycle))
		  {
//			drawCycle(cycle.points, img_out, width, height);
			objects.push_back(cycle);
			return;
		  }
		}
	  }
	}
	for(i=0; i<nextCandidates.size(); i++)
	{
	  index = nextCandidates[i].x*3 + nextCandidates[i].y*width*3;
	  if(mask[index] > 0) mask[index] = 2;
	  currEntry.location = nextCandidates[i];
	  verticeTable.push_back(currEntry);
	}
  }
}

void vectorizeMask2(unsigned char* img_in, 
					unsigned char* img_aux, 
					objectSet& objects, 
					int width, 
					int height)
{
  int i, j, index;
  point pt;

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

	if((i<=1)||(j<=1)||(i>=width-2)||(j>=height-2))
	{
	  img_aux[index  ] = 0;
	  img_aux[index+1] = 0;
	  img_aux[index+2] = 0;
	  continue;
	}

    img_aux[index  ] = img_in[index  ];
    img_aux[index+1] = img_in[index+1];
    img_aux[index+2] = img_in[index+2];
  }

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

	if(img_aux[index] != 0)
	{
	  pt.x = i;
	  pt.y = j;
	  findCycles2(pt, img_aux, objects, width, height);
	}
  }
}

//
//  Find critical points functions
//

template<class T>
vector<T> shiftVector(vector<T>& vec, int d)
{
  vector<T> result;
  int size = vec.size();

  for(int i=0; i<size; i++)
  {
	int index = (i - d) % size;
	if(index < 0) index += size;

	result.push_back(vec[index]);
  }

  return result;
}

void synchronizeCycles(pointSet& cycle1, pointSet& cycle2)
{
  if(cycle1.size() != cycle2.size()) return;
  int size = cycle1.size();

  pointSet newCycle2;

  for(int i=0; i<size; i++)
  {
	float minDist2 = 999999999999.0f;
	int index = -1;

	int x = cycle1[i].x;
	int y = cycle1[i].y;

	for(int j=0; j<size; j++)
	{
	  int dx = cycle2[j].x - x;
	  int dy = cycle2[j].y - y;
	  int currDist = (dx*dx + dy*dy);

	  if(currDist < minDist2)
	  {
		minDist2 = currDist;
		index = j;
	  }
	}

	if(index != -1)
	{
	  newCycle2.push_back(cycle2[index]);
	}
  }

  cycle2 = newCycle2;
}


void adjustCycles(pointSet& outerCycle, pointSet& innerCycle, unsigned char* img, int width, int height)
{
  if(outerCycle.size() != innerCycle.size()) return;
  synchronizeCycles(outerCycle, innerCycle);

  int size = outerCycle.size();
  int minLight = 1000;
  int index0 = -1;

  for(int i=0; i<size; i++)
  {
	int x = outerCycle[i].x + (outerCycle[i].x - innerCycle[i].x) / 2;
	int y = outerCycle[i].y + (outerCycle[i].y - innerCycle[i].y) / 2;

	int sumLight = 1000;

	if((x>=0)&&(x<width)&&(y>=0)&&(y<height))
	{
	  int index = (y*width + x)*3;
	  sumLight = img[index] + img[index+1] + img[index+2];
	}

	if(sumLight < minLight)
	{
	  minLight = sumLight;
	  index0 = i;
	}
  }

  outerCycle = shiftVector(outerCycle, -index0);
  innerCycle = shiftVector(innerCycle, -index0);
}

void getCriticalPoints(objectSet& objects, unsigned char* img, int width, int height)
{
  if(objects.size() < 2) return;

  adjustCycles(objects[0].points, objects[1].points, img, width, height);
}