/// there is some type of alignment issue with my mat4 and the aimatrix4x4
/// class, so the copy must be done manually
void TransformMatrix(const aiMatrix4x4& in, mat4<F32>& out, bool rowMajor) {
    if (rowMajor) {
        out.set({ in.a1, in.a2, in.a3, in.a4,
                  in.b1, in.b2, in.b3, in.b4,
                  in.c1, in.c2, in.c3, in.c4,
                  in.d1, in.d2, in.d3, in.d4 });
    } else {
        out.set({ in.a1, in.b1, in.c1, in.d1,
                  in.a2, in.b2, in.c2, in.d2,
                  in.a3, in.b3, in.c3, in.d3,
                  in.a4, in.b4, in.c4, in.d4 });
    }
}

void Init()
{
	// create a visualiser
	Visualiser::Create();

	// setup matrices
	g_CameraMatrix.SetFrame( vec4(0,10,-10,1), vec4(0,-1,1,0), vec4(0,1,0,0));
	g_ProjectionMatrix.Perspective(PI/6, 1200/720.0f, 0.1f, 100);
//	g_ProjectionMatrix.Orthographic(1280,720,0.1f,100);
	g_ViewMatrix = g_CameraMatrix.ToViewMatrix();
	g_ModelMatrix = mat4(1,0,0,0,
						0,1,0,0,
						0,0,1,0,
						0,0,0,1);

	glEnable(GL_DEPTH_TEST);
	// load shader
	const char* aszInputs[] = { "Position",	"UV" };
	const char* aszOutputs[] = { "outColour" };
	g_ShaderID = LoadShader( 2, aszInputs, 1, aszOutputs,
		"./shaders/vertex.glsl",
		"./shaders/pixel.glsl");

	// build 2-triangle plane
	float fPlaneSize = 2.0f;
	Build3DPlane(fPlaneSize,g_VAO,g_VBO,g_IBO);

	// load texture
	g_TextureID = LoadTexture("./images/crate_sideup.png", GL_BGRA);

	// set matrix uniforms within the shaders
	GLuint ProjectionID = glGetUniformLocation(g_ShaderID,"Projection");
	GLuint ViewID = glGetUniformLocation(g_ShaderID,"View");
	GLuint ModelID = glGetUniformLocation(g_ShaderID,"Model");

	glUniformMatrix4fv(ProjectionID, 1, false, g_ProjectionMatrix);
	glUniformMatrix4fv(ViewID, 1, false, g_ViewMatrix);
	glUniformMatrix4fv(ModelID, 1, false, g_ModelMatrix);

	// set the texture to use slot 0 in the shader
	GLuint texUniformID = glGetUniformLocation(g_ShaderID,"diffuseTexture");
	glUniform1i(texUniformID,0);
	
	// set clear colour
	glClearColor(0.25f,0.25f,0.25f,1);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_CULL_FACE);

	// start our timer
	AIE::ResetTimer();
}

void AnimationSequence::calculateFOV(mat4 &mat, float &lxMax, float &lyMax) const
{
	vec3 vert1, vert2;

	vector<Mesh*> s = getFrame();

	// for each mesh
	for(vector<Mesh*>::iterator i=s.begin(); i!=s.end(); ++i)
	{
		const Mesh *mesh = (*i);

		// For each vertex
		for(int i=0; i<mesh->m_numOfVerts; ++i)
		{
			// Transform the vertex by the matrix
			vert1 = mesh->m_pVerts[i];
			vert1.w = 1.0f;
			vert2 = mat.transformVector(vert1);

			// Calculate the spread, keep the max
			lxMax = max(lxMax, fabsf(vert2.x / vert2.z));
			lyMax = max(lyMax, fabsf(vert2.y / vert2.z));
		}
	}
}

void CameraPathControls::draw(const mat4& projection, const mat4 views[],
                                   const vec3& lightPositionWorld,
                                   const int& selectedControlPoint,
                                   float deltaT) const {

    /// Common drawing.
    predraw();
	
    /// Update the model matrix.
    static float angle = 1.0f;
    angle += deltaT * 1.0f;
    mat4 rotationMatrix = mat4::Identity();
    rotationMatrix(0,0) = +std::cos(angle);
    rotationMatrix(0,1) = -std::sin(angle);
    rotationMatrix(1,0) = +std::sin(angle);
    rotationMatrix(1,1) = +std::cos(angle);

    /// Update the content of the uniforms.
    glUniformMatrix4fv( _projectionID, 1, GL_FALSE, projection.data());
    glUniformMatrix4fv( _viewID, 1, GL_FALSE, views[0].data());
    glUniform3fv(_lightPositionWorldID, 1, lightPositionWorld.data());
    glUniform1i( _selectedControlPointID, selectedControlPoint);
    glUniformMatrix4fv(_rotationMatrixID, 1, GL_FALSE, rotationMatrix.data());

    /// Render from camera point of view to 'normal' FBOs.
    glBindFramebuffer(GL_FRAMEBUFFER, framebufferIDs["controllerView"]);
    _vertices->draw();

}

mat4 transpose( const mat4& m )
{
	mat4 res;

#ifdef SLMATH_SSE2_MSVC
    
	const m128_t* const mp = m.m128();
	m128_t* const resp = res.m128();
    m128_t tmp0 = _mm_shuffle_ps(mp[0], mp[1], 0x44);
    m128_t tmp2 = _mm_shuffle_ps(mp[0], mp[1], 0xEE);
    m128_t tmp1 = _mm_shuffle_ps(mp[2], mp[3], 0x44);
    m128_t tmp3 = _mm_shuffle_ps(mp[2], mp[3], 0xEE);
	resp[0] = _mm_shuffle_ps(tmp0, tmp1, 0x88);
    resp[1] = _mm_shuffle_ps(tmp0, tmp1, 0xDD);
    resp[2] = _mm_shuffle_ps(tmp2, tmp3, 0x88);
    resp[3] = _mm_shuffle_ps(tmp2, tmp3, 0xDD);

#else
	
	for ( size_t j = 0 ; j < 4 ; ++j )
	{
		res[0][j] = m[j][0];
		res[1][j] = m[j][1];
		res[2][j] = m[j][2];
		res[3][j] = m[j][3];
	}

#endif

	return res;
}

void et::decomposeMatrix(const mat4& mat, vec3& translation, quaternion& rotation, vec3& scale)
{
	mat3 rot = mat.mat3();
	translation = mat[3].xyz();
	scale = removeMatrixScale(rot);
	rotation = matrixToQuaternion(rot);
}

float Trans_Mesh::Ray_Tri_Intersect(const vec3& rayorig, const vec3& raydir, mat4& world, uint16_t startindex, uint16_t numindices) const{
	world.inverse();
	vec3 org(rayorig*world), di(raydir);// transform these to the mesh's space so the checks are in object space, not world space
	TransformNormal(di, world);
	// do all checks in OBJECT SPACE!!!
	di*=20000.0f;//make sure the ray is long enough
	return RayTriangleIntersect(org, di, &Vertices[0], &Indices[startindex],numindices);
}

void Geode::draw(mat4 C) {
	glMatrixMode(GL_MODELVIEW); //set to modelview
	glPushMatrix(); //conserve the matrix
	
	glMultMatrixf( C.makeTranspose().ptr() );
	render(); //render the shape
	
	glPopMatrix();
}

void Graphics::setMatrix(ConstantLocation location, const mat4& value) {
	if (location.shaderType == -1) return;
	float floats[16];
	for (int y = 0; y < 4; ++y) {
		for (int x = 0; x < 4; ++x) {
			floats[y * 4 + x] = value.get(y, x);
		}
	}
	if (location.shaderType == 0) device->SetVertexShaderConstantF(location.reg.regindex, floats, 4);
	else device->SetPixelShaderConstantF(location.reg.regindex, floats, 4);
}

void Program::setUniformDirectly(int nLoc, uint32_t type, const mat4& value)
{
#if !defined(ET_CONSOLE_APPLICATION)
	if (nLoc == -1) return;
	
	(void)type;
	ET_ASSERT(type == GL_FLOAT_MAT4);
	ET_ASSERT(apiHandleValid());
	
	glUniformMatrix4fv(nLoc, 1, 0, value.data());
	checkOpenGLError("glUniformMatrix4fv");
#endif
}

/* order.                                                              */
vec4 operator* (const vec4& v, const mat4& m)
{
	vec4 product = vec4();
	
	/* Multiply the rows of m by the vector to get the products components. */
	for(int i = 0; i < 4; i++)
	{
		product[i] = m.getColumn(i) * v;
	}

	/* Return the new product vector*/
	return product;
}

void
BoundingBox::setTransform (mat4 &m)
{
	m_GeometryTransform.copy(m);

	std::vector<vec4> vertices (8);

	vertices[0].set (m_vLocalPoints[MIN].x, m_vLocalPoints[MIN].y, m_vLocalPoints[MIN].z);
	vertices[1].set (m_vLocalPoints[MAX].x, m_vLocalPoints[MIN].y, m_vLocalPoints[MIN].z);
	vertices[2].set (m_vLocalPoints[MAX].x, m_vLocalPoints[MIN].y, m_vLocalPoints[MAX].z);
	vertices[3].set (m_vLocalPoints[MIN].x, m_vLocalPoints[MIN].y, m_vLocalPoints[MAX].z);

	vertices[4].set (m_vLocalPoints[MIN].x, m_vLocalPoints[MAX].y, m_vLocalPoints[MIN].z);
	vertices[5].set (m_vLocalPoints[MAX].x, m_vLocalPoints[MAX].y, m_vLocalPoints[MIN].z);
	vertices[6].set (m_vLocalPoints[MAX].x, m_vLocalPoints[MAX].y, m_vLocalPoints[MAX].z);
	vertices[7].set (m_vLocalPoints[MIN].x, m_vLocalPoints[MAX].y, m_vLocalPoints[MAX].z);

	//vertices[0].set (m_vPoints[MIN].x, m_vPoints[MIN].y, m_vPoints[MIN].z);
	//vertices[1].set (m_vPoints[MAX].x, m_vPoints[MIN].y, m_vPoints[MIN].z);
	//vertices[2].set (m_vPoints[MAX].x, m_vPoints[MIN].y, m_vPoints[MAX].z);
	//vertices[3].set (m_vPoints[MIN].x, m_vPoints[MIN].y, m_vPoints[MAX].z);

	//vertices[4].set (m_vPoints[MIN].x, m_vPoints[MAX].y, m_vPoints[MIN].z);
	//vertices[5].set (m_vPoints[MAX].x, m_vPoints[MAX].y, m_vPoints[MIN].z);
	//vertices[6].set (m_vPoints[MAX].x, m_vPoints[MAX].y, m_vPoints[MAX].z);
	//vertices[7].set (m_vPoints[MIN].x, m_vPoints[MAX].y, m_vPoints[MAX].z);

	for (int i = 0; i < 8; i++) {
		m.transform (vertices[i]);
	}

	//aTransform.getMat44().transform (m_vPoints[MIN]);
	//aTransform.getMat44().transform (m_vPoints[MAX]);
	//aTransform.getMat44().transform (m_vPoints[CENTER]);

	//std::vector<vec3> vertices(3);

	//for (int i = MIN; i <= CENTER; i++) {
	//	vertices[i] = m_vPoints[i];
	//}

	// Need to preserve local points
	vec3 auxMin, auxMax;
	auxMin = m_vLocalPoints[MIN];
	auxMax = m_vLocalPoints[MAX];

	calculate (vertices);

	m_vLocalPoints[MIN] = auxMin;
	m_vLocalPoints[MAX] = auxMax;
}

void Shadow::calculateMatrices(const Light &light, const Actor &actor, int shadowMapSize, mat4& lightProjectionMatrix, mat4& lightViewMatrix, mat4& textureMatrix, float lx, float ly)
{
	const vec3 &lightPosition = light.getPosition();
	const vec3 &lightCenter = actor.getPos();
	const vec3 lightUp(0.0f, 1.0f, 0.0f);

// Calculate the model-view matrix
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();
	gluLookAt(	lightPosition.x,	lightPosition.y,	lightPosition.z,
				lightCenter.x,		lightCenter.y,		lightCenter.z,
				lightUp.x,			lightUp.y,			lightUp.z);
	lightViewMatrix.zero();
	glGetFloatv(GL_MODELVIEW_MATRIX, lightViewMatrix);
	glPopMatrix();

// Calculate the projection matrix
	// row 1
	lightProjectionMatrix.m[0] = (0.995f-1.0f/(float)shadowMapSize)/lx;
	lightProjectionMatrix.m[1] = 0.0f;
	lightProjectionMatrix.m[2] = 0.0f;
	lightProjectionMatrix.m[3] = 0.0f;
	// row 2
	lightProjectionMatrix.m[4] = 0.0f;
	lightProjectionMatrix.m[5] = (0.995f-1.0f/(float)shadowMapSize)/ly;
	lightProjectionMatrix.m[6] = 0.0f;
	lightProjectionMatrix.m[7] = 0.0f;
	// row 3
	lightProjectionMatrix.m[8]  = 0.0f;
	lightProjectionMatrix.m[9]  = 0.0f;
	lightProjectionMatrix.m[10] = (farClip + nearClip) / (nearClip - farClip);
	lightProjectionMatrix.m[11] = -1.0f;
	// row 4
	lightProjectionMatrix.m[12] = 0.0f;
	lightProjectionMatrix.m[13] = 0.0f;
	lightProjectionMatrix.m[14] = 2.0f * (farClip * nearClip) / (nearClip - farClip);
	lightProjectionMatrix.m[15] = 0.0f;

	//Calculate texture matrix for projection
	//This matrix takes us from eye space to the light's clip space
	//It is postmultiplied by the inverse of the current view matrix when specifying texgen
	const float biasMatrixf[] = {	0.5f, 0.0f, 0.0f, 0.0f,
						0.0f, 0.5f, 0.0f, 0.0f,
						0.0f, 0.0f, 0.5f, 0.0f,
						0.5f, 0.5f, 0.5f, 1.0f};	//bias from [-1, 1] to [0, 1]
	mat4 biasMatrix = biasMatrixf;

	// Calculate the texture matrix
	textureMatrix = biasMatrix * lightProjectionMatrix * lightViewMatrix;
}

 mat4 operator*(const mat4& value) const
 {
     mat4 right = value.transpose();
     return mat4(
        vec4(m[0].dot(right.m[0]), m[0].dot(right.m[1]),
             m[0].dot(right.m[2]), m[0].dot(right.m[3])),
        vec4(m[1].dot(right.m[0]), m[1].dot(right.m[1]),
             m[1].dot(right.m[2]), m[1].dot(right.m[3])),
        vec4(m[2].dot(right.m[0]), m[2].dot(right.m[1]),
             m[2].dot(right.m[2]), m[2].dot(right.m[3])),
        vec4(m[3].dot(right.m[0]), m[3].dot(right.m[1]),
             m[3].dot(right.m[2]), m[3].dot(right.m[3]))
    );
 }

void RenderingEngine::Render(float theta) const
{
    const float distance = 10;
    const vec3 target(0, -0.15, 0);
    const vec3 up(0, 1, 0);

    vec3 eye(0, -0.15, distance * 2);
    mat4 view = mat4::LookAt(eye, target, up);
    
    glUseProgram(m_simple.Program);
    glUniformMatrix4fv(m_simple.Uniforms.Modelview, 1, 0, view.Pointer());
    glDepthFunc(GL_ALWAYS);
    glBindTexture(GL_TEXTURE_2D, m_textures.Metal);
    
    RenderDrawable(m_quad, m_simple);
        
    eye = vec3(0, 0, distance);
    view = mat4::LookAt(eye, target, up);
    
    const mat4 model = mat4::RotateY(theta * 180.0f / 3.14f);
    const mat3 model3x3 = model.ToMat3();
    const mat4 modelview = model * view;

    vec4 eyeWorldSpace(0, 0, -10, 1);
    vec4 eyeObjectSpace = model * eyeWorldSpace;

    glUseProgram(m_cubemap.Program);
    glUniform3fv(m_cubemap.Uniforms.EyePosition, 1, eyeObjectSpace.Pointer());
    glUniformMatrix4fv(m_cubemap.Uniforms.Modelview, 1, 0, modelview.Pointer());
    glUniformMatrix3fv(m_cubemap.Uniforms.Model, 1, 0, model3x3.Pointer());
    glBindTexture(GL_TEXTURE_CUBE_MAP, m_textures.Cubemap);
    glEnableVertexAttribArray(m_cubemap.Attributes.Normal);
    glDepthFunc(GL_LESS);
    
    RenderDrawable(m_kleinBottle, m_cubemap);
}

/* Multiply the given two 3x3 matrices and return the new matrix. */
mat4 operator* (const mat4& m1, const mat4& m2)
{
	mat4 product = mat4();
	
	/* Multiply the rows of m1 times the columns of m2. */
	for(int r = 0; r < 4; r++)
	{
		for(int c = 0; c < 4; c++)
		{
			product[r][c] = m1[r] * m2.getColumn(c);
		}
	}
	
	return product;
}

void MutualInformationRegistration::calculateVoreenTrafo(const mat4& itkMatrix) {
    const VolumeBase* fixed = fixedVolumeInport_.getData();

    mat4 invertedITKTransform;
    bool success = itkMatrix.invert(invertedITKTransform);
    if(!success) {
        LERROR("Failed to invert ITK transformation matrix!");
        return;
    }
    mat4 f_physicalToWorld = fixed->getPhysicalToWorldMatrix();

    mat4 voreenMatrix = f_physicalToWorld * invertedITKTransform;
    transformationMatrix_.set(voreenMatrix);
    LINFO("Setting matrix");
}

// Draw the shadow map in the world, so we can see it.
void WorldRenderer::DebugShowShadowMap(const corgi::CameraInterface& camera,
                                       fplbase::Renderer& renderer) {
  fplbase::RenderTarget::ScreenRenderTarget(renderer).SetAsRenderTarget();

  static const mat4 kDebugTextureWorldTransform =
      mat4::FromScaleVector(mathfu::vec3(10.0f, 10.0f, 10.0f));

  const mat4 mvp = camera.GetTransformMatrix() * kDebugTextureWorldTransform;
  const mat4 world_matrix_inverse = kDebugTextureWorldTransform.Inverse();

  renderer.set_camera_pos(world_matrix_inverse * camera.position());
  renderer.set_light_pos(world_matrix_inverse * light_camera_.position());
  renderer.set_model_view_projection(mvp);
  renderer.set_color(vec4(1.0f, 1.0f, 1.0f, 1.0f));

  shadow_map_.BindAsTexture(0);

  textured_shader_->Set(renderer);

  // Render a large quad in the world, with the shadowmap texture on it:
  fplbase::Mesh::RenderAAQuadAlongX(vec3(0.0f, 0.0f, 0.0f),
                                    vec3(10.0f, 0.0f, 10.0f),
                                    vec2(1.0f, 0.0f), vec2(0.0f, 1.0f));
}

void the::plotter::draw(const mat4 & m)
{
	if(lines.empty()) return;
	glUseProgram(mat->getID());
	glBindBuffer(GL_ARRAY_BUFFER, 0);
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
   	glEnableVertexAttribArray(pos);
   	glEnableVertexAttribArray(col);
  	glVertexAttribPointer(pos,3, GL_FLOAT,GL_FALSE,0, &lines[0]);
  	glVertexAttribPointer(col,4, GL_FLOAT,GL_FALSE,0, &colors[0]);
  	glUniformMatrix4fv(mpv, 1,GL_FALSE,m.ptr());
    glDrawArrays(GL_LINES,0, lines.size());
    glDisableVertexAttribArray(pos);
    GL_CHECK("the::plotter::draw");
}

void Program::setUniform(int nLoc, uint32_t type, const mat4& value, bool forced)
{
	if (nLoc == -1) return;
	
	(void)type;
	assert(type == GL_FLOAT_MAT4);
	assert(loaded());
	
	if (forced || ((_mat4Cache.count(nLoc) == 0) || (_mat4Cache[nLoc] != value)))
	{
		_mat4Cache[nLoc] = value;
		glUniformMatrix4fv(nLoc, 1, 0, value.data());
	}
	
	checkOpenGLError("setUniform - mat4");
}