/*
 * This member function is defined in a separate source file to allow different
 * optimization options when using clang as it does not allow per function
 * optimizations as does gcc function attributes.
 */
#include <cmath>
#include <tuple>
#include <boost/math/tools/roots.hpp>
#include "bicycle/bicycle.h"
#include "constants.h"

namespace {
    inline model::real_t square(model::real_t x) {
        return x*x;
    }
}

namespace model {

real_t Bicycle::solve_constraint_pitch(real_t roll, real_t steer, real_t guess, size_t max_iterations) const {
    // constraint function generated by script 'generate_pitch.py'.
    static constexpr int digits = std::numeric_limits<real_t>::digits*2/3;
    static constexpr real_t two = static_cast<real_t>(2.0);
    static constexpr real_t one_five = static_cast<real_t>(1.5);
    static constexpr real_t min = static_cast<real_t>(0.0);
    static constexpr real_t max = constants::pi/2;
    boost::uintmax_t max_it = max_iterations;

    auto constraint_function = [this, roll, steer](real_t pitch)->std::tuple<real_t, real_t> {
        return std::make_tuple(
((m_rf*square(std::cos(pitch))*square(std::cos(roll)) +
(m_d3*std::sqrt(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) + std::sin(roll)*std::sin(steer)) +
square(std::cos(pitch))*square(std::cos(roll))) +
m_rf*(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)))*(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)))*std::abs(std::cos(roll)) +
std::sqrt(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) + std::sin(roll)*std::sin(steer)) +
square(std::cos(pitch))*square(std::cos(roll)))*(-m_d1*std::abs(std::cos(roll))*std::sin(pitch) +
m_d2*std::abs(std::cos(roll))*std::cos(pitch) -
m_rr*std::cos(roll))*std::cos(roll))/(std::sqrt(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)) + square(std::cos(pitch))*square(std::cos(roll)))*std::abs(std::cos(roll)))
                ,
((m_rf*square(std::cos(pitch))*square(std::cos(roll)) +
(m_d3*std::sqrt(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) + std::sin(roll)*std::sin(steer)) +
square(std::cos(pitch))*square(std::cos(roll))) +
m_rf*(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)))*(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)))*std::abs(std::cos(roll)) +
std::sqrt(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) + std::sin(roll)*std::sin(steer)) +
square(std::cos(pitch))*square(std::cos(roll)))*(-m_d1*std::abs(std::cos(roll))*std::sin(pitch) +
m_d2*std::abs(std::cos(roll))*std::cos(pitch) -
m_rr*std::cos(roll))*std::cos(roll))*((-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer))*std::cos(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(pitch)*std::cos(pitch)*square(std::cos(roll)))/(std::pow(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)) +
square(std::cos(pitch))*square(std::cos(roll)), one_five)*std::abs(std::cos(roll))) +
((-m_d1*std::abs(std::cos(roll))*std::cos(pitch) -
m_d2*std::abs(std::cos(roll))*std::sin(pitch))*std::sqrt(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)) + square(std::cos(pitch))*square(std::cos(roll)))*std::cos(roll) +
(-(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer))*std::cos(pitch)*std::cos(roll)*std::cos(steer) -
std::sin(pitch)*std::cos(pitch)*square(std::cos(roll)))*(-m_d1*std::abs(std::cos(roll))*std::sin(pitch) +
m_d2*std::abs(std::cos(roll))*std::cos(pitch) -
m_rr*std::cos(roll))*std::cos(roll)/std::sqrt(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)) + square(std::cos(pitch))*square(std::cos(roll))) +
(-two*m_rf*std::sin(pitch)*std::cos(pitch)*square(std::cos(roll)) -
(m_d3*std::sqrt(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) + std::sin(roll)*std::sin(steer)) +
square(std::cos(pitch))*square(std::cos(roll))) +
m_rf*(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)))*std::cos(pitch)*std::cos(roll)*std::cos(steer) +
(m_d3*(-(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer))*std::cos(pitch)*std::cos(roll)*std::cos(steer) -
std::sin(pitch)*std::cos(pitch)*square(std::cos(roll)))/std::sqrt(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)) +
square(std::cos(pitch))*square(std::cos(roll))) -
m_rf*std::cos(pitch)*std::cos(roll)*std::cos(steer))*(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)))*std::abs(std::cos(roll)))/(std::sqrt(square(-std::sin(pitch)*std::cos(roll)*std::cos(steer) +
std::sin(roll)*std::sin(steer)) +
square(std::cos(pitch))*square(std::cos(roll)))*std::abs(std::cos(roll)))
                );
    };
    return boost::math::tools::newton_raphson_iterate(constraint_function, guess, min, max, digits, max_it);
}

} // namespace model


#include <opengv/optimization_tools/objective_function_tools/OptimalUPnPFunctionInfo.hpp>
#include <Eigen/Dense>
#include <opengv/Indices.hpp>
#include <iostream>

OptimalUPnPFunctionInfo::OptimalUPnPFunctionInfo(const opengv::absolute_pose::AbsoluteAdapterBase & adapter){
  //Initialize class members
  Mr  = Eigen::MatrixXd::Zero(9,9);
  vr  = Eigen::MatrixXd::Zero(9,1);;
  Mrt = Eigen::MatrixXd::Zero(9,3);
  vt  = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd constant = Eigen::MatrixXd::Zero(1,1); //Used for debugging
  n   = 0;
  opengv::Indices indices(adapter.getNumberCorrespondences());
  int total_points = (int) indices.size();
  n = total_points;

  //Used to store all the information needed
  Eigen::MatrixXd C_all = Eigen::MatrixXd::Zero(3, total_points);
  Eigen::MatrixXd V_all = Eigen::MatrixXd::Zero(3, total_points);
  Eigen::MatrixXd X_all = Eigen::MatrixXd::Zero(3, total_points);
  //**************************
  Eigen::Matrix3d id = Eigen::Matrix3d::Identity(3,3);
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * n, n + 3);
  // This time all the information is needed beforehand
  for( int i = 0; i < total_points; i++ )
  {
    C_all.block<3,1>(0,i)  = adapter.getCamOffset(indices[i]);
    X_all.block<3,1>(0,i)  = adapter.getPoint(indices[i]);
    V_all.block<3,1>(0,i)  = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
    A.block<3,1>(3 * i, i) = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
    A.block<3,3>(3 * i, n) = -Eigen::Matrix3d::Identity(3,3);
  }
  //std::cout << "A matrix: " << std::endl << A << std::endl;
  Eigen::MatrixXd U = ((A.transpose() * A).inverse() * A.transpose() ).block(0,0, n, 3 * n);
  //std::cout << "U" << std::endl << U << std::endl;
  /*std::cout << "Data:" << std::endl;
  std::cout << "ci" << std::endl << C_all << std::endl;
  std::cout << "xi" << std::endl << X_all << std::endl;
  std::cout << "vi" << std::endl << V_all << std::endl;*/

  Eigen::MatrixXd D = Eigen::MatrixXd::Zero(3,9);
  Eigen::MatrixXd v = Eigen::MatrixXd::Zero(3,1);

  Eigen::MatrixXd fi = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd pi = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd vi = Eigen::MatrixXd::Zero(3,1);

  Eigen::MatrixXd fj      = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd pj      = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd vj      = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd u_total = Eigen::MatrixXd::Zero(1, n);
  Eigen::MatrixXd uij     = Eigen::MatrixXd::Zero(3,1);

  //Start building the D matrix and v vector from wich the residual can be calculated
  for(int i = 0; i < n; ++i) {

    fi = V_all.block<3,1>(0, i);
    pi = X_all.block<3,1>(0, i);
    vi = C_all.block<3,1>(0, i);
    u_total = U.block(i,0, 1, 3 * n);
    double internal_coefficient = 0;
    Eigen::MatrixXd internal_vector = Eigen::MatrixXd::Zero(1,9);
    D = Eigen::MatrixXd::Zero(3,9);
    v = Eigen::MatrixXd::Zero(3,1);
    for(int j = 0; j < n; ++j){
      uij = u_total.block(0, 3 * j , 1, 3);
      pj = X_all.block<3,1>(0, j);
      vj = C_all.block<3,1>(0, j);
      internal_vector.block<1,3>(0,0) = pj(0,0) * uij;
      internal_vector.block<1,3>(0,3) = pj(1,0) * uij;
      internal_vector.block<1,3>(0,6) = pj(2,0) * uij;

      internal_coefficient = internal_coefficient + (uij * vj)(0,0);

      D = D + (fi * internal_vector);
    }

    Eigen::MatrixXd Dr_i = Eigen::MatrixXd::Zero(3,9);
    Dr_i.block<3,3>(0,0) = pi(0,0) * Eigen::MatrixXd::Identity(3,3);
    Dr_i.block<3,3>(0,3) = pi(1,0) * Eigen::MatrixXd::Identity(3,3);
    Dr_i.block<3,3>(0,6) = pi(2,0) * Eigen::MatrixXd::Identity(3,3);
    D = D - Dr_i;
    v = vi - fi * internal_coefficient;

    Mr  = Mr  + D.transpose() * D;
    Mrt = Mrt - 2 * D.transpose();
    vr  = vr  + 2 * D.transpose() * v;
    vt  = vt  - 2 * v;
    constant = constant + v.transpose() * v;
  }

  /*Eigen::MatrixXd r = Eigen::MatrixXd::Zero(9,1);
  r(0,0) = rot(0,0);
  r(1,0) = rot(1,0);
  r(2,0) = rot(2,0);
  r(3,0) = rot(0,1);
  r(4,0) = rot(1,1);
  r(5,0) = rot(2,1);
  r(6,0) = rot(0,2);
  r(7,0) = rot(1,2);
  r(8,0) = rot(2,2);*/

  //std::cout << "The squared residual: " << std::endl;
  //std::cout << (r.transpose() * Mr * r + vr.transpose() * r + r.transpose() * Mrt * trans + vt.transpose() * trans + constant + n * trans.transpose() * trans) << std::endl;
  /*std::cout << "Mr:  "      << std::endl << Mr       << std::endl;
  std::cout << "vr:  "      << std::endl << vr       << std::endl;
  std::cout << "Mrt: "      << std::endl << Mrt      << std::endl;
  std::cout << "vt:  "      << std::endl << vt       << std::endl;
  std::cout << "n:   "      << std::endl << n        << std::endl;
  std::cout << "constant: " << std::endl << constant << std::endl;
  std::cout << "R_: "       << std::endl << rot      << std::endl;
  std::cout << "t_: "       << std::endl << trans    << std::endl;*/
}

OptimalUPnPFunctionInfo::~OptimalUPnPFunctionInfo(){};

double OptimalUPnPFunctionInfo::objective_function_value(const opengv::rotation_t & rotation, const opengv::translation_t & translation){
  const double * p = &rotation(0);
  Map<const Matrix<double,1,9> > r(p, 1, 9);
  Eigen::MatrixXd e = (r * Mr * r.transpose() + vr.transpose() * r.transpose() + r * Mrt * translation + vt.transpose() * translation + n * translation.transpose() * translation);
  return ( e(0,0) );
}

opengv::rotation_t OptimalUPnPFunctionInfo::rotation_gradient(const opengv::rotation_t & rotation, const opengv::translation_t & translation){
  const double * p = &rotation(0);
  Map<const Matrix<double,1,9> > r(p, 1, 9);
  Eigen::MatrixXd result = (2 * Mr * r.transpose()) + ( Mrt * translation ) + vr;
  double * ptr = &result(0);
  Map<Matrix<double, 3,3> > m(ptr, 3, 3);
  return m ;
}

opengv::translation_t OptimalUPnPFunctionInfo::translation_gradient(const opengv::rotation_t & rotation, const opengv::translation_t & translation){
  const double * p = &rotation(0);
  Map<const Matrix<double,1,9> > r(p, 1, 9);
  return (  2 * n * translation + Mrt.transpose() * r.transpose() + vt )  ;
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

**/
//==================================================================================================
#ifndef BOOST_SIMD_HYPERBOLIC_HPP_INCLUDED
#define BOOST_SIMD_HYPERBOLIC_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-hyperbolic  Hyperbolic functions

    Those functions provides scalar and SIMD version of
    hyperbolic  and inverse hyperbolic functions.
  **/

  /*!
    @ingroup group-callable
    @defgroup group-callable-hyperbolic Hyperbolic Callable Objects
    Callable objects version of @ref group-hyperbolic

    Their specific semantic limitations are similar to those of their function
    equivalents as described in the @ref group-hyperbolic section.
  **/
} }

#include <boost/simd/function/acosh.hpp>
#include <boost/simd/function/acoth.hpp>
#include <boost/simd/function/acsch.hpp>
#include <boost/simd/function/asech.hpp>
#include <boost/simd/function/asinh.hpp>
#include <boost/simd/function/atanh.hpp>
#include <boost/simd/function/cosh.hpp>
#include <boost/simd/function/coth.hpp>
#include <boost/simd/function/csch.hpp>
#include <boost/simd/function/sech.hpp>
#include <boost/simd/function/sinhc.hpp>
#include <boost/simd/function/sinhcosh.hpp>
#include <boost/simd/function/sinh.hpp>
#include <boost/simd/function/tanh.hpp>

#endif


/*--------------------------------------------------------------------

Search for LCG multiplier using the LLL-Spectral Test!  January 2000

Authors: Karl Entacher, Karl.Entacher@fh-sbg.ac.at

         Thomas Schell, Dept. of Scientific Computing, Univ. Salzburg

--------------------------------------------------------------------*/

// Modified by e 2018-05-10 for LCG with power of two modulus

/*-------------Libraries (needs Victor Shoups NTL-Lib----------------*/

#include <iostream>

#include <fstream>

#include <time.h>

#include <sys/resource.h>

#include <math.h>

#include <float.h>

#include <NTL/ZZ.h>

#include <NTL/RR.h>

#include <NTL/mat_ZZ.h>

#include <NTL/LLL.h>

using namespace std;
using namespace NTL;

/*------------------------ Definitions ------------------------------*/

ZZ lambda, modul, seed;

struct {

  ZZ lambda;

  double min_norm_x_x;

} best;

#define MAX_DIMS 24

#define DIMS 24 // TODO: make this a command line arg

double fact[MAX_DIMS];

RR fact_RR[MAX_DIMS];

const int x_xmax = DIMS;       /* Test for dimensions <= DIMS */

const ZZ n_min=to_ZZ(1);

//const ZZ n_max=to_ZZ(10000);
const ZZ n_max=to_ZZ(1000000);

/*-----------------------------------------------------------------------

   Call the function with:

        lll_search ["output-file"] [power-of-two-modulus] [seed]

   Example:

        lll_esearch "output-file" 340282366920938463463374607431768211456 3

  ----------------------------------------------------------------------*/

int main(int argc, char *argv[])
{

/*--------------------------    Input-Check        -----------------------*/

    if (!(argc == 4))
    {
        cout << "lll_search_rnd <output-file-name> <power-of-two-modulus> <seed>" << endl;
        exit(0);
    }

/*--------- reads the input-parameters: "file" modulus seed ------*/

    ofstream out_file(argv[1], ios::out);

    modul = to_ZZ(argv[2]);

    //seed = to_ZZ(argv[3]);
    SetSeed((const unsigned char *)argv[3], strlen(argv[3]));

    long blen = (long )(log(modul)/log(2.0)) - 2L;

/*------------- output of modulus and seed ---------------*/

    out_file << modul;

    //out_file << seed;

    out_file << " " << blen;

    out_file << " " << RandomBits_ZZ(blen);

    out_file << endl;

/*------------- Constants for the normalized Spectral Test   -----------------*/

    fact[0] = 0.0;                                // intentionally left uninitialized -> not used

#if 0

    fact[1] = to_double(to_RR(1.0) / pow(to_RR(4.0/3.0), to_RR(1.0/4.0)) / pow(to_RR(modul), to_RR(1.0/2.0)));

    fact[2] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(1.0/6.0)) / pow(to_RR(modul), to_RR(1.0/3.0)));

    fact[3] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(1.0/4.0)) / pow(to_RR(modul), to_RR(1.0/4.0)));

    fact[4] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(3.0/10.0)) / pow(to_RR(modul), to_RR(1.0/5.0)));

    fact[5] = to_double(to_RR(1.0) / pow(to_RR(64.0/3.0), to_RR(1.0/12.0)) / pow(to_RR(modul), to_RR(1.0/6.0)));

    fact[6] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(3.0/7.0)) / pow(to_RR(modul), to_RR(1.0/7.0)));

    fact[7] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(1.0/2.0)) / pow(to_RR(modul), to_RR(1.0/8.0)));

#else

  fact_RR[0] = to_RR(0.0);  // intentionally left uninitialized -> not used

  fact_RR[1] = to_RR(0.2886751345948128822545744);

  fact_RR[2] = to_RR(0.1767766952966368811002111);

  fact_RR[3] = to_RR(0.125);

  fact_RR[4] = to_RR(0.0883883476483184405501055);

  fact_RR[5] = to_RR(0.0721687836487032205636436);

  fact_RR[6] = to_RR(0.0625);

  fact_RR[7] = to_RR(0.0625);

  fact_RR[8] = to_RR(0.06007);

  fact_RR[9] = to_RR(0.05953);

  fact_RR[10] = to_RR(0.06136);

  fact_RR[11] = to_RR(0.06559);

  fact_RR[12] = to_RR(0.07253);

  fact_RR[13] = to_RR(0.08278);

  fact_RR[14] = to_RR(0.09735);

  fact_RR[15] = to_RR(0.11774);

  fact_RR[16] = to_RR(0.14624);

  fact_RR[17] = to_RR(0.18629);

  fact_RR[18] = to_RR(0.24308);

  fact_RR[19] = to_RR(0.32454);

  fact_RR[20] = to_RR(0.44289);

  fact_RR[21] = to_RR(0.61722);

  fact_RR[22] = to_RR(0.87767);

  fact_RR[23] = to_RR(1.27241);

  for (int i = 1; i < x_xmax; i++)
  {
    fact[i] = to_double(1 / (to_RR(2) * pow(fact_RR[i] * to_RR(modul), to_RR(1.0) / to_RR(i+1))));
  }

#endif

/*------------Search, Matrix (Basis) Input, LLL und Output -------------*/

    best.lambda = to_ZZ(0);

    best.min_norm_x_x = 0.0;

    ZZ t_h = to_ZZ(10);

    struct rusage cur_ru;

    for (ZZ n = n_min; n <= n_max; n++)
    {
        double min_norm_x_x = 0.0;

        lambda = RandomBits_ZZ(blen) * 4 + 1;

        mat_ZZ x;

        x.SetDims(x_xmax, x_xmax);

        min_norm_x_x = 1.0;

        for (int j = 2; j <= x_xmax; j++)
        {

            x.SetDims(j,j);

            x[0][0] = modul;     // first index = rows, second index = columns

            for (int i = 1; i < j; i++)          // fill in the 1s

                x[i][i] = 1;

            for (int i = 1; i < j; i++)

                x[i][0] =-(power(lambda, i));

            ZZ det, rg;

            rg = LLL(det, x, 0);

            double min_x_x = to_double(x[0] * x[0]);

            for (int i = 1; i < j; i++)
            {
                double x_x = to_double(x[i] * x[i]);

                if (min_x_x > x_x)

                    min_x_x = x_x;
            }

            double norm_x_x = fact[j-1] * sqrt(min_x_x);

            if (min_norm_x_x > norm_x_x)

                min_norm_x_x = norm_x_x;
        }

        if (min_norm_x_x > best.min_norm_x_x)
        {
            best.min_norm_x_x = min_norm_x_x;

            best.lambda = lambda;

            getrusage(RUSAGE_SELF, &cur_ru);

            out_file << "time\t" << cur_ru.ru_utime.tv_sec << "\tn\t" << n << "\tl\t" << lambda << "\t" << min_norm_x_x << endl;
        }

        ZZ n_ = n - n_min;

        if (n_ >= t_h)
        {
            getrusage(RUSAGE_SELF, &cur_ru);

            cout << n_ << "\t" << cur_ru.ru_utime.tv_sec << endl;

            t_h *= 10;
        }
    }

    getrusage(RUSAGE_SELF, &cur_ru);

    cout << "total time elapsed\t" << cur_ru.ru_utime.tv_sec << endl;

    out_file << "time\t" << cur_ru.ru_utime.tv_sec << "\tl\t" << best.lambda << "\t" << best.min_norm_x_x << endl;

    out_file.close();

}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <boost/make_shared.hpp>
#include "tudat/astro/gravitation/gravityFieldModel.h"

namespace tudat
{
namespace gravitation
{

//! Set predefined central gravity field settings.
std::shared_ptr< GravityFieldModel > getPredefinedCentralGravityField(
    BodiesWithPredefinedCentralGravityFields bodyWithPredefinedCentralGravityField )
{
    double gravitationalParameter = 0.0;

    // Select body with prefined central gravity field.
    switch( bodyWithPredefinedCentralGravityField )
    {
    case sun:

        // Set gravitational parameter [m^3 s^-2].
        // Reference: http://ssd.jpl.nasa.gov/?constants#ref.
        gravitationalParameter = 1.32712440018e20;

        break;

    case mercury:

        // Set gravitational parameter [m^3 s^-2].
        // Reference: Mass taken from Table 1.2, pg. 6, (de Pater, 2010), value
        //            of gravitational constant taken from
        //            http://ssd.jpl.nasa.gov/?constants#ref.
        gravitationalParameter = 2.203289218e13;

        break;

    case venus:

        // Set gravitational parameter [m^3 s^-2].
        // Reference: Mass taken from Table 1.2, pg. 6, (de Pater, 2010), value
        //            of gravitational constant taken from
        //            http://ssd.jpl.nasa.gov/?constants#ref.
        gravitationalParameter = 3.2485504415e14;

        break;

    case earth:

        // Set gravitational parameter [m^3 s^-2].
        // Reference: Mass taken from Table 1.2, pg. 6, (de Pater, 2010), value
        //            of gravitational constant taken from
        //            http://ssd.jpl.nasa.gov/?constants#ref.
        gravitationalParameter = 3.9859383624e14;

        break;

    case moon:

        // Set gravitational parameter [m^3 s^-2].
        // Reference: Mass taken from Table 1.2, pg. 6, (de Pater, 2010), value
        //            of gravitational constant taken from
        //            http://ssd.jpl.nasa.gov/?constants#ref.
        gravitationalParameter = 4.903686391e12;

        break;

    case mars:

        // Set gravitational parameter [m^3 s^-2].
        // Reference: Mass taken from Table 1.2, pg. 6, (de Pater, 2010), value
        //            of gravitational constant taken from
        //            http://ssd.jpl.nasa.gov/?constants#ref.
        gravitationalParameter = 4.2828018915e13;

        break;

    case jupiter:

        // Set gravitational parameter [m^3 s^-2].
        // Reference: Mass taken from Table 1.3, pg. 6, (de Pater, 2010), value
        //            of gravitational constant taken from
        //            http://ssd.jpl.nasa.gov/?constants#ref.
        gravitationalParameter = 1.2668579374e17;

        break;

    case saturn:

        // Set gravitational parameter [m^3 s^-2].
        // Reference: Mass taken from Table 1.3, pg. 6, (de Pater, 2010), value
        //            of gravitational constant taken from
        //            http://ssd.jpl.nasa.gov/?constants#ref.
        gravitationalParameter = 3.793100511400001e16;

        break;

    case uranus:

        // Set gravitational parameter [m^3 s^-2].
        // Reference: Mass taken from Table 1.3, pg. 6, (de Pater, 2010), value
        //            of gravitational constant taken from
        //            http://ssd.jpl.nasa.gov/?constants#ref.
        gravitationalParameter = 5.793943348799999e15;

        break;

    case neptune:

        // Set gravitational parameter [m^3 s^-2].
        // Reference: Mass taken from Table 1.3, pg. 6, (de Pater, 2010), value
        //            of gravitational constant taken from
        //            http://ssd.jpl.nasa.gov/?constants#ref.
        gravitationalParameter = 6.834733937e15;

        break;

    default:

        std::string errorMessage = "Desired predefined central gravity field " +
                std::to_string( bodyWithPredefinedCentralGravityField ) +
                " does not exist";
        throw std::runtime_error( errorMessage );
    }
    return std::make_shared< GravityFieldModel >( gravitationalParameter );
}

} // namespace gravitation
} // namespace tudat


/*
 * MatrixCreator.cc
 *
 *  Created on: 28.06.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/fe/fe_update_flags.h>
#include <forward/MatrixCreator.h>

#include <functional>

namespace wavepi {
namespace forward {

using namespace dealii;

inline double square(const double x) {
   return x * x;
}

template<int dim>
MatrixCreator<dim>::LaplaceAssemblyScratchData::LaplaceAssemblyScratchData(const FiniteElement<dim> &fe,
      const Quadrature<dim> &quad)
      : fe_values(fe, quad, update_values | update_gradients | update_quadrature_points | update_JxW_values) {
}

template<int dim>
MatrixCreator<dim>::LaplaceAssemblyScratchData::LaplaceAssemblyScratchData(
      const LaplaceAssemblyScratchData &scratch_data)
      :
            fe_values(scratch_data.fe_values.get_fe(), scratch_data.fe_values.get_quadrature(),
                  update_values | update_gradients | update_quadrature_points | update_JxW_values) {
}

template<int dim>
MatrixCreator<dim>::MassAssemblyScratchData::MassAssemblyScratchData(const FiniteElement<dim> &fe,
      const Quadrature<dim> &quad)
      : fe_values(fe, quad, update_values | update_quadrature_points | update_JxW_values) {
}

template<int dim>
MatrixCreator<dim>::MassAssemblyScratchData::MassAssemblyScratchData(const MassAssemblyScratchData &scratch_data)
      :
            fe_values(scratch_data.fe_values.get_fe(), scratch_data.fe_values.get_quadrature(),
                  update_values | update_quadrature_points | update_JxW_values) {
}

template<int dim>
void MatrixCreator<dim>::local_assemble_A_cc(const LightFunction<dim> * const rho, const LightFunction<dim> * const q,
      const double time, const typename DoFHandler<dim>::active_cell_iterator &cell,
      LaplaceAssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      const double val_a = 1.0 / rho->evaluate(scratch_data.fe_values.quadrature_point(q_point), time);
      const double val_q = q->evaluate(scratch_data.fe_values.quadrature_point(q_point), time);

      for (unsigned int i = 0; i < dofs_per_cell; ++i) {
         for (unsigned int j = 0; j < dofs_per_cell; ++j)
            copy_data.cell_matrix(i, j) += (val_a * scratch_data.fe_values.shape_grad(i, q_point)
                  * scratch_data.fe_values.shape_grad(j, q_point)
                  + val_q * scratch_data.fe_values.shape_value(i, q_point)
                        * scratch_data.fe_values.shape_value(j, q_point)) * scratch_data.fe_values.JxW(q_point);
      }
   }

   cell->get_dof_indices(copy_data.local_dof_indices);
}

template<int dim>
void MatrixCreator<dim>::local_assemble_A_dd(const Vector<double> &rho, const Vector<double> &q,
      const typename DoFHandler<dim>::active_cell_iterator &cell, LaplaceAssemblyScratchData &scratch_data,
      AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   cell->get_dof_indices(copy_data.local_dof_indices);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      double val_a = 0;
      double val_q = 0;

      for (unsigned int k = 0; k < dofs_per_cell; ++k) {
         const double val_shape = scratch_data.fe_values.shape_value(k, q_point);

         val_a += rho[copy_data.local_dof_indices[k]] * val_shape;
         val_q += q[copy_data.local_dof_indices[k]] * val_shape;
      }
      val_a = 1.0 / val_a;

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
         for (unsigned int j = 0; j < dofs_per_cell; ++j)
            copy_data.cell_matrix(i, j) += (val_a * scratch_data.fe_values.shape_grad(i, q_point)
                  * scratch_data.fe_values.shape_grad(j, q_point)
                  + val_q * scratch_data.fe_values.shape_value(i, q_point)
                        * scratch_data.fe_values.shape_value(j, q_point)) * scratch_data.fe_values.JxW(q_point);
   }
}

template<int dim>
void MatrixCreator<dim>::local_assemble_A_cd(const LightFunction<dim> * const rho, const Vector<double> &q,
      const double time, const typename DoFHandler<dim>::active_cell_iterator &cell,
      LaplaceAssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   cell->get_dof_indices(copy_data.local_dof_indices);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      const double val_a = 1.0 / rho->evaluate(scratch_data.fe_values.quadrature_point(q_point), time);

      for (unsigned int i = 0; i < dofs_per_cell; ++i) {
         for (unsigned int j = 0; j < dofs_per_cell; ++j) {
            copy_data.cell_matrix(i, j) += val_a * scratch_data.fe_values.shape_grad(i, q_point)
                  * scratch_data.fe_values.shape_grad(j, q_point) * scratch_data.fe_values.JxW(q_point);

            for (unsigned int k = 0; k < dofs_per_cell; ++k)
               copy_data.cell_matrix(i, j) += q[copy_data.local_dof_indices[k]]
                     * scratch_data.fe_values.shape_value(k, q_point) * scratch_data.fe_values.shape_value(i, q_point)
                     * scratch_data.fe_values.shape_value(j, q_point) * scratch_data.fe_values.JxW(q_point);
         }
      }
   }
}

template<int dim>
void MatrixCreator<dim>::local_assemble_A_dc(const Vector<double> &rho, const LightFunction<dim> * const q,
      const double time, const typename DoFHandler<dim>::active_cell_iterator &cell,
      LaplaceAssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   cell->get_dof_indices(copy_data.local_dof_indices);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      const double val_q = q->evaluate(scratch_data.fe_values.quadrature_point(q_point), time);

      double val_a = 0;
      for (unsigned int k = 0; k < dofs_per_cell; ++k)
         val_a += rho[copy_data.local_dof_indices[k]] * scratch_data.fe_values.shape_value(k, q_point);
      val_a = 1.0 / val_a;

      for (unsigned int i = 0; i < dofs_per_cell; ++i) {
         for (unsigned int j = 0; j < dofs_per_cell; ++j) {
            copy_data.cell_matrix(i, j) += (val_q * scratch_data.fe_values.shape_value(i, q_point)
                  * scratch_data.fe_values.shape_value(j, q_point)
                  + val_a * scratch_data.fe_values.shape_grad(i, q_point)
                        * scratch_data.fe_values.shape_grad(j, q_point)) * scratch_data.fe_values.JxW(q_point);
         }
      }
   }
}

template<int dim>
void MatrixCreator<dim>::local_assemble_mass_d(const Vector<double> &c,
      const typename DoFHandler<dim>::active_cell_iterator &cell, MassAssemblyScratchData &scratch_data,
      AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   cell->get_dof_indices(copy_data.local_dof_indices);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      for (unsigned int i = 0; i < dofs_per_cell; ++i)
         for (unsigned int j = 0; j < dofs_per_cell; ++j)
            for (unsigned int k = 0; k < dofs_per_cell; ++k)
               copy_data.cell_matrix(i, j) += c[copy_data.local_dof_indices[k]]
                     * scratch_data.fe_values.shape_value(k, q_point) * scratch_data.fe_values.shape_value(i, q_point)
                     * scratch_data.fe_values.shape_value(j, q_point) * scratch_data.fe_values.JxW(q_point);
   }
}

template<int dim>
void MatrixCreator<dim>::local_assemble_mass_c(const LightFunction<dim> * const c, const double time,
      const typename DoFHandler<dim>::active_cell_iterator &cell, MassAssemblyScratchData &scratch_data,
      AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      const double val = c->evaluate(scratch_data.fe_values.quadrature_point(q_point), time);

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
         for (unsigned int j = 0; j < dofs_per_cell; ++j)
            copy_data.cell_matrix(i, j) += val * scratch_data.fe_values.shape_value(i, q_point)
                  * scratch_data.fe_values.shape_value(j, q_point) * scratch_data.fe_values.JxW(q_point);
   }

   cell->get_dof_indices(copy_data.local_dof_indices);
}

template<int dim>
void MatrixCreator<dim>::local_assemble_C_cc(const LightFunction<dim> * const rho, const LightFunction<dim> * const c,
      const double time_rho, const double time_c, const typename DoFHandler<dim>::active_cell_iterator &cell,
      MassAssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      const double val_a = 1.0 / rho->evaluate(scratch_data.fe_values.quadrature_point(q_point), time_rho);
      const double val_c = 1.0 / square(c->evaluate(scratch_data.fe_values.quadrature_point(q_point), time_c));

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
         for (unsigned int j = 0; j < dofs_per_cell; ++j)
            copy_data.cell_matrix(i, j) += val_c * val_a * scratch_data.fe_values.shape_value(i, q_point)
                  * scratch_data.fe_values.shape_value(j, q_point) * scratch_data.fe_values.JxW(q_point);
   }

   cell->get_dof_indices(copy_data.local_dof_indices);
}

template<int dim>
void MatrixCreator<dim>::local_assemble_C_dd(const Vector<double> &rho, const Vector<double> &c,
      const typename DoFHandler<dim>::active_cell_iterator &cell, MassAssemblyScratchData &scratch_data,
      AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   cell->get_dof_indices(copy_data.local_dof_indices);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      double val_a = 0;
      double val_c = 0;

      for (unsigned int k = 0; k < dofs_per_cell; ++k) {
         const double val_shape = scratch_data.fe_values.shape_value(k, q_point);

         val_a += rho[copy_data.local_dof_indices[k]] * val_shape;
         val_c += c[copy_data.local_dof_indices[k]] * val_shape;
      }

      val_a = 1.0 / val_a;
      val_c = 1.0 / (val_c * val_c);

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
         for (unsigned int j = 0; j < dofs_per_cell; ++j)
            copy_data.cell_matrix(i, j) += val_c * val_a * scratch_data.fe_values.shape_value(i, q_point)
                  * scratch_data.fe_values.shape_value(j, q_point) * scratch_data.fe_values.JxW(q_point);
   }
}

template<int dim>
void MatrixCreator<dim>::local_assemble_C_cd(const LightFunction<dim> * const rho, const Vector<double> &c,
      const double time_rho, const typename DoFHandler<dim>::active_cell_iterator &cell,
      MassAssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   cell->get_dof_indices(copy_data.local_dof_indices);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      const double val_a = 1.0 / rho->evaluate(scratch_data.fe_values.quadrature_point(q_point), time_rho);

      double val_c = 0;
      for (unsigned int k = 0; k < dofs_per_cell; ++k)
         val_c += c[copy_data.local_dof_indices[k]] * scratch_data.fe_values.shape_value(k, q_point);
      val_c = 1.0 / (val_c * val_c);

      for (unsigned int i = 0; i < dofs_per_cell; ++i) {
         for (unsigned int j = 0; j < dofs_per_cell; ++j)
            copy_data.cell_matrix(i, j) += val_c * val_a * scratch_data.fe_values.shape_value(i, q_point)
                  * scratch_data.fe_values.shape_value(j, q_point) * scratch_data.fe_values.JxW(q_point);
      }
   }
}

template<int dim>
void MatrixCreator<dim>::local_assemble_C_dc(const Vector<double> &rho, const LightFunction<dim> * const c,
      const double time_c, const typename DoFHandler<dim>::active_cell_iterator &cell,
      MassAssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   cell->get_dof_indices(copy_data.local_dof_indices);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      const double val_c = 1.0 / square(c->evaluate(scratch_data.fe_values.quadrature_point(q_point), time_c));

      double val_a = 0;
      for (unsigned int k = 0; k < dofs_per_cell; ++k)
         val_a += rho[copy_data.local_dof_indices[k]] * scratch_data.fe_values.shape_value(k, q_point);
      val_a = 1.0 / val_a;

      for (unsigned int i = 0; i < dofs_per_cell; ++i) {
         for (unsigned int j = 0; j < dofs_per_cell; ++j)
            copy_data.cell_matrix(i, j) += val_c * val_a * scratch_data.fe_values.shape_value(i, q_point)
                  * scratch_data.fe_values.shape_value(j, q_point) * scratch_data.fe_values.JxW(q_point);
      }
   }
}

template<int dim>
void MatrixCreator<dim>::local_assemble_D_intermediate_d(const Vector<double> &rho_current,
      const Vector<double> &rho_next, const typename DoFHandler<dim>::active_cell_iterator &cell,
      MassAssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   cell->get_dof_indices(copy_data.local_dof_indices);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      double val_current = 0;
      double val_next = 0;

      for (unsigned int k = 0; k < dofs_per_cell; ++k) {
         const double val_shape = scratch_data.fe_values.shape_value(k, q_point);

         val_current += rho_current[copy_data.local_dof_indices[k]] * val_shape;
         val_next += rho_next[copy_data.local_dof_indices[k]] * val_shape;
      }

      val_next = 1.0 / val_next;

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
         for (unsigned int j = 0; j < dofs_per_cell; ++j)
            copy_data.cell_matrix(i, j) += val_current * val_next * scratch_data.fe_values.shape_value(i, q_point)
                  * scratch_data.fe_values.shape_value(j, q_point) * scratch_data.fe_values.JxW(q_point);
   }
}

template<int dim>
void MatrixCreator<dim>::local_assemble_D_intermediate_c(const LightFunction<dim> * const rho,
      const double time_current, const double time_next, const typename DoFHandler<dim>::active_cell_iterator &cell,
      MassAssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
   const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
   const unsigned int n_q_points = scratch_data.fe_values.get_quadrature().size();

   copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
   copy_data.local_dof_indices.resize(dofs_per_cell);
   scratch_data.fe_values.reinit(cell);

   cell->get_dof_indices(copy_data.local_dof_indices);

   for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
      const double val_current = rho->evaluate(scratch_data.fe_values.quadrature_point(q_point), time_current);
      const double val_next = 1.0 / rho->evaluate(scratch_data.fe_values.quadrature_point(q_point), time_next);

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
         for (unsigned int j = 0; j < dofs_per_cell; ++j)
            copy_data.cell_matrix(i, j) += val_current * val_next * scratch_data.fe_values.shape_value(i, q_point)
                  * scratch_data.fe_values.shape_value(j, q_point) * scratch_data.fe_values.JxW(q_point);
   }
}

template<int dim>
void MatrixCreator<dim>::copy_local_to_global(SparseMatrix<double> &matrix, const AssemblyCopyData &copy_data) {
   for (unsigned int i = 0; i < copy_data.local_dof_indices.size(); ++i) {
      for (unsigned int j = 0; j < copy_data.local_dof_indices.size(); ++j)
         matrix.add(copy_data.local_dof_indices[i], copy_data.local_dof_indices[j], copy_data.cell_matrix(i, j));
   }
}

template<int dim>
void MatrixCreator<dim>::create_A_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, std::shared_ptr<LightFunction<dim>> rho, std::shared_ptr<LightFunction<dim>> q,
      const double time) {
   AssertThrow(rho, ExcZero());
   AssertThrow(q, ExcZero());

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_A_cc, rho.get(), q.get(), time, std::placeholders::_1,
               std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         LaplaceAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_A_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, std::shared_ptr<LightFunction<dim>> rho, const Vector<double> &q,
      const double time) {
   AssertThrow(rho, ExcZero());
   Assert(q.size() == dof->n_dofs(), ExcDimensionMismatch(q.size(), dof->n_dofs()));

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_A_cd, rho.get(), std::ref(q), time, std::placeholders::_1,
               std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         LaplaceAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_A_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, const Vector<double> &rho, std::shared_ptr<LightFunction<dim>> q,
      const double time) {
   AssertThrow(q, ExcZero());
   Assert(rho.size() == dof->n_dofs(), ExcDimensionMismatch(rho.size(), dof->n_dofs()));

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_A_dc, std::ref(rho), q.get(), time, std::placeholders::_1,
               std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         LaplaceAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_A_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, const Vector<double> &rho, const Vector<double> &q) {
   Assert(rho.size() == dof->n_dofs(), ExcDimensionMismatch(rho.size(), dof->n_dofs()));
   Assert(q.size() == dof->n_dofs(), ExcDimensionMismatch(q.size(), dof->n_dofs()));

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_A_dd, std::ref(rho), std::ref(q), std::placeholders::_1,
               std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         LaplaceAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_C_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, std::shared_ptr<LightFunction<dim>> rho, std::shared_ptr<LightFunction<dim>> c,
      const double time_rho, const double time_c) {
   AssertThrow(rho, ExcZero());
   AssertThrow(c, ExcZero());

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_C_cc, rho.get(), c.get(), time_rho, time_c,
               std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         MassAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_C_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, std::shared_ptr<LightFunction<dim>> rho, const Vector<double> &c,
      const double time_rho) {
   AssertThrow(rho, ExcZero());
   Assert(c.size() == dof->n_dofs(), ExcDimensionMismatch(c.size(), dof->n_dofs()));

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_C_cd, rho.get(), std::ref(c), time_rho, std::placeholders::_1,
               std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         MassAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_C_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, const Vector<double> &rho, std::shared_ptr<LightFunction<dim>> c,
      const double time_c) {
   AssertThrow(c, ExcZero());
   Assert(rho.size() == dof->n_dofs(), ExcDimensionMismatch(rho.size(), dof->n_dofs()));

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_C_dc, std::ref(rho), c.get(), time_c, std::placeholders::_1,
               std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         MassAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_C_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, const Vector<double> &rho, const Vector<double> &c) {
   Assert(rho.size() == dof->n_dofs(), ExcDimensionMismatch(rho.size(), dof->n_dofs()));
   Assert(c.size() == dof->n_dofs(), ExcDimensionMismatch(c.size(), dof->n_dofs()));

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_C_dd, std::ref(rho), std::ref(c), std::placeholders::_1,
               std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         MassAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_D_intermediate_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, const Vector<double> &rho_current, const Vector<double> &rho_next) {
   Assert(rho_current.size() == dof->n_dofs(), ExcDimensionMismatch(rho_current.size(), dof->n_dofs()));
   Assert(rho_next.size() == dof->n_dofs(), ExcDimensionMismatch(rho_next.size(), dof->n_dofs()));

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_D_intermediate_d, std::ref(rho_current), std::ref(rho_next),
               std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         MassAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_D_intermediate_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, std::shared_ptr<LightFunction<dim>> rho, double current_time, double next_time) {
   AssertThrow(rho, ExcZero());

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_D_intermediate_c, rho.get(), current_time, next_time,
               std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         MassAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_mass_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, const Vector<double> &c) {
   Assert(c.size() == dof->n_dofs(), ExcDimensionMismatch(c.size(), dof->n_dofs()));

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_mass_d, std::ref(c), std::placeholders::_1,
               std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         MassAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template<int dim>
void MatrixCreator<dim>::create_mass_matrix(std::shared_ptr<DoFHandler<dim>> dof, const Quadrature<dim> &quad,
      SparseMatrix<double> &matrix, std::shared_ptr<LightFunction<dim>> c, const double time) {
   AssertThrow(c, ExcZero());

   WorkStream::run(dof->begin_active(), dof->end(),
         std::bind(&MatrixCreator<dim>::local_assemble_mass_c, c.get(), time, std::placeholders::_1,
               std::placeholders::_2, std::placeholders::_3),
         std::bind(&MatrixCreator<dim>::copy_local_to_global, std::ref(matrix), std::placeholders::_1),
         MassAssemblyScratchData(dof->get_fe(), quad), AssemblyCopyData());
}

template class MatrixCreator<1> ;
template class MatrixCreator<2> ;
template class MatrixCreator<3> ;

}  // namespace forward
} /* namespace wavepi */


/**
 *          Copyright Matthias Walter 2010.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 **/

#ifndef SIGNING_HPP_
#define SIGNING_HPP_

#include "total_unimodularity.hpp"

#include <set>

#include <boost/type_traits/is_const.hpp>

#include "matrix_transposed.hpp"
#include "matrix_permuted.hpp"
#include "matrix_reorder.hpp"
#include "matrix.hpp"
#include "bipartite_graph_bfs.hpp"

namespace unimod
{

  /**
   * Generic function to find a non-zero column and swap it to a given position.
   *
   * @param matrix The given matrix
   * @param column_first First index of a column range
   * @param column_beyond Beyond index of a column range
   * @param row_first First index of a row range
   * @param row_beyond Beyond index of a row range
   * @param target_column Column to be swapped to
   * @return Whether a non-zero column was found.
   */

  template <typename MatrixType>
  bool find_nonzero_column(MatrixType& matrix, size_t column_first, size_t column_beyond, size_t row_first, size_t row_beyond, size_t target_column)
  {
    for (size_t column = column_first; column < column_beyond; ++column)
    {
      for (size_t row = row_first; row < row_beyond; ++row)
      {
        if (matrix(row, column) != 0)
        {
          matrix_permute2(matrix, target_column, column);
          return true;
        }

      }
    }
    return false;
  }

  /**
   * Takes a spanning tree in the bipartite graph of a matrix and a set of nodes to be signed.
   *
   * @param matrix The given matrix
   * @param spanning_tree A spanning tree
   * @param dim Index-mapping for bipartite graph
   * @param nodes Set of nodes
   * @param current_index Current index in the spanning tree
   * @param column The column to be signed
   * @param changes Necessary changes to the entries in the column
   */

  template <typename MatrixType>
  void check_sign(const MatrixType& matrix, const std::vector <bipartite_graph_bfs_node>& spanning_tree, const bipartite_graph_dimensions& dim,
      const std::set <size_t>& nodes, size_t current_index, size_t column, std::map <size_t, bool>& changes)
  {
    /// Root does never change.
    if (spanning_tree[current_index].predecessor == current_index)
    {
      changes[dim.index_to_row(current_index)] = false;
      return;
    }

    /// Search for ancestors until reaching one of the given nodes.
    int value = matrix(dim.index_to_row(current_index), column);
    size_t last, index = current_index;
    do
    {
      last = index;
      index = spanning_tree[index].predecessor;
      std::pair <size_t, size_t> coords = dim.indexes_to_coordinates(index, last);
      value += matrix(coords.first, coords.second);
    }
    while (nodes.find(index) == nodes.end());

    /// If the ancestor is not yet processed, we recurse.
    if (changes.find(dim.index_to_row(index)) == changes.end())
    {
      check_sign(matrix, spanning_tree, dim, nodes, index, column, changes);
    }

    value += matrix(dim.index_to_row(index), column);
    if (changes[dim.index_to_row(index)])
    {
      value += 2;
    }

    value = (value >= 0 ? value : -value) % 4;
    /// If sum (modulo 4) is not 0, we'd like to change the current one
    changes[dim.index_to_row(current_index)] = (value == 2);

    if (value != 0 && value != 2)
    {
      throw std::logic_error("Signing procedure: modulo-sum of cycle was neither 0, nor 2!");
    }
  }

  /**
   * A functor which compares the absolute values.
   */

  template <typename T>
  struct abs_greater
  {
    /**
     * Comparison function
     *
     * @param first First value
     * @param second Second value
     * @return true if and only if the |first| > |second|
     */

    bool operator()(const T& first, const T& second)
    {
      T abs_first = first >= 0 ? first : -first;
      T abs_second = second >= 0 ? second : -second;
      return abs_first > abs_second;
    }
  };

  /**
   * Generic signing function of a matrix, which might also be const.
   * Running time: O (height * width^2)
   *
   * @param matrix The given matrix
   * @param violator Pointer to violator indices to be filled.
   * @return true if and only if the matrix is signed already.
   */

  template <typename M>
  bool sign_matrix(M& matrix, submatrix_indices* violator)
  {
    bool result = true;
    matrix_permuted <M> permuted(matrix);
    size_t handled_rows = 0;

    /// Go trough column by column.
    for (size_t handled_columns = 0; handled_columns < permuted.size2(); ++handled_columns)
    {
      if (find_nonzero_column(permuted, handled_columns, permuted.size2(), 0, handled_rows, handled_columns))
      {
        /// There is a non-zero column right of the already-handled submatrix.

        std::set <size_t> start_nodes;
        std::set <size_t> end_nodes;
        std::set <size_t> all_nodes;

        bipartite_graph_dimensions dim(handled_rows, handled_columns);
        for (size_t row = 0; row < handled_rows; ++row)
        {
          if (permuted(row, handled_columns) != 0)
          {
            size_t index = dim.row_to_index(row);
            if (start_nodes.empty())
              start_nodes.insert(index);
            else
              end_nodes.insert(index);
            all_nodes.insert(index);
          }
        }

        /// Start a BFS on bipartite graph of the submatrix and look for shortest paths from first 1 to all others

        std::vector <bipartite_graph_bfs_node> bfs_result;
        if (!bipartite_graph_bfs(permuted, dim, start_nodes, end_nodes, true, bfs_result))
          throw std::logic_error("Signing procedure: Did not reach all nodes via bfs!");

        /// Evaluate matrix-entries on the shortest paths
        std::map <size_t, bool> changes;
        for (typename std::set <size_t>::const_iterator iter = end_nodes.begin(); iter != end_nodes.end(); ++iter)
        {
          check_sign(permuted, bfs_result, dim, all_nodes, *iter, handled_columns, changes);
        }

        /// Checking changes
        for (std::map <size_t, bool>::iterator iter = changes.begin(); iter != changes.end(); ++iter)
        {
          if (!iter->second)
            continue;

          if (boost::is_const <M>::value)
          {
            if (violator)
            {
              /// Find the violator, going along the path
              std::set <size_t> violator_rows, violator_columns;

              size_t index = iter->first;
              do
              {
                if (dim.is_row(index))
                  violator_rows.insert(permuted.perm1()(dim.index_to_row(index)));
                else
                  violator_columns.insert(permuted.perm2()(dim.index_to_column(index)));

                index = bfs_result[index].predecessor;
              }
              while (all_nodes.find(index) == all_nodes.end());
              violator_rows.insert(permuted.perm1()(dim.index_to_row(index)));
              violator_columns.insert(permuted.perm2()(handled_columns));

              /// Fill violator data
              violator->rows = submatrix_indices::indirect_array_type(violator_rows.size());
              violator->columns = submatrix_indices::indirect_array_type(violator_columns.size());
              size_t i = 0;
              for (std::set <size_t>::const_iterator iter = violator_rows.begin(); iter != violator_rows.end(); ++iter)
                violator->rows[i++] = *iter;
              i = 0;
              for (std::set <size_t>::const_iterator iter = violator_columns.begin(); iter != violator_columns.end(); ++iter)
                violator->columns[i++] = *iter;
            }
            return false;
          }
          else
          {
            /// We are not just testing, so swap the sign on a one.
            size_t real_row = permuted.perm1()(dim.index_to_row(iter->first));
            size_t real_column = permuted.perm2()(handled_columns);
            matrix_set_value(matrix, real_row, real_column, -matrix(real_row, real_column));

            result = false;
          }
        }

        matrix_reorder_rows(permuted, handled_rows, permuted.size1(), handled_columns, permuted.size2(), abs_greater <int> ());

        /// Augment submatrix by rows with 1 in the new column.
        while (handled_rows < permuted.size1())
        {
          if (permuted(handled_rows, handled_columns) == 0)
            break;
          else
            ++handled_rows;
        }
      }
      else
      {
        /// Handled upper-left submatrix and lower-right submatrix are disconnected
        for (size_t column = handled_columns; column < permuted.size2(); ++column)
        {
          size_t count = 0;
          for (size_t row = handled_rows; row < permuted.size1(); ++row)
          {
            if (permuted(row, column) != 0)
              ++count;
          }

          /// A zero column can be skipped, as it is handled by definition.
          if (count > 0)
          {
            /// Found a nonzero column and swap ones to the top.
            matrix_reorder_rows(permuted, handled_rows, permuted.size1(), handled_columns, permuted.size2(), abs_greater <int> ());
            while (handled_rows < permuted.size1())
            {
              if (permuted(handled_rows, handled_columns) == 0)
                break;
              else
                ++handled_rows;
            }

            break;
          }
        }
      }
    }

    return result;
  }

}

#endif /* SIGNING_HPP_ */


/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkSparseKernelTransform.txx,v $
  Language:  C++
  Date:      $Date: 2006-11-28 14:22:18 $
  Version:   $Revision: 1.1 $

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#ifndef _itkSparseKernelTransform_txx
#define _itkSparseKernelTransform_txx
#include "itkSparseKernelTransform.h"

// Report timings
#include <itkTimeProbe.h>
#include <itkTimeProbesCollectorBase.h>

#include <Eigen/Sparse>
#include <Eigen/SparseLU>

#include <vector>

namespace itk
{

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
SparseKernelTransform<TScalarType, NDimensions>::
SparseKernelTransform()
  : Transform<TScalarType, NDimensions,NDimensions>(1)
//:Superclass(
                         //   NDimensions,
                           // NDimensions )
  // the second NDimensions is associated is provided as
  // a tentative number for initializing the Jacobian.
  // The matrix can be resized at run time so this number
  // here is irrelevant. The correct size of the Jacobian
  // will be NDimension X NDimension.NumberOfLandMarks.
{

    // m_I.set_identity();
    m_I               = IMatrixType::Identity();
    m_SourceLandmarks = PointSetType::New();
    m_TargetLandmarks = PointSetType::New();
    m_Displacements   = VectorSetType::New();
    m_WMatrixComputed = false;

    m_LMatrixComputed  = false;
    m_LInverseComputed = false;

    m_Stiffness = 0.0;

    Eigen::setNbThreads(8);
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
SparseKernelTransform<TScalarType, NDimensions>::
~SparseKernelTransform()
{
}

template<class TScalarType, unsigned int NDimensions>
inline void SparseKernelTransform<TScalarType,
  NDimensions>::ComputeJacobianWithRespectToParameters(
    const InputPointType & in, JacobianType & jacobian) const {}

template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
SetSourceLandmarks(PointSetType * landmarks)
{
    itkDebugMacro("setting SourceLandmarks to " << landmarks );
    if (this->m_SourceLandmarks != landmarks)
    {
        this->m_SourceLandmarks = landmarks;
        this->UpdateParameters();
        this->Modified();

        // these are invalidated when the source lms change
        m_WMatrixComputed  = false;
        m_LMatrixComputed  = false;
        m_LInverseComputed = false;

        // you must recompute L and Linv - this does not require the targ lms
        // Linverse is only needed ofr Jacobian computation, I will defer this in case Jacobian is needed
        // we will assume by default that this transform is used only for warping, if it is a part of optimization
        // the GetJacobian will feel that the inverse is not computed and will compute it
        //this->ComputeLInverse();
        this->ComputeL();

    }
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
SetTargetLandmarks(PointSetType * landmarks)
{
    itkDebugMacro("setting TargetLandmarks to " << landmarks );
    if (this->m_TargetLandmarks != landmarks)
    {
        this->m_TargetLandmarks = landmarks;
        // this is invalidated when the target lms change
        m_WMatrixComputed=false;
        this->ComputeWMatrix();
        this->UpdateParameters();
        this->Modified();
    }

}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
const typename SparseKernelTransform<TScalarType, NDimensions>::GMatrixType &
SparseKernelTransform<TScalarType, NDimensions>::
ComputeG( const InputVectorType & ) const
{
    //
    // Should an Exception be thrown here  ?
    //
    itkWarningMacro(<< "ComputeG() should be reimplemented in the subclass !!");
    return m_GMatrix;
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
const typename SparseKernelTransform<TScalarType, NDimensions>::GMatrixType &
SparseKernelTransform<TScalarType, NDimensions>::
ComputeReflexiveG( PointsIterator ) const
{
    m_GMatrix = GMatrixType::Zero();
    for(unsigned d = 0; d < NDimensions; d++)
        m_GMatrix(d,d) = m_Stiffness;

    //m_GMatrix.fill( NumericTraits< TScalarType >::Zero );
    //m_GMatrix.fill_diagonal( m_Stiffness );

    return m_GMatrix;
}

/**
 * Default implementation of the the method. This can be overloaded
 * in transforms whose kernel produce diagonal G matrices.
 */
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
ComputeDeformationContribution( const InputPointType  & thisPoint,
                                OutputPointType & result     ) const
{

    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();

    PointsIterator sp  = m_SourceLandmarks->GetPoints()->Begin();

    for(unsigned int lnd=0; lnd < numberOfLandmarks; lnd++ )
    {
        const GMatrixType & Gmatrix = ComputeG( thisPoint - sp->Value() );
        for(unsigned int dim=0; dim < NDimensions; dim++ )
        {
            for(unsigned int odim=0; odim < NDimensions; odim++ )
            {
                result[ odim ] += Gmatrix(dim, odim ) * m_DMatrix(dim,lnd);
            }
        }
        ++sp;
    }

}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>
::ComputeD(void) const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();

    PointsIterator sp  = m_SourceLandmarks->GetPoints()->Begin();
    PointsIterator tp  = m_TargetLandmarks->GetPoints()->Begin();
    PointsIterator end = m_SourceLandmarks->GetPoints()->End();

    m_Displacements->Reserve( numberOfLandmarks );
    typename VectorSetType::Iterator vt = m_Displacements->Begin();

    while( sp != end )
    {
        vt->Value() = tp->Value() - sp->Value();
        vt++;
        sp++;
        tp++;
    }
    //	std::cout<<" Computed displacements "<<m_Displacements<<std::endl;
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>
::ComputeWMatrix(void) const
{
    itk::TimeProbe clock;

    //	std::cout<<"Computing W matrix"<<std::endl;

    //typedef vnl_svd<TScalarType>  SVDSolverType;

    if(!m_LMatrixComputed) {
        this->ComputeL();
    }
    this->ComputeY();

    //SVDSolverType svd( m_LMatrix, 1e-8 );
    //m_WMatrix = svd.solve( m_YMatrix );

    clock.Start();

    //Eigen::BiCGSTAB<LMatrixType>  solver;
    Eigen::BiCGSTAB<LMatrixType, Eigen::IncompleteLUT<double> >  solver;
    solver.preconditioner().setDroptol(1e-10);
    solver.preconditioner().setFillfactor(1000);

    //    Eigen::SparseLU<LMatrixType> solver;
    solver.compute(m_LMatrix);

    if(solver.info()!= Eigen::Success) {
        // decomposition failed
        std::cerr << "LMatrix failed to decompose ...!" << std::endl;
        return;
    }

    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();
    m_WMatrix = WMatrixType::Zero(NDimensions*(numberOfLandmarks+NDimensions+1), 1);
    m_WMatrix = solver.solve(m_YMatrix);

    //    m_WMatrix = WMatrixType::Random(NDimensions*(numberOfLandmarks+NDimensions+1), 1);
    //    solver.setMaxIterations(1);
    //    int i = 0;
    //    do {
    //        m_WMatrix = solver.solveWithGuess(m_YMatrix,m_WMatrix);
    //        std::cout << "#iteration: " << i << " " << "estimated error: " << solver.error() << std::endl;
    //        ++i;
    //    } while (solver.info()!= Eigen::Success && i<100);

    // std::cout << "#iterations: " << solver.iterations() << std::endl;
    // std::cout << "estimated error: " << solver.error() << std::endl;

    std::cout  << solver.error() << std::endl;
    if(solver.info() != Eigen::Success) {
        // solving failed
        std::cerr << "solving sparse system failed ...!" << std::endl;
        return;
    }

    clock.Stop();
    // std::cout << "Computing Wmatrix:" << std::endl;
    // std::cout << "Mean: " << clock.GetMean() << std::endl;
    // std::cout << "Total: " << clock.GetTotal() << std::endl;

    this->ReorganizeW();
    m_WMatrixComputed=true;
}

/**
 * postponed till needing the jacobian for this class
 */
//template <class TScalarType, unsigned int NDimensions>
//void SparseKernelTransform<TScalarType, NDimensions>::
//ComputeLInverse(void) const
//{
//    // Assumes that L has already been computed
//    // Necessary for the jacobian
//    if(!m_LMatrixComputed) {
//        this->ComputeL();
//    }
//    //std::cout<<"LMatrix is:"<<std::endl;
//    //std::cout<<m_LMatrix<<std::endl;

//    itk::TimeProbesCollectorBase timeCollector;
//    if (0){
//        timeCollector.Start( "ComputeLInverse" );
//        m_LMatrixInverse=vnl_matrix_inverse<TScalarType> (m_LMatrix);
//        timeCollector.Stop( "ComputeLInverse" );
//    }

//    // Convert to sparse matrix
//    // Because of the special storage scheme of a SparseMatrix, special care has to be taken when adding new nonzero entries.
//    // For instance, the cost of a single purely random insertion into a SparseMatrix is O(nnz),
//    // where nnz is the current number of non-zero coefficients.
//    // The simplest way to create a sparse matrix while guaranteeing good performance is thus to first build a list of
//    // so-called triplets, and then convert it to a SparseMatrix.
//    typedef Eigen::SparseMatrix<ScalarType> SpMat; // declares a column-major sparse matrix type of double
//    typedef Eigen::Triplet<ScalarType> Triplet;
//    SpMat A(m_LMatrix.rows(),m_LMatrix.cols());

//    std::vector<Triplet> tripletList;
//    for ( unsigned int r = 0; r < m_LMatrix.rows(); r++ )
//        for ( unsigned int c = 0; c < m_LMatrix.cols(); c++ )
//        {
//            ScalarType val = m_LMatrix.get( r, c );
//            if ( val != 0 )
//                tripletList.push_back(Triplet(r,c,val));
//        }
//    A.setFromTriplets(tripletList.begin(), tripletList.end());
//    timeCollector.Stop( "ConvertLToSparse" );

//    // Method 4: LU Decomposition
//    // Depends on local ITK vnl_sparse_lu modification
//    timeCollector.Start( "ComputeLSparseInverse" );
//    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();
//    //    vnl_sparse_symmetric_eigensystem eigSys;
//    //    eigSys.CalculateNPairs(lSparseMatrix,NDimensions*(numberOfLandmarks+NDimensions+1), false);
//    //    //LSparseMatrixType lMatrixInverse = vnl_sparse_lu( lSparseMatrix ).inverse();
//    //    timeCollector.Stop( "ComputeLSparseInverse" );

//    m_LInverseComputed=true;
//    //std::cout<<"LMatrix inverse is:"<<std::endl;
//    //std::cout<<m_LMatrixInverse<<std::endl;
//}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>::
ComputeL(void) const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();
    //vnl_matrix<TScalarType> O2(NDimensions*(NDimensions+1),
    //                           NDimensions*(NDimensions+1), 0);

    this->ComputeP();
    this->ComputeK();

    //m_LMatrix.set_size( NDimensions*(numberOfLandmarks+NDimensions+1),
    //                    NDimensions*(numberOfLandmarks+NDimensions+1) );
    //m_LMatrix.fill( 0.0 );
    m_LMatrix = LMatrixType( NDimensions*(numberOfLandmarks+NDimensions+1),
                             NDimensions*(numberOfLandmarks+NDimensions+1) );

    //std::vector<TripletType> tripletList;

    // putting KMATRIX
    //m_LMatrix.update( m_KMatrix, 0, 0 );
    //it.value();
    //it.row(); // row index
    //it.col(); // col index (here it is equal to k)
    //it.index(); // inner index, here it is equal to it.row()
    for (int k = 0; k < m_KMatrix.outerSize(); ++k) // column index
        for (typename KMatrixType::InnerIterator it(m_KMatrix,k); it; ++it)
            m_LMatrix.insert(it.row(), it.col()) = it.value() ;
    //tripletList.push_back( TripletType ( it.row(), it.col(), it.value() ) );

    // putting PMATRIX - will keep the lower only ??
    //m_LMatrix.update( m_PMatrix, 0, m_KMatrix.columns() );
    //m_LMatrix.update( m_PMatrix.transpose(), m_KMatrix.rows(), 0);
    for (int p = 0; p < m_PMatrix.outerSize(); ++p) // column index
        for (typename PMatrixType::InnerIterator it(m_PMatrix,p); it; ++it)
        {
            // fill P -> upper
            m_LMatrix.insert(it.row(), NDimensions*numberOfLandmarks + it.col()) = it.value() ;
            //tripletList.push_back( TripletType ( it.row(), NDimensions*numberOfLandmarks + it.col(), it.value() ) );

            // fill P.transpose -> lower
            m_LMatrix.insert(NDimensions*numberOfLandmarks + it.col(),  it.row()) = it.value() ;
            //tripletList.push_back( TripletType ( NDimensions*numberOfLandmarks + it.col(),  it.row(), it.value() ) );
        }
    //m_LMatrix.update( O2, m_KMatrix.rows(), m_KMatrix.columns());

    //    // shireen: for debugging let's make sure that the L matrix is sparse (based on gaussian basis)
    //    std::ofstream ofs;
    //    ofs.open("Lsparse.csv");
    //    for (int k = 0; k < m_LMatrix.outerSize(); ++k) // column index
    //        for (typename LMatrixType::InnerIterator it(m_LMatrix,k); it; ++it)
    //            ofs << it.row()  << ", " << it.col()  << ", " << it.value() << std::endl;
    //    ofs.close();

    // std::cout << "Lmatrix - nnz = "  << m_LMatrix.nonZeros() << std::endl;
    m_LMatrix.makeCompressed();

    //m_LMatrix.setFromTriplets(tripletList.begin(), tripletList.end());
    m_LMatrixComputed=1;
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>::
ComputeK(void) const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();
    GMatrixType G;
    //std::vector<TripletType> tripletList;

    m_KMatrix = KMatrixType( NDimensions * numberOfLandmarks,
                             NDimensions * numberOfLandmarks );
    //m_KMatrix.set_size( NDimensions * numberOfLandmarks,
    //                    NDimensions * numberOfLandmarks );

    //m_KMatrix.fill( 0.0 );

    PointsIterator p1  = m_SourceLandmarks->GetPoints()->Begin();
    PointsIterator end = m_SourceLandmarks->GetPoints()->End();

    // K matrix is symmetric, so only evaluate the upper triangle and
    // store the values in both the upper and lower triangle
    unsigned int i = 0;
    while( p1 != end )
    {
        PointsIterator p2 = p1; // start at the diagonal element
        unsigned int j = i;

        // Compute the block diagonal element, i.e. kernel for pi->pi
        //G = ComputeReflexiveG(p1);

        // force to compute the basis on the diagonal
        const InputVectorType s = p1.Value() - p1.Value();
        G = ComputeG(s); // the basis

        //m_KMatrix.update(G, i*NDimensions, i*NDimensions);
        for(unsigned int d = 0; d < NDimensions; d++)
        {
            if(G(d,d) != 0 )
                m_KMatrix.insert(i*NDimensions+d ,i*NDimensions+d) = G(d,d) + m_Stiffness; // this is as a regularizer
            //tripletList.push_back(TripletType(i*NDimensions+d ,i*NDimensions+d, m_GMatrix(d,d)));
        }

        p2++;
        j++;

        // Compute the upper (and copy into lower) triangular part of K
        // only save the lower part, don't need it
        while( p2 != end )
        {
            const InputVectorType s = p1.Value() - p2.Value();
            G = ComputeG(s); // the basis

            // write value in upper and lower triangle of matrix
            for(unsigned int ii = 0 ; ii < NDimensions; ii++)
                for(unsigned int jj = 0 ; jj < NDimensions; jj++)
                {
                    if (G(ii,jj) != 0)
                    {
                        m_KMatrix.insert(i*NDimensions+ii ,j*NDimensions+jj) = G(ii,jj); // upper
                        m_KMatrix.insert(j*NDimensions+ii ,i*NDimensions+jj) = G(ii,jj); // lower
                    }

                    //tripletList.push_back(TripletType(i*NDimensions+ii ,j*NDimensions+jj, G(ii,jj)));
                    //tripletList.push_back(TripletType(j*NDimensions+ii ,i*NDimensions+jj, G(ii,jj)));
                }

            // m_KMatrix.update(G, i*NDimensions, j*NDimensions);
            // m_KMatrix.update(G, j*NDimensions, i*NDimensions);
            p2++;
            j++;
        }
        p1++;
        i++;
    }
    //std::cout<<"K matrix: "<<std::endl;
    //std::cout<<m_KMatrix<<std::endl;

    //    // shireen: for debugging let's make sure that the L matrix is sparse (based on gaussian basis)
    //    std::ofstream ofs;
    //    ofs.open("Ksparse.csv");
    //    for (int k = 0; k < m_KMatrix.outerSize(); ++k) // column index
    //        for (typename KMatrixType::InnerIterator it(m_KMatrix,k); it; ++it)
    //            ofs << it.row()  << ", " << it.col()  << ", " << it.value() << std::endl;
    //    ofs.close();

    // std::cout << "Kmatrix - nnz = "  << m_KMatrix.nonZeros() << std::endl;
    m_KMatrix.makeCompressed();

    //m_KMatrix.setFromTriplets(tripletList.begin(), tripletList.end());
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>::
ComputeP() const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();

    //IMatrixType I = IMatrixType::Identity();
    //IMatrixType temp;
    InputPointType p;

    //I.set_identity();

    //std::vector<TripletType> tripletList;

    //m_PMatrix.set_size( NDimensions*numberOfLandmarks,
    //                    NDimensions*(NDimensions+1) );
    //m_PMatrix.fill( 0.0 );
    m_PMatrix = PMatrixType(NDimensions*numberOfLandmarks,
                            NDimensions*(NDimensions+1) );
    for (unsigned int i = 0; i < numberOfLandmarks; i++)
    {
        m_SourceLandmarks->GetPoint(i, &p);
        for (unsigned int j = 0; j < NDimensions; j++)
        {
            //temp = I * p[j];
            for(unsigned int d = 0 ; d < NDimensions; d++)
            {
                m_PMatrix.insert(i*NDimensions + d, j*NDimensions + d) = p[j];
                //tripletList.push_back( TripletType( i*NDimensions + d, j*NDimensions + d, p[j] ) );
            }
            //m_PMatrix.update(temp, i*NDimensions, j*NDimensions);
        }

        for(unsigned int d = 0 ; d < NDimensions; d++)
        {
            m_PMatrix.insert(i*NDimensions + d, NDimensions*NDimensions + d) = 1;
            //tripletList.push_back( TripletType( i*NDimensions + d, NDimensions*NDimensions + d, 1 ) );
        }
        //m_PMatrix.update(I, i*NDimensions, NDimensions*NDimensions);
    }

    // std::cout << "Pmatrix - nnz = "  << m_PMatrix.nonZeros() << std::endl;
    m_PMatrix.makeCompressed();
    //m_PMatrix.setFromTriplets(tripletList.begin(), tripletList.end());
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void SparseKernelTransform<TScalarType, NDimensions>::
ComputeY(void) const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();

    this->ComputeD();

    typename VectorSetType::ConstIterator displacement =
            m_Displacements->Begin();

    //m_YMatrix.set_size( NDimensions*(numberOfLandmarks+NDimensions+1), 1);
    //m_YMatrix.fill( 0.0 );
    m_YMatrix = YMatrixType::Zero(NDimensions*(numberOfLandmarks+NDimensions+1), 1);

    for (unsigned int i = 0; i < numberOfLandmarks; i++)
    {
        for (unsigned int j = 0; j < NDimensions; j++)
        {
            m_YMatrix(i*NDimensions+j, 0) = displacement.Value()[j];
            //m_YMatrix.put(i*NDimensions+j, 0, displacement.Value()[j]);
        }
        displacement++;
    }

    for (unsigned int i = 0; i < NDimensions*(NDimensions+1); i++)
    {
        m_YMatrix(numberOfLandmarks*NDimensions+i, 0) = 0;
        //m_YMatrix.put(numberOfLandmarks*NDimensions+i, 0, 0);
    }
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>
::ReorganizeW(void) const
{
    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();

    // The deformable (non-affine) part of the registration goes here
    m_DMatrix = DMatrixType::Zero(NDimensions,numberOfLandmarks);
    //m_DMatrix.set_size(NDimensions,numberOfLandmarks);

    unsigned int ci = 0;
    for(unsigned int lnd=0; lnd < numberOfLandmarks; lnd++ )
    {
        for(unsigned int dim=0; dim < NDimensions; dim++ )
        {
            //std::cout << m_WMatrix(ci,0) << std::endl;
            m_DMatrix(dim,lnd) = m_WMatrix(ci++,0);
        }
    }

    // This matrix holds the rotational part of the Affine component
    m_AMatrix = AMatrixType::Zero(NDimensions, NDimensions);
    for(unsigned int j=0; j < NDimensions; j++ )
    {
        for(unsigned int i=0; i < NDimensions; i++ )
        {
            m_AMatrix(i,j) = m_WMatrix(ci++,0);
        }
    }

    // This vector holds the translational part of the Affine component
    m_BVector = BMatrixType::Zero(NDimensions,1);
    for(unsigned int k=0; k < NDimensions; k++ )
    {
        m_BVector(k) = m_WMatrix(ci++,0);
    }

    // release WMatrix memory by assigning a small one.
    m_WMatrix = WMatrixType(1,1);

    m_WMatrixComputed=1;

    //    std::ofstream ofs;
    //    ofs.open("D.csv");
    //    for (int k = 0; k < m_DMatrix.outerSize(); ++k) // column index
    //        for (typename DMatrixType::InnerIterator it(m_DMatrix,k); it; ++it)
    //            ofs << it.row()  << ", " << it.col()  << ", " << it.value() << std::endl;
    //    ofs.close();
}

/**
 *
 */
template <class TScalarType, unsigned int NDimensions>
typename SparseKernelTransform<TScalarType, NDimensions>::OutputPointType
SparseKernelTransform<TScalarType, NDimensions>
::TransformPoint(const InputPointType& thisPoint) const
{

    OutputPointType result;

    typedef typename OutputPointType::ValueType ValueType;

    result.Fill( NumericTraits< ValueType >::Zero );

    this->ComputeDeformationContribution( thisPoint, result );

    // Add the rotational part of the Affine component
    for(unsigned int j=0; j < NDimensions; j++ )
    {
        for(unsigned int i=0; i < NDimensions; i++ )
        {
            result[i] += m_AMatrix(i,j) * thisPoint[j];
        }
    }

    // This vector holds the translational part of the Affine component
    for(unsigned int k=0; k < NDimensions; k++ )
    {
        result[k] += m_BVector(k) + thisPoint[k];
    }

    return result;

}

//// Compute the Jacobian in one position - POSTPONED
//template <class TScalarType, unsigned int NDimensions>
//const typename SparseKernelTransform<TScalarType,NDimensions>::JacobianType &
//SparseKernelTransform< TScalarType,NDimensions>::
//GetJacobian( const InputPointType & thisPoint) const
//{
//    if(!m_LInverseComputed) {
//        this->ComputeLInverse();
//    }
//    unsigned long numberOfLandmarks = m_SourceLandmarks->GetNumberOfPoints();
//    Superclass::m_Jacobian.SetSize(NDimensions, numberOfLandmarks*NDimensions);
//    Superclass::m_Jacobian.Fill( 0.0 );

//    PointsIterator sp  = m_SourceLandmarks->GetPoints()->Begin();
//    for(unsigned int lnd=0; lnd < numberOfLandmarks; lnd++ )
//    {
//        const GMatrixType & Gmatrix = ComputeG( thisPoint - sp->Value() );
//        ///std::cout<<"G for landmark "<<lnd<<std::endl<<Gmatrix<<std::endl;
//        for(unsigned int dim=0; dim < NDimensions; dim++ )
//        {
//            for(unsigned int odim=0; odim < NDimensions; odim++ )
//            {
//                for(unsigned int lidx=0; lidx < numberOfLandmarks*NDimensions; lidx++ )
//                {
//                    Superclass::m_Jacobian[ odim ] [lidx] += Gmatrix(dim, odim ) *
//                            m_LMatrixInverse[lnd*NDimensions+dim][lidx];
//                }
//            }
//        }
//        ++sp;

//    }
//    for(unsigned int odim=0; odim < NDimensions; odim++ )
//    {
//        for(unsigned int lidx=0; lidx < numberOfLandmarks*NDimensions; lidx++ )
//        {
//            for(unsigned int dim=0; dim < NDimensions; dim++ )
//            {
//                Superclass::m_Jacobian[ odim ] [lidx] += thisPoint[dim] *
//                        m_LMatrixInverse[(numberOfLandmarks+dim)*NDimensions+odim][lidx];
//            }

//            Superclass::m_Jacobian[ odim ] [lidx] += m_LMatrixInverse[(numberOfLandmarks+NDimensions)*NDimensions+odim][lidx];
//        }
//    }

//    return Superclass::m_Jacobian;

//}

// Set to the identity transform - ie make the  Source and target lm the same
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
SetIdentity()
{
    this->SetParameters(this->GetFixedParameters());
}

// Set the parameters
// NOTE that in this transformation both the Source and Target
// landmarks could be considered as parameters. It is assumed
// here that the Target landmarks are provided by the user and
// are not changed during the optimization process required for
// registration.
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
SetParameters( const ParametersType & parameters )
{
    //	std::cout<<"Setting parameters to "<<parameters<<std::endl;
    typename PointsContainer::Pointer landmarks = PointsContainer::New();
    const unsigned int numberOfLandmarks =  parameters.Size() / NDimensions;
    landmarks->Reserve( numberOfLandmarks );

    PointsIterator itr = landmarks->Begin();
    PointsIterator end = landmarks->End();

    InputPointType  landMark;

    unsigned int pcounter = 0;
    while( itr != end )
    {
        for(unsigned int dim=0; dim<NDimensions; dim++)
        {
            landMark[ dim ] = parameters[ pcounter ];
            pcounter++;
        }
        itr.Value() = landMark;
        itr++;
    }

    // m_SourceLandmarks->SetPoints( landmarks );
    m_TargetLandmarks->SetPoints( landmarks );

    // W MUST be recomputed if the target lms are set
    this->ComputeWMatrix();

    //  if(!m_LInverseComputed) {
    //  this->ComputeLInverse();
    //  }

    // Modified is always called since we just have a pointer to the
    // parameters and cannot know if the parameters have changed.
    this->Modified();

}

// Set the fixed parameters
// Since the API of the SetParameters() function sets the
// source landmarks, this function was added to support the
// setting of the target landmarks, and allowing the Transform
// I/O mechanism to be supported.
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
SetFixedParameters( const ParametersType & parameters )
{
    typename PointsContainer::Pointer landmarks = PointsContainer::New();
    const unsigned int numberOfLandmarks =  parameters.Size() / NDimensions;

    landmarks->Reserve( numberOfLandmarks );

    PointsIterator itr = landmarks->Begin();
    PointsIterator end = landmarks->End();

    InputPointType  landMark;

    unsigned int pcounter = 0;
    while( itr != end )
    {
        for(unsigned int dim=0; dim<NDimensions; dim++)
        {
            landMark[ dim ] = parameters[ pcounter ];
            pcounter++;
        }
        itr.Value() = landMark;
        itr++;
    }

    //  m_TargetLandmarks->SetPoints( landmarks );
    m_SourceLandmarks->SetPoints( landmarks );

    // these are invalidated when the source lms change
    m_WMatrixComputed=false;
    m_LMatrixComputed=false;
    m_LInverseComputed=false;

    // you must recompute L and Linv - this does not require the targ lms
    //this->ComputeLInverse();
    this->ComputeL();

}

// Update parameters array
// They are the components of all the landmarks in the source space
template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
UpdateParameters( void ) const
{
    this->m_Parameters = ParametersType( m_TargetLandmarks->GetNumberOfPoints() * NDimensions );

    PointsIterator itr = m_TargetLandmarks->GetPoints()->Begin();
    PointsIterator end = m_TargetLandmarks->GetPoints()->End();

    unsigned int pcounter = 0;
    while( itr != end )
    {
        InputPointType  landmark = itr.Value();
        for(unsigned int dim=0; dim<NDimensions; dim++)
        {
            this->m_Parameters[ pcounter ] = landmark[ dim ];
            pcounter++;
        }
        itr++;
    }
}

// Get the parameters
// They are the components of all the landmarks in the source space
template <class TScalarType, unsigned int NDimensions>
const typename SparseKernelTransform<TScalarType, NDimensions>::ParametersType &
SparseKernelTransform<TScalarType, NDimensions>::
GetParameters( void ) const
{
    this->UpdateParameters();
    return this->m_Parameters;

}

// Get the fixed parameters
// This returns the target landmark locations
// This was added to support the Transform Reader/Writer mechanism
template <class TScalarType, unsigned int NDimensions>
const typename SparseKernelTransform<TScalarType, NDimensions>::ParametersType &
SparseKernelTransform<TScalarType, NDimensions>::
GetFixedParameters( void ) const
{
    this->m_FixedParameters = ParametersType( m_SourceLandmarks->GetNumberOfPoints() * NDimensions );

    PointsIterator itr = m_SourceLandmarks->GetPoints()->Begin();
    PointsIterator end = m_SourceLandmarks->GetPoints()->End();

    unsigned int pcounter = 0;
    while( itr != end )
    {
        InputPointType  landmark = itr.Value();
        for(unsigned int dim=0; dim<NDimensions; dim++)
        {
            this->m_FixedParameters[ pcounter ] = landmark[ dim ];
            pcounter++;
        }
        itr++;
    }

    return this->m_FixedParameters;

}

template <class TScalarType, unsigned int NDimensions>
void
SparseKernelTransform<TScalarType, NDimensions>::
PrintSelf(std::ostream& os, Indent indent) const
{
    Superclass::PrintSelf(os,indent);
    if (m_SourceLandmarks)
    {
        os << indent << "SourceLandmarks: " << std::endl;
        m_SourceLandmarks->Print(os,indent.GetNextIndent());
    }
    if (m_TargetLandmarks)
    {
        os << indent << "TargetLandmarks: " << std::endl;
        m_TargetLandmarks->Print(os,indent.GetNextIndent());
    }
    if (m_Displacements)
    {
        os << indent << "Displacements: " << std::endl;
        m_Displacements->Print(os,indent.GetNextIndent());
    }
    os << indent << "Stiffness: " << m_Stiffness << std::endl;
}
} // namespace itk

#endif