/* Image Processing and Recognition Module for the IUPGLCAP application. */
/* It should not contain interface code, neither OpenGL nor IUP.         */

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

#include <iup.h>

#include <im.h>
#include <im_capture.h>
#include <im_image.h>
#include <im_convert.h>
#include <im_process.h>
#include <im_format_avi.h>

#include "pca.h"

#define NLINES 8
#define EPSILON 0.00001

/* Line atibuttes */
struct stLine {
	/* A line point */
	int x;
    int y;
	/* Line normal vector */
	double nx;
	double ny;
};


/* Function: given a point (x,y) in the image, it computes the offset in the image array. */
#define OFFSET(image, x, y) (y*image->width + x)

/* Function: given an offset in the image array, it turns to 255 its value. 
             The same is done to its 8 neighbours */
static void Mark(imImage* image, int offset)
{
  unsigned char* im_data = (unsigned char*)image->data[0];
  int offset1, offset2, offset3, offset4,
	  offset5, offset6, offset7, offset8;

  im_data[offset] = 255;

  offset1=offset+1; offset2=offset+image->width;
  offset3=offset-1; offset4=offset-image->width;
  offset5=offset+1+image->width;
  offset6=offset-1+image->width;
  offset7=offset-1-image->width;
  offset8=offset+1-image->width;

  if(offset1 < image->size)
      im_data[offset1] = 255;
  if(offset2 < image->size)
      im_data[offset2] = 255;
  if(offset3 >= 0)
      im_data[offset3] = 255;
  if(offset4 >= 0)
      im_data[offset4] = 255;

  if(offset5 < image->size)
      im_data[offset5] = 255;
  if(offset6 < image->size)
      im_data[offset6] = 255;
  if(offset7 >= 0)
      im_data[offset7] = 255;
  if(offset8 >= 0)
      im_data[offset8] = 255;

}

/* Function: given a point (x,y) in the image, it turns to 255 its value. 
             The same is done to its 8 neighbours */
void MarkPoint(imImage* image, int x, int y) 
{
	Mark(image, OFFSET(image,x,y));
}

/* Function: it draws the line identified by att */
void MarkLine(imImage* image, struct stLine * att) 
{
	int i, j;
	unsigned char* im_data = (unsigned char*)image->data[0];

	if((fabs(att->ny) > EPSILON) && (fabs(att->ny) > fabs(att->nx))) {
		double m = att->nx / att->ny;
		for(i=0; i < image->width; i++) {
			j = (int)(att->y - m * (i - att->x) + 0.5);
			if((j < image->height) && (j >= 0))
				im_data[j * image->width + i] = 255;
		}
	}
	else if(fabs(att->nx) > EPSILON) {
		double m = att->ny / att->nx;
		for(j=0; j < image->height; j++) {
			i = (int)(att->x - m * (j - att->y) + 0.5);
			if((i < image->width) && (i >= 0))
				im_data[j * image->width + i] = 255;
		}
	}
	else
		return;
}


/* Function: returns the intersect point of 2 lines */
static int	IntersectLines(struct stLine * l1, struct stLine * l2, double * x, double * y)
{
	double m1, m2;

	if((l1->nx == l2->nx) && (l1->ny == l2->ny))
		return -1;
	if((l1->nx == -l2->nx) && (l1->ny == -l2->ny))
		return -1;

	m1 = l1->nx / l1->ny;
	m2 = l2->nx / l2->ny;
	*x = (l1->y - l2->y + m1*l1->x - m2*l2->x) / (m1 - m2);
	*y = l1->y - m1 * (*x - l1->x);
	return 0;
}

static int	IdentifyCorners(imImage* amps, imImage* image, struct stLine * att, int r)
{
	double x, y;
	int ix, iy;

	/* Internal corners of the pattern */
	if(IntersectLines(&(att[0]), &(att[3]), &x, &y) != 0)
		return -1;
	ix = (int)(x + 0.5);
	iy = (int)(y + 0.5);
	if((ix<0) || (ix>=image->width) || (iy<0) || (iy>=image->height))
		return -1;
	GetCornerPCA(amps, ix, iy, 3, r, &ix, &iy);
	MarkPoint(image, ix, iy);

	if(IntersectLines(&(att[0]), &(att[1]), &x, &y) != 0)
		return -1;
	ix = (int)(x + 0.5);
	iy = (int)(y + 0.5);
	if((ix<0) || (ix>=image->width) || (iy<0) || (iy>=image->height))
		return -1;
	GetCornerPCA(amps, ix, iy, 3, r, &ix, &iy);
	MarkPoint(image, ix, iy);

	if(IntersectLines(&(att[2]), &(att[1]), &x, &y) != 0)
		return -1;
	ix = (int)(x + 0.5);
	iy = (int)(y + 0.5);
	if((ix<0) || (ix>=image->width) || (iy<0) || (iy>=image->height))
		return -1;
	GetCornerPCA(amps, ix, iy, 3, r, &ix, &iy);
	MarkPoint(image, ix, iy);

	if(IntersectLines(&(att[2]), &(att[3]), &x, &y) != 0)
		return -1;
	ix = (int)(x + 0.5);
	iy = (int)(y + 0.5);
	if((ix<0) || (ix>=image->width) || (iy<0) || (iy>=image->height))
		return -1;
	GetCornerPCA(amps, ix, iy, 3, r, &ix, &iy);
	MarkPoint(image, ix, iy);

	/* External corners of the pattern */

	if(IntersectLines(&(att[4]), &(att[7]), &x, &y) != 0)
		return -1;
	ix = (int)(x + 0.5);
	iy = (int)(y + 0.5);
	if((ix<0) || (ix>=image->width) || (iy<0) || (iy>=image->height))
		return -1;
	GetCornerPCA(amps, ix, iy, 3, r, &ix, &iy);
	MarkPoint(image, ix, iy);

	if(IntersectLines(&(att[4]), &(att[5]), &x, &y) != 0)
		return -1;
	ix = (int)(x + 0.5);
	iy = (int)(y + 0.5);
	if((ix<0) || (ix>=image->width) || (iy<0) || (iy>=image->height))
		return -1;
	GetCornerPCA(amps, ix, iy, 3, r, &ix, &iy);
	MarkPoint(image, ix, iy);

	if(IntersectLines(&(att[6]), &(att[5]), &x, &y) != 0)
		return -1;
	ix = (int)(x + 0.5);
	iy = (int)(y + 0.5);
	if((ix<0) || (ix>=image->width) || (iy<0) || (iy>=image->height))
		return -1;
	GetCornerPCA(amps, ix, iy, 3, r, &ix, &iy);
	MarkPoint(image, ix, iy);

	if(IntersectLines(&(att[6]), &(att[7]), &x, &y) != 0)
		return -1;
	ix = (int)(x + 0.5);
	iy = (int)(y + 0.5);
	if((ix<0) || (ix>=image->width) || (iy<0) || (iy>=image->height))
		return -1;
	GetCornerPCA(amps, ix, iy, 3, r, &ix, &iy);
	MarkPoint(image, ix, iy);

	return 0;
}

static int IdentifyLine(imImage* input, struct stLine * att) 
{
	double eig1;
	double eigv1_x, eigv1_y;
    double eig2;
	double eigv2_x, eigv2_y;

	if( GetEigPCA(input, att->x, att->y, 3, &eig1, &eigv1_x, &eigv1_y, &eig2, &eigv2_x, &eigv2_y)) {
		//IupMessage("ERRO", "Autovalores complexos!");
		return -1;
	}

	if((fabs(eig1)< EPSILON) || ((fabs(eigv1_x) < EPSILON) && (fabs(eigv1_y) < EPSILON)))
		return -1;
	att->nx = eigv1_x;
	att->ny = eigv1_y;

	return 0;
}


static void IdentifyLines(imImage* input, int n, struct stLine * att) 
{
	int i;

	for(i=0; i < n; i++)
		IdentifyLine(input, &(att[i]));
}

static void IdentifyPoint(int x, int y, struct stLine * att) 
{
	att->x = x;
	att->y = y;
	att->nx = 0;
	att->ny = 0;
}

static int IdentifyCandidates(imImage* image, int center_x, int center_y, 
							   int nCandidates, struct stLine * att)
{
  unsigned char* im_data = (unsigned char*)image->data[0];

  int n = 0;

  int finished = 0;
  int find_xp = 0;
  int find_yp = 0;
  int find_xn = 0;
  int find_yn = 0;
  int xp = center_x+1;
  int yp = center_y+1;
  int xn = center_x-1;
  int yn = center_y-1;
  int in = 0;
  int init = 0;
  int end = 0;

  while(! finished) {

	  /* positive X */
	  for(;(xp < image->width) && (! find_xp); xp++) {
		if(im_data[OFFSET(image,xp,center_y)] != 0)
			if(!in) {
                in = 1;
                init = xp;
                end = xp;
			}
			else {
				end = xp;
			}
		else {
			if(in) {
				int z0;
                in = 0;
                z0 = (init + end) / 2;
				IdentifyPoint(z0, center_y, &(att[n]));
				find_xp = 1;
				if(++n == nCandidates)
					return n;
			}
		}
	  }
	  if(!find_xp)
		  return n;

	  /* positive Y */
	  for(;(yp < image->height) && (! find_yp); yp++) {
		if(im_data[OFFSET(image,center_x,yp)] != 0)
			if(!in) {
                in = 1;
                init = yp;
                end = yp;
			}
			else {
				end = yp;
			}
		else {
			if(in) {
				int z0;
                in = 0;
                z0 = (init + end) / 2;
				IdentifyPoint(center_x, z0, &(att[n]));
				find_yp = 1;
				if(++n == nCandidates)
					return n;
			}
		}
	  }
	  if(!find_yp)
		  return n;

	  /* negative X */
	  for(;(xn >= 0) && (! find_xn); xn--) {
		if(im_data[OFFSET(image,xn,center_y)] != 0)
			if(!in) {
                in = 1;
                init = xn;
                end = xn;
			}
			else {
				end = xn;
			}
		else {
			if(in) {
				int z0;
                in = 0;
                z0 = (init + end) / 2;
				IdentifyPoint(z0, center_y, &(att[n]));
				find_xn = 1;
				if(++n == nCandidates)
					return n;
			}
		}
	  }
	  if(! find_xn)
		  return n;

	  /* negative Y */
	  for(;(yn >= 0) && (! find_yn); yn--) {
		if(im_data[OFFSET(image,center_x,yn)] != 0)
			if(!in) {
                in = 1;
                init = yn;
                end = yn;
			}
			else {
				end = yn;
			}
		else {
			if(in) {
				int z0;
                in = 0;
                z0 = (init + end) / 2;
				IdentifyPoint(center_x, z0, &(att[n]));
				find_yn = 1;
				if(++n == nCandidates)
					return n;
			}
		}
	  }
	  if(! find_yn)
		  return n;

	  finished = !(find_xp || find_yp || find_xn || find_yn);
	  find_xp = find_yp = find_xn = find_yn = 0;
  }
  return n;
}

void arProcessMarcos5(unsigned char* gl_data, imImage *image,imImage *aux_image, int *flags, double value1, double value2)
{
  static imImage *amps = NULL;
  struct stLine att[NLINES];
  int i;
  int ret;

  if (flags[0] || flags[1] || flags[2])
  {

    /* eliminate noise with a gaussian filter of sigma=1 */
    imProcessGaussianConvolve(image, aux_image, 1.0f);

	/* Save image amplitudes */
	if(amps)
		imImageCopyData(image, amps);
	else
        amps = imImageDuplicate(image);

	/* Magnitude of the sobel convolution */
	imProcessSobelConvolve(aux_image, image);

    /* Apply a threshold and convert to 0-127 */
	imProcessThreshold(image, aux_image, (int)(value1*255), 127); 

	ret = IdentifyCandidates(aux_image, image->width/2, image->height/2, NLINES, att);

  	if (flags[0]) {
        for(i=0; i<ret; i++)
            MarkPoint(aux_image, att[i].x, att[i].y);
	}

	if (flags[1] || flags[2]) {
        IdentifyLines(amps, ret, att);

		if (flags[1]) {
            for(i=0; i<ret; i++)
                MarkLine(aux_image, &(att[i]));
		}
	 }

	if (flags[2] && (ret == NLINES))
		IdentifyCorners(amps, aux_image, att, 12);

	imImageCopyData(aux_image, image);

  }
}
