/* -*- 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/>.
 *
 */

#include <fstream>
#include <mia/core/msgstream.hh>
#include <mia/2d/transform/rotation.hh>


NS_MIA_BEGIN
using namespace std;

C2DRotationTransformation::C2DRotationTransformation(const C2DBounds& size, 
					       const C2DFVector& relative_rot_center, 
					       const C2DInterpolatorFactory& ipf):
	C2DTransformation(ipf),
	m_size(size),
	m_rotation(0.0),
	m_relative_rot_center(relative_rot_center), 
	m_t(6),
	m_matrix_valid(false)
{
	initialize();
}

C2DRotationTransformation::C2DRotationTransformation(const C2DBounds& size, double rotation, 
					       const C2DFVector& relative_rot_center,
					       const C2DInterpolatorFactory& ipf):
	C2DTransformation(ipf),
	m_size(size),
	m_rotation(rotation),
	m_relative_rot_center(relative_rot_center), 
	m_t(6),
	m_matrix_valid(false)
{
	initialize(); 
}

C2DRotationTransformation::C2DRotationTransformation(const C2DRotationTransformation& other):
	C2DTransformation(other),
	m_size(other.m_size),
	m_rotation(other.m_rotation),
	m_relative_rot_center(other.m_relative_rot_center), 
	m_rot_center(other.m_rot_center), 
	m_t(other.m_t),
	m_matrix_valid(other.m_matrix_valid)
{
}

void C2DRotationTransformation::initialize()
{
	m_rot_center = C2DFVector(m_size -C2DBounds::_1) * m_relative_rot_center; 
}

C2DFVector C2DRotationTransformation::get_displacement_at(const C2DFVector& x) const
{
	return x - transform(x);
}



C2DFVector C2DRotationTransformation::transform(const C2DFVector& x)const
{
	if (!m_matrix_valid)
		evaluate_matrix();

	return C2DFVector(
		m_t[0] * x.x + m_t[1] * x.y + m_t[2],
		m_t[3] * x.x + m_t[4] * x.y + m_t[5]
			  );
}


C2DTransformation *C2DRotationTransformation::do_clone()const
{
	return new C2DRotationTransformation(*this);
}

C2DTransformation *C2DRotationTransformation::invert()const
{
	C2DRotationTransformation *result = new C2DRotationTransformation(*this); 
	result->m_rotation = -m_rotation;  
	result->m_matrix_valid = false; 
	return result;
}


size_t C2DRotationTransformation::degrees_of_freedom() const
{
	return 1;
}

void C2DRotationTransformation::update(float /*step*/, const C2DFVectorfield& /*a*/)
{
	assert(0 && "not implemented");
}

void C2DRotationTransformation::rotate(double angle)
{
	m_matrix_valid = false;
	m_rotation += angle;
}

CDoubleVector C2DRotationTransformation::get_parameters() const
{
	CDoubleVector result(degrees_of_freedom());
	result[0] = m_rotation;
	return result;
}

void C2DRotationTransformation::set_parameters(const CDoubleVector& params)
{
	assert(degrees_of_freedom() == params.size());
	m_rotation = params[0];
	m_matrix_valid = false;
}


const C2DBounds& C2DRotationTransformation::get_size() const
{
	return m_size;
}

P2DTransformation C2DRotationTransformation::do_upscale(const C2DBounds& size) const
{
	return P2DTransformation(new C2DRotationTransformation(size, m_rotation, 
							    m_relative_rot_center, get_interpolator_factory()));
}

C2DFMatrix C2DRotationTransformation::derivative_at(const C2DFVector& MIA_PARAM_UNUSED(x)) const
{
	if (!m_matrix_valid)
		evaluate_matrix(); 
	
	return C2DFMatrix(C2DFVector(m_t[0], m_t[1]),
			  C2DFVector(m_t[3], m_t[4]));
}

C2DFMatrix C2DRotationTransformation::derivative_at(int MIA_PARAM_UNUSED(x), int MIA_PARAM_UNUSED(y)) const
{
	if (!m_matrix_valid)
		evaluate_matrix(); 
	
	return C2DFMatrix(C2DFVector(m_t[0], m_t[1]),
			  C2DFVector(m_t[3], m_t[4]));
}

void C2DRotationTransformation::set_identity()
{
	cvdebug() << "set identity\n";
	m_rotation = 0.0;
}

void C2DRotationTransformation::evaluate_matrix() const
{
	const double cosa = cos(m_rotation);
	const double sina = sin(m_rotation);

	m_t[0] = cosa;
	m_t[1] = -sina;
	m_t[3] = sina;
	m_t[4] = cosa;

	C2DFVector d(m_t[0] * m_rot_center.x + m_t[1] * m_rot_center.y,
		     m_t[3] * m_rot_center.x + m_t[4] * m_rot_center.y); 
		
	m_t[2] = m_rot_center.x - d.x;
	m_t[5] = m_rot_center.y - d.y;

	m_matrix_valid = true;
}

float C2DRotationTransformation::get_max_transform() const
{
	// check the corners
	float m =      (C2DFVector(get_size() - C2DBounds::_1) -  transform(C2DFVector(get_size()  - C2DBounds::_1))).norm2();
	float test0Y = (C2DFVector(0, get_size().y - 1) - transform(C2DFVector(0, get_size().y - 1))).norm2();
	float testX0 = (C2DFVector(get_size().x - 1, 0) - transform(C2DFVector(get_size().x - 1, 0))).norm2();
	float test00 = transform(C2DFVector(0, 0)).norm2();

	if (m < test0Y)
		m = test0Y;

	if (m < testX0)
		m = testX0;

	if (m < test00)
		m = test00;
	return sqrt(m);
}


C2DFVector C2DRotationTransformation::operator () (const C2DFVector& x) const
{
	return transform(x); 
}

float C2DRotationTransformation::get_jacobian(const C2DFVectorfield& /*v*/, float /*delta*/) const
{
	assert(!"not implemented");
}

void C2DRotationTransformation::translate(const C2DFVectorfield& gradient, CDoubleVector& params) const
{
	assert(gradient.get_size() == m_size);
	assert(params.size() == degrees_of_freedom());

	vector<double> r(params.size(), 0.0);

	auto g = gradient.begin();
	double fy = - m_rot_center.y; 
	for (size_t y = 0; y < m_size.y; ++y, fy += 1.0) {
		double fx = - m_rot_center.x; 
		for (size_t x = 0; x < m_size.x; ++x, fx += 1.0, ++g) {
			r[0] += - fy * g->x + fx * g->y; 
		}
	}
	params[0] = r[0];
}



C2DRotationTransformation::iterator_impl::iterator_impl(const C2DBounds& pos, const C2DBounds& size, 
						      const C2DRotationTransformation& trans):
	C2DTransformation::iterator_impl(pos, size),
	m_trans(trans), 
	m_value(trans.transform(C2DFVector(pos)))
{
	m_dx = m_trans.transform(C2DFVector(pos.x + 1.0, pos.y)) - m_value;
}

C2DTransformation::iterator_impl * C2DRotationTransformation::iterator_impl::clone() const
{
	return new iterator_impl(get_pos(), get_size(), m_trans); 
}

const C2DFVector&  C2DRotationTransformation::iterator_impl::do_get_value()const
{
	return m_value; 
}

void C2DRotationTransformation::iterator_impl::do_x_increment()
{
	m_value += m_dx; 
}

void C2DRotationTransformation::iterator_impl::do_y_increment()
{
	m_value = m_trans.transform(C2DFVector(get_pos())); 
	m_dx = m_trans.transform(C2DFVector(get_pos().x + 1.0, get_pos().y)) - m_value;
}


C2DTransformation::const_iterator C2DRotationTransformation::begin() const
{
	return C2DTransformation::const_iterator(new iterator_impl(C2DBounds(0,0), get_size(), *this)); 
}

C2DTransformation::const_iterator C2DRotationTransformation::end() const
{
	return C2DTransformation::const_iterator(new iterator_impl(get_size(), get_size(), *this)); 
}

float C2DRotationTransformation::pertuberate(C2DFVectorfield& /*v*/) const
{
	assert(!"not implemented");
}

C2DRotationTransformCreator::C2DRotationTransformCreator(const C2DFVector& relative_rot_center, const C2DInterpolatorFactory& ipf):
	C2DTransformCreator(ipf), 
	m_relative_rot_center(relative_rot_center)
{
}

P2DTransformation C2DRotationTransformCreator::do_create(const C2DBounds& size, const C2DInterpolatorFactory& ipf) const
{
	return P2DTransformation(new C2DRotationTransformation(size, m_relative_rot_center, ipf));
}

C2DRotationTransformCreatorPlugin::C2DRotationTransformCreatorPlugin():
	C2DTransformCreatorPlugin("rotation")
{
	add_parameter("rot-center", make_param(m_relative_rot_center, false, 
					       "Relative rotation center, i.e.  <0.5,0.5> corresponds "
					       "to the center of the support rectangle"));
}

C2DTransformCreator *C2DRotationTransformCreatorPlugin::do_create(const C2DInterpolatorFactory& ipf) const
{
	return new C2DRotationTransformCreator(m_relative_rot_center, ipf);
}

const std::string C2DRotationTransformCreatorPlugin::do_get_descr() const
{
	return "Rotation transformations (i.e. rotation about a given center, one degree of freedom).";
}

extern "C" EXPORT CPluginBase *get_plugin_interface()
{
	return new C2DRotationTransformCreatorPlugin();
}


NS_MIA_END
