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

#include <mia/3d/transform/affine.hh>

NS_MIA_USE

using namespace std;
using namespace ::boost;
using namespace boost::unit_test;
namespace bfs=boost::filesystem;

struct ipfFixture {
	ipfFixture():
		ipf("bspline:d=3", "mirror")
		{
		} 
	C3DInterpolatorFactory ipf; 
}; 

void affin_add_translate(C3DAffineTransformation& rtrans, double x, double y, double z) 
{
	auto params = rtrans.get_parameters(); 
	params[3] += x; 
	params[7] += y; 
	params[11] += z; 
	rtrans.set_parameters(params); 
}

void affin_add_scale(C3DAffineTransformation& rtrans, double x, double y, double z) 
{
	auto params = rtrans.get_parameters(); 
	const double expx = exp(x);
	const double expy = exp(y);
	const double expz = exp(z);
	params[0] *= expx;
	params[1] *= expx;
	params[2] *= expx;
	params[3] *= expx;
	params[4] *= expy;
	params[5] *= expy;
	params[6] *= expy;
	params[7] *= expy;
	params[8] *= expz;
	params[9] *= expz;
	params[10] *= expz;
	params[11] *= expz;
	rtrans.set_parameters(params); 
}


struct TranslateTransFixture: public  ipfFixture{
	TranslateTransFixture():size(60, 80, 20),
				rtrans(size, ipf)
		{
			affin_add_translate(rtrans, 1.0, 2.0, 3.0);
		}
	C3DBounds size;
	C3DAffineTransformation rtrans;
};


BOOST_FIXTURE_TEST_CASE(basics_TranslateTransFixture, TranslateTransFixture)
{
	C3DFVector x(2, 4, 5);
	BOOST_CHECK_EQUAL(rtrans.degrees_of_freedom(), 12u);
	BOOST_CHECK_EQUAL(C3DFVector(3.0, 6.0, 8.0),  rtrans(x));
}

BOOST_FIXTURE_TEST_CASE(max_TranslateTransFixture, TranslateTransFixture)
{
	BOOST_CHECK_CLOSE(rtrans.get_max_transform(), sqrtf(14.0), 0.1);
}

BOOST_FIXTURE_TEST_CASE(set_identity_TranslateTransFixture, TranslateTransFixture)
{
	rtrans.set_identity();

	C3DAffineTransformation::const_iterator ti = rtrans.begin();

	for (size_t z = 0; z < size.z; ++z)
		for (size_t y = 0; y < size.y; ++y)
			for (size_t x = 0; x < size.x; ++x, ++ti) {
				BOOST_CHECK_EQUAL(*ti, C3DFVector(x, y, z));
			}
}

BOOST_FIXTURE_TEST_CASE(derivative_TranslateTransFixture, TranslateTransFixture)
{
	C3DFMatrix d = 	rtrans.derivative_at(10,10,10);
	BOOST_CHECK_EQUAL(d.x.x, 1.0f);
	BOOST_CHECK_EQUAL(d.x.y, 0.0f);
	BOOST_CHECK_EQUAL(d.x.z, 0.0f);
	BOOST_CHECK_EQUAL(d.y.x, 0.0f);
	BOOST_CHECK_EQUAL(d.y.y, 1.0f);
	BOOST_CHECK_EQUAL(d.y.z, 0.0f);
	BOOST_CHECK_EQUAL(d.z.x, 0.0f);
	BOOST_CHECK_EQUAL(d.z.y, 0.0f);
	BOOST_CHECK_EQUAL(d.z.z, 1.0f);

}

BOOST_FIXTURE_TEST_CASE(derivative_TranslateTransFixture_off_grid, TranslateTransFixture)
{
	C3DFMatrix d = 	rtrans.derivative_at(C3DFVector(10.1,10.2,10.3));
	BOOST_CHECK_EQUAL(d.x.x, 1.0f);
	BOOST_CHECK_EQUAL(d.x.y, 0.0f);
	BOOST_CHECK_EQUAL(d.x.z, 0.0f);
	BOOST_CHECK_EQUAL(d.y.x, 0.0f);
	BOOST_CHECK_EQUAL(d.y.y, 1.0f);
	BOOST_CHECK_EQUAL(d.y.z, 0.0f);
	BOOST_CHECK_EQUAL(d.z.x, 0.0f);
	BOOST_CHECK_EQUAL(d.z.y, 0.0f);
	BOOST_CHECK_EQUAL(d.z.z, 1.0f);

}


BOOST_FIXTURE_TEST_CASE(test_affine3d, ipfFixture)
{
	C3DAffineTransformation t1(C3DBounds(10,20,15), ipf); 

	BOOST_CHECK_EQUAL(t1.degrees_of_freedom(), 12u);

	C3DFVector x0(1.0f, 2.0f, 3.0);

	C3DFVector y0 = t1(x0);
	BOOST_CHECK_EQUAL(y0, x0);

	affin_add_scale(t1, std::log(2.0), std::log(3.0), std::log(4.0));

	C3DFVector y1 = t1(x0);
	BOOST_CHECK_EQUAL(y1, C3DFVector(2.0f, 6.0f, 12.0));

	affin_add_translate(t1, 1.0f, 2.0f, 1.5f);
	BOOST_CHECK_EQUAL(t1(x0), C3DFVector(3.0f, 8.0f, 13.5));

}


BOOST_FIXTURE_TEST_CASE( test_affine3d_iterator, ipfFixture )
{
	C3DBounds size(10,20,15);

	C3DAffineTransformation t1(size, ipf);
	C3DAffineTransformation::const_iterator ti = t1.begin();
	
	for (size_t z = 0; z < size.z; ++z)
		for (size_t y = 0; y < size.y; ++y)
			for (size_t x = 0; x < size.x; ++x, ++ti) {
				BOOST_CHECK_EQUAL(*ti, C3DFVector(x, y, z));
			}
	
	BOOST_CHECK(ti == t1.end());
}

BOOST_FIXTURE_TEST_CASE( test_affine3d_ranged_iterator, ipfFixture)
{
	C3DBounds size(10,20,30);
	C3DBounds delta(1,2,3); 

	C3DAffineTransformation t1(size, ipf);
	auto ti = t1.begin_range(delta, size - delta);

	for (size_t z = delta.z; z < size.z - delta.z; ++z)
		for (size_t y = delta.y; y < size.y - delta.y; ++y)
			for (size_t x = delta.x; x < size.x - delta.x; ++x, ++ti) {
				BOOST_CHECK_EQUAL(*ti, C3DFVector(x, y, z));
			}

	BOOST_CHECK(ti == t1.end_range(delta, size - delta));
}


BOOST_FIXTURE_TEST_CASE( test_affine_clone, TranslateTransFixture )
{
	P3DTransformation clone(rtrans.clone());

	C3DAffineTransformation& spclone = dynamic_cast<C3DAffineTransformation&>(*clone);

	BOOST_CHECK_EQUAL(spclone.get_size(), rtrans.get_size());

	C3DTransformation::const_iterator ic = spclone.begin();
	C3DTransformation::const_iterator ec = spclone.end();

	C3DTransformation::const_iterator io = rtrans.begin();

	while (ic != ec ) {
		BOOST_CHECK_EQUAL(*ic, *io);
		++ic;
		++io;
	}
}

BOOST_FIXTURE_TEST_CASE (test_invers, ipfFixture)
{
	C3DBounds size(10,20, 30); 
	C3DAffineTransformation trans(size, ipf); 

	auto params = trans.get_parameters(); 
	BOOST_REQUIRE(params.size()== 12); 

	const float init_matrix[12] = {1,2,3,4, 2,3,1,2, 3, 2, 2, 2}; 
	const float inv_matrix[12]  = { 
		-4.0/13.0, -2.0/13.0,  7.0/13.0,   6.0/13.0, 
		 1.0/13.0,  7.0/13.0, -5.0/13.0,  -8.0/13.0, 
		 5.0/13.0, -4.0/13.0,  1.0/13.0, -14.0/13.0
	}; 
	copy(init_matrix, init_matrix+12, params.begin()); 

	
	trans.set_parameters(params); 
	
	unique_ptr<C3DTransformation> inverse( trans.invert()); 
	BOOST_CHECK_EQUAL(inverse->get_size(), size);

	params = inverse->get_parameters(); 
	BOOST_REQUIRE(params.size()== 12); 
	for(int i = 0; i < 12; ++i) 
		BOOST_CHECK_CLOSE(params[i], inv_matrix[i], 0.1); 
	
}
