/* -*- 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 <cmath>
#include <mia/core/msgstream.hh>
#include <mia/3d/transform/rotbend.hh>

namespace mia_3dtransform_rotbend
{

using namespace mia;
using namespace std;



C3DFVector C3DRotBendTransformation::get_displacement_at(const C3DFVector& x) const
{
       return transform(x) - x;
}



C3DFVector C3DRotBendTransformation::transform(const C3DFVector& x)const
{
       int offset = m_norot ? 0 : 2;
       auto y = m_pre_matrix * x;

       if (y.y > 0) {
              const float max_distance = get_size().y - m_rotation_center.y;
              y.z += y.y * y.y * m_params[offset] / (max_distance * max_distance + 1);
       } else if (y.y < 0) {
              const float max_distance = m_rotation_center.y;
              y.z += y.y * y.y * m_params[offset + 1] / (max_distance * max_distance + 1);
       }

       return m_post_matrix * y;
}

C3DRotBendTransformation::C3DRotBendTransformation(const C3DBounds& size, const C3DFVector& orig, bool norot,
              const C3DInterpolatorFactory& ipf):
       C3DTransformation(ipf),
       m_params(norot ? 2 : 4),
       m_relative_origin(orig),
       m_rotation_center(C3DFVector(size) * m_relative_origin),
       m_size(size),
       m_norot(norot)
{
       m_pre_matrix.identity();
       m_post_matrix.identity();
}

C3DTransformation *C3DRotBendTransformation::do_clone()const
{
       return new C3DRotBendTransformation(*this);
}

C3DTransformation *C3DRotBendTransformation::invert()const
{
       C3DRotBendTransformation *result = new C3DRotBendTransformation(*this);
       assert(0 && "not implemented");
       return result;
}

size_t C3DRotBendTransformation::degrees_of_freedom() const
{
       return m_params.size();
}

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

CDoubleVector C3DRotBendTransformation::get_parameters() const
{
       CDoubleVector result(degrees_of_freedom());
       copy(m_params.begin(), m_params.end(), result.begin());
       return result;
}

void C3DRotBendTransformation::set_parameters(const CDoubleVector& params)
{
       assert(degrees_of_freedom() == params.size());
       copy(params.begin(), params.end(), m_params.begin());
       cvmsg() << "Params: " << m_params << "\n";
       m_pre_matrix.identity();
       m_post_matrix.identity();
       m_pre_matrix.translate( -1.0f * m_rotation_center);

       if (!m_norot)  {
              m_pre_matrix.rotate_x(m_params[0]);
              m_pre_matrix.rotate_y(m_params[1]);
       }

       m_post_matrix.translate(m_rotation_center);
}

const C3DBounds& C3DRotBendTransformation::get_size() const
{
       return m_size;
}

P3DTransformation C3DRotBendTransformation::do_upscale(const C3DBounds& size) const
{
       auto result = new C3DRotBendTransformation(size, m_relative_origin, m_norot, get_interpolator_factory());
       result->set_parameters(m_params);
       return P3DTransformation(result);
}


C3DFMatrix C3DRotBendTransformation::derivative_at(const C3DFVector& MIA_PARAM_UNUSED(x)) const
{
       assert(0 && "not implemented");
}

C3DFMatrix C3DRotBendTransformation::derivative_at(int /*x*/, int /*y*/, int /*z*/) const
{
       assert(0 && "not implemented");
}

void C3DRotBendTransformation::set_identity()
{
       cvdebug() << "set identity\n";
       m_pre_matrix.identity();
       m_post_matrix.identity();
       fill(m_params.begin(), m_params.end(), 0.0);
}

float C3DRotBendTransformation::get_max_transform() const
{
       C3DFVector corners[7] = {
              C3DFVector(get_size().x, 0, 0),
              C3DFVector(get_size().x, get_size().y,            0),
              C3DFVector(           0, get_size().y,            0),
              C3DFVector(           0, get_size().y, get_size().z),
              C3DFVector(get_size().x,            0, get_size().z),
              C3DFVector(           0,            0, get_size().z),
              C3DFVector(get_size())
       };
       float result = transform(C3DFVector()).norm2();

       for (int i = 0; i < 7; ++i) {
              float h = (transform(corners[i]) - corners[i]).norm2();

              if (result < h)
                     result = h;
       }

       return sqrt(result);
}


C3DFVector C3DRotBendTransformation::operator () (const C3DFVector& x) const
{
       return transform(x);
}

float C3DRotBendTransformation::get_jacobian(const C3DFVectorfield& /*v*/, float /*delta*/) const
{
       DEBUG_ASSERT_RELEASE_THROW(false, "C3DRotBendTransformation doesn't implement a jacobian.");
}

void C3DRotBendTransformation::translate(const C3DFVectorfield& MIA_PARAM_UNUSED(gradient), CDoubleVector& MIA_PARAM_UNUSED(params)) const
{
       assert(0 && !"not yet implemented");
}



C3DRotBendTransformation::iterator_impl::iterator_impl(const C3DBounds& pos, const C3DBounds& size,
              const C3DRotBendTransformation& trans):
       C3DTransformation::iterator_impl(pos, size),
       m_trans(trans),
       m_value(trans.transform(C3DFVector(pos)))
{
       m_dx = m_trans.transform(C3DFVector(pos.x + 1.0, pos.y, pos.z)) - m_value;
}

C3DRotBendTransformation::iterator_impl::iterator_impl(const C3DBounds& pos, const C3DBounds& begin,
              const C3DBounds& end, const C3DBounds& size,
              const C3DRotBendTransformation& trans):
       C3DTransformation::iterator_impl(pos, begin, end, size),
       m_trans(trans),
       m_value(trans.transform(C3DFVector(pos)))
{
       m_dx = m_trans.transform(C3DFVector(pos.x + 1.0, pos.y, pos.z)) - m_value;
}

C3DTransformation::iterator_impl *C3DRotBendTransformation::iterator_impl::clone() const
{
       return new iterator_impl(*this);
}

const C3DFVector&  C3DRotBendTransformation::iterator_impl::do_get_value()const
{
       return m_value;
}

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

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

void C3DRotBendTransformation::iterator_impl::do_z_increment()
{
       m_value = m_trans.transform(C3DFVector(get_pos()));
       m_dx = m_trans.transform(C3DFVector(get_pos().x + 1.0, get_pos().y, get_pos().z)) - m_value;
}


C3DTransformation::const_iterator C3DRotBendTransformation::begin() const
{
       return C3DTransformation::const_iterator(new iterator_impl(C3DBounds(0, 0, 0), get_size(), *this));
}

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

C3DTransformation::const_iterator C3DRotBendTransformation::begin_range(const C3DBounds& begin, const C3DBounds& end) const
{
       return C3DTransformation::const_iterator(new iterator_impl(begin, begin, end, get_size(), *this));
}

C3DTransformation::const_iterator C3DRotBendTransformation::end_range(const C3DBounds& begin, const C3DBounds& end) const
{
       return C3DTransformation::const_iterator(new iterator_impl(end, begin, end, get_size(), *this));
}



float C3DRotBendTransformation::pertuberate(C3DFVectorfield& /*v*/) const
{
       DEBUG_ASSERT_RELEASE_THROW(false, "C3DRotBendTransformation doesn't implement pertuberate.");
}

C3DRotBendTransformCreator::C3DRotBendTransformCreator(const C3DFVector& origin, int norot,
              const C3DInterpolatorFactory& ipf):
       C3DTransformCreator(ipf),
       m_origin(origin),
       m_norot(norot)
{
}

P3DTransformation C3DRotBendTransformCreator::do_create(const C3DBounds& size,
              const C3DInterpolatorFactory& ipf) const
{
       return P3DTransformation(new C3DRotBendTransformation(size, m_origin, m_norot, ipf));
}




C3DRotBendTransformCreatorPlugin::C3DRotBendTransformCreatorPlugin():
       C3DTransformCreatorPlugin("rotbend"),
       m_norot(false)
{
       add_parameter("origin", new C3DFVectorParameter(m_origin, true, "center of the transformation"));
       add_parameter("norot", make_param(m_norot, false, "Don't optimize the rotation"));
}


C3DTransformCreator *C3DRotBendTransformCreatorPlugin::do_create(const C3DInterpolatorFactory& ipf) const
{
       return new C3DRotBendTransformCreator(m_origin, m_norot, ipf);
}

const std::string C3DRotBendTransformCreatorPlugin::do_get_descr() const
{
       return "Restricted transformation (4 degrees of freedom). "
              "The transformation is restricted to the rotation around the x and y axis "
              "and a bending along the x axis, independedn in each direction, with the bending "
              "increasing with the squared distance from the rotation axis."
              ;
}

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



}
