/* -*- mia-c++  -*-
 *
 * This file is part of MIA - a toolbox for medical image analysis 
 * Copyright (c) Leipzig, Madrid 1999-2016 Gert Wollny
 *
 * MIA is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MIA; if not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <cmath>
#include <mia/3d/affine_matrix.hh>
#include <mia/core/utils.hh>

NS_MIA_BEGIN

using std::vector; 
using std::runtime_error; 

CAffinTransformMatrix::CAffinTransformMatrix():m_matrix(16, 0.0f)
{
        identity();
}

CAffinTransformMatrix::CAffinTransformMatrix(float a11, float a12, float a13, float a14, 
					     float a21, float a22, float a23, float a24, 
					     float a31, float a32, float a33, float a34):m_matrix(16, 0.0f)
{
	m_matrix[0] = a11; 
	m_matrix[4] = a12; 
	m_matrix[8] = a13; 
	m_matrix[12]= a14; 

	m_matrix[1] = a21; 
	m_matrix[5] = a22; 
	m_matrix[9] = a23; 
	m_matrix[13]= a24; 

	m_matrix[2] = a31; 
	m_matrix[6] = a32; 
	m_matrix[10]= a33; 
	m_matrix[14]= a34; 

	m_matrix[15]=1.0f;

}

void CAffinTransformMatrix::rotate_x(float angle, const C3DFVector& center)
{
        float c, s; 
        sincosf(angle, &s, &c); 

	float ac[8]; 
	float as[8]; 

	const int a1 = 0; 
	const int a2 = 1;
	const int a5 = 2; 
	const int a6 = 3;
	const int a9 = 4; 
	const int aA = 5;
	const int aD = 6; 
	const int aE = 7;

	ac[a1] = m_matrix[1] * c; as[a1] = m_matrix[1] * s; 
	ac[a2] = m_matrix[2] * c; as[a2] = m_matrix[2] * s; 
	ac[a5] = m_matrix[5] * c; as[a5] = m_matrix[5] * s; 
	ac[a6] = m_matrix[6] * c; as[a6] = m_matrix[6] * s; 
	ac[a9] = m_matrix[9] * c; as[a9] = m_matrix[9] * s; 
	ac[aA] = m_matrix[10]* c; as[aA] = m_matrix[10] * s; 
	ac[aD] = m_matrix[13]* c; as[aD] = m_matrix[13] * s; 
	ac[aE] = m_matrix[14]* c; as[aE] = m_matrix[14] * s; 

	m_matrix[1] = ac[a1] - as[a2]; 
	m_matrix[5] = ac[a5] - as[a6]; 
	m_matrix[9] = ac[a9] - as[aA]; 
	m_matrix[13] = ac[aD] - as[aE] + center.z * s - center.y * c + center.y; 

	m_matrix[2] = as[a1] + ac[a2]; 
	m_matrix[6] = as[a5] + ac[a6]; 
	m_matrix[10]= as[a9] + ac[aA]; 
	m_matrix[14]= as[aD] + ac[aE] - center.y * s - center.z * c + center.z;
	

}

void CAffinTransformMatrix::rotate_y(float angle, const C3DFVector& center)
{
        float c, s; 
        sincosf(angle, &s, &c); 

	float ac[8]; 
	float as[8]; 

	const int a0 = 0; 
	const int a2 = 1;
	const int a4 = 2; 
	const int a6 = 3;
	const int a8 = 4; 
	const int aA = 5;
	const int aC = 6; 
	const int aE = 7;

	ac[a0] = m_matrix[0] * c; as[a0] = m_matrix[0] * s; 
	ac[a2] = m_matrix[2] * c; as[a2] = m_matrix[2] * s; 
	ac[a4] = m_matrix[4] * c; as[a4] = m_matrix[4] * s; 
	ac[a6] = m_matrix[6] * c; as[a6] = m_matrix[6] * s; 
	ac[a8] = m_matrix[8] * c; as[a8] = m_matrix[8] * s; 
	ac[aA] = m_matrix[10]* c; as[aA] = m_matrix[10] * s; 
	ac[aC] = m_matrix[12]* c; as[aC] = m_matrix[12] * s; 
	ac[aE] = m_matrix[14]* c; as[aE] = m_matrix[14] * s; 

	m_matrix[0] = ac[a0] - as[a2]; 
	m_matrix[4] = ac[a4] - as[a6]; 
	m_matrix[8] = ac[a8] - as[aA]; 
	m_matrix[12] = ac[aC] - as[aE] + center.z * s - center.x * c + center.x; 

	m_matrix[2] = as[a0] + ac[a2]; 
	m_matrix[6] = as[a4] + ac[a6]; 
	m_matrix[10]= as[a8] + ac[aA]; 
	m_matrix[14]= as[aC] + ac[aE] - center.x * s - center.z * c + center.z;

}

void CAffinTransformMatrix::rotate_z(float angle, const C3DFVector& center)
{
        float c, s; 
        sincosf(angle, &s, &c); 

	float ac[8]; 
	float as[8]; 

	const int a0 = 0; 
	const int a1 = 1;
	const int a4 = 2; 
	const int a5 = 3;
	const int a8 = 4; 
	const int a9 = 5;
	const int aC = 6; 
	const int aD = 7;

	ac[a0] = m_matrix[0] * c; as[a0] = m_matrix[0] * s; 
	ac[a1] = m_matrix[1] * c; as[a1] = m_matrix[1] * s; 
	ac[a4] = m_matrix[4] * c; as[a4] = m_matrix[4] * s; 
	ac[a5] = m_matrix[5] * c; as[a5] = m_matrix[5] * s; 
	ac[a8] = m_matrix[8] * c; as[a8] = m_matrix[8] * s; 
	ac[a9] = m_matrix[9] * c; as[a9] = m_matrix[9] * s; 
	ac[aC] = m_matrix[12]* c; as[aC] = m_matrix[12] * s; 
	ac[aD] = m_matrix[13]* c; as[aD] = m_matrix[13] * s; 

	m_matrix[0] = ac[a0] - as[a1]; 
	m_matrix[4] = ac[a4] - as[a5]; 
	m_matrix[8] = ac[a8] - as[a9]; 
	m_matrix[12] = ac[aC] - as[aD] + center.y * s - center.x * c + center.x; 

	m_matrix[1] = as[a0] + ac[a1]; 
	m_matrix[5] = as[a4] + ac[a5]; 
	m_matrix[9]= as[a8] + ac[a9]; 
	m_matrix[13]= as[aC] + ac[aD] - center.x * s - center.y * c + center.y;
}


void CAffinTransformMatrix::transform_centered(const C3DFMatrix& m, const C3DFVector& center)
{
	const auto shift = center - m * center; 
	vector<float> help(16,0.0f); 
	
	// multiplying from right side with m transposed 
	help[0] = m_matrix[0] * m.x.x + m_matrix[1] * m.x.y + m_matrix[2] * m.x.z;  
	help[1] = m_matrix[0] * m.y.x + m_matrix[1] * m.y.y + m_matrix[2] * m.y.z;  
	help[2] = m_matrix[0] * m.z.x + m_matrix[1] * m.z.y + m_matrix[2] * m.z.z;  

	help[4] = m_matrix[4] * m.x.x + m_matrix[5] * m.x.y + m_matrix[6] * m.x.z;  
	help[5] = m_matrix[4] * m.y.x + m_matrix[5] * m.y.y + m_matrix[6] * m.y.z;  
	help[6] = m_matrix[4] * m.z.x + m_matrix[5] * m.z.y + m_matrix[6] * m.z.z;  

	help[8] = m_matrix[8] * m.x.x + m_matrix[9] * m.x.y + m_matrix[10] * m.x.z;
	help[9] = m_matrix[8] * m.y.x + m_matrix[9] * m.y.y + m_matrix[10] * m.y.z;
	help[10]= m_matrix[8] * m.z.x + m_matrix[9] * m.z.y + m_matrix[10] * m.z.z;

	help[12] = m_matrix[12] * m.x.x + m_matrix[13] * m.x.y + m_matrix[14] * m.x.z + shift.x; 
	help[13] = m_matrix[12] * m.y.x + m_matrix[13] * m.y.y + m_matrix[14] * m.y.z + shift.y;
	help[14] = m_matrix[12] * m.z.x + m_matrix[13] * m.z.y + m_matrix[14] * m.z.z + shift.z;


	help[15] = 1.0f; 

	swap(help, m_matrix); 

}

void CAffinTransformMatrix::rotate(const Quaternion& q, const C3DFVector& center)
{
	const auto rot = C3DFMatrix(q.get_rotation_matrix()); 
	const auto shift = center - rot * center; 
	vector<float> help(16,0.0f); 
	
	// multiplying from right side with rot transposed 
	help[0] = m_matrix[0] * rot.x.x + m_matrix[1] * rot.x.y + m_matrix[2] * rot.x.z;  
	help[1] = m_matrix[0] * rot.y.x + m_matrix[1] * rot.y.y + m_matrix[2] * rot.y.z;  
	help[2] = m_matrix[0] * rot.z.x + m_matrix[1] * rot.z.y + m_matrix[2] * rot.z.z;  

	help[4] = m_matrix[4] * rot.x.x + m_matrix[5] * rot.x.y + m_matrix[6] * rot.x.z;  
	help[5] = m_matrix[4] * rot.y.x + m_matrix[5] * rot.y.y + m_matrix[6] * rot.y.z;  
	help[6] = m_matrix[4] * rot.z.x + m_matrix[5] * rot.z.y + m_matrix[6] * rot.z.z;  

	help[8] = m_matrix[8] * rot.x.x + m_matrix[9] * rot.x.y + m_matrix[10] * rot.x.z;
	help[9] = m_matrix[8] * rot.y.x + m_matrix[9] * rot.y.y + m_matrix[10] * rot.y.z;
	help[10]= m_matrix[8] * rot.z.x + m_matrix[9] * rot.z.y + m_matrix[10] * rot.z.z;

	help[12] = m_matrix[12] * rot.x.x + m_matrix[13] * rot.x.y + m_matrix[14] * rot.x.z + shift.x; 
	help[13] = m_matrix[12] * rot.y.x + m_matrix[13] * rot.y.y + m_matrix[14] * rot.y.z + shift.y;
	help[14] = m_matrix[12] * rot.z.x + m_matrix[13] * rot.z.y + m_matrix[14] * rot.z.z + shift.z;


	help[15] = 1.0f; 

	swap(help, m_matrix); 

}

void CAffinTransformMatrix::scale(const C3DFVector& scale, const C3DFVector& center)
{
	const C3DFVector sh = (C3DFVector::_1 - scale) * center; 

	for (int i = 0; i < 4; ++i) {
		m_matrix[4*i] *= scale.x; 
		m_matrix[4*i + 1] *= scale.y; 
		m_matrix[4*i + 2] *= scale.z; 
	}
	m_matrix[12]  += sh.x; 
	m_matrix[13]  += sh.y; 
	m_matrix[14] += sh.z; 
	
}

const std::vector<float>& CAffinTransformMatrix::data() const
{
	return m_matrix; 
}

void CAffinTransformMatrix::translate(const C3DFVector& shift)
{
	m_matrix[12] += shift.x; 
	m_matrix[13] += shift.y; 
	m_matrix[14] += shift.z; 
}

void CAffinTransformMatrix::identity()
{
	fill(m_matrix.begin(), m_matrix.end(), 0.0f); 
        m_matrix[0] = m_matrix[5] = m_matrix[10] = m_matrix[15] = 1.0f; 
}

CAffinTransformMatrix& CAffinTransformMatrix::operator *= (const CAffinTransformMatrix& other)
{
	vector<float> help(16,0.0f); 

	help[0] = m_matrix[0] * other.m_matrix[0] + m_matrix[4] * other.m_matrix[1] + m_matrix[8] * other.m_matrix[2];  
	help[1] = m_matrix[1] * other.m_matrix[0] + m_matrix[5] * other.m_matrix[1] + m_matrix[9] * other.m_matrix[2];  
	help[2] = m_matrix[2] * other.m_matrix[0] + m_matrix[6] * other.m_matrix[1] + m_matrix[10]* other.m_matrix[2];

	help[4] = m_matrix[0] * other.m_matrix[4] + m_matrix[4] * other.m_matrix[5] + m_matrix[8] * other.m_matrix[6];  
	help[5] = m_matrix[1] * other.m_matrix[4] + m_matrix[5] * other.m_matrix[5] + m_matrix[9] * other.m_matrix[6];  
	help[6] = m_matrix[2] * other.m_matrix[4] + m_matrix[6] * other.m_matrix[5] + m_matrix[10] * other.m_matrix[6];

	help[8] = m_matrix[0] * other.m_matrix[8] + m_matrix[4] * other.m_matrix[9] + m_matrix[8] * other.m_matrix[10];  
	help[9] = m_matrix[1] * other.m_matrix[8] + m_matrix[5] * other.m_matrix[9] + m_matrix[9] * other.m_matrix[10];  
	help[10]= m_matrix[2] * other.m_matrix[8] + m_matrix[6] * other.m_matrix[9] + m_matrix[10]* other.m_matrix[10];

	help[12]= other.m_matrix[12] * m_matrix[0] + other.m_matrix[13] * m_matrix[4] + other.m_matrix[14] * m_matrix[8] + m_matrix[12]; 
	help[13]= other.m_matrix[12] * m_matrix[1] + other.m_matrix[13] * m_matrix[5] + other.m_matrix[14] * m_matrix[9] + m_matrix[13];
	help[14]= other.m_matrix[12] * m_matrix[2] + other.m_matrix[13] * m_matrix[6] + other.m_matrix[14] * m_matrix[10]+ m_matrix[14];

	help[15] = 1.0f; 

	swap(m_matrix, help); 
	return *this; 
}

C3DFVector CAffinTransformMatrix::rotate(const C3DFVector& x) const
{
	return C3DFVector(m_matrix[0] * x.x + m_matrix[4] * x.y + m_matrix[8] * x.z, 
			  m_matrix[1] * x.x + m_matrix[5] * x.y + m_matrix[9] * x.z, 
			  m_matrix[2] * x.x + m_matrix[6] * x.y + m_matrix[10] * x.z); 
}

const CAffinTransformMatrix CAffinTransformMatrix::inverse() const
{
	const float det = (m_matrix[0]*m_matrix[5]-m_matrix[4]*m_matrix[1])*m_matrix[10]+
		(m_matrix[8]*m_matrix[1]-m_matrix[0]*m_matrix[9])*m_matrix[6]+
		(m_matrix[4]*m_matrix[9]-m_matrix[8]*m_matrix[5])*m_matrix[2]; 

	if (::fabs(det) < 1e-8)
		throw runtime_error("CAffinTransformMatrix:inverse(): Matrix is (numerically) singular"); 
	
	const float inv_det = 1.0f/det; 
	
	CAffinTransformMatrix result; 

	result.m_matrix[0] = inv_det * (m_matrix[5]*m_matrix[10]- m_matrix[9]*m_matrix[6]); 
	result.m_matrix[4] = inv_det * (m_matrix[8]*m_matrix[6] - m_matrix[4]*m_matrix[10]); 
	result.m_matrix[8] = inv_det * (m_matrix[4]*m_matrix[9] - m_matrix[8]*m_matrix[5]); 
	result.m_matrix[12] = inv_det * ((m_matrix[8]*m_matrix[5] - m_matrix[4]*m_matrix[9])*m_matrix[14]+
					(m_matrix[4]*m_matrix[13] - m_matrix[12]*m_matrix[5])*m_matrix[10]+
					(m_matrix[12]*m_matrix[9] - m_matrix[8]*m_matrix[13])*m_matrix[6]); 
		
	result.m_matrix[1] = inv_det *  (m_matrix[9]*m_matrix[2] - m_matrix[1]*m_matrix[10]); 
	result.m_matrix[5] = inv_det *  (m_matrix[0]*m_matrix[10]- m_matrix[8]*m_matrix[2]); 
	result.m_matrix[9] = inv_det *  (m_matrix[8]*m_matrix[1] - m_matrix[0]*m_matrix[9]);
	result.m_matrix[13] = inv_det *  ((m_matrix[0]*m_matrix[9]-m_matrix[8]*m_matrix[1])*m_matrix[14]+
					 (m_matrix[12]*m_matrix[1]-m_matrix[0]*m_matrix[13])*m_matrix[10]+
					 (m_matrix[8]*m_matrix[13]-m_matrix[12]*m_matrix[9])*m_matrix[2]); 

	result.m_matrix[2] = inv_det *  (m_matrix[1]*m_matrix[6] - m_matrix[5]*m_matrix[2]); 
	result.m_matrix[6] = inv_det *  (m_matrix[4]*m_matrix[2] - m_matrix[0]*m_matrix[6]); 
	result.m_matrix[10]= inv_det *  (m_matrix[0]*m_matrix[5] - m_matrix[4]*m_matrix[1]); 
	result.m_matrix[14]= inv_det *  ((m_matrix[4]*m_matrix[1] - m_matrix[0]*m_matrix[5])*m_matrix[14]+
					 (m_matrix[0]*m_matrix[13] - m_matrix[12]*m_matrix[1])*m_matrix[6]+
					 (m_matrix[12]*m_matrix[5] - m_matrix[4]*m_matrix[13])*m_matrix[2]); 
	
	result.m_matrix[15] = 1.0f; 
	
	return result; 
}

void CAffinTransformMatrix::shear(const C3DFVector& shear, const C3DFVector& origin)
{
	vector<float> help(m_matrix); 
	
	help[ 0] += m_matrix[ 1] * shear.x; 
	help[ 4] += m_matrix[ 5] * shear.x; 
	help[ 8] += m_matrix[ 9] * shear.x; 

	help[ 1] += m_matrix[ 2] * shear.y; 
	help[ 5] += m_matrix[ 6] * shear.y; 
	help[ 9] += m_matrix[10] * shear.y; 

	help[ 2] += m_matrix[ 0] * shear.z; 
	help[ 6] += m_matrix[ 4] * shear.z; 
	help[10] += m_matrix[ 8] * shear.z; 

	help[12] += m_matrix[13] * shear.x; 
	help[13] += m_matrix[14] * shear.y; 
	help[14] += m_matrix[12] * shear.z;

	if (origin != C3DFVector::_0) {
                help[12] += origin.x
			-(m_matrix[9]*shear.x + m_matrix[8])*origin.z
                        -(m_matrix[5]*shear.x + m_matrix[4])*origin.y
                        -(m_matrix[1]*shear.x + m_matrix[0])*origin.x; 

		help[13] += origin.y 
			-(m_matrix[10]*shear.y + m_matrix[9]) * origin.z
			-(m_matrix[ 6]*shear.y + m_matrix[5]) * origin.y
			-(m_matrix[ 2]*shear.y + m_matrix[1]) * origin.x; 
			
		
		help[14] += origin.z 
			-(m_matrix[8]*shear.z + m_matrix[10])*origin.z
			-(m_matrix[4]*shear.z + m_matrix[ 6])*origin.y
			-(m_matrix[0]*shear.z + m_matrix[ 2])*origin.x;
	}
	swap(m_matrix, help); 
}



C3DFVector CAffinTransformMatrix::operator * (const C3DFVector& x) const
{
	return C3DFVector(
		m_matrix[0] * x.x + m_matrix[4] * x.y + m_matrix[8] * x.z + m_matrix[12], 
		m_matrix[1] * x.x + m_matrix[5] * x.y + m_matrix[9] * x.z + m_matrix[13], 
		m_matrix[2] * x.x + m_matrix[6] * x.y + m_matrix[10] * x.z + m_matrix[14]); 
}


CAffinTransformMatrix operator * (const CAffinTransformMatrix& lhs, const CAffinTransformMatrix& rhs)
{
	CAffinTransformMatrix h(lhs); 
	h*=rhs; 
	return h; 
}

NS_MIA_END
