/**********************************************************
	** This is C-code which will read in a raw Odetics image,
	** split it into range and reflectance images, and write
	** them out in PGM format.
	**********************************************************/

#include <stdio.h>

main(argc,argv)

int	argc;
char	*argv[];

{
char		text[300];
unsigned char	*combined,*range,*ref,*pgm;
int		r,c,i;
FILE		*fpt;

if ((combined=(unsigned char *)calloc(1,128*128*2)) == NULL  ||
	(range=(unsigned char *)calloc(1,128*128)) == NULL  ||
	(pgm=(unsigned char *)calloc(1,128*128*3)) == NULL  ||
	(ref=(unsigned char *)calloc(1,128*128)) == NULL)
  {
  printf("Unable to acquire necessary memory\n");
  exit(0);
  }
if (argc < 2  ||  argc > 3  ||  (argc == 3  &&  strcmp(argv[2],"rotate") != 0))
  {
  printf("Usage:  odetics-to-pgm [filename] [rotate]\n");
  exit(0);
  }
if ((fpt=fopen(argv[1],"r")) == NULL)
  {
  printf("Unable to open %s\n",argv[1]);
  exit(0);
  }
if (fread(combined,1,2*128*128,fpt) != 2*128*128)
  {
  printf("Problem reading odetics data\n");
  exit(0);
  }
fclose(fpt);
	/* splits the raw data into range and reflectance images */
for (r=0; r<128; r++)
  for (c=0; c<128; c++)
    {
    if (argc == 2)
      {	/* no rotation */
      range[r*128+c]=combined[(r*128+c)*2+1];
      ref[r*128+c]=combined[(r*128+c)*2];
      }
    else
      {	/* rotation 90 degrees CW */
      range[r*128+c]=combined[((127-c)*128+r)*2+1];
      ref[r*128+c]=combined[((127-c)*128+r)*2];
      }
    }
for (i=0; i<2; i++) /* once for range, reflectance */
  {
  if (i == 0)
    {
    for (r=0; r<128*128; r++)
      pgm[r]=range[r];
    sprintf(text,"%s.range.pgm",argv[1]);
    }
  else
    {
    for (r=0; r<128*128; r++)
      pgm[r]=ref[r];
    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,"P5\n128 128 255\n");
  if (fwrite(pgm,1,128*128,fpt) != 128*128)
    {
    printf("Problem writing pgm file\n");
    exit(0);
    }
  fclose(fpt);
  }
free(range);
free(ref);
free(combined);
free(pgm);
}






/*
**	This routine converts the data in an Odetics range image into 3D
**	cartesian coordinate data.  The range image is 8-bit, and comes
**	already separated from the intensity image.  The cartesian
**	coordinates are returned in P, where P[0] is X, P[1] is Y and
**	P[2] is Z.  ImageTypeFlag, for all these images available, is 0.
*/

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

ConvertOdeticsRangeToCartesian(RangeImage,P,ImageTypeFlag)

unsigned char	*RangeImage;
double		*P[3];
int             ImageTypeFlag;

{
int	r,c;
double	cp[7];
double	xangle,yangle,dist;
double	ScanDirectionFlag,SlantCorrection;


cp[0]=1220.7;		/* horizontal mirror angular velocity in rpm */
cp[1]=32.0;		/* scan time per single pixel in microseconds */
cp[2]=(COLS/2)-0.5;		/* middle value of columns */
cp[3]=1220.7/192.0;	/* vertical mirror angular velocity in rpm */
cp[4]=6.14;		/* scan time (with retrace) per line in milliseconds */
cp[5]=(ROWS/2)-0.5;		/* middle value of rows */
cp[6]=10.0;		/* standoff distance in range units (3.66cm per r.u.) */

cp[0]=cp[0]*3.1415927/30.0;	/* convert rpm to rad/sec */
cp[3]=cp[3]*3.1415927/30.0;	/* convert rpm to rad/sec */
cp[0]=2.0*cp[0];		/* beam ang. vel. is twice mirror ang. vel. */
cp[3]=2.0*cp[3];		/* beam ang. vel. is twice mirror ang. vel. */
cp[1]/=1000000.0;		/* units are microseconds : 10^-6 */
cp[4]/=1000.0;			/* units are milliseconds : 10^-3 */

switch(ImageTypeFlag)
  {
  case 1:		/* Odetics image -- scan direction upward */
    ScanDirectionFlag=-1;
    break;
  case 0:		/* Odetics image -- scan direction downward */
    ScanDirectionFlag=1;
    break;
  default:		/* in case we want to do this on synthetic model */
    ScanDirectionFlag=0;
    break;
  }

	/* start with semi-spherical coordinates from laser-range-finder: */
	/*			(r,c,RangeImage[r*COLS+c])		  */
	/* convert those to axis-independant spherical coordinates:	  */
	/*			(xangle,yangle,dist)			  */
	/* then convert the spherical coordinates to cartesian:           */
	/*			(P => X[] Y[] Z[])			  */

for (r=0; r<ROWS; r++)
  {
  for (c=0; c<COLS; c++)
    {
    SlantCorrection=cp[3]*cp[1]*((double)c-cp[2]);
    xangle=cp[0]*cp[1]*((double)c-cp[2]);
    yangle=(cp[3]*cp[4]*(cp[5]-(double)r))+	/* Standard Transform Part */
	SlantCorrection*ScanDirectionFlag;	/*  + slant correction */
    dist=(double)RangeImage[r*COLS+c]+cp[6];
    P[2][r*COLS+c]=sqrt((dist*dist)/(1.0+(tan(xangle)*tan(xangle))
	+(tan(yangle)*tan(yangle))));
    P[0][r*COLS+c]=tan(xangle)*P[2][r*COLS+c];
    P[1][r*COLS+c]=tan(yangle)*P[2][r*COLS+c];
    }
  }

}