/*
**	This code fragment 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.
**      (needs stdio.h and math.h)
*/

void ConvertRangeToCartesian(unsigned char *RangeImage, double **P)
{
	int	r,c;
	double	cp[8];
	double	bvav;
	double	xangle,yangle,dist;
	double	ScanDirectionFlag,SlantCorrection;

	ScanDirectionFlag=-1;

		/******* BVAV => Beam Vertical Angular Velocity *******/

	cp[0]=1220.7;		/* horizontal mirror angular velocity in rpm */
	cp[1]=32.0;		/* scan time per single pixel in microseconds */
	cp[2]=63.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]=63.5;		/* middle value of rows */

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

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

	bvav=cp[3];

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

}


/*  This function computes the unit normal vector at each 
    point in the image and marks particular pixels with a 
	gray level to indicate that it is a part of a surface
*/

unsigned char * ComputeUnitNormals(double **P)
{
	double vector1[3], vector2[3], cross[3];
	int x, y, i, max;
	double magnitude;
	int	alpha, beta;
	int accumulator[72][36] = {0};
	int normal[128*128][2];
	unsigned char *image;


	/* Allocate memory for the image */
	image = (unsigned char *)malloc(128*128);


	/* Compute the cross product and unit normals and 
	   increment the accumulator based alpha and beta */
	for (x=20; x<100; x++)
	{
		for (y=20; y<100; y++)
		{
			/* Get two vectors */
			 vector1[0] = (P[0][(x-1)*128+y] - P[0][x*128+y]);
			 vector1[1] = (P[1][(x-1)*128+y] - P[1][x*128+y]);
			 vector1[2] = (P[2][(x-1)*128+y] - P[2][x*128+y]);

			 vector2[0] = (P[0][x*128+y+1]   - P[0][x*128+y]);
			 vector2[1] = (P[1][x*128+y+1]   - P[1][x*128+y]);
			 vector2[2] = (P[2][x*128+y+1]   - P[2][x*128+y]);

			 /* The cross product */
			 cross[0] = vector1[1]*vector2[2] - vector2[1]*vector1[2];
			 cross[1] = vector1[2]*vector2[0] - vector2[2]*vector1[0];
			 cross[2] = vector1[0]*vector2[1] - vector2[0]*vector1[1];
			
			 /* Magnitude of the normal vector */
			 magnitude = sqrt(cross[0]*cross[0] + cross[1]*cross[1] + cross[2]*cross[2]);


			 /*The alpha and beta */
			alpha = (int)(57.2958 * acos(cross[2]/magnitude));
			beta = (int)(57.2958 * atan(cross[1]/cross[0]));

			
			// if (alpha > 90) alpha -= 90;
			if (beta < 0 ) beta += 360;  // Make sure angle betwwen 0 - 360

			alpha = alpha/5;
			beta = beta/5;

			++accumulator[beta][alpha];

			normal[(x-1)*128 + y][0] = alpha;
			normal[(x-1)*128 + y][1] = beta;
		}
	}

	
	/* Find the 7th highest accumulator value because 1 - 6 highest
	   values are from the floor and walls */
	for (i = 0; i < 7; i++)
	{
		max = 0;
		for (x=0; x<72; x++)
			for (y=0; y<36; y++)
				if (accumulator[x][y] > max)
				{
					max = accumulator[x][y];
					alpha = y ;
					beta = x ;
				}
		accumulator[beta][alpha] = 0;
	}
	

	/* Mark pixels as surface pixels if alpha and beta within tolerance */
	for (i = 0; i < 128*128; i++)
	{
		if (((normal[i][0] == alpha) || (normal[i][0] == alpha-1) || (normal[i][0] == alpha+1)) && 
			((normal[i][1] == beta)  || (normal[i][1] == beta-1)  || (normal[i][1] == beta+1)))
			image[i] = 255;
		else
			image[i] = 0;
	}


	/* Find the next highest accumulator array value */
	max = 0;
	for (x=0; x<72; x++)
		for (y=0; y<36; y++)
			if (accumulator[x][y] > max)
			{
				max = accumulator[x][y];
				alpha = y ;
				beta = x ;
			}
	accumulator[beta][alpha] = 0;

	
	/* Mark surface pixels */
	for (i = 0; i < 128*128; i++)
	{
		if (((normal[i][0] == alpha) || (normal[i][0] == alpha-1) || (normal[i][0] == alpha+1)) && 
			((normal[i][1] == beta)  || (normal[i][1] == beta-1)  || (normal[i][1] == beta+1)))
			image[i] = 170;
	}


	/* Find the next highest accumulator value */
	max = 0;
	for (x=0; x<72; x++)
		for (y=0; y<36; y++)
			if (accumulator[x][y] > max)
			{
				max = accumulator[x][y];
				alpha = y ;
				beta = x ;
			}
	accumulator[beta][alpha] = 0;

	
	/* Mark surface pixels */
	for (i = 0; i < 128*128; i++)
	{
		if (((normal[i][0] == alpha) || (normal[i][0] == alpha-1) || (normal[i][0] == alpha+1)) && 
			((normal[i][1] == beta)  || (normal[i][1] == beta-1)  || (normal[i][1] == beta+1)))
			image[i] = 100;
	}

	return image;
	


}