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


NS_MIA_BEGIN
using namespace std;

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

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

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

void C2DRigidTransformation::initialize()
{
       m_rot_center = C2DFVector(m_size - C2DBounds::_1) * m_relative_rot_center;
       cvdebug() << "m_rot_center= " << m_rot_center << "\n";
}

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



C2DFVector C2DRigidTransformation::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 *C2DRigidTransformation::do_clone()const
{
       return new C2DRigidTransformation(*this);
}

C2DTransformation *C2DRigidTransformation::invert()const
{
       C2DRigidTransformation *result = new C2DRigidTransformation(*this);
       result->m_rotation = -m_rotation;
       const double sina = sin(m_rotation);
       const double cosa = cos(m_rotation);
       result->m_translation.x = - cosa * m_translation.x - sina * m_translation.y;
       result->m_translation.y =   sina * m_translation.x - cosa * m_translation.y;
       result->m_matrix_valid = false;
       return result;
}


size_t C2DRigidTransformation::degrees_of_freedom() const
{
       return 3;
}

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

void C2DRigidTransformation::translate(float x, float y)
{
       m_matrix_valid = false;
       m_translation.x +=  x;
       m_translation.y +=  y;
}

void C2DRigidTransformation::rotate(float angle)
{
       m_matrix_valid = false;
       m_rotation += angle;
       const double sina = sin(angle);
       const double cosa = cos(angle);
       const double tx      = cosa * m_translation.x - sina * m_translation.y;
       m_translation.y = sina * m_translation.x + cosa * m_translation.y;
       m_translation.x = tx;
}

CDoubleVector C2DRigidTransformation::get_parameters() const
{
       CDoubleVector result(degrees_of_freedom());
       result[0] = m_translation.x;
       result[1] = m_translation.y;
       result[2] = m_rotation;
       return result;
}

void C2DRigidTransformation::set_parameters(const CDoubleVector& params)
{
       assert(degrees_of_freedom() == params.size());
       m_translation.x = params[0];
       m_translation.y = params[1];
       m_rotation = params[2];
       cvdebug() << "Rigid transform = (" << m_translation << ", " << m_rotation << ")\n";
       m_matrix_valid = false;
}

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

P2DTransformation C2DRigidTransformation::do_upscale(const C2DBounds& size) const
{
       C2DFVector new_trans(float(size.x) / (float)get_size().x * m_translation.x,
                            float(size.y) / (float)get_size().y * m_translation.y);
       return P2DTransformation(new C2DRigidTransformation(size, new_trans, m_rotation,
                                m_relative_rot_center, get_interpolator_factory()));
}

C2DFMatrix C2DRigidTransformation::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 C2DRigidTransformation::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 C2DRigidTransformation::set_identity()
{
       cvdebug() << "set identity\n";
       m_translation.x = m_translation.y = m_rotation = 0.0;
}

void C2DRigidTransformation::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_translation.x - d.x + m_rot_center.x;
       m_t[5] = m_translation.y - d.y + m_rot_center.y;
       m_matrix_valid = true;
}

float C2DRigidTransformation::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 C2DRigidTransformation::operator () (const C2DFVector& x) const
{
       return transform(x);
}

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

void C2DRigidTransformation::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] += g->x;
                     r[1] += g->y;
                     r[2] += - fy * g->x + fx * g->y;
              }
       }

       copy(r.begin(), r.end(), params.begin());
}



C2DRigidTransformation::iterator_impl::iterator_impl(const C2DBounds& pos, const C2DBounds& size,
              const C2DRigidTransformation& 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 *C2DRigidTransformation::iterator_impl::clone() const
{
       return new iterator_impl(get_pos(), get_size(), m_trans);
}

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

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

void C2DRigidTransformation::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 C2DRigidTransformation::begin() const
{
       return C2DTransformation::const_iterator(new iterator_impl(C2DBounds(0, 0), get_size(), *this));
}

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

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

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

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

C2DRigidTransformCreatorPlugin::C2DRigidTransformCreatorPlugin():
       C2DTransformCreatorPlugin("rigid")
{
       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 *C2DRigidTransformCreatorPlugin::do_create(const C2DInterpolatorFactory& ipf) const
{
       return new C2DRigidTransformCreator(m_relative_rot_center, ipf);
}

const std::string C2DRigidTransformCreatorPlugin::do_get_descr() const
{
       return "Rigid transformations (i.e. rotation and translation, three degrees of freedom).";
}

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


NS_MIA_END
