/* --------------------------------------------------------------------- */
/* -                        CALIB.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 
#define PLUG_IN 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];
double DIAG[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 AxY, AyY, AzY, CaY; 

FILE *fp1, *fp2;

 
main ( argc, argv )
  int argc;
  char *argv[];
{ 
   printf ("starting matrix normalization and Gaussian elimination program \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] );
    	       
  	if (argc > 1) 
  	   answer = (*argv[1] == 'x');
  	if (argc == 1) 
  	   answer = 1;
  	printf ("answer is %d\n", answer);
	if (answer)	
	{
	fill_AB_for_X();
#ifdef ROGER	
	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(); 
#ifdef ROGER	
	Householder_method();
#endif
	Hx = Unknowns[1];
	Hy = Unknowns[2];
	Hz = Unknowns[3];
	Ay = Unknowns[4];
	Az = Unknowns[5];
	Ch = Unknowns[6];
	Ca = Unknowns[7];
  	printf ("Hx: %60.55f\n", Hx);  
  	printf ("Hy: %60.55f\n", Hy);  
  	printf ("Hz: %60.55f\n", Hz);  
  	printf ("Ch: %60.55f\n", Ch);  
  	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;
	Ch = Ch / 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 ("Ch: %60.55f\n", Ch);  
  	printf ("Ay: %60.55f\n", Ay);  
  	printf ("Az: %60.55f\n", Az);  
  	printf ("Ca: %60.55f\n", Ca);  

	Ax = Ax/magA;	
  	printf ("Ax: %60.55f\n", Ax);  
  	printf ("unity = %60.55f\n", Ax*Ax + Az*Az + Ay*Ay);  
	yong_checkX();
	rww_checkX();
	}
	
#ifdef DEBUG	
#endif	
  	if (argc > 1) 
  	   answer = (*argv[1] == 'y');
  	if (argc == 1) 
  	   answer = 1;
  	printf ("answer is %d\n", answer);
	if (answer)
	{
 	fill_AB_Y();
	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]);  
#ifdef ROGER	
#endif	
	normalize();
	gaussian(); 
#ifdef ROGER	
	Householder_method();
#endif
	Vx = Unknowns[1];
	Vy = Unknowns[2];
	Vz = Unknowns[3];
	AyY = Unknowns[4];
	AzY = Unknowns[5];
	Cv = Unknowns[6];
	CaY = Unknowns[7];
#ifdef PLUG_IN	
	Cv = Unknowns[4];
#endif
   	printf ("********************************\n");	
	printf ("Vx: %60.55f\n", Vx);  
  	printf ("Vy: %60.55f\n", Vy);  
  	printf ("Vz: %60.55f\n", Vz);  
  	printf ("Cv: %60.55f\n", Cv);  
	printf ("Ay: %60.55f\n", AyY);  
  	printf ("Az: %60.55f\n", AzY);  
  	printf ("Ca: %60.55f\n", CaY);  
#ifdef ROGERIN	
	AxY = 1.0;	
	magA = AxY*AxY + AyY*AyY + AzY*AzY;
  	printf ("MagA: %60.55f\n", magA);  
	magA = sqrt(magA);	
  	printf ("MagA: %60.55f\n", magA);  
	Vx = Vx / magA;
	Vy = Vy / magA;
	Vz = Vz / magA;
	Cv = Cv / magA;
	AxY = AxY/magA;	
	AyY = AyY / magA;
	AzY = AzY / magA;
	CaY = CaY / magA;
  	printf ("scaled by magA YYYYYYYYY data: \n");  
	printf ("Vx: %60.55f\n", Vx);  
  	printf ("Vy: %60.55f\n", Vy);  
  	printf ("Vz: %60.55f\n", Vz);  
  	printf ("Cv: %60.55f\n", Cv);  
  	printf ("Ay: %60.55f\n", AyY);  
  	printf ("Az: %60.55f\n", AzY);  
  	printf ("Ca: %60.55f\n", CaY);  
  	printf ("unity = %60.55f\n", AxY*AxY + AzY*AzY + AyY*AyY);  
  	printf ("Ax: %60.55f\n", AxY);  
#endif
#ifdef PLUG_IN	
	AxY = Ax;	
	AyY = Ay;
	AzY = Az;
	CaY = Ca;
#endif
	yong_checkY();
	rww_checkY();
	compute_C_vectorX();
	compute_C_vectorY();
	}

#ifdef ROGER
#endif
  	printf ("scaled by magA YYYYYYYYY data: \n");  
	printf ("Vx: %60.55f\n", Vx);  
  	printf ("Vy: %60.55f\n", Vy);  
  	printf ("Vz: %60.55f\n", Vz);  
  	printf ("Cv: %60.55f\n", Cv);  
  	printf ("AyY: %60.55f\n", AyY);  
  	printf ("AzY: %60.55f\n", AzY);  
  	printf ("CaY: %60.55f\n", CaY);  
	AxY = AxY/magA;	
  	printf ("unity Y = %60.55f\n", AxY*AxY + AzY*AzY + AyY*AyY);  
  	printf ("AxY: %60.55f\n", AxY);  
  	printf ("scaled by magA XXXXXX data: \n");  
  	printf ("Hx: %60.55f\n", Hx);  
  	printf ("Hy: %60.55f\n", Hy);  
  	printf ("Hz: %60.55f\n", Hz);  
  	printf ("Ch: %60.55f\n", Ch);  
  	printf ("Ay: %60.55f\n", Ay);  
  	printf ("Az: %60.55f\n", Az);  
  	printf ("Ca: %60.55f\n", Ca);  

  	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");
} /* end main function */

/* read in N&M */
read_in_M()
{
	N = 7; /* fixed */
	fscanf(fp2, "%4d", &M);
	printf ("The number of data points (M rows) is: ");
	printf("M: %d\n", M);
	printf("N: %d (should be 7) \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_X()
{
	double Im;

	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[i] = Im * X[i];
  	   
	   A[i][1] = X[i];  
  	   A[i][2] = Y[i];  
  	   A[i][3] = Z[i];  
	   A[i][4] = -(Im * Y[i]);
	   A[i][5] = -(Im * Z[i]);
	   A[i][6] =  -1;          
	   A[i][7] = Im;        
	}
} /* end function */

fill_AB_Y()
{
	double Jm;

	for (i=1;i<=M;i++)
	{  
	   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[i] = Jm * X[i]; 		/* diff */
  	   
	   A[i][1] = X[i];  
  	   A[i][2] = Y[i];  
  	   A[i][3] = Z[i];  
	   A[i][4] = -(Jm * Y[i]);    	/* diff */
	   A[i][5] = -(Jm * Z[i]);
	   A[i][6] =  -1; 
	   A[i][7] = Jm;     
#ifdef PLUG_IN	
	   B[i] = Jm*X[i]*Ax + Jm*Y[i]*Ay + Jm*Z[i]*Az - Jm*Ca;
	   A[i][1] = X[i];  
  	   A[i][2] = Y[i];  
  	   A[i][3] = Z[i];  
	   A[i][4] =  -1;          
	   N = 4;
#endif	
	}
} /* end function */

compute_C_vectorX()
{
	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 ("XXXXXXXXXXXXXXXXXX\n"); 
	printf ("Cx: %60.55f mm\n", Cx); 
	printf ("Cy: %60.55f mm\n", Cy); 
	printf ("Cz: %60.55f mm\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 */

compute_C_vectorY()
{
	BT[1] = CaY; 	/*  */
	BT[2] = Ch; 	/*  */
	BT[3] = Cv; 	/*  */
  	   
	G[1][1] = AxY;  
  	G[1][2] = AyY;  
  	G[1][3] = AzY;  
	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 ("YYYYYYYYYYYYYYYYYY\n"); 
	printf ("Cx: %60.55f mm\n", Cx); 
	printf ("Cy: %60.55f mm\n", Cy); 
	printf ("Cz: %60.55f mm\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_checkX()
{
	double Im, num, denom, answer;

	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); 
   	/* 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);	 
		printf ("DIFFERENCE[%d]: %60.55f\n", i, fabs(Im - answer) );	 
	}
} /* end function */

rww_checkY()
{
	double Jm, num, denom, answer;

	for (i=1;i<=M;i++)
	{  
		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*Hx + Cy*Hy + Cz*Hz; */
   	/* test =  Cx*Vx + Cy*Vy + Cz*Vz; */
	   	num =   X[i]*Vx + Y[i]*Vy + Z[i]*Vz - Cv;
	   	denom = X[i]*AxY + Y[i]*AyY + Z[i]*AzY - CaY;
	  	answer = num / denom; 
		printf ("RWW answer[%d]: %60.55f\n", i, answer);	 
		printf ("DIFFERENCE[%d]: %60.55f\n", i, fabs(Jm - answer) );	 
	}
} /* end function */

yong_checkX()
{
	double Im, answer;

	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); 
	   	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 answer[%d]: %60.55f\n", i, answer);	 
	}
} /* end function */

yong_checkY()
{
	double Jm, answer;

	for (i=1;i<=M;i++)
	{  
		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]*AxY) + 
 X[i]*Vx + Y[i]*Vy + Z[i]*Vz  - (Jm*Y[i]*AyY) - (Jm*Z[i]*AzY) -Cv + Jm*CaY;
	   
		printf ("---> YONG 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];  
	   }	
#ifdef ROGER	
	for (i=1;i<=N;i++)
	   for (j=1;j<= M;j++)
	       printf ("AT[%d][%d]: %60.55f\n", i,j, AT[i][j]);
#endif	
/* 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 ROGER	
	     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 ROGER	
	     printf ("BT[%d]: %60.55f\n", k, BT[k]);
#endif	
	   }	
} /* end function */

/* --------------------------------------------------------------------- */
/* -                        GAUSSIAN.C                                 - */
/* --------------------------------------------------------------------- */
/* - This function 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 */

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 */

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 */
/* --------------------------------------------------------------------- */
/* - This function performs the Householder Method of matrix            - */
/* --------------------------------------------------------------------- */
Householder_method()
{ 
   	int i,j, k,HM;	
	double sum = 0.0, S,P,T=0.0;

	printf ("starting Householder Method \n");	
	init_arrays();
	for (i=1;i<=M;i++)
	   for (j=1;j<= N;j++)
  	   { 
	       G[i][j] = A[i][j];  
	   }	
	   for (j=1;j<= M;j++)
  	       BT[j] = B[j];  
	
	HM = M;	 /* for this program only */
	for (k=1;k <= N;k++)
	{
	     for (i=k;i <= HM;i++)
	     {
	        sum = sum + G[i][k] * G[i][k];
	     }
	   
#ifdef ROGER	
	     	S = sqrt(sum);
#endif	
	     if (G[k][k] > 0.0)
	     	S = sqrt(sum);
	     else
	     	S = -sqrt(sum);

	     DIAG[k] = -S;
	     G[k][k] = G[k][k] + S;
	     P = G[k][k] * S;

	printf ("Householder k: %d \n",k);	
	     for (j=k+1;j <= N;j++)
	     {
	        for (i=k;i <= HM;i++)
	        {
	           T = T + G[i][k] * G[i][j];
	        }
	        T = T / P;
	        for (i=k;i <= HM;i++)
	        {
	           G[i][j] = G[i][j] - T * G[i][k];
	        }

	     } /* end for j */
	        
	     T = 0.0;
	     for (i=k;i <= HM;i++)
	     {
	         T = T + G[i][k] * BT[i];
	     }
	     T = T / P;
	     for (i=k;i <= HM;i++)
	     {
	         BT[i] = BT[i] - T * G[i][k];
	     }
	} /* end for k */
 /* BACK SUBSTITUTION   */
	for (i=N;i >= 1;i--)
	{
	printf ("Householder Back sub i: %d \n",i);	
	     sum = 0.0;
	     for (j=i+1;j <= N;j++)
	     {
	        sum = sum + G[i][j] * Unknowns[j];
	     }
	     Unknowns[i] = (BT[i]-sum) / DIAG[i];  
	} /* end for i */
	
} /* end function */

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