/* --------------------------------------------------------------------- */
/* -                   P.C                                             - */
/* --------------------------------------------------------------------- */
/* -    Some necessary includes...                                     - */
/* --------------------------------------------------------------------- */
/* written by: Roger W. Webster, Ph.D.
/* --------------------------------------------------------------------- */
#include "calibration.h"
/* #define DEBUG 1 */

/* --------------------------------------------------------------------- */
/* -    Global variables and structures...                             - */
/* --------------------------------------------------------------------- */
int i,j, k, r, answer;
int N=3,M=3; 

double RWWDxR,	RWWDyR, RWWDzR;		
double RWWPx,	RWWPy, RWWPz;		
double G[MAX][MAX];
double BT[MAX];
double Unknowns[MAX];
double U[MAX][MAX];
int PIVOT[MAX];
double ImL, JmL, ImR, JmR;  /* image points of object in mm */

double Pixels_per_mm; 
double HxL, HyL, HzL,
       VxL, VyL, VzL,
       AxL, AyL, AzL, 
       ChL, CvL, CaL, 
       CxL, CyL, CzL;
double HxR, HyR, HzR,
       VxR, VyR, VzR,
       AxR, AyR, AzR, 
       ChR, CvR, CaR, 
       CxR, CyR, CzR;
double DxL, DyL, DzL,  /* left direction vector */
       DxR, DyR, DzR;  /* right direction vector */
double Px, Py, Pz;	 /* this is where the object really is in 3D */

FILE *fp1, *fp2;

main ( argc, argv )
  int argc;
  char *argv[];
{ 
   printf (" -------------------- P.C --------------------------- \n");
   printf ("starting to find P[px, py, pz] Matrix  \n");
   printf (" -------------------- P.C --------------------------- \n");
	if (argc > 1) 
  	   for ( j=1; j < argc; j++ ) 
    	       printf ( "arv is %s\n", argv[j] );
       
	 if ((fp1 = fopen( "Camera.Left","r")) == NULL )
    		printf("cant open file: Camera.Left \n" );
        if ((fp2 = fopen( "Camera.Right","r")) == NULL )
    		printf("cant open file: Camera.Right \n" );
	 
	
	Pixels_per_mm = (double)(image_height_pixels)/(double)image_vert_mm; 
   	
	read_camera_data_LEFT();
	read_camera_data_RIGHT();
	get_image_points(&ImL, &JmL, &ImR, &JmR);
   	printf("ImL: %60.55f\n", ImL);
   	printf("JmL: %60.55f\n", JmL);
   	printf("ImR: %60.55f\n", ImR);
   	printf("JmR: %60.55f\n", JmR);

	compute_point_P(&Px, &Py, &Pz);	
   	printf("Px mm: %60.55f\n", Px);
   	printf("Py mm: %60.55f\n", Py);
   	printf("Pz mm: %60.55f\n", Pz);
   	printf("Px INCHES: %60.55f\n", Px * 0.03937);
   	printf("Py INCHES: %60.55f\n", Py * 0.03937);
   	printf("Pz INCHES: %60.55f\n", Pz * 0.03937);
  	
    	       
  	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_camera_data_RIGHT()
{
        char s;
	double d=0.0;
		
   printf (" --------------- RIGHT Camera Data ------------------- \n");
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	CaR = d; 	/*  */
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	ChR = d; 	/*  */
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	CvR = d; 	/*  */
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
  	   
	AxR = d;  
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
  	AyR = d;  
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
  	AzR = d;  
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	HxR = d;  /*  */
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	HyR = d;
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	HzR = d; 
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	VxR = d;     
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	VyR = d;     
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	VzR = d;     
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	CxR = d; /* in millimeters was in inches */
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	CyR = d;
	printf ("fscanf result %d\n", fscanf (fp2, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp2 );
	CzR = d;
   printf (" --------------- RIGHT Camera Data ------------------- \n");
	printf ("HxR: %60.55f\n", HxR);  
  	printf ("HyR: %60.55f\n", HyR);  
  	printf ("HzR: %60.55f\n", HzR);  
	printf ("VxR: %60.55f\n", VxR);  
  	printf ("VyR: %60.55f\n", VyR);  
  	printf ("VzR: %60.55f\n", VzR);  
  	printf ("ChR: %60.55f\n", ChR);  
  	printf ("CvR: %60.55f\n", CvR);  
  	printf ("AxR: %60.55f\n", AxR);  
  	printf ("AyR: %60.55f\n", AyR);  
  	printf ("AzR: %60.55f\n", AzR);  
  	printf ("CaR: %60.55f\n", CaR);  
  	printf ("CxR: %60.55f\n", CxR);  
  	printf ("CyR: %60.55f\n", CyR);  
  	printf ("CzR: %60.55f\n", CzR);  
  	printf ("CxR INCHES: %60.55f\n", CxR * 0.03937);  
  	printf ("CyR INCHES: %60.55f\n", CyR * 0.03937);  
  	printf ("CzR INCHES: %60.55f\n", CzR * 0.03937);  
   printf (" ---------------------------------------------------- \n");
} /* end function */


read_camera_data_LEFT()
{
        char s;
	double d=0.0;
		
   printf (" --------------- LEFT Camera Data ------------------- \n");
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	CaL = d; 	/*  */
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	ChL = d; 	/*  */
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	CvL = d; 	/*  */
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
  	   
	AxL = d;  
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
  	AyL = d;  
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
  	AzL = d;  
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	HxL = d;  /*  */
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	HyL = d;
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	HzL = d; 
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	VxL = d;     
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	VyL = d;     
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	VzL = d;     
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	CxL = d; /* in millimeters was in inches */
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	CyL = d;
	printf ("fscanf result %d\n", fscanf (fp1, "%lf", &d) ); /* RWW */
	printf ("%60.55f\n", d );
  	fflush( fp1 );
	CzL = d;
   printf (" --------------- LEFT Camera Data ------------------- \n");
	printf ("HxL: %60.55f\n", HxL);  
  	printf ("HyL: %60.55f\n", HyL);  
  	printf ("HzL: %60.55f\n", HzL);  
	printf ("VxL: %60.55f\n", VxL);  
  	printf ("VyL: %60.55f\n", VyL);  
  	printf ("VzL: %60.55f\n", VzL);  
  	printf ("ChL: %60.55f\n", ChL);  
  	printf ("CvL: %60.55f\n", CvL);  
  	printf ("AxL: %60.55f\n", AxL);  
  	printf ("AyL: %60.55f\n", AyL);  
  	printf ("AzL: %60.55f\n", AzL);  
  	printf ("CaL: %60.55f\n", CaL);  
  	printf ("CxL: %60.55f\n", CxL);  
  	printf ("CyL: %60.55f\n", CyL);  
  	printf ("CzL: %60.55f\n", CzL);  
  	printf ("CxL INCHES: %60.55f\n", CxL * 0.03937);  
  	printf ("CyL INCHES: %60.55f\n", CyL * 0.03937);  
  	printf ("CzL INCHES: %60.55f\n", CzL * 0.03937);  
   printf (" ---------------------------------------------------- \n");
} /* end function */


compute_point_P(Px, Py, Pz)	
double *Px, *Py, *Pz;
{
	double numer, denom, r, jr;
	double Cdiff_X, Cdiff_Y, Cdiff_Z;	
	double Delta_VR, Delta_AR;
	
	compute_DL_vector();	 /* get DL matrix */
	Cdiff_X = CxR - CxL;	
	Cdiff_Y = CyR - CyL;	
	Cdiff_Z = CzR - CzL;	
  	printf ("C Diff X inches (BASELINE): %60.55f\n", Cdiff_X * 0.03937);  
  	printf ("C DIFF Y inches: %60.55f\n", Cdiff_Y * 0.03937);  
  	printf ("C DIFF Z inches: %60.55f\n", Cdiff_Z * 0.03937);  

	numer =  (ImR*Cdiff_X*AxR) + (ImR*Cdiff_Y*AyR) + (ImR*Cdiff_Z* AzR)
		- (Cdiff_X * HxR) - (Cdiff_Y * HyR) - (Cdiff_Z * HzR);

	denom =  (DxL * HxR) + (DyL * HyR) + (DzL * HzR)
 		-(ImR * DxL * AxR) - (ImR * DyL * AyR) - (ImR * DzL * AzR);

	r = numer / denom;
  	printf ("r: %60.55f\n", r);  
	
	numer =  (JmR*Cdiff_X*AxR) + (JmR*Cdiff_Y*AyR) + (JmR*Cdiff_Z* AzR)
		- (Cdiff_X * VxR) - (Cdiff_Y * VyR) - (Cdiff_Z * VzR);

	denom =  (DxL * VxR) + (DyL * VyR) + (DzL * VzR)
 		-(JmR * DxL * AxR) - (JmR * DyL * AyR) - (JmR * DzL * AzR);

	jr = numer / denom;
  	printf ("jr: %60.55f\n", jr);  

/* RWW TRY
	DxR = (-r * DxL) + (CxR - CxL);		
	DyR = (-r * DyL) + (CyR - CyL);		
	DzR = (-r * DzL) + (CzR - CzL);		
*/
	DxR = (-r * DxL) + (CxL - CxR);		
	DyR = (-r * DyL) + (CyL - CyR);		
	DzR = (-r * DzL) + (CzL - CzR);		
	
	RWWDxR = (-jr * DxL) + (CxL - CxR);		
	RWWDyR = (-jr * DyL) + (CyL - CyR);		
	RWWDzR = (-jr * DzL) + (CzL - CzR);		
	RWWPx = RWWDxR + CxR;
	RWWPy = RWWDyR + CyR;
	RWWPz = RWWDzR + CzR;
   	printf("JrPx INCHES: %60.55f\n", RWWPx * 0.03937);
   	printf("JrPy INCHES: %60.55f\n", RWWPy * 0.03937);
   	printf("JrPz INCHES: %60.55f\n", RWWPz * 0.03937);
	
	*Px = DxR + CxR;
	*Py = DyR + CyR;
	*Pz = DzR + CzR;
/* RWW TRY
	*Px = DxR + CxL;
	*Py = DyR + CyL;
	*Pz = DzR + CzL;
*/
  	Delta_VR = Cdiff_X * VxR + Cdiff_Y * VyR + Cdiff_Z * VzR;  
  	Delta_AR = Cdiff_X * AxR + Cdiff_Y * AyR + Cdiff_Z * AzR;  
  	printf ("Delta . VR: %60.55f\n", Delta_VR );  
  	printf ("Delta . AR: %60.55f\n", Delta_AR );  

} /* end function */

get_image_points(ImL, JmL, ImR, JmR)
double *ImL, *JmL, *ImR, *JmR;
{
   	/* int IL=165, JL=337, IR=132, JR=328; rww works !!! */
   	/* int IL=377, JL=128, IR=354, JR=121; rww doesn't work */
   	int IL=586, JL=424, IR=530, JR=414; 

	printf("what is LEFT Image point X:");
   	scanf("%d", &IL);
   	printf("what is LEFT Image point Y:");
   	scanf("%d", &JL);
   	
   	printf("what is RIGHT Image point X:");
   	scanf("%d", &IR);
   	printf("what is RIGHT Image point Y:");
   	scanf("%d", &JR);

	printf ("IL: %d JL: %d IR: %d Jr: %d\n", IL, JL, IR, JR);	
	*ImL = (double)(IL - Io) / Pixels_per_mm;
	*JmL = (double)(JL - Jo) / Pixels_per_mm;
	*ImR = (double)(IR - Io) / Pixels_per_mm;
	*JmR = (double)(JR - Jo) / Pixels_per_mm;

} /* end function */

compute_DL_vector()
{
	G[1][1] = AxL;  G[1][2] = AyL;  G[1][3] = AzL;  BT[1] = 1; 	/*  */
	G[2][1] = VxL;  G[2][2] = VyL;  G[2][3] = VzL;  BT[2] = JmL; 	/*  */
	G[3][1] = HxL;  G[3][2] = HyL;  G[3][3] = HzL;  BT[3] = ImL; 	/*  */
	N= 3;
	M = 3;
	gaussian();
	DxL = Unknowns[1];
	DyL = Unknowns[2];
	DzL = Unknowns[3];
	printf ("-----------------------------------------------------------\n"); 
	printf ("DL is a unit vector\n"); 
	printf ("DxL millimeters: %60.55f \n", DxL); 
	printf ("DyL millimeters: %60.55f \n", DyL); 
	printf ("DzL millimeters: %60.55f \n", DzL); 
	printf ("-----------------------------------------------------------\n"); 
} /* 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 DEBUG	
	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 DEBUG	
} /* 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;
} /* 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 */

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

#ifdef ROGER	
compute_T_Matrix()
{
	int i,j;
	double T[4][5]; /* Perspective Transform Matrix NxM is really 3x4 */

	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 */
#endif ROGER