/* --------------------------------------------------------------------- */
/* -                   CALIB_BOTH.C                                    - */
/* --------------------------------------------------------------------- */
/* -    Some necessary includes...                                     - */
/* --------------------------------------------------------------------- */
/* written by: Roger W. Webster, Ph.D.
   original code stolen with permission from Robert Smith, Ph.D.        */
/* --------------------------------------------------------------------- */
#include "calibration.h"
/* #define DEBUG 1 */

/* --------------------------------------------------------------------- */
/* -    Global variables and structures...                             - */
/* --------------------------------------------------------------------- */
int i,j, k, r, answer;
int N=299,M=399; 
double d = 0.0;
double A[MAX][MAX];
double AT[MAX][MAX];
double B[MAX];
double G[MAX][MAX];
double BT[MAX];
double Unknowns[MAX];
double U[MAX][MAX];
int PIVOT[MAX];

float XF, YF, ZF;

double X[MAX], Y[MAX], Z[MAX];
int ImX[MAX], ImY[MAX];
double Pixels_per_mm; 
double Hx, Hy, Hz,
       Vx, Vy, Vz,
       Ax, Ay, Az, 
       Ch, Cv, Ca, 
       Cx, Cy, Cz;
double magA; 
double T[4][5]; /* Perspective Transform Matrix NxM is really 3x4 */

FILE *fp1, *fp2, *fp3;

 
main ( argc, argv )
  int argc;
  char *argv[];
{ 
   printf (" -------------------- CALIB_BOTH.C --------------------------- \n");
   printf ("starting matrix normalization and Gaussian elimination program \n");
   printf (" -------------------- CALIB_BOTH.C --------------------------- \n");
        if ((fp1 = fopen( "CALIBPTS","r")) == NULL )
    		printf("cant open file: CALIBPTS \n" );
        if ((fp2 = fopen( "M","r")) == NULL )
    		printf("cant open file: M \n" );
	Pixels_per_mm = (double)(image_height_pixels)/(double)image_vert_mm; 
	printf ("The Pixels_per_mm is: %60.55f\n", Pixels_per_mm);
   	
	read_in_M();
	read_in_data_pts();
	
  	if (argc > 1) 
  	   for ( j=1; j < argc; j++ ) 
    	       printf ( "arv is %s\n", argv[j] );
    	       
	fill_AB_for_XandY();
#ifdef DEBUG	
	for (i=1;i<=M;i++)
           printf ("B[%d]: %60.55f\n", i, B[i]);  
	for (i=1;i<=M;i++)
	   for (j=1;j<= N;j++)
  	       printf ("A[%d][%d]: %60.55f\n", i,j, A[i][j]);  
#endif
	normalize();
	gaussian();
	Hx = Unknowns[1];
	Hy = Unknowns[2];
	Hz = Unknowns[3];
	Ay = Unknowns[4];
	Az = Unknowns[5];
	Ch = Unknowns[6];
	Ca = Unknowns[7];
	Vx = Unknowns[8];
	Vy = Unknowns[9];
	Vz = Unknowns[10];
	Cv = Unknowns[11];
  	printf ("Hx: %60.55f\n", Hx);  
  	printf ("Hy: %60.55f\n", Hy);  
  	printf ("Hz: %60.55f\n", Hz);  
	printf ("Vx: %60.55f\n", Vx);  
  	printf ("Vy: %60.55f\n", Vy);  
  	printf ("Vz: %60.55f\n", Vz);  
  	printf ("Ch: %60.55f\n", Ch);  
  	printf ("Cv: %60.55f\n", Cv);  
  	printf ("Ay: %60.55f\n", Ay);  
  	printf ("Az: %60.55f\n", Az);  
  	printf ("Ca: %60.55f\n", Ca);  
	Ax = 1.0;	
	magA = Ax*Ax + Ay*Ay + Az*Az;
  	printf ("MagA: %60.55f\n", magA);  
	magA = sqrt(magA);	
  	printf ("MagA: %60.55f\n", magA);  
	Hx = Hx / magA;
	Hy = Hy / magA;
	Hz = Hz / magA;
	Vx = Vx / magA;
	Vy = Vy / magA;
	Vz = Vz / magA;
	Ch = Ch / magA;
	Cv = Cv / magA;
	Ay = Ay / magA;
	Az = Az / magA;
	Ca = Ca / magA;
  	printf ("scaled by magA\n");  
  	printf ("Hx: %60.55f\n", Hx);  
  	printf ("Hy: %60.55f\n", Hy);  
  	printf ("Hz: %60.55f\n", Hz);  
	printf ("Vx: %60.55f\n", Vx);  
  	printf ("Vy: %60.55f\n", Vy);  
  	printf ("Vz: %60.55f\n", Vz);  
  	printf ("Ch: %60.55f\n", Ch);  
  	printf ("Cv: %60.55f\n", Cv);  
	Ax = Ax/magA;	
  	printf ("Ax: %60.55f\n", Ax);  
  	printf ("Ay: %60.55f\n", Ay);  
  	printf ("Az: %60.55f\n", Az);  
  	printf ("Ca: %60.55f\n", Ca);  
	printf ("unity = %60.55f\n", Ax*Ax + Az*Az + Ay*Ay);  
	yong_check();
	rww_check();
	compute_T_Matrix();
	compute_C_vector();
	dump_camera_data();
#ifdef DEBUG	
#endif	
  	if ( fp1 != NULL )
    	    if (r = fclose( fp1 ) != 0)
	       printf ("fp1 didnt close properly \n");
  	if ( fp2 != NULL )
    	    if (r = fclose( fp2 ) != 0)
	        printf ("fp2 didnt close properly \n");
  	if ( fp3 != NULL )
    	    if (r = fclose( fp3 ) != 0)
	        printf ("fp3 didnt close properly \n");
} /* end main function */

/* read in N&M */
read_in_M()
{
	N = 11; /* fixed */
	fscanf(fp2, "%4d", &M);
	printf ("The number of data points (M rows) is: ");
	printf("M: %d\n", M);
	printf("N: %d \n", N);
} /* end function */
	
/* read in the data points */
read_in_data_pts()
{
	for (i=1;i<=M;i++)
	{  
	   fscanf(fp1, "%15f", &XF);
  	   fflush( fp1 );
	   fscanf(fp1, "%15f", &YF);
  	   fflush( fp1 );
	   fscanf(fp1, "%15f", &ZF);
  	   fflush( fp1 );
	   fscanf(fp1, "%6d", &ImX[i]);
  	   fflush( fp1 );
	   fscanf(fp1, "%6d", &ImY[i]);
  	   fflush( fp1 );
	   printf ("X %f inches ", XF);
	   printf ("Y: %f inches ", YF);
	   printf ("Z: %f inches \n ", ZF);
  	   X[i] = (double)XF / (double)inches_per_mm;	 
  	   Y[i] = (double)YF / (double)inches_per_mm;	 
  	   Z[i] = (double)ZF / (double)inches_per_mm;	 
	}
	printf ("this is the data\n");	
	for (i=1;i<=M;i++)
	{  
	   printf ("X[%d]: %60.55f mm \n",i, X[i]);
	   printf ("Y[%d]: %60.55f mm \n",i, Y[i]);
	   printf ("Z[%d]: %60.55f mm \n",i, Z[i]);
	   printf ("xm -> ImX[%d]: %d ",i, ImX[i]);
	   printf ("ym -> ImY[%d]: %d\n",i, ImY[i]);
	}
} /* end function */

fill_AB_for_XandY()
{
	double Im, Jm;
	int sub=1;
	
	for (i=1;i<=M;i++)
	{  
	   Im = (double)(ImX[i] - Io) / Pixels_per_mm;
	   printf ("ImX[%d]: %d Io: %d \n", i, ImX[i], Io); 
	   printf ("Im: %60.55f \n", Im); 
	   B[sub] = Im * X[i];
	   
	   A[sub][1] = X[i];  /* Hx */
  	   A[sub][2] = Y[i];  /* Hy */ 
  	   A[sub][3] = Z[i];  /* Hz */ 
	   A[sub][4] = -(Im * Y[i]);  /* Ay */
	   A[sub][5] = -(Im * Z[i]);  /* Az */
	   A[sub][6] =  -1;          /* Ch */
	   A[sub][7] = Im;           /* Ca */
	   A[sub][8] = 0;        
	   A[sub][9] = 0;        
	   A[sub][10] = 0;        
	   A[sub][11] = 0;        
	   
	   sub++;
	   Jm = (double)(ImY[i] - Jo) / Pixels_per_mm; /* diff */
	   printf ("ImY[%d]: %d Jo: %d \n", i, ImY[i], Jo); 
	   printf ("Jm: %60.55f \n", Jm); 
	   B[sub] = Jm * X[i]; 		/* diff */
 
	   A[sub][1] = 0;        
	   A[sub][2] = 0;        
	   A[sub][3] = 0;        
	   A[sub][4] = -(Jm * Y[i]);  	/* Ay */
	   A[sub][5] = -(Jm * Z[i]);    /* Az */
	   A[sub][6] =  0;  	       /* Ch */
	   A[sub][7] = Jm;     	       /* Ca */
	   A[sub][8] = X[i];     /* Vx */
  	   A[sub][9] = Y[i];     /* Vy */
  	   A[sub][10] = Z[i];    /* Vz */
	   A[sub][11] =  -1;     /* Cv */
	   
	   sub++;
	}
        M = M * 2; /* to set the number of rows to twice M */
} /* end function */

compute_T_Matrix()
{
	int i,j;

	T[1][1] = Hx;  /*  */
	T[1][2] = Hy;
	T[2][3] = Hz; 
	T[1][4] = Ch; 	/*  */
  	   
	T[2][1] = Vx;     
	T[2][2] = Vy;     
	T[2][3] = Vz;     
	T[2][4] = Cv; 	/*  */
	
	T[3][1] = Ax;  
  	T[3][2] = Ay;  
  	T[3][3] = Az;  
	T[3][4] = Ca; 	/*  */
  	printf (" ----------------------------------------------------- \n");  
  	printf ("PERSPECTIVE TRANSFORM MATRIX T[i][j] \n");  
	for (i=1;i<=3;i++)
  	{  printf ("T[%d]: \n", i);  
	   for (j=1;j<= 4;j++)
  	       printf ("T[%d][%d]: %60.55f\n", i,j, T[i][j]);  
	}
  	printf (" ----------------------------------------------------- \n");  
} /* end function */

compute_C_vector()
{
	BT[1] = Ca; 	/*  */
	BT[2] = Ch; 	/*  */
	BT[3] = Cv; 	/*  */
  	   
	G[1][1] = Ax;  
  	G[1][2] = Ay;  
  	G[1][3] = Az;  
	G[2][1] = Hx;  /*  */
	G[2][2] = Hy;
	G[2][3] = Hz; 
	G[3][1] = Vx;     
	G[3][2] = Vy;     
	G[3][3] = Vz;     
	N= 3;
	M = 3;
	gaussian();
	Cx = Unknowns[1];
	Cy = Unknowns[2];
	Cz = Unknowns[3];
	printf ("-----------------------------------------------------------\n"); 
	printf ("Cx millimeters: %60.55f \n", Cx); 
	printf ("Cy millimeters: %60.55f \n", Cy); 
	printf ("Cz millimeters: %60.55f \n", Cz); 
	Cx = Cx * 0.03937; 
	Cy = Cy * 0.03937; 
	Cz = Cz * 0.03937; 
	printf ("Cx inches: %60.55f \n", Cx); 
	printf ("Cy inches: %60.55f \n", Cy); 
	printf ("Cz inches: %60.55f \n", Cz); 
	printf ("-----------------------------------------------------------\n"); 
} /* end function */

rww_check()
{
	double Im, Jm, num, denom, answer, diff, total_diff=0.0;

	for (i=1;i<=M/2;i++)
	{  
		Im = (double)(ImX[i] - Io) / Pixels_per_mm;
		printf ("ImX[%d]: %d Io: %d \n", i, ImX[i], Io); 
		printf ("Im: %60.55f \n", Im); 
   	/* test =  Cx*Hx + Cy*Hy + Cz*Hz; */
	   	num =   X[i]*Hx + Y[i]*Hy + Z[i]*Hz - Ch;
	   	denom = X[i]*Ax + Y[i]*Ay + Z[i]*Az - Ca;
	  	answer = num / denom; 
		printf ("RWW answer[%d]: %60.55f\n", i, answer);	 
		diff = fabs(Im - answer);	 
		total_diff =  total_diff + diff;	 
		printf ("DIFFERENCE[%d]: %60.55f\n", i, diff );	 
		
		Jm = (double)(ImY[i] - Jo) / Pixels_per_mm;
		printf ("ImY[%d]: %d Jo: %d \n", i, ImY[i], Jo); 
		printf ("Jm: %60.55f \n", Jm); 
   	/* test =  Cx*Vx + Cy*Vy + Cz*Vz; */
	   	num =   X[i]*Vx + Y[i]*Vy + Z[i]*Vz - Cv;
	   	denom = X[i]*Ax + Y[i]*Ay + Z[i]*Az - Ca;
	  	answer = num / denom; 
		printf ("RWW answer[%d]: %60.55f\n", i, answer);	 
		diff = fabs(Jm - answer);	 
		total_diff =  total_diff + diff;	 
		printf ("DIFFERENCE[%d]: %60.55f\n", i, diff );	 
	}
	printf ("\n TOTAL Difference: %60.55f\n", total_diff );	 
	printf ("AVERAGE Difference: %60.55f\n", total_diff/M );	 
} /* end function */

yong_check()
{
	double Im, Jm, answer;
	
	for (i=1;i<=M/2;i++)
	{  
		Im = (double)(ImX[i] - Io) / Pixels_per_mm;
		printf ("ImX[%d]: %d Io: %d \n", i, ImX[i], Io); 
		printf ("Im: %60.55f \n", Im); 
	   answer =  -(Im*X[i]*Ax) + 
 X[i]*Hx + Y[i]*Hy + Z[i]*Hz  - (Im*Y[i]*Ay) - (Im*Z[i]*Az) -Ch + Im*Ca;
	   
		printf ("---> YONG XXX answer[%d]: %60.55f\n", i, answer);	 
	
		Jm = (double)(ImY[i] - Jo) / Pixels_per_mm;
		printf ("ImY[%d]: %d Jo: %d \n", i, ImY[i], Jo); 
		printf ("Jm: %60.55f \n", Jm); 
   		answer =  -(Jm*X[i]*Ax) + 
 X[i]*Vx + Y[i]*Vy + Z[i]*Vz  - (Jm*Y[i]*Ay) - (Jm*Z[i]*Az) -Cv + Jm*Ca;
	   
		printf ("---> YONG YYY answer[%d]: %60.55f\n", i, answer);	 
	}
} /* end function */

normalize()
{
/* transpose A[] */
	for (i=1;i<=M;i++)
	   for (j=1;j<= N;j++)
  	       AT[j][i] = A[i][j];  
	
/* create Matrix G[] */
	for (k=1;k<=N;k++)
	  for (i=1;i<=N;i++)
	  {
	     d = 0.0;
	     for (j=1;j<= M;j++)
  	     {
		d = d + AT[k][j] * A[j][i];  
	     }
	     G[k][i] = d;	
#ifdef DEBUG	
	     printf ("G[%d][%d]: %60.55f\n", k,i, G[k][i]);
#endif	
	   }	
/* create Matrix BT[] */
	  for (k=1;k<=N;k++)
	  {
	     d = 0.0;
	     for (j=1;j<= M;j++)
  	     {
		d = d + AT[k][j] * B[j];  
	     }	
	     BT[k] = d;	
#ifdef DEBUG	
	     printf ("BT[%d]: %60.55f\n", k, BT[k]);
#endif	
	   }	
} /* end function */

/* --------------------------------------------------------------------- */
/* -                        GAUSSIAN.C                                 - */
/* --------------------------------------------------------------------- */
/* - This program performs the Gaussian Elimination using Pivoting     - */
/* --------------------------------------------------------------------- */

gaussian()
{ 
	double rel = 0.0;
   	printf ("starting GAUSSIAN Elimination matrix program \n");
   	
        init_arrays();  
	printf("\n N: %d M: %d\n", N, M);
	
	factor(N);
	solve_it(N);
	resid(N, &rel);
	printf ("The relative residual error is: %60.55f\n", rel);
#ifdef ROGER
	printf ("The U[][] matrix is \n");
	for (i=1;i<=N;i++)
	   for (j=1;j<= N;j++)
  	       printf ("U[%d][%d] = %60.55f\n",  i,j, U[i][j]);  
	printf ("****************************************  \n");
	printf ("The ******** Unknown matrix ******** is \n");
	printf ("****************************************  \n");
	for (i=1;i<=N;i++)
  	       printf ("x%d => %60.55f\n",  i, Unknowns[i]);  
#endif
} /* end main function */

/*---------------------- Factor -----------------------*/
factor(N)
 int N;
{
   	int i,j, count, it;	
	double b, am, fm, fact, comp;
	for (i=1;i<=N;i++)
	   for (j=1;j<= N;j++)
  	       U[i][j] = G[i][j];  
	for (it=1;it <= N-1;it++)
 	{
	  fm = 0.0;
	  count = it;
	  for (i=it;i <= N;i++)
	  {
	     am = 0.0;
	     for (j=it;j <= N;j++)
	        am = max(am, fabs(U[i][j]) );
   /* printf ("the max row: %d is: %60.55f\n", i, am);  */
	     comp = fabs( U[i][it]) / am; 
	     if (fm <= comp)
	     { 
	        fm = comp;
	        count = i;
	     } 
	   } /* end for i */
	   PIVOT[it] = count;
	   if (count != it)
	   { 
	      	for (j=it;j<= N;j++)
  	        {
		   b = U[it][j];  
		   U[it][j] = U[count][j];  
		   U[count][j] = b;  
	  	}  
   	   } /* end if */ 
	  for (i=it+1;i <= N;i++)
	  {
	     fact = -U[i][it] / U[it][it];
	     U[i][it] = fact;
	     for (j=it+1;j<= N;j++)
		U[i][j] = U[i][j] + fact * U[it][j];  
	  }

        } /* end for it */ 
} /* end function */


resid (N, rel)
 int N; double *rel;
{
  	int i, j, k;
  	double sum=0.0, norm1=0.0, normb=0.0;
	
	for (i=1;i<=N;i++)
	{
  	   sum = -BT[i];  
	   for (j=1;j <= N;j++)
  	        sum = sum + G[i][j] * Unknowns[j];  
	   norm1 = max(norm1, fabs(sum) );
	}
	normb = 0.0;
	for (i=1;i <= N;i++)
	{
	   normb = max(normb, fabs(BT[i]) );
	}
	*rel = norm1 / normb;
#ifdef DEBUG	
	printf ("norm1: %60.55f normb: %60.55f \n", norm1, normb);
#endif	
} /* end function */

solve_it(N)
 int N;
{
  	int i, j, k;
  	double sw, sum;
	for (i=1;i<=N;i++)
  	   Unknowns[i] = BT[i];  
	for (i=1;i <= N-1;i++)
	{
	   if (PIVOT[i] != i)
	   {
		sw = Unknowns[i];
  	        Unknowns[i] = Unknowns[ PIVOT[i] ];  
  	        Unknowns[ PIVOT[i] ] = sw;  
	   } /* end if */ 

	   for (j=i+1;j <= N;j++)
  	        Unknowns[j] = Unknowns[j] + U[j][i] * Unknowns[i];  
	} /* end for i */
 /* backsolve Ux = b */
	for (i=N;i >= 1;i--)
	{
	   sum = 0.0;
	   for (k=i+1;k <= N;k++)
  	        sum = sum + U[i][k] * Unknowns[k];  
  	   Unknowns[i] = (Unknowns[i] - sum) /  U[i][i];  
	}

} /* end function */
#ifdef ROGER
#endif

init_arrays()
{
   	int i,j;	
	for (i=1;i<MAX;i++)
	{
	   Unknowns[i]= 0.0; 
	   PIVOT[i] = 0;
	   for (j=1;j< MAX;j++)
		U[i][j] = 0.0;  
	 }
} /* end function */

  	     
dump_camera_data()
{
        char s;
	double d;
		
	printf ("which camera Left or Right type r or l \n"); 
	scanf("%c", &s);
	printf ("s = %c\n", s);	
	if (s == 'l')
	    if ((fp3 = fopen( "Camera.Left","w+")) == NULL )
    		printf("cant open file: Camera.Left \n" );
	if (s == 'r')
	    if ((fp3 = fopen( "Camera.Right","w+")) == NULL )
    		printf("cant open file: Camera.Left \n" );
	
	d = Ca; 	/*  */
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	d = Ch; 	/*  */
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	d = Cv; 	/*  */
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
  	   
	d = Ax;  
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
  	d = Ay;  
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
  	d = Az;  
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	d = Hx;  /*  */
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	d = Hy;
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	d = Hz; 
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	d = Vx;     
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	d = Vy;     
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	d = Vz;     
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	Cx = Cx / 0.03937; 
	Cy = Cy / 0.03937; 
	Cz = Cz / 0.03937; 
	d = Cx; /* in millimeters was in inches */
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	d = Cy;
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
	d = Cz;
	fprintf (fp3, "%60.55f \n", d );
  	fflush( fp3 );
} /* end function */