#include <stdio.h>
#include <math.h>


/*
**	This routine converts a K2T GRF-2 image to Cartesian.
**	It utilizes some routines found in ``Numerical Recipes in C''.
**	Please note that they require users of this code to have
**	purchased at least one copy of their book.
*/

ConvertK2TRangeToCartesian(RangeImage,P,CalibrationFilename,
	Origin,LightSource,Calibration)

unsigned char	*RangeImage;
double		*P[3];
char		*CalibrationFilename;
double		Origin[3];	/* where is focal point of CCD? */
double		LightSource[3];	/* where is location of light projector? */
double		Calibration[];	/* calib info for XYZ-RCD transform */

{
int		r,c,IMAGE_ROWS=480,IMAGE_COLS=640,gaussj(),i;
double		**a,**b,**dmatrix(),range;
float		cal[22];
double		valX,valY,valZ,val4,val5,val6,val7,val8,val9,det;
FILE		*fpt;
char		text[300];

if ((fpt=fopen(CalibrationFilename,"r")) == NULL)
  {
  if (strcmp(&(CalibrationFilename[strlen(CalibrationFilename)-10]),
	".range.rpa") == 0)
    {
    strcpy(text,CalibrationFilename);
    text[strlen(text)-10]='\0';
    strcat(text,".rpa");
    if ((fpt=fopen(text,"r")) == NULL)
      {
      printf("Unable to open calibration file %s\n",text);
      exit(0);
      }
    }
  else
    printf("Unable to open calibration file %s\n",CalibrationFilename);
  }
for (i=0; i<22; i++)
  if (fscanf(fpt,"%f",&cal[i]) != 1)
    {
    printf("Bad calibration file: %s\n",CalibrationFilename);
    exit(0);
    }
  else
    Calibration[i]=cal[i];
		/* The light source point location can be there, or not */
if (fscanf(fpt,"%lf %lf %lf",&LightSource[0],&LightSource[1],
	&LightSource[2]) != 3)
  LightSource[0]=LightSource[1]=LightSource[2]=0.0;
fclose(fpt);

a=dmatrix(1,4,1,4);
b=dmatrix(1,4,1,1);
for (r=0; r<IMAGE_ROWS; r++)
  {
  for (c=0; c<IMAGE_COLS; c++)
    {
    if (RangeImage[r*IMAGE_COLS+c] == 0)
      range=255.0;
    else
      range=(((double)RangeImage[r*IMAGE_COLS+c])*
		(cal[21]-cal[20])/254.0)+cal[20];
    for (i=0; i<4; i++)
      {
      a[1][i+1]=(double)cal[i];
      a[2][i+1]=(double)cal[4+i];
      a[3][i+1]=(double)cal[8+i];
      a[4][i+1]=(double)0.0;
      }
    a[4][4]=(double)1.0;
    b[1][1]=(double)range*(double)c;
    b[2][1]=(double)range*(double)r;
    b[3][1]=(double)range;
    b[4][1]=(double)1.0;
    if (gaussj(a,4,b,1) == 1)
      for (i=0; i<3; i++)
        P[i][r*IMAGE_COLS+c]=b[i+1][1];
    else
      for (i=0; i<3; i++)
        P[i][r*IMAGE_COLS+c]=-1.0;
    }
  }
free_dmatrix(a,1,4,1,4);
free_dmatrix(b,1,4,1,1);
	/* from Apurva -- finds origin */
valX = (cal[5] * cal[10] - cal[6] * cal[9]);
valY = (cal[6] * cal[8] - cal[4] * cal[10]);
valZ = (cal[4] * cal[9] - cal[5] * cal[8]);
val4 = (cal[2] * cal[9] - cal[1] * cal[10]);
val5 = (cal[0] * cal[10] - cal[2] * cal[8]);
val6 = (cal[1] * cal[8] - cal[0] * cal[9]);
val7 = (cal[1] * cal[6] - cal[5] * cal[2]);
val8 = (cal[4] * cal[2] - cal[0] * cal[6]);
val9 = (cal[0] * cal[5] - cal[4] * cal[1]);
det=valX*cal[0] +  valY*cal[1] + valZ*cal[2];
Origin[0] = -(valX*cal[3]+ val4*cal[7] + val7*cal[11])/det;
Origin[1] = -(valY*cal[3]+ val5*cal[7] + val8*cal[11])/det;
Origin[2] = -(valZ*cal[3]+ val6*cal[7] + val9*cal[11])/det;
}



/*
**	This code is taken from ``Numerical Recipes in C'', 2nd
**	and 3rd editions, by Press, Teukolsky, Vetterling and
**	Flannery, Cambridge University Press, 1992, 1994.
*/



#include <stdio.h>              /* standard I/O file */
#include <math.h> 	       /* math library */
#define SWAP(a,b) {double temp=(a);(a)=(b);(b)=temp;}

/**************************************************************************/
/* assigns power(0,x) zero, power(x,0) 1, log(x<1) zero,and sqrt(0) a zero*/
/* without raising exception error messanges                              */
/**************************************************************************/
double POW(x,y)
double x,y;
{
  if( x == 0.00)
    return 0.00;
  else if (y == 0.00)
    return 1.00;
  else
    return pow(x,y);
 }

double LOG(x)
double x;
{
  if (x < 0.00 || x == 0.00 )
     return 0.00;
   else 
     return log(x);
}
double SQRT(x)
double x;
{
  if (x < 0.00 || x == 0.00 )
     return 0.00;
   else 
     return sqrt(x);
}


/*****************************************************************************/
/* This is a gauss jordan elimination method for matrices                    */
/* a[1..n][1..n] is an input *matrix of n by n elements. b[1..n][1..m] is an */
/* input *matrix of size n by m containing m right hand side vectors. On     */
/* output, a is replaced by its *matrix inverse, and b is replaced by the    */
/* corresponding set of solution vectors.                                    */
/*****************************************************************************/

int gaussj(a,n,b,m)
double **a,**b;
int n,m;

{
	int *indxc, *indxr,*ipiv;

       /* arrays ipiv[1..n], indxr[1..n], and indxc[1..n] are  */
       /* used for bookeeping on the pivoting                  */

	int i,icol,irow,j,k,l,ll,*ivector();
	double big,dum, pivinv;
	void nrerror(),free_ivector();
	indxc = ivector(1,n);
	indxr=ivector(1,n);
	ipiv=ivector(1,n);

	for (j=1;j<=n;j++)
	    ipiv[j]=0;

	for(i=1;i<=n;i++) /* *main loop for columns to be reduced */
	  { big = 0.0;
	      for (j=1;j<=n;j++) /* outer loop for search of a pivot element*/
		if (ipiv[j] !=1)
		   for(k=1;k<=n;k++)
		     {if (ipiv[k] ==0)
			{if (fabs(a[j][k]) >= big)
			   {big =fabs(a[j][k]);
			      irow=j;
			      icol=k;
	 		    }
			}
	           else if (ipiv[k] > 1) return(0);
				/* nrerror("GAUSSJ: singular *matrix -1"); */
		      }
		++(ipiv[icol]);
/******************************************************************************/
/* we now have the pivot element, so we interchage rows if neede to put the   */
/* pivot element on the diagonol. The columns are not a[8]sically interchaged,*/
/* only relabled: indx[i],the  column of the ith pivt element, is the ith     */
/* column that is reduced, while indxr is the row in which that pivot element */
/* was originally located. If indxr[i]!=indxc[i] there is an implied column   */
/* interchage. With this form of bookkeeping the solution b's will end up in  */
/* the correct order, and the inverse matrix will be scrabled by columns.     */
/******************************************************************************/
	if (irow !=icol)
	  {for (l=1;l<=n;l++)
	       SWAP(a[irow][l],a[icol][l])
	    for (l=1;l<=m;l++)
		SWAP(b[irow][l],b[icol][l])
	}

/******************************************************************/
/* We are now ready to divide the pivot row by the pivot element, */
/* located at irow, icol.                                         */
/******************************************************************/

	indxr[i]=irow;
	indxc[i]=icol;
	if (a[icol][icol] == 0.0)/* nrerror("GAUSSJ: singular matrix-2");*/
				return(0);
	pivinv=1.0/a[icol][icol];
	a[icol][icol]=1.0;
	for (l=1;l<=n;l++)
	    a[icol][l] *=pivinv;
	for (l=1;l<=m;l++)
	    b[icol][l] *=pivinv;
	for (ll=1;ll<=n;ll++) 
	    if (ll!= icol) /*next we reduce the rows except for the pivot one*/ 
	     {  dum=a[ll][icol];
		a[ll][icol]=0.0;
		for (l=1;l<=n;l++)
	    a[ll][l] -=a[icol][l]*dum;
	for (l=1;l<=m;l++)
	    b[ll][l] -=b[icol][l]*dum;
	      }
	}
  
/******************************************************************************/
/* This is the end of the main loop over columns of the reduction. It only    */
/* remains to unscrable the solution in view of the column interchages. We do */
/* this by interchaging pairs of columns in the reverse order that the        */
/* permutation was build up						                                 */
/******************************************************************************/

	for (l=1;l>=1;l--)
	  { if (indxr[l] != indxc[l])
	    for (k=1;k<=n;k++)
		SWAP(a[k][indxr[l]],a[k][indxc[l]]);
	}

/* done */

	free_ivector(ipiv,1,n);
	free_ivector(indxr,1,n);
	free_ivector(indxc,1,n);
return(1);
	}

/****************************************************************************/
/*This routine is used by many recipes programs. Function nrerror is        */
/*invoked to terminate program execution with an appropriate messange       */
/*when fatal error is encountered. The other routines are used to allocate  */
/* and deallocate memory for vectors and matrices.                          */
/* numerical recipes std. error handler                                     */
/****************************************************************************/

void nrerror(error_text)
char error_text[];
{

	fprintf(stderr,"numerical recipes run time error ... \n");
	fprintf(stderr,"%s\n",error_text);
	fprintf(stderr,"..now exiting to system ... \n");
	exit(1);
}

/*******************************************************/
/* allocates  double matrix of size specified by user  */
/*******************************************************/
double **dmatrix(nrl,nrh,ncl,nch)
int nrl,nrh,ncl,nch;
{
  register i;
  double **m;

  /* Allocate pointers to rows */

  m = (double **) malloc((unsigned) (nrh-nrl+1)*sizeof(double*));
  if(!m) printf("Allocation falure in row alloc. in matrix()\n");
  m -= nrl;

  /* Allocate rows and set pointers to them */

  for(i=nrl;i<=nrh;i++)
  {
    m[i]=(double *) malloc((unsigned) (nch-ncl+1)*sizeof(double));
    if(!m[i]) printf("Allocation falure 2  in matrix()\n");
    m[i] -= ncl;
  }

  /* return pointer to array of pointers to rows */

  return m;
}

/*******************************************************/
/* allocaates single matrix of size specified by user  */
/*******************************************************/
float **matrix(nrl,nrh,ncl,nch)
int nrl,nrh,ncl,nch;
{
  register i;
  float **m;

  /* Allocate pointers to rows */

  m = (float **) calloc( (nrh-nrl+1),sizeof(float*));
  if(!m) printf("Allocation falure in row alloc. in matrix()\n");
  m -= nrl;

  /* Allocate rows and set pointers to them */

  for(i=nrl;i<=nrh;i++)
  {
    m[i]=(float *) calloc(  (nch-ncl+1),sizeof(float));
    if(!m[i]) printf("Allocation falure 2  in matrix()\n");
    m[i] -= ncl;
  }

  /* return pointer to array of pointers to rows */

  return m;
}

/*******************************************************/
/* allocaates integer matrix of size specified by user  */
/*******************************************************/
int **imatrix(nrl,nrh,ncl,nch)
int nrl,nrh,ncl,nch;
{
  register i;
  int **m;

  /* Allocate pointers to rows */

  m = (int **) calloc( (nrh-nrl+1),sizeof(int*));
  if(!m) printf("Allocation falure in row alloc. in imatrix()\n");
  m -= nrl;

  /* Allocate rows and set pointers to them */

  for(i=nrl;i<=nrh;i++)
  {
    m[i]=(int *) calloc(  (nch-ncl+1),sizeof(int));
    if(!m[i]) printf("Allocation falure 2  in imatrix()\n");
    m[i] -= ncl;
  }

  /* return pointer to array of pointers to rows */

  return m;
}


/*****************************/
/* allocates double vector   */
/*****************************/
double *dvector(nl,nh)
int nl,nh;
{
  double *v;

  v = (double *) malloc((unsigned) (nh-nl+1)*sizeof(double));
  if(!v) nrerror("Allocation falure in dvector()\n");
  return(v-nl);
}

/*****************************/
/* allocates float vector   */
/*****************************/
float *fvector(nl,nh)
int nl,nh;
{
  float *v;

  v = (float *) malloc( (nh-nl+1)*sizeof(float));
  if(!v) printf("Allocation falure in vector()\n");
  return(v-nl);
}

/*************************************************/
/*allocates an int. vector with range [nl...nh] */
/*************************************************/
int *ivector(nl,nh)
int nl,nh;
{
  int *v;

  v = (int *) malloc((unsigned) (nh-nl+1)*sizeof(int));
  if(!v) nrerror("Allocation falure in ivector()\n");
  return(v-nl);
}

/***********************************************/
/* frees a double vector allocated by dvector() */
/***********************************************/
void free_dvector(v,nl,nh)
double *v;
int nl,nh;
{
  free((char*) (v+nl));
}

/************************************************/
/* frees a float vector allocated by fvector()  */
/************************************************/
void free_fvector(v,nl,nh)
float *v;
int nl,nh;

{
  free((char*) (v+nl));
}

/*********************************************/
/* frees a int vector allocated by ivector() */
/*********************************************/
void free_ivector(v,nl,nh)
int *v,nl,nh;

{
  free((char*) (v +nl));
}

/******************************************/
/* frees a matrix allocated with dmatrix() */
/******************************************/
void free_dmatrix(m,nrl,nrh,ncl,nch)
double **m;
int nrl,nrh,ncl,nch;

{
  register i;

  for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
  free((char*) (m+nrl));
}

/******************************************/
/* frees a matrix allocated with matrix() */
/******************************************/
void free_matrix(m,nrl,nrh,ncl,nch)
float **m;
int nrl,nrh,ncl,nch;

{
  register i;

  for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
  free((char*) (m+nrl));
}

/******************************************/
/* frees a matrix allocated with imatrix() */
/******************************************/
void free_imatrix(m,nrl,nrh,ncl,nch)
int **m;
int nrl,nrh,ncl,nch;

{
  register i;

  for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
  free((char*) (m+nrl));
}