//
// matrix.cc
//
// Handle matrices
//
// Copyright (C) J. Belson	1998.01.21
//

#include <iomanip>

#include "matrix.h"


/**
 * Constructor
 * Create a matrix using data in the form used by OpenGL, which
 *  is unfortunately different to our format:
 *
 * 0   4   8  12
 * 1   5   9  13
 * 2   6  10  14
 * 3   7  11  15
 */
matrix::matrix(float *m)
{
	for (int i=0; i<NUM_ELEMENTS; i++) {
		//elem[i] = m[i];
		elem[i] = m[((i%4)<<2) + i/4];
	}
}


/**
 * Constructor
 */
matrix::matrix()
{
	;
}


/**
 * Constructor
 * @param t	type of matrix to be created
 * @param a argument for matrix construction
 * @param b argument for matrix construction
 * @param c argument for matrix construction
 */
matrix::matrix(matrix_type t, float a, float b, float c)
{
	if (MATRIX_WIDTH != 4) {
		std::cout << "Warning - other classes require matrix to be 4x4" << std::endl;
	}

	switch (t) {
		case IDENTITY:
			make_identity();
			break;

		case ROTATION:
			make_rotation(a, b, c);
			break;

		case TRANSLATION:
			make_translation(a, b, c);
			break;

		case PROJECTION:
			make_projection(a);
			break;

		case SCALING:
			make_scaling(a, b, c);
			break;
	}

}


/**
 * Construct a matrix from three vectors
 */
matrix::matrix(const vector3d& v1, const vector3d& v2, const vector3d& v3)
{
	make_identity();

	elem[0 + 0] = v1.i;
	elem[0 + 4] = v1.j;
	elem[0 + 8] = v1.k;

	elem[1 + 0] = v2.i;
	elem[1 + 4] = v2.j;
	elem[1 + 8] = v2.k;

	elem[2 + 0] = v3.i;
	elem[2 + 4] = v3.j;
	elem[2 + 8] = v3.k;
}


/**
 * Make this matrix an indentity matrix
 */
void matrix::make_identity()
{
	for (int i=0; i<MATRIX_WIDTH; i++) {
		for (int j=0; j<MATRIX_WIDTH; j++) {
			elem[i*MATRIX_WIDTH + j] = (i==j) ? 1.0 : 0.0;
		}
	}
}


/**
 * Multiply together two matrices.
 * This is an O(n^3) operation.
 */
matrix operator*(matrix &m, matrix &n)
{
	matrix new_m;

	for (int i=0; i<MATRIX_WIDTH; i++) {
		for (int j=0; j<MATRIX_WIDTH; j++) {
			double sum = 0.0;
			for (int k=0; k<MATRIX_WIDTH; k++) {
				sum += m[k*MATRIX_WIDTH + i] * n[j*MATRIX_WIDTH + k];
			}
			new_m[j*MATRIX_WIDTH + i] = sum;
		}
	}
	return new_m;
}



#if 0
matrix matrix::operator*(const matrix &m)
{
	matrix new_m;
	for (int i=0; i<MATRIX_WIDTH; i++) {
		for (int j=0; j<MATRIX_WIDTH; j++) {
			double sum = 0.0;
			for (int k=0; k<MATRIX_WIDTH; k++) {
				sum += elem[k*MATRIX_WIDTH + i] * m[j*MATRIX_WIDTH + k];
			}
			new_m[j*MATRIX_WIDTH + i] = sum;
		}
	}
	return new_m;
}
#endif


/**
 * Assign to this matrix
 */
matrix matrix::operator=(const matrix& m)
{
	for (int i=0; i<NUM_ELEMENTS; i++) {
		elem[i] = m[i];
	}

	return *this;
}


/**
 * Add together two matrices
 */
matrix operator+(matrix &m, const matrix &n)
{
	matrix new_m;
	for (int i=0; i<NUM_ELEMENTS; i++) {
		new_m[i] = m[i] + n[i];
	}
	return new_m;
}


/**
 * Subtract one matrix from another
 */
matrix operator-(matrix &m, const matrix &n)
{
	matrix new_m;
	for (int i=0; i<NUM_ELEMENTS; i++) {
		new_m[i] = m[i] - n[i];
	}
	return new_m;
}


/**
 * Reference an element of this matrix
 */
float &matrix::operator[](int index)
{
	if (index >= 0 && index <= NUM_ELEMENTS) { 
		return elem[index];
	} else {
		std::cerr << "operator[] index out of range!" << std::endl;
		return elem[0];
	}
}


/**
 * Reference an element of this matrix
 */
const float &matrix::operator[](int index) const
{
	if (index >= 0 && index <= NUM_ELEMENTS) {
		return elem[index];
	} else {
		std::cerr << "operator[] index out of range!" << std::endl;
		return elem[0];
	}
}


/**
 * Make this matrix a rotation matrix
 */
void matrix::make_rotation(float a, float b, float c)
{
	make_identity();

	if (a != 0.0) {
		matrix m(IDENTITY);
		m[5]  =  cos(a);
		m[6]  = -sin(a);
		m[9]  =  sin(a);
		m[10] =  cos(a);
		*this = m * *this;
	}
	if (b != 0.0) {
		matrix m(IDENTITY);
		m[0]  =  cos(b);
		m[2]  =  sin(b);
		m[8]  = -sin(b);
		m[10] =  cos(b);
		*this = m * *this;
	}
	if (c != 0.0) {
		matrix m(IDENTITY);
		m[0] =  cos(c);
		m[1] = -sin(c);
		m[4] =  sin(c);
		m[5] =  cos(c);
		*this = m * *this;
	}
}


/**
 * Make this matrix a translation matrix
 */
void matrix::make_translation(float x, float y, float z)
{
	make_identity();

	elem[3]  = x;
	elem[7]  = y;
	elem[11] = z;
}


/**
 * Make this matrix a perspective projection matrix
 */
void matrix::make_projection(float depth)
{
	make_identity();

	elem[14] = -1.0/depth;
	elem[15] = 0.0;
}


/**
 * Make this matrix a scaling matrix
 */
void matrix::make_scaling(float a, float b, float c)
{
	make_identity();

	elem[0] = a;
	elem[5] = b;
	elem[10] = c;
}


/**
 * Transpose this matrix
 */
matrix matrix::transpose(void)
{
	matrix m;

	for (int i=0; i<MATRIX_WIDTH; i++) {
		for (int j=0; j<MATRIX_WIDTH; j++) {
			m.elem[i*MATRIX_WIDTH + j] = elem[j*MATRIX_WIDTH + i];
		}
	}

	return m;
}


/**
 * Apply this matrix to a point
 */
point3d matrix::operator*(const point3d& p)
{
	point3d result = p;
	result.set(	p.x*elem[0]  + p.y*elem[1]  + p.z*elem[2]  + 1*elem[3],
				p.x*elem[4]  + p.y*elem[5]  + p.z*elem[6]  + 1*elem[7],
				p.x*elem[8]  + p.y*elem[9]  + p.z*elem[10] + 1*elem[11]
				);

	return result;
}


/**
 * Apply this matrix to a vector
 */
vector3d matrix::operator*(const vector3d& v)
{
	vector3d result(v.i*elem[0]  + v.j*elem[1]  + v.k*elem[2]  + 1*elem[3],
					v.i*elem[4]  + v.j*elem[5]  + v.k*elem[6]  + 1*elem[7],
					v.i*elem[8]  + v.j*elem[9]  + v.k*elem[10] + 1*elem[11]
					);

	return result;
}


/**
 * Apply a scaling factor to this matrix
 */
matrix matrix::operator*(float scale)
{
	matrix m;
	for (int i=0; i<NUM_ELEMENTS; i++) {
		m.elem[i] = elem[i]*scale;
	}

	return m;
}


/**
 * Dump matrix elements to stdout
 */
void matrix::dump(void)
{
	using namespace std;

	cout << std::setprecision(4);

	for (int i=0; i<NUM_ELEMENTS; i++) {
		if (i && !(i%4)) cout << endl;
		cout.width(4);
		//cout << setfill('#');
		cout << elem[i] << "\t";;
	}
	cout << endl << endl;
}