/*These files contain basic structures used frequently in the program and in computer
graphics in general.  For more information, see the header file.

-David Mayerich, 8/20/05*/

#include "rtsMath.h"

//#include<iostream>

//using namespace std;



point3D::point3D()
{
	x=0; y=0; z=0;
}

point3D::point3D(double newx, double newy, double newz)
{
	x=newx; y=newy; z=newz;
}

point3D point3D::operator +(vector3D param)
{
	point3D result;
	result.x=x+param.x;
	result.y=y+param.y;
	result.z=z+param.z;

	return result;
}

point3D point3D::operator -(vector3D param)
{
	point3D result;
	result.x=x-param.x;
	result.y=y-param.y;
	result.z=z-param.z;

	return result;
}

vector3D point3D::operator -(point3D param)
{
	vector3D result;
	result.x=x-param.x;
	result.y=y-param.y;
	result.z=z-param.z;

	return result;
}

void point3D::print()
{
	cout<<x<<","<<y<<","<<z<<endl;
}

vector3D vector3D::operator+(vector3D param)
{
	vector3D result;
	result.x=x+param.x;
	result.y=y+param.y;
	result.z=z+param.z;

	return result;
}

vector3D vector3D::operator -(vector3D param)
{
	vector3D result;
	result.x=x-param.x;
	result.y=y-param.y;
	result.z=z-param.z;

	return result;
}
double vector3D::operator*(vector3D param)
{
	return x*param.x + y*param.y + z*param.z;
}
	
vector3D::vector3D()
{
	x=0; y=0; z=0;
}

vector3D::vector3D(double newx, double newy, double newz)
{
	x=newx;
	y=newy;
	z=newz;
}

int vector3D::normalize()
{
	double length=sqrt(x*x + y*y + z*z);
	if(length == 0)
	{
		x=0.0;
		y=0.0;
		z=0.0;
	}
	else
	{
		x=x/length;
		y=y/length;
		z=z/length;
	}
	
	return 1;
}

double vector3D::length()
{
	return sqrt(x*x + y*y + z*z);
}

vector3D vector3D::cross(vector3D param)
{
	vector3D result;
	result.x=y*param.z - z*param.y;
	result.y=z*param.x - x*param.z;
	result.z=x*param.y - y*param.x;

	return result;
}

vector3D vector3D::operator *(double param)
{
	vector3D result;
	result.x=x*param;
	result.y=y*param;
	result.z=z*param;

	return result;
}

void vector3D::print()
{
	cout<<x<<","<<y<<","<<z<<endl;
}

matrix4x4::matrix4x4()
{
	//initialize to the identity matrix
	for(int i=0; i<16; i++)
		matrix[i] = 0.0;
	matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1.0;
}

matrix4x4::matrix4x4(float m00, float m01, float m02, float m03,
			  float m10, float m11, float m12, float m13,
			  float m20, float m21, float m22, float m23,
			  float m30, float m31, float m32, float m33)
{
	float new_matrix[16] = {m00, m01, m02, m03,m10, m11, m12, m13, m20, m21, m22, m23,m30, m31, m32, m33};
	for(int i=0; i<16; i++)
		matrix[i] = new_matrix[i];
}

matrix4x4::matrix4x4(vector3D basis_x, vector3D basis_y, vector3D basis_z)
{
	//initialize to the identity matrix
	for(int i=0; i<16; i++)
		matrix[i] = 0.0;
	matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1.0;
	//insert the vectors
	matrix[0] = basis_x.x;
	matrix[1] = basis_x.y;
	matrix[2] = basis_x.z;

	matrix[4] = basis_y.x;
	matrix[5] = basis_y.y;
	matrix[6] = basis_y.z;

	matrix[8] = basis_z.x;
	matrix[9] = basis_z.y;
	matrix[10] = basis_z.z;
}

double matrix4x4::operator()(unsigned int row, unsigned int col)
{
	return matrix[row*4 + col];
}

void matrix4x4::gl_set_matrix(float* gl_matrix)
{
	//for(int row = 0; row<4; row++)
	//	for(int col = 0; col<4; col++)
	//		matrix[row*4 + col] = gl_matrix[col*4 + row];
	for(int i=0; i<16; i++)
		matrix[i] = gl_matrix[i];
}


void matrix4x4::gl_get_matrix(float* gl_matrix)
{
	//for(int row = 0; row<4; row++)
	//	for(int col = 0; col<4; col++)
	//		gl_matrix[col*4 + row] = matrix[row*4 + col];
	for(int i=0; i<16; i++)
		gl_matrix[i] = matrix[i];

}

matrix4x4 matrix4x4::submatrix(unsigned int start_col, unsigned int start_row, unsigned int end_col, unsigned int end_row)
{
	matrix4x4 result;
	int col, row;
	for(row = start_row; row<=end_row; row++)
		for(col = start_col; col <= end_col; col++)
			result.matrix[col*4 + row] = matrix[col*4 + row];
	return result;

}

matrix4x4 matrix4x4::transpose()
{
	matrix4x4 result;
	int col, row;
	for(row = 0; row <4; row++)
		for(col=0; col<4; col++)
			result.matrix[col*4 + row] = matrix[row*4+ col];
	return result;
}



vector3D matrix4x4::basis_x()
{
	vector3D basis(matrix[0], matrix[1], matrix[2]);
	return basis;
}

vector3D matrix4x4::basis_y()
{
	vector3D basis(matrix[4], matrix[5], matrix[6]);
	return basis;
}

vector3D matrix4x4::basis_z()
{
	vector3D basis(matrix[8], matrix[9], matrix[10]);
	return basis;
}



point3D matrix4x4::operator *(point3D param)
{
	point3D result;
	//result.x = matrix[0][0]*param.x + matrix[1][0]*param.y + matrix[2][0]*param.z + matrix[3][0];
	//result.y = matrix[0][1]*param.x + matrix[1][1]*param.y + matrix[2][1]*param.z + matrix[3][1];
	//result.z = matrix[0][2]*param.x + matrix[1][2]*param.y + matrix[2][2]*param.z + matrix[3][2];
	//float homogeneous = matrix[0][3] + matrix[1][3] + matrix[2][3] + matrix[3][3];
	result.x = matrix[0]*param.x + matrix[4]*param.y + matrix[8]*param.z + matrix[12];
	result.y = matrix[1]*param.x + matrix[5]*param.y + matrix[9]*param.z + matrix[13];
	result.z = matrix[2]*param.x + matrix[6]*param.y + matrix[10]*param.z + matrix[14];
	float homogeneous = matrix[3] + matrix[7] + matrix[11] + matrix[15];
	result.x/=homogeneous;
	result.y/=homogeneous;
	result.z/=homogeneous;
	
	return result;
}

vector3D matrix4x4::operator*(vector3D param)
{
	vector3D result;
	result.x = matrix[0]*param.x + matrix[4]*param.y + matrix[8]*param.z;
	result.y = matrix[1]*param.x + matrix[5]*param.y + matrix[9]*param.z;
	result.z = matrix[2]*param.x + matrix[6]*param.y + matrix[10]*param.z;
	
	return result;
}

void matrix4x4::print()
{
	for(int row = 0; row<4; row++)
	{
		for(int col = 0; col<4; col++)
		{
			cout<<matrix[col*4 + row]<<" ";
		}
		cout<<endl;
	}
}
		

RGBA::RGBA(double red, double green, double blue, double ambient)
{
	r=red;
	g=green;
	b=blue;
	a=ambient;
}

RGBA::RGBA()
{
	r=1.0;
	g=1.0;
	b=1.0;
	a=1.0;
}

int quaternion::normalize()
{
	double length=sqrt(w*w + x*x + y*y + z*z);
	w=w/length;
	x=x/length;
	y=y/length;
	z=z/length;

	return 1;
}

quaternion quaternion::operator *(quaternion param)
{
	float A, B, C, D, E, F, G, H;


	A = (w + x)*(param.w + param.x);
	B = (z - y)*(param.y - param.z);
	C = (w - x)*(param.y + param.z); 
	D = (y + z)*(param.w - param.x);
	E = (x + z)*(param.x + param.y);
	F = (x - z)*(param.x - param.y);
	G = (w + y)*(param.w - param.z);
	H = (w - y)*(param.w + param.z);

	quaternion result;
	result.w = B + (-E - F + G + H) /2;
	result.x = A - (E + F + G + H)/2; 
	result.y = C + (E - F + G - H)/2; 
	result.z = D + (E - F - G + H)/2;

	return result;
}

double* quaternion::toMatrix()
{


    double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;


    // calculate coefficients
    x2 = x + x; y2 = y + y;
    z2 = z + z;
    xx = x * x2; xy = x * y2; xz = x * z2;
    yy = y * y2; yz = y * z2; zz = z * z2;
    wx = w * x2; wy = w * y2; wz = w * z2;

	double m[4][4];
    m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
    m[2][0] = xz + wy; m[3][0] = 0.0;

    m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
    m[2][1] = yz - wx; m[3][1] = 0.0;


    m[0][2] = xz - wy; m[1][2] = yz + wx;
    m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;


    m[0][3] = 0; m[1][3] = 0;
    m[2][3] = 0; m[3][3] = 1;

	double* orientationmatrix=(double*)m;
	char c;


	double* result=new double[16];
	double* array=(double*)m;
	for(int i=0; i<16; i++)
		result[i]=array[i];

	return result;
}

quaternion::quaternion()
{
	w=0.0; x=0.0; y=0.0; z=0.0;
}

quaternion::quaternion(double c, double i, double j, double k)
{
	w=c;  x=i;  y=j;  z=k;
}

ray3D::ray3D(point3D start_point, vector3D dir)
{
	point=start_point;
	direction=dir;
	direction.normalize();
}

ray3D::ray3D(point3D pointA, point3D pointB)
{
	point = pointA;
	direction = pointB - pointA;
	direction.normalize();
}

ray3D::ray3D()
{
	point = point3D(0, 0, 0);
	direction = vector3D(0, 0, 1);
}

int ray3D::intersect(plane3D plane, point3D& intersection_point)
{
	double NdotD = plane.normal * direction;		//determine the angle between the plane normal and the ray direction
	if(NdotD == 0.0)								//if they are orthogonal, the ray is parallel to the plane
		return RTS_NO_INTERSECTION;

	vector3D E = point - plane.point;			//determine the vector from the ray point to the plane point
	double NdotE = plane.normal * E;			//find the angle between the plane normal and the above calculated E
	double t = -NdotE/NdotD;					//the intersection occurrs at time t

	if(t<0)
		return RTS_NO_INTERSECTION;

	intersection_point = point + direction*t;	//find the intersection point
	return RTS_OK;
}



plane3D::plane3D()
{
	point = point3D(0.0, 0.0, 0.0);
	normal = vector3D(0.0, 0.0, 1.0);
	normal.normalize();
}

plane3D::plane3D(point3D point_on_plane, vector3D plane_normal)
{
	point = point_on_plane;
	normal = plane_normal;
	normal.normalize();
}


function2D::function2D(const function2D &copy)
{
	m_x0 = copy.m_x0;
	m_y0 = copy.m_y0;
	m_x1 = copy.m_x1;
	m_y1 = copy.m_y1;
	m_x_resolution = copy.m_x_resolution;
	m_y_resolution = copy.m_y_resolution;
	m_values = new float[m_x_resolution*m_y_resolution];

	for(int i=0; i < m_x_resolution*m_y_resolution; i++)
		m_values[i] = copy.m_values[i];
}
function2D& function2D::operator=(const function2D& f)
{
    if (this != &f)							// make sure not same object
	{						
        delete m_values;
		m_x0 = f.m_x0;
		m_x1 = f.m_x1;
		m_y0 = f.m_y0;
		m_y1 = f.m_y1;
		m_x_resolution = f.m_x_resolution;
		m_y_resolution = f.m_y_resolution;
		m_values = new float[m_x_resolution*m_y_resolution];
		for(int i=0; i<m_x_resolution * m_y_resolution; i++)
			m_values[i] = f.m_values[i];
    }
    return *this;    // Return ref for multiple assignment
}//end operator=

function2D function2D::operator*(function2D param)
{
	//this is a quick and dirty multiplication method (mostly for images)

	if((m_x_resolution != param.m_x_resolution) || (m_y_resolution != param.m_y_resolution))
		exit(0);

	float* a_bits = m_values;
	float* b_bits = param.getBits();

	function2D result(m_x0, m_x1, m_y0, m_y1, m_x_resolution, m_y_resolution);
	//result.m_values = new T[m_x_resolution * m_y_resolution];

	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
		result.m_values[i] = a_bits[i]*b_bits[i];

	//result.setBits((T*)result_bits);
	return result;
}
function2D function2D::operator*(float param)
{
	function2D result(m_x0, m_x1, m_y0, m_y1, m_x_resolution, m_y_resolution);
	for(int i = 0; i<m_x_resolution*m_y_resolution; i++)
		result.m_values[i] = m_values[i]*param;

	return result;
}

function2D function2D::operator+(function2D param)
{
	function2D result(m_x0, m_x1, m_y0, m_y1, m_x_resolution, m_y_resolution);
	for(int i=0; i<m_x_resolution * m_y_resolution; i++)
		result.m_values[i] = m_values[i] + param.m_values[i];
	return result;
}

function2D function2D::operator+(float param)
{
	function2D result(m_x0, m_x1, m_y0, m_y1, m_x_resolution, m_y_resolution);
	for(int i=0; i<m_x_resolution * m_y_resolution; i++)
		result.m_values[i] = m_values[i] + param;
	return result;
}

function2D function2D::operator-(function2D param)
{
	function2D result(m_x0, m_x1, m_y0, m_y1, m_x_resolution, m_y_resolution);
	for(int i=0; i<m_x_resolution * m_y_resolution; i++)
		result.m_values[i] = m_values[i] - param.m_values[i];
	return result;
}

function2D::function2D(float domain_x0, float domain_x1, float domain_y0, float domain_y1,
											 int x_resolution, int y_resolution)
{
	//set all of the member variables describing the size of the function domain
	m_x0=domain_x0;
	m_y0=domain_y0;
	m_x1=domain_x1;
	m_y1=domain_y1;

	m_x_resolution = x_resolution;
	m_y_resolution = y_resolution;

	//allocate memory for the function
	m_values = new float[x_resolution * y_resolution];
}

function2D::function2D()
{
	m_x0 = 0;
	m_x1 = 0;
	m_y0 = 0;
	m_y1 = 0;
	m_x_resolution = 0;
	m_y_resolution = 0;
	m_values = NULL;
}
float function2D::operator ()(float x, float y)
{
	if(x < m_x0 || x > m_x1)
		return 0.0f;
	if(y < m_y0 || y > m_y1)
		return 0.0f;

	float x_size = m_x1 - m_x0;
	float y_size = m_y1 - m_y0;
	float scaled_x = (x-m_x0)/x_size;
	float scaled_y = (y-m_y0)/y_size;
	int x_index = scaled_x * m_x_resolution;
	int y_index = scaled_y * m_y_resolution;
	return m_values[y_index * m_x_resolution + x_index];
}

float* function2D::getBits()
{
	return m_values;
}

void function2D::setBits(unsigned char* bits)
{
	//copy the given bits to the current function
	//the member m_values can't just be set to bits because the datatypes may be different
	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
		m_values[i] = bits[i];
}

void function2D::setBits(float* bits)
{
	//copy the given bits to the current function
	//the member m_values can't just be set to bits because the datatypes may be different
	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
			m_values[i] = bits[i];
}

void function2D::setBits(double* bits)
{
	//copy the given bits to the current function
	//the member m_values can't just be set to bits because the datatypes may be different
	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
			m_values[i] = bits[i];
}
void function2D::setBits(int* bits)
{
	//copy the given bits to the current function
	//the member m_values can't just be set to bits because the datatypes may be different
	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
		m_values[i] = bits[i];
}



function2D::~function2D()
{
	delete m_values;
}

void function2D::CreateGaussian(float mean_x, float mean_y, float std_dev)
{
	//this function creates a 2D gaussian with a mean at the center of the image
	//and a standard deviation as specified
	//unsigned char* gaussian = new unsigned char[width*height];
	//create a few variables to store details about the function domain
	float domain_x;
	float domain_y;
	float x_domain_length = m_x1 - m_x0;
	float y_domain_length = m_y1 - m_y0;
	//double high_value = (1.0/(2.0*PI*std_dev*std_dev))*exp(0.0);
	//cucle through all values in the functions resolution
	for(int x = 0; x<m_x_resolution; x++)
		for(int y=0; y<m_y_resolution; y++)
		{
			//find the appropriate domain values associated with the array index
			domain_x = ((x/(double)m_x_resolution)*(x_domain_length))+m_x0;
			domain_y = ((y/(double)m_y_resolution)*(y_domain_length))+m_y0;
			double exponent = exp(-((domain_x-mean_x)*(domain_x-mean_x) + (domain_y-mean_y)*(domain_y-mean_y))/(2.0*std_dev*std_dev));
			m_values[y*m_x_resolution+x] = (1.0/(2.0*RTS_PI*std_dev*std_dev))*exponent;
		}
}

void function2D::CreateConstant(float value)
{
	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
	{
		m_values[i] = value;
	}
}

void function2D::CreateCircleMask(float center_x, float center_y, float radius)
{
	float domain_x;
	float domain_y;
	float x_domain_length = m_x1 - m_x0;
	float y_domain_length = m_y1 - m_y0;

	//cycle through all values in the functions resolution
	for(int x = 0; x<m_x_resolution; x++)
		for(int y=0; y<m_y_resolution; y++)
		{
			//find the appropriate domain values associated with the array index
			domain_x = ((x/(double)m_x_resolution)*(x_domain_length))+m_x0;
			domain_y = ((y/(double)m_y_resolution)*(y_domain_length))+m_y0;
			//find the distance between the current point and the circle center
			point3D center(center_x, center_y, 0.0);
			point3D current(domain_x, domain_y, 0.0);
			vector3D difference = current - center;
			float distance = difference.length();

			//if the pixel is inside the circle, set it to 1.0
			//otherwise, set the pixel to 0.0
			if(distance < radius)
				m_values[y*m_x_resolution+x] = 1.0f;
			else
				m_values[y*m_x_resolution+x] = 0.0f;
		}
}

void function2D::Scale(float min, float max)
{
	//this method scales the function so that it exists between the values min and max

	//first, find the minimum and maximum values for the function
	int current_min_index = 0;
	int current_max_index = 0;
	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
	{
		if(m_values[i] < m_values[current_min_index])
			current_min_index = i;
		if(m_values[i] > m_values[current_max_index])
			current_max_index = i;
	}
		//cout<<"current max: "<<m_values[current_max_index]<<endl;
		//cout<<"current min: "<<m_values[current_min_index]<<endl;
		//char c;cin>>c;
	float current_min = m_values[current_min_index];
	float current_max = m_values[current_max_index];
	//now loop through the function again and scale to the appropriate values
	float scaled;
	for(int i=0; i<m_x_resolution * m_y_resolution; i++)
	{
		scaled = ((m_values[i] - current_min) / (current_max - current_min));
		m_values[i] = (scaled *(max-min)) + min;
	}
}

void function2D::Clip(float min, float max)
{
	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
	{
		if(m_values[i] < min)
			m_values[i] = min;
		else if(m_values[i] > max)
			m_values[i] = max;
	}
}

void function2D::Abs()
{
	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
	{
		m_values[i] = fabs(m_values[i]);
	}
}

float function2D::Integral()
{
	//this function returns the sum of all values of the function
	float result=0.0f;
	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
		result+= m_values[i];
	return result;
}

float function2D::Average()
{
	//this function returns the average of all of the values of the function
	float result=0.0f;
	for(int i=0; i<m_x_resolution*m_y_resolution; i++)
		result+= m_values[i];
	result = result/(m_x_resolution*m_y_resolution);
	return result;
}


line3D::line3D()
{
	m_p0 = point3D(0.0, 0.0, 0.0);
	m_p1 = point3D(0.0, 0.0, 0.0);
}

line3D::line3D(point3D p0, point3D p1)
{
	m_p0 = p0;
	m_p1 = p1;
}

point3D line3D::get_point(double pos)
{
	return m_p0 + (m_p1 - m_p0)*pos;
}