/** Linear fitting of spheres to a set of 3D 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

And:
SUHAIL A ISLAM - MATHS NOTES
http://www.bmm.icnet.uk/people/suhail/plane.html

Fraga, 3/03/2009
 **/

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


/**
   Compiled 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
#define MM 16

int main( int argc, char *argv[] )
{
	int i,j;
	char namein[64];
	double px, py, pz;
	double a, b, c, d;
	double x0, y0, z0;
	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= 195;
	int INFO;

	double S[MM], Z[MM];
	double alphar[M];
	double alphai[M];
	double beta[M];
	double vl[LDVL*M];
	double vr[LDVR*M];
	double wk[LWORK];


	double C[MM]={  1.0, 0.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0,
	                0.0, 0.0, 1.0, 0.0,
	                0.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<MM; i++) S[i] = 0.0;

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

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

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

			Z[10] = pz*pz;
			Z[11] = Z[14] = pz;

			Z[15] = 1.0;

			for( j=0; j<MM; j++ ) S[j] += Z[j];
			i++;
		}
		else {
			fprintf( stderr, "Error reading input file\n" );
			fclose(file);
			return( 1 );
		}
	}
	fclose(file);
	printf( "# n = %d %lf\n", i, S[24] );

 	// Ccalculating plane's centroid
	x0 = S[3]/i;
	y0 = S[7]/i;
	z0 = S[11]/i;

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


	printf("\n INFO=%d", INFO );

	printf("\nalphar\n ");
	for(j=0; j < M; j++){
		printf(" %g \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*M+i]);
		}
		printf("\n ");
	}

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

	a = vr[i*M+0];
	b = vr[i*M+1];
	c = vr[i*M+2];
	d = vr[i*M+3];

	printf("%lf %lf %lf %lf c(%lf, %lf, %lf)\n", a, b, c, d, x0, y0, z0 );

	return 0;
}