/** Linear fitting of circles to a set of 2D points

Idea based in papers:

V. Pratt. Direct least-squares fitting of algebraic surfaces
ACM SIGGRAPH Computer Graphics, (21) 4, 1987, pp. 145 - 152

A. Fitzgibbon, M. Pilu, and R.B. Fisher,
Direct least square fitting of ellipses
IEEE Trans, of Pattern Anal. & Mach. Intell. (21) 5, 1999,  pp. 476-480
 **/

#include <stdio.h>
#include <math.h>
#include "f2c.h"
#include "clapack.h"

/**
   Compile with command line:
gcc -O2 algeb_fit.c -o algeb_fit lapack_LINUX.a libF77.a blas_LINUX.a -lm

gcc -O2 algeb_fit.c -o algeb_fit -L/opt/local/lib -lapack -lF77 -lblas -lm
**/

#define M 4

int main( int argc, char *argv[] )
{

	int i,j;
	char namein[64];
	double px, py;
	double a, d, e, f;
	double x0, y0, r;
	FILE *file;

	////////////////////////////////////////
	/** Parametros para la función dggev **/

	char JOBVL = 'N';
	char JOBVR = 'V';
	int N = M;
	int LDA = M;
	int LDB = M;
	int LDVL = 1;
	int LDVR = M;
	int LWORK=156;
	int INFO;

	double S[M*M], Z[M*M];
	double alphar[M];
	double alphai[M];
	double beta[M];
	double vl[LDVL*M];
	double vr[LDVR*M];
	double wk[LWORK];
	// double wk;
	double C[M*M]={ 0.0, 0.0, 0.0, -2.0,
	                0.0, 1.0, 0.0,  0.0,
	                0.0, 0.0, 1.0,  0.0,
	               -2.0, 0.0, 0.0,  0.0};



	////////////////////////////////////////////

	if ( argc != 2 ) {
		fprintf( stderr, "Args: file_name\n" ); 
		exit(1);
	}

	strncpy( namein, argv[1], 64 );
	if ( (file = fopen (namein,"r") ) == NULL ) {
		fprintf( stderr, "Can't open %s file\n", file );
		exit(1);
	}

	for( i=0; i<M*M; i++ ) S[i] = 0.0;

	//Construye la matriz Design X
	for (i=0; ;) {
		j = fscanf(file,"%lf %lf\n", &px, &py );

		if ( j == EOF ) break;
		else if( j==2 ) {
			// printf( "%lf %lf\n", px, py );
			r = px*px + py*py;
			Z[0] = r*r;
			Z[1] = Z[4] = r*px;
			Z[2] = Z[8] = r*py;
			Z[3] = Z[12] = r;

			Z[5] = px*px;
			Z[6] = Z[9] = px*py;
			Z[7] = Z[13] = px;

			Z[10] = py*py;
			Z[11] = Z[14] = py;
			Z[15] = 1.0;

			for(j=0; j<M*M; j++ ) S[j] += Z[j];
			i++;
		}
	}
	printf( "# n = %d\n", i );


	dggev_(&JOBVL, &JOBVR, &N, S, &LDA, C, &LDB, alphar, alphai, beta, vl, &LDVL, vr, &LDVR, wk, &LWORK, &INFO);


	// printf("\n INFO=%d WORK=%lf", INFO, wk );
	printf("\n INFO=%d", INFO );

	printf("\nalphar\n ");
	for(j=0; j < M; j++){
		printf(" %lf \n ", alphar[j]);
	}

	printf("\nalphai\n ");
	for(j=0; j < M; j++){
		printf(" %lf \n ", alphai[j]);
	}

	printf("\nbeta\n ");
	for(j=0; j < M; j++){
		printf(" %lf \n ", beta[j]);
	}

	printf("\nEigenvectores (transpuestos)\n ");
	for(j=0; j < M; j++){
		for (i=0; i < M; i++){
			printf(" %lf  ", vr[j*N+i]);
		}
		printf("\n ");
	}

	a = MAXFLOAT;
    for(j=0; j<M; j++) {
		b = fabs( alphar[j] );
        if( b < a && b < 1e10 ){
			a = b;
			i=j;
		}
	}
	printf("\nMínimo eigenvalor: %d \n",i);

	a = vr[i*M+0];
	d = vr[i*M+1];
	e = vr[i*M+2];
	f = vr[i*M+3];

	x0 = -d/(2*a);
	y0 = -e/(2*a);

	r = sqrt( (d*d + e*e)/(4.0*a*a) -f/a );

	printf("\nCentro, r\n" );
	printf("%lf %lf %lf\n", x0, y0, r );

	return 0;
}