/* -*- mia-c++  -*-
 *
 * This file is part of MIA - a toolbox for medical image analysis 
 * Copyright (c) Leipzig, Madrid 1999-2017 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/>.
 *
 */

#define BOOST_TEST_MODULE TEST_ORIENTATION 
#include <sstream>
#include <mia/internal/autotest.hh>
#include <mia/core/cmdlineparser.hh>
#include <mia/3d/orientation.hh>


NS_MIA_USE
using namespace std;

static void run_test_read(const string& id, E3DImageOrientation test_orient, 
			  const string& expect)
{
	E3DImageOrientation orient;
	istringstream is(id);
	is >> orient;
	BOOST_CHECK_EQUAL(orient, test_orient);

	stringstream os;
	os << test_orient;
	BOOST_CHECK_EQUAL(os.str(), expect);

}

BOOST_AUTO_TEST_CASE( test_orientation_streamio )
{
	run_test_read("axial", ior_axial, "axial");
	run_test_read("coronal", ior_coronal, "coronal");
	run_test_read("saggital", ior_saggital, "saggital");
	run_test_read("unknown", ior_undefined, "(undefined)");
}

BOOST_AUTO_TEST_CASE( test_orientation_attribute )
{
	COrientationTranslator::register_for("orientation");
	PAttribute attr = CStringAttrTranslatorMap::instance().to_attr("orientation", "axial");
	const C3DImageOrientation *io = dynamic_cast<const C3DImageOrientation *>(attr.get());
	BOOST_REQUIRE(io);

	E3DImageOrientation orient = *io;
	BOOST_CHECK_EQUAL(orient, ior_axial);
}

struct OrientationTestFixture {
	void check_transform(const C3DOrientationAndPosition& op, const vector<double>& expect); 
	void check_inv_transform(const C3DOrientationAndPosition& op, const vector<double>& expect); 
private: 
	void check(const CDoubleVector& params, const vector<double>& expect); 
	
}; 

BOOST_FIXTURE_TEST_CASE(test_scale_x, OrientationTestFixture) 
{
	C3DOrientationAndPosition op(ior_default, C3DFVector::_0, C3DFVector(2.0, 1, 1), 
				     Quaternion::_1);

	vector<double> expect = {
		2.0, 0.0, 0.0, 0.0, 
		0.0, 1.0, 0.0, 0.0, 
		0.0, 0.0, 1.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};

	check_transform(op, expect); 

	vector<double> inv_expect = {
		0.5, 0.0, 0.0, 0.0, 
		0.0, 1.0, 0.0, 0.0, 
		0.0, 0.0, 1.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	
	check_inv_transform(op, inv_expect); 
	
}


BOOST_FIXTURE_TEST_CASE(test_shift_x, OrientationTestFixture) 
{
	C3DOrientationAndPosition op(ior_default, C3DFVector(3.0,0,0), C3DFVector::_1, 
				     Quaternion::_1);

	vector<double> expect = {
		1.0, 0.0, 0.0, 3.0, 
		0.0, 1.0, 0.0, 0.0, 
		0.0, 0.0, 1.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};

	check_transform(op, expect); 

	vector<double> inv_expect = {
		1.0, 0.0, 0.0,-3.0, 
		0.0, 1.0, 0.0, 0.0, 
		0.0, 0.0, 1.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	
	check_inv_transform(op, inv_expect); 
	
}

BOOST_FIXTURE_TEST_CASE(test_rot_x, OrientationTestFixture) 
{
	C3DOrientationAndPosition op(ior_default, C3DFVector::_0, C3DFVector::_1, 
				     Quaternion(C3DDVector(0.5 * M_PI, 0, 0)));

	vector<double> expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 0.0,-1.0, 0.0, 
		0.0, 1.0, 0.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};

	check_transform(op, expect); 

	vector<double> inv_expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 0.0, 1.0, 0.0, 
		0.0,-1.0, 0.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	
	check_inv_transform(op, inv_expect); 
}


BOOST_FIXTURE_TEST_CASE(test_scale_y, OrientationTestFixture) 
{
	C3DOrientationAndPosition op(ior_default, C3DFVector::_0, C3DFVector(1.0, 2.0, 1), 
				     Quaternion::_1);

	vector<double> expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 2.0, 0.0, 0.0, 
		0.0, 0.0, 1.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	check_transform(op, expect); 

	vector<double> inv_expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 0.5, 0.0, 0.0, 
		0.0, 0.0, 1.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	
	check_inv_transform(op, inv_expect); 

}

BOOST_FIXTURE_TEST_CASE(test_shift_y, OrientationTestFixture) 
{
	C3DOrientationAndPosition op(ior_default,  C3DFVector(0.0, 2.0, 0), C3DFVector::_1,
				     Quaternion::_1);

	vector<double> expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 1.0, 0.0, 2.0, 
		0.0, 0.0, 1.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	check_transform(op, expect); 

	vector<double> inv_expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 1.0, 0.0,-2.0, 
		0.0, 0.0, 1.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	
	check_inv_transform(op, inv_expect); 

}

BOOST_FIXTURE_TEST_CASE(test_rot_y, OrientationTestFixture) 
{
	C3DOrientationAndPosition op(ior_default, C3DFVector::_0, C3DFVector::_1, 
				     Quaternion(C3DDVector(0, 0.5 * M_PI, 0)));

	vector<double> expect = {
		0.0, 0.0, 1.0, 0.0, 
		0.0, 1.0, 0.0, 0.0, 
	       -1.0, 0.0, 0.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};

	check_transform(op, expect); 

	vector<double> inv_expect = {
		0.0, 0.0,-1.0, 0.0, 
		0.0, 1.0, 0.0, 0.0, 
		1.0, 0.0, 0.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	
	check_inv_transform(op, inv_expect); 
	
}


BOOST_FIXTURE_TEST_CASE(test_scale_z, OrientationTestFixture) 
{
	C3DOrientationAndPosition op(ior_default, C3DFVector::_0, C3DFVector(1.0, 1.0, 2.0), 
				     Quaternion::_1);

	vector<double> expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 1.0, 0.0, 0.0, 
		0.0, 0.0, 2.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	check_transform(op, expect); 

	vector<double> inv_expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 1.0, 0.0, 0.0, 
		0.0, 0.0, 0.5, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	
	check_inv_transform(op, inv_expect); 

}

BOOST_FIXTURE_TEST_CASE(test_shift_z, OrientationTestFixture) 
{
	C3DOrientationAndPosition op(ior_default, C3DFVector(0.0, 0.0, 7.0), C3DFVector::_1, 
				     Quaternion::_1);

	vector<double> expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 1.0, 0.0, 0.0, 
		0.0, 0.0, 1.0, 7.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	check_transform(op, expect); 

	vector<double> inv_expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 1.0, 0.0, 0.0, 
		0.0, 0.0, 1.0,-7.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	
	check_inv_transform(op, inv_expect); 

}

BOOST_FIXTURE_TEST_CASE(test_rot_z, OrientationTestFixture) 
{
	C3DOrientationAndPosition op(ior_default, C3DFVector::_0, C3DFVector::_1, 
				     Quaternion(C3DDVector(0, 0, 0.5 * M_PI)));

	vector<double> expect = {
		0.0,-1.0, 0.0, 0.0, 
		1.0, 0.0, 0.0, 0.0, 
	        0.0, 0.0, 1.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};

	check_transform(op, expect); 

	vector<double> inv_expect = {
		0.0, 1.0, 0.0, 0.0, 
	       -1.0, 0.0, 0.0, 0.0, 
	        0.0, 0.0, 1.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	
	check_inv_transform(op, inv_expect); 
	
}



// this is the left handed rotation 
BOOST_FIXTURE_TEST_CASE(test_rot_xyz, OrientationTestFixture) 
{
	C3DOrientationAndPosition op(ior_default, C3DFVector::_0, C3DFVector::_1, 
				     Quaternion(C3DDVector(M_PI / 6.0, M_PI / 3.0, 3 * M_PI/4.0)));

	double cos_psi = -sqrt(2.0) / 2.0; 
	double sin_psi = sqrt(2.0) / 2.0; 

	double cos_theta = 0.5; 
	double sin_theta = sqrt(3)/2.0; 

	double cos_phi = sqrt(3)/2.0; 
	double sin_phi = 0.5; 

	double a11 = cos_theta * cos_psi; 
	double a12 = - cos_phi * sin_psi + sin_phi * sin_theta * cos_psi; 
	double a13 = sin_phi * sin_psi + cos_phi * sin_theta * cos_psi; 
	
	double a21 = cos_theta * sin_psi; 
	double a22 = cos_phi * cos_psi + sin_phi * sin_theta * sin_psi; 
	double a23 = - sin_phi * cos_psi + cos_phi * sin_theta * sin_psi; 
	
	double a31 = - sin_theta; 
	double a32 = sin_phi * cos_theta;
	double a33 = cos_phi * cos_theta;


	vector<double> expect = {
		a11, a12, a13, 0.0, 
		a21, a22, a23, 0.0, 
		a31, a32, a33, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};

	check_transform(op, expect); 

	vector<double> inv_expect = {
		1.0, 0.0, 0.0, 0.0, 
		0.0, 0.0, 1.0, 0.0, 
		0.0,-1.0, 0.0, 0.0, 
		0.0, 0.0, 0.0, 1.0, 
	};
	
//	check_inv_transform(op, inv_expect); 
}


void OrientationTestFixture::check_transform(const C3DOrientationAndPosition& op, const vector<double>& expect)
{
	CDoubleVector params(16); 
	op.get_transform_parameters(params); 
	check(params, expect); 
}

void OrientationTestFixture::check_inv_transform(const C3DOrientationAndPosition& op, const vector<double>& expect)
{
	CDoubleVector params(16); 
	op.get_inverse_transform_parameters(params); 
	check(params, expect); 
}

void OrientationTestFixture::check(const CDoubleVector& params, const vector<double>& expect)
{
	for (int i = 0; i < 16; ++i) {
		cvdebug() << "[" << i << "]=" <<  params[i] << ", expect " << expect[i] << "\n"; 
		if (expect[i] == 0.0) {
			BOOST_CHECK_SMALL(params[i], 1e-5); 
		}else{
			BOOST_CHECK_CLOSE(params[i], expect[i], 1e-5);
		}
	}
}
