#include "matrix.h"
#include "structures.h"
#include <string.h>
#include <math.h>
#include <iostream> 
//RCOO == ROW_COLUMN_ORDER_OFFSET
//colum - СТОЛБЕЦ!
//row - СТРОКА!
#define RCOO(row, column, total_rows) row+column*total_rows

namespace M3D
{
GLMatrix4x4::GLMatrix4x4()
{
	identity();
}
void GLMatrix4x4::rotate( float angle, float x, float y, float z )
{
	float c = cos( angle * GRAD2RAD ), s = sin( angle * GRAD2RAD );
	float cm1 = 1 - c;
	float zs = z * s, xs = x * s, ys = y * s;
	GLMatrix4x4 rot;

	//Сформируем элементы диагонали
	rot._matrix4x4_[ RCOO( 0, 0, 4 ) ] = x * x * cm1 + c;
	rot._matrix4x4_[ RCOO( 1, 1, 4 ) ] = y * y * cm1 + c;
	rot._matrix4x4_[ RCOO( 2, 2, 4 ) ] = z * z * cm1 + c;
	//Верхняя часть над диагональю матрицы
	rot._matrix4x4_[ RCOO( 0, 1, 4 ) ] = x * y * cm1 - zs;
	rot._matrix4x4_[ RCOO( 0, 2, 4 ) ] = x * z * cm1 + ys;
	rot._matrix4x4_[ RCOO( 1, 2, 4 ) ] = y * z * cm1 - xs;
	//Нижняя часть под диагональю матрицы
	rot._matrix4x4_[ RCOO( 1, 0, 4 ) ] = y * x * cm1 + zs;
	rot._matrix4x4_[ RCOO( 2, 0, 4 ) ] = x * z * cm1 - ys;
	rot._matrix4x4_[ RCOO( 2, 1, 4 ) ] = y * z * cm1 + xs;
	( *this ) *= rot;
	this->mult3x3_4x4( rot );
}
void GLMatrix4x4::translate( float x, float y, float z )
{
	GLMatrix4x4 trans;
	trans._matrix4x4_[ RCOO( 0, 3, 4 ) ] = x;
	trans._matrix4x4_[ RCOO( 1, 3, 4 ) ] = y;
	trans._matrix4x4_[ RCOO( 2, 3, 4 ) ] = z;
	( *this ) *= trans;
	return;
}
void GLMatrix4x4::scale( float x, float y, float z )
{
	for ( int a = 0;a < 4;++a )
	{
		_matrix4x4_[ RCOO ( 0, a, 4 ) ] *= x;
		_matrix4x4_[ RCOO ( 1, a, 4 ) ] *= y;
		_matrix4x4_[ RCOO ( 2, a, 4 ) ] *= z;
	}
}
void GLMatrix4x4::zero()
{
	memset ( _matrix4x4_, 0, 16 * sizeof ( float ) );
	memset ( _matrix_3x3_rotate_, 0, 9 * sizeof ( float ) );
}
void GLMatrix4x4::identity()
{
	zero();
	for ( int a = 0;a < 4;++a ) _matrix4x4_[ RCOO ( a, a, 4 ) ] = 1;
	for ( int a = 0;a < 3;++a ) _matrix_3x3_rotate_[ RCOO ( a, a, 3 ) ] = 1;
}
void GLMatrix4x4::operator*=( const GLMatrix4x4 &matrix )
{
	float tmp[ 16 ];

	for ( int a = 0;a < 4;++a )
	{
		tmp[ RCOO ( 0, a, 4 ) ] = _matrix4x4_[ RCOO ( 0, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 0, 0, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 1, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 0, 1, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 2, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 0, 2, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 3, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 0, 3, 4 ) ];
		tmp[ RCOO ( 1, a, 4 ) ] = _matrix4x4_[ RCOO ( 0, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 1, 0, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 1, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 1, 1, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 2, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 1, 2, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 3, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 1, 3, 4 ) ];
		tmp[ RCOO ( 2, a, 4 ) ] = _matrix4x4_[ RCOO ( 0, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 2, 0, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 1, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 2, 1, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 2, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 2, 2, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 3, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 2, 3, 4 ) ];
		tmp[ RCOO ( 3, a, 4 ) ] = _matrix4x4_[ RCOO ( 0, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 3, 0, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 1, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 3, 1, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 2, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 3, 2, 4 ) ] +
		                          _matrix4x4_[ RCOO ( 3, a, 4 ) ] * matrix._matrix4x4_[ RCOO ( 3, 3, 4 ) ];
	}
	memcpy( _matrix4x4_, tmp, 16 * sizeof( float ) );
}
void GLMatrix4x4::mult3x3_4x4( const GLMatrix4x4 &matrix )
{
	float tmp[ 9 ];

	for ( int a = 0;a < 3;++a )
	{
		tmp[ RCOO ( 0, a, 3 ) ] =
		    _matrix_3x3_rotate_[ RCOO ( 0, a, 3 ) ] * matrix._matrix4x4_[ RCOO ( 0, 0, 4 ) ] +
		    _matrix_3x3_rotate_[ RCOO ( 1, a, 3 ) ] * matrix._matrix4x4_[ RCOO ( 0, 1, 4 ) ] +
		    _matrix_3x3_rotate_[ RCOO ( 2, a, 3 ) ] * matrix._matrix4x4_[ RCOO ( 0, 2, 4 ) ];
		tmp[ RCOO ( 1, a, 3 ) ] =
		    _matrix_3x3_rotate_[ RCOO ( 0, a, 3 ) ] * matrix._matrix4x4_[ RCOO ( 1, 0, 4 ) ] +
		    _matrix_3x3_rotate_[ RCOO ( 1, a, 3 ) ] * matrix._matrix4x4_[ RCOO ( 1, 1, 4 ) ] +
		    _matrix_3x3_rotate_[ RCOO ( 2, a, 3 ) ] * matrix._matrix4x4_[ RCOO ( 1, 2, 4 ) ];
		tmp[ RCOO ( 2, a, 3 ) ] =
		    _matrix_3x3_rotate_[ RCOO ( 0, a, 3 ) ] * matrix._matrix4x4_[ RCOO ( 2, 0, 4 ) ] +
		    _matrix_3x3_rotate_[ RCOO ( 1, a, 3 ) ] * matrix._matrix4x4_[ RCOO ( 2, 1, 4 ) ] +
		    _matrix_3x3_rotate_[ RCOO ( 2, a, 3 ) ] * matrix._matrix4x4_[ RCOO ( 2, 2, 4 ) ];
	}
	memcpy( _matrix_3x3_rotate_, tmp, 9 * sizeof( float ) );
}
void GLMatrix4x4::projection( const CameraParams &params )
{
	GLMatrix4x4 proj;
	float z_nf = ( params.z_near - params.z_far );
	float f = 1.0 / ( tan ( params.fov_y / 2. ) );
	
	proj._matrix4x4_[ RCOO( 0, 0, 4 ) ] = f / params.aspect;
	proj._matrix4x4_[ RCOO( 1, 1, 4 ) ] = f;
	proj._matrix4x4_[ RCOO( 2, 2, 4 ) ] = ( params.z_far + params.z_near ) / z_nf;
	proj._matrix4x4_[ RCOO( 2, 3, 4 ) ] = ( 2 * params.z_far * params.z_near ) / z_nf;
	proj._matrix4x4_[ RCOO( 3, 2, 4 ) ] = -1;
	proj._matrix4x4_[ RCOO( 3, 3, 4 ) ] = 0;
	( *this ) *= proj;
}
void GLMatrix4x4::print()
{
	for ( int a = 0;a < 4;++a )
	{
		std::cerr << "[";
		for ( int b = 0;b < 4;b++ )
		{
			std::cerr << _matrix4x4_[ RCOO( a, b, 4 ) ] << " ";
		}
		std::cerr << "]" << std::endl;
	}
}
}
