/*
   CMSC 427 - Term Project Candidate
   ---------------------------------

   Beth Weinstein - bweinste@wam.umd.edu
   Ben Stewart - stewartb@wam.umd.edu
   Nawfel Tricha - Topaz@endorproductions.com

   Minimal OpenGL project using Microsoft Visual C++.
   This project is a premilinary candidate for the term 
   project required by CMSC 427.

   ABSTRACT:
      In this project, we will develop several fast 
      rendering and collision routines which would 
      require C/C++ speed and accuracy and then 
      implement a VisualJ++ interface.

   MODULE:
      Utilities module.
*/

#include <math.h>
#include <stdlib.h>
#include "Utils.h"

/*********************************************************************************************\
   NT:
   Given 3 Euler angles, we get three scaled vectors: (Forward, Right, Up).
   The Matrix is:

      | cY.cZ              cY.sZ             -sY       0 |
      | sX.sY.cZ-cX.sZ     sX.sY.sZ+cX.cZ    sX.cY     0 |
      | cX.sY.cZ+sX.sZ     cX.sY.sZ-sX.cZ    cX.cY     0 |
      |       0                  0             0       1 |

   X = Pitch;
   Y = Yaw;
   Z = Roll;

\**********************************************************************************************/
/*
void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
{
	float		angle;
	static float		sr, sp, sy, cr, cp, cy;
	// static to help MS compiler fp bugs

	angle = angles[YAW] * (M_PI*2 / 360);
	sy = sin(angle);
	cy = cos(angle);
	angle = angles[PITCH] * (M_PI*2 / 360);
	sp = sin(angle);
	cp = cos(angle);
	angle = angles[ROLL] * (M_PI*2 / 360);
	sr = sin(angle);
	cr = cos(angle);

	if (up)
	{

		up[0] = cp*cy;
		up[1] = cp*sy;
		up[2] = -sp;
		//forward[2] = cp*sy;
		//forward[1] = sp;
	}
	if (right)
	{
		right[0] = (-1*sr*sp*cy+-1*cr*-sy);
		right[1] = (-1*sr*sp*sy+-1*cr*cy);
		right[2] = -1*sr*cp;
	}
	if (forward)
	{
		forward[0] = (cr*sp*cy+-sr*-sy);
		forward[1] = (cr*sp*sy+-sr*cy);
		forward[2] = cr*cp;
	}
}
*/
void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
{
	float		angle;
	static float		sz, sx, sy, cz, cx, cy;
	
   //  Switch cos and sin for pitch/yaw:

	angle = angles[PITCH] * (M_PI*2 / 360);
	sx = sin(angle);
	cx = cos(angle);
	angle = angles[YAW] * (M_PI*2 / 360);
	sy = sin(angle);
	cy = cos(angle);
	angle = angles[ROLL] * (M_PI*2 / 360);
	sz = sin(angle);
	cz = cos(angle);

	if (right)
	   {
		right[0] = cy*cz;
		right[1] = cy*sz;
		right[2] = -sy;
	   }
	if (forward)
	   {
		forward[0] = -cx*sy*cz-sx*sz;
		forward[1] = -cx*sy*sz+sx*cz;
      forward[2] = -cx*cy;
	   }
	if (up)
	   {
		up[0] = sx*sy*cz-cx*sz;
      up[1] = sx*sy*sz+cx*cz;
		up[2] = sx*cy;
		}
}
/*********************************************************************************************\
   NT:
   Return true if two vectors are identical, false otherwise.
\*********************************************************************************************/
inline bool VectorCompare (vec3_t v1, vec3_t v2)
{
	if (v1[0] != v2[0] || v1[1] != v2[1] || v1[2] != v2[2])
			return true;
	return false;
}


/*********************************************************************************************\
   NT:
   Return length of a normalized vector.
\*********************************************************************************************/
float VectorNormalize (vec3_t &v)
{
	float	length, ilength;

	length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
	length = (float) sqrt (length);		// FIXME
	if (length)
	   {
		ilength = 1/length;
		v[0] *= ilength;
		v[1] *= ilength;
		v[2] *= ilength;
	   }		
	return length;
}

/*********************************************************************************************\
   NT:
   Return length of a vector sqrt(v.v).
\*********************************************************************************************/
double sqrt(double x);
inline float VectorLength(vec3_t v)
{
	float	length;
   length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
	length = (float) sqrt (length);		// FIXME
	return length;
}

/*********************************************************************************************\
   NT:
   Return distance between two points.
\*********************************************************************************************/
float VectorDistance(vec3_t v1, vec3_t v2)
{
	float	length;
   vec3_t delta;
   VectorSubtract(v2, v1, delta);
   length = delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2];
	length = (float) sqrt (length);		// FIXME
	return length;
}

/*********************************************************************************************\
   NT:
   Return true if point (end) is in front of point (start) oriented in (angles).
\*********************************************************************************************/
bool InFront (vec3_t start, vec3_t angles, vec3_t end)
{
	vec3_t vec, forward;
	float	dot;
	
	AngleVectors (angles, forward, NULL, NULL);
	VectorSubtract (end, start, vec);
	VectorNormalize (vec);
	dot = DotProduct (vec, forward);
	 
	if (dot > 0.3) //.3
		return true;
	return false;
}

/*********************************************************************************************\
   NT:
   Return YAW (Y-Angle) of vector.
\*********************************************************************************************/
float VectorYaw (vec3_t vec)
{
	float	yaw;
	if (vec[2] == 0)
      {
      if (vec[0] <= 0) return 90;
      else return 270;
      }
	if (vec[0] == 0)
      {
      if (vec[2] <= 0) return 0;
      else return 180;
      }
	yaw = atan2(-vec[0], -vec[2]) * 180 / M_PI;
	if (yaw < 0) yaw += 360;
	return yaw;
}

/*********************************************************************************************\
   NT:
   Convert vector to angles.
\*********************************************************************************************/
void VectorAngles (vec3_t value1, vec3_t angles)
{
	float	forward;
	float	yaw, pitch;
	
	if (value1[2] == 0)
      {
      if (value1[0] <= 0) yaw = 90;
      else yaw = 270;      
      }
	else if (value1[0] == 0)
      {
      if (value1[2] <= 0) yaw = 0;
      else yaw = 180;
      }
	else
	   {
		yaw = (atan2(-value1[0], -value1[2]) * 180 / M_PI);
		if (yaw < 0) yaw += 360;
      }

   forward = sqrt (value1[0]*value1[0] + value1[2]*value1[2]);
	pitch = (atan2(-value1[1], forward) * 180 / M_PI);
	if (pitch < 0) pitch += 360;

	angles[PITCH] = -pitch;
	angles[YAW] = yaw;
	angles[ROLL] = 0;
}
/*********************************************************************************************\
   NT:
   Convert vector to matrix (OpenGL compatible).
\*********************************************************************************************/
void AngleMatrix(vec3_t angle, float *mat)
{
   float cos_pitch = (float) cos(angle[PITCH]),
         sin_pitch = (float) sin(angle[PITCH]),
         cos_yaw = (float) cos(angle[YAW]),
         sin_yaw = (float) sin(angle[YAW]),
         cos_roll = (float) cos(angle[ROLL]),
         sin_roll = (float) sin(angle[ROLL]);

   mat[0] = cos_roll * cos_yaw;
   mat[4] = sin_yaw;
   mat[8] = -sin_roll;
   
   mat[1] = -sin_yaw;
   mat[5] = cos_pitch * cos_yaw;
   mat[9] = sin_pitch;
   
   mat[2] = sin_roll;
   mat[6] = -sin_pitch;
   mat[10]= cos_roll * cos_pitch;

   mat[3] = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0;
   mat[15] = 1;
}

/*
 * BS: Generates a surface of revolution given a set of data points, and an origin
 *	   and axis to revolve around
 */
void GenerateSurface(vec3_t dataPts[], int numPoints, int numSlices, int axis, vec3_t **surface)
{
	int i, j;
	float angle;

	switch(axis)
	{
		// BS: Assumes silohette is drawn in the xy plane
		case XAXIS:
			for(j = 0; j < numSlices; j++)
			{
				angle = (j / (float) numSlices) * 2 * M_PI;
				for(i = 0; i < numPoints; i++)
				{
					surface[i][j][0] = dataPts[i][0];
					surface[i][j][1] = dataPts[i][1] * cos(angle);
					surface[i][j][2] = dataPts[i][1] * sin(angle) * -1.0f;
				}
			}
			break;
		// BS: Assumes silohette is drawn in the xy plane
		case YAXIS:
			for(j = 0; j < numSlices; j++)
			{
				angle = (j / (float) numSlices) * 2 * M_PI;
				for(i = 0; i < numPoints; i++)
				{
					surface[i][j][0] = dataPts[i][0] * cos(angle);
					surface[i][j][1] = dataPts[i][1];
					surface[i][j][2] = dataPts[i][0] * sin(angle) * -1.0f;
				}
			}
			break;
		// BS: Assumes silohette is drawn in the yz plane
		case ZAXIS:
			for(j = 0; j < numSlices; j++)
			{
				angle = (j / (float) numSlices) * 2 * M_PI;
				for(i = 0; i < numPoints; i++)
				{
					surface[i][j][0] = dataPts[i][1] * sin(angle) * -1.0f;
					surface[i][j][1] = dataPts[i][1] * cos(angle);
					surface[i][j][2] = dataPts[i][2];
				}
			}
			break;
	}
}

/*
 * BS: draws a series of polygons defined by the surface data points generated by
 *     GenerateSurface
 */

void DrawSurface(vec3_t **surface, text_coord_t **text_coords, int numPoints, int numSlices)
{
	int i, j;

	glBegin(GL_QUAD_STRIP);
		for(i = 0; i < numPoints - 1; i++)
		{
			for(j = 0; j < numSlices; j++)
			{
				glTexCoord2f(text_coords[i][j].s,text_coords[i][j].t);
				glVertex3f(surface[i][j][0],
						   surface[i][j][1],
						   surface[i][j][2]);
				glTexCoord2f(text_coords[i + 1][j].s,text_coords[i][j].t);
				glVertex3f(surface[i + 1][j][0],
						   surface[i + 1][j][1],
						   surface[i + 1][j][2]);
			}
			glTexCoord2f(text_coords[i][0].s,text_coords[i][0].t);
			glVertex3f(surface[i][0][0],
					   surface[i][0][1],
					   surface[i][0][2]);
			glTexCoord2f(text_coords[i + 1][0].s,text_coords[i + 1][0].t);
			glVertex3f(surface[i + 1][0][0],
					   surface[i + 1][0][1],
					   surface[i + 1][0][2]);
		}	

		for(j = 0; j < numSlices; j++)
		{
			glTexCoord2f(text_coords[numPoints - 1][j].s,text_coords[numPoints - 1][j].t);
			glVertex3f(surface[numPoints - 1][j][0],
					   surface[numPoints - 1][j][1],
					   surface[numPoints - 1][j][2]);
			glTexCoord2f(text_coords[0][j].s,text_coords[0][j].t);
			glVertex3f(surface[0][j][0],
					   surface[0][j][1],
					   surface[0][j][2]);
		}
		
		glTexCoord2f(text_coords[numPoints - 1][0].s,text_coords[numPoints - 1][0].t);	
		glVertex3f(surface[numPoints - 1][0][0],
				   surface[numPoints - 1][0][1],
				   surface[numPoints - 1][0][2]);
		glTexCoord2f(text_coords[0][0].s,text_coords[0][0].t);
		glVertex3f(surface[0][0][0],
				   surface[0][0][1],
				   surface[0][0][2]);
	glEnd();

}
/*
void DrawSurface(vec3_t **surface, text_coord_t **text_coords, int numPoints, int numSlices)
{
	int i, j;

	glBegin(GL_QUAD_STRIP);
		for(i = 0; i < numPoints - 1; i++)
		{
			for(j = 0; j < numSlices; j++)
			{
				glTexCoord2f(text_coords[i + 1][j].s,text_coords[i][j].t);
				glVertex3f(surface[i + 1][j][0],
						   surface[i + 1][j][1],
						   surface[i + 1][j][2]);
				glTexCoord2f(text_coords[i][j].s,text_coords[i][j].t);
				glVertex3f(surface[i][j][0],
						   surface[i][j][1],
						   surface[i][j][2]);
			}
			glTexCoord2f(text_coords[i + 1][0].s,text_coords[i + 1][0].t);
			glVertex3f(surface[i + 1][0][0],
					   surface[i + 1][0][1],
					   surface[i + 1][0][2]);
			glTexCoord2f(text_coords[i][0].s,text_coords[i][0].t);
			glVertex3f(surface[i][0][0],
					   surface[i][0][1],
					   surface[i][0][2]);
		}	

		for(j = 0; j < numSlices; j++)
		{
			glTexCoord2f(text_coords[0][j].s,text_coords[0][j].t);
			glVertex3f(surface[0][j][0],
					   surface[0][j][1],
					   surface[0][j][2]);
			glTexCoord2f(text_coords[numPoints - 1][j].s,text_coords[numPoints - 1][j].t);
			glVertex3f(surface[numPoints - 1][j][0],
					   surface[numPoints - 1][j][1],
					   surface[numPoints - 1][j][2]);
		}
		
		glTexCoord2f(text_coords[0][0].s,text_coords[0][0].t);
		glVertex3f(surface[0][0][0],
				   surface[0][0][1],
				   surface[0][0][2]);
		glTexCoord2f(text_coords[numPoints - 1][0].s,text_coords[numPoints - 1][0].t);	
		glVertex3f(surface[numPoints - 1][0][0],
				   surface[numPoints - 1][0][1],
				   surface[numPoints - 1][0][2]);
	glEnd();

}
*/