#pragma once

#include "Vector.hpp"
#include "Float.h"
#include <math.h>

typedef float m3x3Type[3][3];

/*!
\brief 3D linear transformation + translation class.
Used by the ray-tracing system to store entity modelling transformations. Class Transformed is an Intersecable 
that refers to an Intersectable and contains a Transformation.
*/
class Transformation {
public:
	float m[3][3];
	float t[3];

	Transformation(std::istream& isc)
	{
		for(int i=0;i < 3;i++)
			for(int j=0;j < 3;j++)
				isc >> m[i][j];
		for(int ti=0;ti < 3;ti++)
				isc >> t[ti];
	}

	Transformation()
	{
		m[0][0] = m[1][1] = m[2][2] = 1.0f;
		m[0][1] = m[0][2] = m [1][0] = m[1][2] = m[2][0] = m[2][1] = 0.0f;
		t[0] = t[1] = t[2] = 0.0f;
	}
	
	inline void fill (float m00, float m01, float m02,
					float m10, float m11, float m12,
					float m20, float m21, float m22,
					float t0, float t1, float t2){
/*		m[0][0] = m00;
		m[0][1] = m01;
		m[0][2] = m02;

		m[1][0] = m10;
		m[1][1] = m11;
		m[1][2] = m12;

		m[2][0] = m20;
		m[2][1] = m21;
		m[2][2] = m22;*/
		
		m[0][0] = m00;
		m[1][0] = m01;
		m[2][0] = m02;

		m[0][1] = m10;
		m[1][1] = m11;
		m[2][1] = m12;

		m[0][2] = m20;
		m[1][2] = m21;
		m[2][2] = m22;

		t[0] = t0;
		t[1] = t1;
		t[2] = t2;
	}

	inline void setInvert (Transformation& mA);
	inline void transformPoint (const Vector& vIn, Vector& vOut) const;
	inline void transformDirection (const Vector& vIn, Vector& vOut) const;
	inline void transformPointTransposed (const Vector& vIn, Vector& vOut) const;
	inline void transformDirectionTransposed (const Vector& vIn, Vector& vOut) const;
	inline void rotateX(float angle);
	inline void rotateY(float angle);
	inline void rotateZ(float angle);
	inline void scale(float factor);
};


void Transformation::setInvert (Transformation& mA)
{
	m3x3Type& A = mA.m;
	
	// generated by maple  C(A_inv,optimized);       
	float		t4 = A[0][0]*A[1][1];
	float 	    t6 = A[0][0]*A[1][2];
	float 	    t8 = A[0][1]*A[1][0];
	float 	    t10 = A[0][2]*A[1][0];
	float 	    t12 = A[0][1]*A[2][0];
	float 	    t14 = A[0][2]*A[2][0];
	float 	    t17 = 1/(t4*A[2][2]-t6*A[2][1]-t8*A[2][2]+t10*A[2][1]+t12*A[1][2]-t14*A[1][1]);
	
//	assert (!_isnan (t17));
	
	m[0][0] = (A[1][1]*A[2][2]-A[1][2]*A[2][1])*t17;
	m[0][1] = -(A[0][1]*A[2][2]-A[0][2]*A[2][1])*t17;
	m[0][2] = -(-A[0][1]*A[1][2]+A[0][2]*A[1][1])*t17;
	m[1][0] = -(A[1][0]*A[2][2]-A[1][2]*A[2][0])*t17;
	m[1][1] = (A[0][0]*A[2][2]-t14)*t17;
	m[1][2] = -(t6-t10)*t17;
	m[2][0] = -(-A[1][0]*A[2][1]+A[1][1]*A[2][0])*t17;
	m[2][1] = -(A[0][0]*A[2][1]-t12)*t17;
	m[2][2] = (t4-t8)*t17;

	t[0] = -(mA.t[0] * m[0][0] + mA.t[1] * m[0][1] + mA.t[2] * m[0][2]);
	t[1] = -(mA.t[0] * m[1][0] + mA.t[1] * m[1][1] + mA.t[2] * m[1][2]);
	t[2] = -(mA.t[0] * m[2][0] + mA.t[1] * m[2][1] + mA.t[2] * m[2][2]);

}

//! vOut = Matrix * vIn,  matrix is on the left side
inline void Transformation::transformPoint (const Vector& vIn, Vector& vOut) const
{
	vOut.x = vIn.x * m[0][0] + vIn.y * m[0][1] + vIn.z * m[0][2] + t[0];
	vOut.y = vIn.x * m[1][0] + vIn.y * m[1][1] + vIn.z * m[1][2] + t[1];
	vOut.z = vIn.x * m[2][0] + vIn.y * m[2][1] + vIn.z * m[2][2] + t[2];
}

//! vOut = vIn * Matrix,  matrix is on the right side
inline void Transformation::transformPointTransposed (const Vector& vIn, Vector& vOut) const
{
	vOut.x = vIn.x * m[0][0] + vIn.y * m[1][0] + vIn.z * m[2][0] + t[0];
	vOut.y = vIn.x * m[0][1] + vIn.y * m[1][1] + vIn.z * m[2][1] + t[1];
	vOut.z = vIn.x * m[0][2] + vIn.y * m[1][2] + vIn.z * m[2][2] + t[2];
}

//! vOut = Matrix * vIn,  matrix is on the left side
inline void Transformation::transformDirection (const Vector& vIn, Vector& vOut) const
{
	vOut.x = vIn.x * m[0][0] + vIn.y * m[0][1] + vIn.z * m[0][2];
	vOut.y = vIn.x * m[1][0] + vIn.y * m[1][1] + vIn.z * m[1][2];
	vOut.z = vIn.x * m[2][0] + vIn.y * m[2][1] + vIn.z * m[2][2];
}

//! vOut = vIn * Matrix,  matrix is on the right side
inline void Transformation::transformDirectionTransposed (const Vector& vIn, Vector& vOut) const
{
	vOut.x = vIn.x * m[0][0] + vIn.y * m[1][0] + vIn.z * m[2][0];
	vOut.y = vIn.x * m[0][1] + vIn.y * m[1][1] + vIn.z * m[2][1];
	vOut.z = vIn.x * m[0][2] + vIn.y * m[1][2] + vIn.z * m[2][2];
}

inline void Transformation::rotateZ(float angle)
{
	float cosx = cosf(angle);
	float sinx = sinf(angle);

	float tmp = m[0][0];
	m[0][0] = cosx * tmp - sinx * m[1][0];
	m[1][0] = sinx * tmp + cosx * m[1][0];

	tmp = m[0][1];
	m[0][1] = cosx * tmp - sinx * m[1][1];
	m[1][1] = sinx * tmp + cosx * m[1][1];

	tmp = m[0][2];
	m[0][2] = cosx * tmp - sinx * m[1][2];
	m[1][2] = sinx * tmp + cosx * m[1][2];

	tmp = t[0];
	t[0] = cosx * tmp - sinx * t[1];
	t[1] = sinx * tmp + cosx * t[1];
}

inline void Transformation::rotateY(float angle)
{
	float cosx = cosf(angle);
	float sinx = sinf(angle);

	float tmp = m[0][0];
	m[0][0] = cosx * tmp - sinx * m[2][0];
	m[2][0] = sinx * tmp + cosx * m[2][0];

	tmp = m[0][2];
	m[0][2] = cosx * tmp - sinx * m[2][2];
	m[2][2] = sinx * tmp + cosx * m[2][2];

	tmp = m[0][1];
	m[0][1] = cosx * tmp - sinx * m[2][1];
	m[2][1] = sinx * tmp + cosx * m[2][1];

	tmp = t[0];
	t[0] = cosx * tmp - sinx * t[2];
	t[2] = sinx * tmp + cosx * t[2];
}

inline void Transformation::rotateX(float angle)
{
	float cosx = cosf(angle);
	float sinx = sinf(angle);

	float tmp = m[1][1];
	m[1][1] = cosx * tmp - sinx * m[2][1];
	m[2][1] = sinx * tmp + cosx * m[2][1];

	tmp = m[1][2];
	m[1][2] = cosx * tmp - sinx * m[2][2];
	m[2][2] = sinx * tmp + cosx * m[2][2];

	tmp = m[1][0];
	m[1][0] = cosx * tmp - sinx * m[2][0];
	m[2][0] = sinx * tmp + cosx * m[2][0];

	tmp = t[1];
	t[1] = cosx * tmp - sinx * t[2];
	t[2] = sinx * tmp + cosx * t[2];
}

inline void Transformation::scale(float factor)
{

	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
			m[i][j] *= factor;
		t[i] *= factor;
	}
	
}