/**********************************************************
	** This is C-code which will read in a raw Perceptron image,
	** split it into range and reflectance images, and write
	** them out in PGM format.  WARNING:  Note that the Perceptron
	** data is 12-bit, so must be stored in the ASCII version
	** of PGM.  Many image readers (like xv) will load this data,
	** but will often clip it to 8-bit.
	**********************************************************/

#include <stdio.h>

main(argc,argv)

int	argc;
char	*argv[];

{
char		text[300],header[6][800];
int		*LoadingImage;
short int	*pgm;
int		i,j,value,r,c;
FILE		*fpt;

if ((LoadingImage=(int *)calloc(4,512*512)) == NULL  ||
	(pgm=(short int *)calloc(2,512*512)) == NULL)
  {
  printf("Unable to acquire necessary memory\n");
  exit(0);
  }
if (argc != 2)
  {
  printf("Usage:  perc-to-pgm [filename] \n");
  exit(0);
  }
if ((fpt=fopen(argv[1],"r")) == NULL)
  {
  printf("Unable to open %s\n",argv[1]);
  exit(0);
  }
fscanf(fpt,"%s %s",header[0],header[1]);
fscanf(fpt,"%s %s",header[0],header[1]);
fscanf(fpt,"%s %s %s",header[0],header[1],header[2]);
fscanf(fpt,"%s %s %s",header[0],header[1],header[2]);
fscanf(fpt,"%s %s %s",header[0],header[1],header[2]);
fscanf(fpt,"%s %s %s",header[0],header[1],header[2]);
fscanf(fpt,"%s %s %s %s",header[0],header[1],header[2],header[3]);
fscanf(fpt,"%s %s %s %s",header[0],header[1],header[2],header[3]);
fscanf(fpt,"%s %s %s",header[0],header[1],header[2]);
fscanf(fpt,"%s %s %s %s %s",header[0],header[1],header[2],
	header[3],header[4]);
fscanf(fpt,"%c%c",&(header[0][0]),&(header[0][1])); /* extra new-line bytes */
if (fread(LoadingImage,4,512*512,fpt) != 512*512)
  {
  printf("Problem reading perceptron data\n");
  exit(0);
  }
fclose(fpt);
for (i=0; i<2; i++)
  {
  for (j=0; j<512*512; j++)
    {
    if (i == 0)
      value=(LoadingImage[j] & 0x00000fff) ;	/* range data for pixel */
    else
      value=(LoadingImage[j] & 0x0fff0000) >> 16;	/* reflectance */
    pgm[j]=(short int)(value);
    }

  if (i == 0)
    sprintf(text,"%s.range.pgm",argv[1]);
  else
    sprintf(text,"%s.refl.pgm",argv[1]);
  if ((fpt=fopen(text,"w")) == NULL)
    {
    printf("Unable to open %s for writing\n",text);
    exit(0);
    }
  fprintf(fpt,"P2 512 512 4095\n");
  for (r=0; r<512; r++)
    {
    for (c=0; c<511; c++)
      fprintf(fpt,"%d ",(int)pgm[r*512+c]);
    fprintf(fpt,"%d\n",(int)pgm[r*512+511]);
    }
  fclose(fpt);
  }

free(LoadingImage);
free(pgm);
}







/*
**      Convert Perceptron image to 3D cartesian coordinates.
**
**
**                                   beta > 0
**
**                                       ^ y
**                      alpha > 0        |        alpha < 0
** Seen from camera:              x <----|----                (z-axis points
**                                                             out of camera)
**
**                                   beta < 0
*/

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


ConvertPerceptronRangeToCartesian(RangeImage,P,ROWS,COLS)

short int	*RangeImage;
double		*P[3];
int		ROWS,COLS;

{
double		ra, r_0, r_1, r_2, r_3, h_1, h_2, dx, dy, dz;
double		alpha, beta, alpha_0, beta_0;
			/* alpha = Horizontal, beta = Vertical angle */
double		gamma, theta;
double		H,V,delta;
int		r,c;

h_1=3.0;			/* dist (y) between rotating mirror axis
				** and the parallel laser beam. */
h_2=5.5; 			/* dist (y) between nodding mirror axis
				** and rotating mirror laser intersection. */
gamma=45.0*(M_PI/180.0);	/* slope of rotating mirror */
theta=45.0*(M_PI/180.0);	/* slope of nodding mirror in mid position */
alpha_0=beta_0=0.0;
H=51.65;
V=36.73;
r_0=830.3;
delta=0.20236;

for (r=0; r<ROWS; r++)
  {
  for (c=0; c<COLS; c++)
    {
    alpha = alpha_0 + H*(255.5 - c)/512.0;
    alpha = alpha*M_PI/180.0;
    beta  = beta_0  + V*(255.5 - r)/512.0;
    beta  = beta*M_PI/180.0;

	/* (dx,dy,dz): where the laser leaves the nodding mirror */

    dz = -(h_1*(1.0-cos(alpha))/tan(gamma));/* Motion of the laser intersection
				               point with the rotating mirror 
				               as a function of alpha. */

    dy = dz*tan(theta + 0.5*beta);  /* Vertical offset of nodding mirror
				       intersection due to dz */

    dx = (h_2 + dy)*tan(alpha);     /* Horizontal offset of laser intersection
				       point with nodding mirror due to alpha */
 
	/*
	** R  : true range: laser light path length (laser travel) from
	**      laser emission point to target.
	** ra : range value returned by camera.
	** r_0: (standoff distance) length of laser ray to point for which r=0
	** r_1: laser travel (z) from laserdiode to intersection with rotating
	**      mirror: r1 = d1 + dz
	** r_2: laser travel (vector length) from rotating mirror
	**	to nodding mirror.
	** r_3: laser travel (vector length) from nodding mirror to target.
	**
        ** Thus: R = r + r0 = r1 + r2 + r3
	*/

    ra  = (double) RangeImage[r*COLS+c]; /* range returned by camera */

    r_1 = (dz - h_2)/delta;
    r_2 = sqrt(dx*dx + (h_2+dy)*(h_2+dy))/delta;
    r_3 = (ra + r_0 - (r_1 +r_2))*delta;

    P[0][r*COLS+c] = dx + r_3 * sin(alpha);
    P[1][r*COLS+c] = dy + r_3 * cos(alpha)*sin(beta);
    P[2][r*COLS+c] = dz + r_3 * cos(alpha)*cos(beta); 
    }
  }
}