/* Principal Component Analysis Module for the IUPGLCAP application. */
/* Author: Marcos de Carvalho Machado (mmachado@tecgraf.puc-rio.br)  */
/* Date: 25/09/2004 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <im.h>
#include <im_image.h>

#include "pca.h"

#define EPSILON 0.00001

static void getSobel(imImage* image, int x, int y, int * grad_x, int * grad_y)
{
	unsigned char* im_data = (unsigned char*)image->data[0];
    int offset_up;
    int offset;
    int offset_dw;

	/* Taking care of the borders */
	if(x<=0)
		x = 1;
	else if(x >= (image->width - 1))
		x = image->width - 2;
	if(y<=0)
		y = 1;
	else if(y >= (image->height - 1))
		x = image->height - 2;

	offset    = y*image->width;
	offset_up = (y+1)*image->width;
	offset_dw = (y-1)*image->width;

	/* Computing the derivatives */
    *grad_x =     im_data[offset_up + x+1] -     im_data[offset_up + x-1] +
              2 * im_data[offset    + x+1] - 2 * im_data[offset    + x-1] + 
                  im_data[offset_dw + x+1] -     im_data[offset_dw + x-1];

    *grad_y =     im_data[offset_up + x+1] -     im_data[offset_dw + x+1] +
              2 * im_data[offset_up + x  ] - 2 * im_data[offset_dw + x  ] + 
                  im_data[offset_up + x-1] -     im_data[offset_dw + x-1];
}

static void getDerivative(imImage* image, int x, int y, int * grad_x, int * grad_y)
{
	unsigned char* im_data = (unsigned char*)image->data[0];
    int offset_up;
    int offset;
    int offset_dw;

	/* Taking care of the borders */
	if(x<=0)
		x = 1;
	else if(x >= (image->width - 1))
		x = image->width - 2;
	if(y<=0)
		y = 1;
	else if(y >= (image->height - 1))
		x = image->height - 2;

	offset    = y*image->width;
	offset_up = (y+1)*image->width;
	offset_dw = (y-1)*image->width;

	/* Computing the derivatives */
    *grad_x =   im_data[offset    + x+1] - im_data[offset    + x-1];
    *grad_y =   im_data[offset_up + x  ] - im_data[offset_dw + x  ];
}

/* n must be odd */
static void getStructureMatrix(imImage* image, int x, int y, int n, 
							   double *s11, double *s22, double *s12)
{
	/* Number of positions previously allocated */
	static int n_allocated = 0;
	/* X partial derivative in the neighborhood of (x,y) */
	static int ** ex = NULL;
	/* Y partial derivative in the neighborhood of (x,y) */
	static int ** ey = NULL;
	int h = (n-1)/2;
	int i, j;
	int ne;
	/* Auxiliary variables to compute variances */
	double s_x2, sx, s_y2, sy;
	/* Auxiliary variables to compute covariances */
	double sxy;

	if(n > n_allocated) {
		if(ex) {
			for(i=0; i<n; i++)
				free(ex[i]);
			free(ex);
		}
		if(ey) {
			for(i=0; i<n; i++)
				free(ey[i]);
			free(ey);
		}
		if((n_allocated = n) <= 0) {
			ex = NULL;
			ey = NULL;
			return;
		}
		else {
            ex = (int ** )malloc(sizeof(int*)*n);
            for(i=0; i<n; i++)
                ex[i] = (int * )malloc(sizeof(int)*n);
            ey = (int ** )malloc(sizeof(int*)*n);
            for(i=0; i<n; i++)
                ey[i] = (int * )malloc(sizeof(int)*n);
		}
	}

	/* Compute ex and ey */
	for(i=0; i<n; i++) {
		for(j=0; j<n; j++) {
			getSobel(image, x+(i-h), y+(j-h), &ex[i][j], &ey[i][j]); 
			//getDerivative(image, x+(i-h), y+(j-h), &ex[i][j], &ey[i][j]); 
		}
	}

	/* Compute structure matrix elements */
	s_x2 = sx = s_y2 = sxy = sx = sy = 0;
	for(i=0; i<n; i++) {
		for(j=0; j<n; j++) {
			s_x2 += ex[i][j] * ex[i][j];
			sx   += ex[i][j];
			s_y2 += ey[i][j] * ey[i][j];
			sy   += ey[i][j];
			sxy  += ex[i][j] * ey[i][j];
		}
	}
	ne = n*n;
	*s11 = (s_x2 / (ne-1)) - ((sx*sx) / (ne*(ne-1)));
	*s22 = (s_y2 / (ne-1)) - ((sy*sy) / (ne*(ne-1)));
	*s12 = (sxy  / (ne-1)) - ((sx*sy) / (ne*(ne-1)));
}

static int getEig(double s11, double s22, double s12, double * peig1, double * peigv1_x, double * peigv1_y,
			double * peig2, double * peigv2_x, double * peigv2_y)
{
	double b;
	double ac;
	double delta2, delta;
	double norm;
    double eig1;
	double eigv1_x, eigv1_y;
    double eig2;
	double eigv2_x, eigv2_y;

	b = -(s11 + s22);
	ac = s11 * s22 - s12 * s12;
	delta2 = b*b - 4*ac;
	if(delta2 < 0)
		return -1;
	delta = sqrt(delta2);

	eig1 = (-b + delta) / 2.;
	eig2 = (-b - delta) / 2.;

	if(fabs(eig2) > fabs(eig1)) {
		double aux = eig1;
		eig1 = eig2;
		eig2 = aux;
	}

	if(fabs(s12) > EPSILON) {
		eigv1_x = (eig1 - s22) / s12;
		eigv1_y = 1;
		norm = sqrt(1 + eigv1_x * eigv1_x);
		eigv1_x /= norm;
		eigv1_y /= norm;	

		eigv2_x = (eig2 - s22) / s12;
		eigv2_y = 1;
		norm = sqrt(1 + eigv2_x * eigv2_x);
		eigv2_x /= norm;
		eigv2_y /= norm;
	}
	else {
		if(fabs(s11) < EPSILON) {
			eigv1_x = 0;
			eigv1_y = 1;
			eigv2_x = 1;
			eigv2_y = 0;
		}
		else {
			eigv1_x = 1;
			eigv1_y = 0;
			eigv2_x = 0;
			eigv2_y = 1;
		}
	}

//	if((eig1 < (-EPSILON)) || (eig2 < (-EPSILON)))
//		IupMessage("ERRO","Autovalor negativo!");

    *peig1 = eig1;
	*peigv1_x = eigv1_x;
	*peigv1_y = eigv1_y;
    *peig2 = eig2;
	*peigv2_x = eigv2_x;
	*peigv2_y = eigv2_y;

	return 0;
}

/* Function: it obtains the 2 eigenvalues and respectives eigenvectors of the image structure
             matrix around the point (x,y). 
			 The parameter "n" is the size of the neighbourhood; it must be odd. 
			 Internally allocated memory can be reused in subsequent calls. */
int GetEigPCA(imImage* image, int x, int y, int n, double * eig1, double * eigv1_x, double * eigv1_y,
			double * eig2, double * eigv2_x, double * eigv2_y)
{
	double s11, s22, s12;

    getStructureMatrix(image, x, y, n, &s11, &s22, &s12);
	return getEig(s11, s22, s12, eig1, eigv1_x, eigv1_y, eig2, eigv2_x, eigv2_y);
}

/* Function: it frees the allocated memory used by GetEigPCA. */
void FreePCA()
{
    getStructureMatrix(NULL, 0, 0, 0, NULL, NULL, NULL);
}


/* Function: it obtains the best candidate to be a corner in a circular neighbourhood of 
             radius r around the point (x,y) of the image. It calls GetEigPCA.
			 The parameter "n" is the size of the neighbourhood for GetPCA; it must be odd 
			 It fails if the ratio beetween the lesser over the greather eigenvalue is less
			 than threshold parameter (0 <= threshold <= 1). */
int GetCornerPCA(imImage* image, int x, int y, int n, int r, double threshold, 
				 int* xc, int* yc)
{
	double s11, s22, s12;
	double max_eig2;
    double eig1;
	double eigv1_x, eigv1_y;
    double eig2;
	double eigv2_x, eigv2_y;
	int xt, yt;
	int i, j;
	double r_cur, rc;

	max_eig2 = 0;
	rc = r;
	for(j = -r; j <= r; j++) {
		yt = y + j;
		if((yt <= 0) || (yt >= image->height))
			continue;
		for(i = -r; i <= r; i++) {
			xt = x + i;
			if((xt <= 0) || (xt >= image->width))
				continue;
			if((r_cur=sqrt(i*i + j*j)) > r)
				continue;
            getStructureMatrix(image, xt, yt, n, &s11, &s22, &s12);
            if(getEig(s11, s22, s12, &eig1, &eigv1_x, &eigv1_y, &eig2, &eigv2_x, &eigv2_y) != 0)
				continue;
//			if((eig2 < max_eig2) || ((eig2/eig1) < threshold)) {
			if((eig2 < max_eig2) || (eig2 == 0)) {
				continue;
			} else if(eig2 > max_eig2) {
				max_eig2 = eig2;
				rc = r_cur;
				*xc = xt;
				*yc = yt;
			}
			else if(rc > r_cur) {
				max_eig2 = eig2;
				rc = r_cur;
				*xc = xt;
				*yc = yt;
			}
		}
	}
	if(max_eig2 == 0)
		return -1;
	else 
		return 0;
}

#ifdef TESTE1
void main(void)
{
	double eig1;
	double eigv1_x, eigv1_y;
    double eig2;
	double eigv2_x, eigv2_y;
	
	int s11, s22, s12;
	s11 = 6; s22 = 6; s12 = 8;

	if( getEig(s11, s22, s12, &eig1, &eigv1_x, &eigv1_y, &eig2, &eigv2_x, &eigv2_y) != 0) {
		printf("ERRO: Autovalores complexos!\n");
		return;
	}
		
	printf("eig1: %f  eigv1_x: %f eigv1_y: %f\n", eig1,eigv1_x,eigv1_y);
	printf("eig2: %f  eigv2_x: %f eigv2_y: %f\n", eig2,eigv2_x,eigv2_y);

}
#elif TESTE
void main(void)
{
	double eig1;
	double eigv1_x, eigv1_y;
    double eig2;
	double eigv2_x, eigv2_y;
	int width, height;
	int i, j;	
	imImage* image;
	unsigned char* im_data;
	int xc, yc;

	width = 40;
	height = 40;
	image = imImageCreate(width, height, IM_GRAY, IM_BYTE);
	im_data = (unsigned char*)image->data[0];
/*/
	for(j=0; j<height; j++)
		for(i=0; i<width; i++)
			im_data[j*image->width + i] = (i>=j)? 1 : 0;
/*/
/*
	for(j=0; j<(height/2); j++)
		for(i=0; i<width; i++)
			im_data[j*image->width + i] = 1;
	for(; j<height; j++)
		for(i=0; i<width; i++)
			im_data[j*image->width + i] = 0;
/*/
	for(j=0; j<(height/4); j++)
		for(i=0; i<width; i++)
			im_data[j*image->width + i] = 0;

	for(; j<(3*height/4); j++) {
		for(i=0; i<(width/4); i++)
			im_data[j*image->width + i] = 0;
		for(; i<(3*width/4); i++)
			im_data[j*image->width + i] = 1;
		for(; i<(width); i++)
			im_data[j*image->width + i] = 0;
	}

	for(; j<height; j++)
		for(i=0; i<width; i++)
			im_data[j*image->width + i] = 0;

	if( GetPCA(image, width/4, height/4, 3, &eig1, &eigv1_x, &eigv1_y,
				&eig2, &eigv2_x, &eigv2_y)) {
		printf("ERRO: Autovalores complexos!\n");
		return;
	}
		
	printf("eig1: %f  eigv1_x: %f eigv1_y: %f\n", eig1,eigv1_x,eigv1_y);
	printf("eig2: %f  eigv2_x: %f eigv2_y: %f\n", eig2,eigv2_x,eigv2_y);


	if( GetCorner(image, 3*width/4, 3*height/4, 3, 2, &xc, &yc)) {
		printf("ERRO: Nao identificou corner!\n");
		return;
	}
		
	printf("CORNER: xc: %d  yc: %d\n", xc, yc);

}

#endif

