/* Author: Ivan Christov, Wolfgang Bangerth, Texas A&M University, 2006 */

/*    $Id: step-25.cc 27657 2012-11-21 13:19:08Z bangerth $ */
/*    Copyright (C) 2006-2009, 2011-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the text and       */
/*    further information on this license.                        */

// @sect3{Include files and global variables}

// For an explanation of the include files, the reader should refer to the
// example programs step-1 through step-4. They are in the standard order,
// which is base -- lac -- grid --
// dofs -- fe -- numerics (since each
// of these categories roughly builds upon previous ones), then a few C++
// headers for file input/output and string streams.
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

#include <fstream>
#include <iostream>

// The last step is as in all previous programs:
namespace Step25
{
  using namespace dealii;

  // @sect3{The SineGordonProblem class template}

  // The entire algorithm for solving the problem is encapsulated in this
  // class. As in previous example programs, the class is declared with a
  // template parameter, which is the spatial dimension, so that we can solve
  // the sine-Gordon equation in one, two or three spatial dimensions. For
  // more on the dimension-independent class-encapsulation of the problem, the
  // reader should consult step-3 and step-4.
  //
  // Compared to step-23 and step-24, there isn't anything newsworthy in the
  // general structure of the program (though there is of course in the inner
  // workings of the various functions!). The most notable difference is the
  // presence of the two new functions compute_nl_term and
  // compute_nl_matrix that compute the nonlinear contributions
  // to the system matrix and right-hand side of the first equation, as
  // discussed in the Introduction. In addition, we have to have a vector
  // solution_update that contains the nonlinear update to the
  // solution vector in each Newton step.
  //
  // As also mentioned in the introduction, we do not store the velocity
  // variable in this program, but the mass matrix times the velocity. This is
  // done in the M_x_velocity variable (the "x" is intended to
  // stand for "times").
  //
  // Finally, the output_timestep_skip variable stores the number
  // of time steps to be taken each time before graphical output is to be
  // generated. This is of importance when using fine meshes (and consequently
  // small time steps) where we would run lots of time steps and create lots
  // of output files of solutions that look almost the same in subsequent
  // files. This only clogs up our visualization procedures and we should
  // avoid creating more output than we are really interested in. Therefore,
  // if this variable is set to a value $n$ bigger than one, output is
  // generated only every $n$th time step.
  template <int dim>
  class SineGordonProblem
  {
  public:
    SineGordonProblem ();
    void run ();

  private:
    void make_grid_and_dofs ();
    void assemble_system ();
    void compute_nl_term (const Vector<double> &old_data,
                          const Vector<double> &new_data,
                          Vector<double>       &nl_term) const;
    void compute_nl_matrix (const Vector<double> &old_data,
                            const Vector<double> &new_data,
                            SparseMatrix<double> &nl_matrix) const;
    unsigned int solve ();
    void output_results (const unsigned int timestep_number) const;

    Triangulation<dim>   triangulation;
    FE_Q<dim>            fe;
    DoFHandler<dim>      dof_handler;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;

    const unsigned int n_global_refinements;

    double time;
    const double final_time, time_step;
    const double theta;

    Vector<double>       solution, solution_update, old_solution;
    Vector<double>       M_x_velocity;
    Vector<double>       system_rhs;

    const unsigned int output_timestep_skip;
  };

  // @sect3{Initial conditions}

  // In the following two classes, we first implement the exact solution for
  // 1D, 2D, and 3D mentioned in the introduction to this program. This
  // space-time solution may be of independent interest if one wanted to test
  // the accuracy of the program by comparing the numerical against the
  // analytic solution (note however that the program uses a finite domain,
  // whereas these are analytic solutions for an unbounded domain). This may,
  // for example, be done using the VectorTools::integrate_difference
  // function. Note, again (as was already discussed in step-23), how we
  // describe space-time functions as spatial functions that depend on a time
  // variable that can be set and queried using the FunctionTime::set_time()
  // and FunctionTime::get_time() member functions of the FunctionTime base
  // class of the Function class.
  template <int dim>
  class ExactSolution : public Function<dim>
  {
  public:
    ExactSolution (const unsigned int n_components = 1,
                   const double time = 0.) : Function<dim>(n_components, time) {}
    virtual double value (const Point<dim> &p,
                          const unsigned int component = 0) const;
  };

  template <int dim>
  double ExactSolution<dim>::value (const Point<dim> &p,
                                    const unsigned int /*component*/) const
  {
    double t = this->get_time ();

    switch (dim)
      {
      case 1:
      {
        const double m = 0.5;
        const double c1 = 0.;
        const double c2 = 0.;
        return -4.*std::atan (m /
                              std::sqrt(1.-m*m) *
                              std::sin(std::sqrt(1.-m*m)*t+c2) /
                              std::cosh(m*p[0]+c1));
      }

      case 2:
      {
        const double theta  = numbers::PI/4.;
        const double lambda  = 1.;
        const double a0  = 1.;
        const double s   = 1.;
        const double arg = p[0] * std::cos(theta) +
                           std::sin(theta) *
                           (p[1] * std::cosh(lambda) +
                            t * std::sinh(lambda));
        return 4.*std::atan(a0*std::exp(s*arg));
      }

      case 3:
      {
        double theta  = numbers::PI/4;
        double phi = numbers::PI/4;
        double tau = 1.;
        double c0  = 1.;
        double s   = 1.;
        double arg = p[0]*std::cos(theta) +
                     p[1]*std::sin(theta) * std::cos(phi) +
                     std::sin(theta) * std::sin(phi) *
                     (p[2]*std::cosh(tau)+t*std::sinh(tau));
        return 4.*std::atan(c0*std::exp(s*arg));
      }

      default:
        Assert (false, ExcNotImplemented());
        return -1e8;
      }
  }

  // In the second part of this section, we provide the initial conditions. We
  // are lazy (and cautious) and don't want to implement the same functions as
  // above a second time. Rather, if we are queried for initial conditions, we
  // create an object ExactSolution, set it to the correct time,
  // and let it compute whatever values the exact solution has at that time:
  template <int dim>
  class InitialValues : public Function<dim>
  {
  public:
    InitialValues (const unsigned int n_components = 1,
                   const double time = 0.)
      :
      Function<dim>(n_components, time)
    {}

    virtual double value (const Point<dim> &p,
                          const unsigned int component = 0) const;
  };

  template <int dim>
  double InitialValues<dim>::value (const Point<dim> &p,
                                    const unsigned int component) const
  {
    return ExactSolution<dim>(1, this->get_time()).value (p, component);
  }

  // @sect3{Implementation of the SineGordonProblem class}

  // Let's move on to the implementation of the main class, as it implements
  // the algorithm outlined in the introduction.

  // @sect4{SineGordonProblem::SineGordonProblem}

  // This is the constructor of the SineGordonProblem class. It
  // specifies the desired polynomial degree of the finite elements,
  // associates a DoFHandler to the triangulation
  // object (just as in the example programs step-3 and step-4), initializes
  // the current or initial time, the final time, the time step size, and the
  // value of $\theta$ for the time stepping scheme. Since the solutions we
  // compute here are time-periodic, the actual value of the start-time
  // doesn't matter, and we choose it so that we start at an interesting time.
  //
  // Note that if we were to chose the explicit Euler time stepping scheme
  // ($\theta = 0$), then we must pick a time step $k \le h$, otherwise the
  // scheme is not stable and oscillations might arise in the solution. The
  // Crank-Nicolson scheme ($\theta = \frac{1}{2}$) and the implicit Euler
  // scheme ($\theta=1$) do not suffer from this deficiency, since they are
  // unconditionally stable. However, even then the time step should be chosen
  // to be on the order of $h$ in order to obtain a good solution. Since we
  // know that our mesh results from the uniform subdivision of a rectangle,
  // we can compute that time step easily; if we had a different domain, the
  // technique in step-24 using GridTools::minimal_cell_diameter would work as
  // well.
  template <int dim>
  SineGordonProblem<dim>::SineGordonProblem ()
    :
    fe (1),
    dof_handler (triangulation),
    n_global_refinements (6),
    time (-5.4414),
    final_time (2.7207),
    time_step (10*1./std::pow(2.,1.*n_global_refinements)),
    theta (0.5),
    output_timestep_skip (1)
  {}

  // @sect4{SineGordonProblem::make_grid_and_dofs}

  // This function creates a rectangular grid in dim dimensions
  // and refines it several times. Also, all matrix and vector members of the
  // SineGordonProblem class are initialized to their appropriate
  // sizes once the degrees of freedom have been assembled. Like step-24, we
  // use the MatrixCreator class to generate a mass matrix $M$
  // and a Laplace matrix $A$ and store them in the appropriate variables for
  // the remainder of the program's life.
  template <int dim>
  void SineGordonProblem<dim>::make_grid_and_dofs ()
  {
    GridGenerator::hyper_cube (triangulation, -10, 10);
    triangulation.refine_global (n_global_refinements);

    std::cout << "   Number of active cells: "
              << triangulation.n_active_cells()
              << std::endl
              << "   Total number of cells: "
              << triangulation.n_cells()
              << std::endl;

    dof_handler.distribute_dofs (fe);

    std::cout << "   Number of degrees of freedom: "
              << dof_handler.n_dofs()
              << std::endl;

    sparsity_pattern.reinit (dof_handler.n_dofs(),
                             dof_handler.n_dofs(),
                             dof_handler.max_couplings_between_dofs());
    DoFTools::make_sparsity_pattern (dof_handler, sparsity_pattern);
    sparsity_pattern.compress ();

    system_matrix.reinit  (sparsity_pattern);
    mass_matrix.reinit    (sparsity_pattern);
    laplace_matrix.reinit (sparsity_pattern);

    MatrixCreator::create_mass_matrix (dof_handler,
                                       QGauss<dim>(3),
                                       mass_matrix);
    MatrixCreator::create_laplace_matrix (dof_handler,
                                          QGauss<dim>(3),
                                          laplace_matrix);

    solution.reinit       (dof_handler.n_dofs());
    solution_update.reinit     (dof_handler.n_dofs());
    old_solution.reinit   (dof_handler.n_dofs());
    M_x_velocity.reinit    (dof_handler.n_dofs());
    system_rhs.reinit     (dof_handler.n_dofs());
  }

  // @sect4{SineGordonProblem::assemble_system}

  // This functions assembles the system matrix and right-hand side vector for
  // each iteration of Newton's method. The reader should refer to the
  // Introduction for the explicit formulas for the system matrix and
  // right-hand side.
  //
  // Note that during each time step, we have to add up the various
  // contributions to the matrix and right hand sides. In contrast to step-23
  // and step-24, this requires assembling a few more terms, since they depend
  // on the solution of the previous time step or previous nonlinear step. We
  // use the functions compute_nl_matrix and
  // compute_nl_term to do this, while the present function
  // provides the top-level logic.
  template <int dim>
  void SineGordonProblem<dim>::assemble_system ()
  {
    // First we assemble the Jacobian matrix $F'_h(U^{n,l})$, where $U^{n,l}$
    // is stored in the vector solution for convenience.
    system_matrix = 0;
    system_matrix.copy_from (mass_matrix);
    system_matrix.add (std::pow(time_step*theta,2), laplace_matrix);

    SparseMatrix<double> tmp_matrix (sparsity_pattern);
    compute_nl_matrix (old_solution, solution, tmp_matrix);
    system_matrix.add (-std::pow(time_step*theta,2), tmp_matrix);

    // Then, we compute the right-hand side vector $-F_h(U^{n,l})$.
    system_rhs = 0;

    tmp_matrix = 0;
    tmp_matrix.copy_from (mass_matrix);
    tmp_matrix.add (std::pow(time_step*theta,2), laplace_matrix);

    Vector<double> tmp_vector (solution.size());
    tmp_matrix.vmult (tmp_vector, solution);
    system_rhs += tmp_vector;

    tmp_matrix = 0;
    tmp_matrix.copy_from (mass_matrix);
    tmp_matrix.add (-std::pow(time_step,2)*theta*(1-theta), laplace_matrix);

    tmp_vector = 0;
    tmp_matrix.vmult (tmp_vector, old_solution);
    system_rhs -= tmp_vector;

    system_rhs.add (-time_step, M_x_velocity);

    tmp_vector = 0;
    compute_nl_term (old_solution, solution, tmp_vector);
    system_rhs.add (std::pow(time_step,2)*theta, tmp_vector);

    system_rhs *= -1;
  }

  // @sect4{SineGordonProblem::compute_nl_term}

  // This function computes the vector $S(\cdot,\cdot)$, which appears in the
  // nonlinear term in the both equations of the split formulation. This
  // function not only simplifies the repeated computation of this term, but
  // it is also a fundamental part of the nonlinear iterative solver that we
  // use when the time stepping is implicit (i.e. $\theta\ne 0$). Moreover, we
  // must allow the function to receive as input an "old" and a "new"
  // solution. These may not be the actual solutions of the problem stored in
  // old_solution and solution, but are simply the
  // two functions we linearize about. For the purposes of this function, let
  // us call the first two arguments $w_{\mathrm{old}}$ and $w_{\mathrm{new}}$
  // in the documentation of this class below, respectively.
  //
  // As a side-note, it is perhaps worth investigating what order quadrature
  // formula is best suited for this type of integration. Since $\sin(\cdot)$
  // is not a polynomial, there are probably no quadrature formulas that can
  // integrate these terms exactly. It is usually sufficient to just make sure
  // that the right hand side is integrated up to the same order of accuracy
  // as the discretization scheme is, but it may be possible to improve on the
  // constant in the asympotitic statement of convergence by choosing a more
  // accurate quadrature formula.
  template <int dim>
  void SineGordonProblem<dim>::compute_nl_term (const Vector<double> &old_data,
                                                const Vector<double> &new_data,
                                                Vector<double>       &nl_term) const
  {
    const QGauss<dim> quadrature_formula (3);
    FEValues<dim>     fe_values (fe, quadrature_formula,
                                 update_values |
                                 update_JxW_values |
                                 update_quadrature_points);

    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points    = quadrature_formula.size();

    Vector<double> local_nl_term (dofs_per_cell);
    std::vector<unsigned int> local_dof_indices (dofs_per_cell);
    std::vector<double> old_data_values (n_q_points);
    std::vector<double> new_data_values (n_q_points);

    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();

    for (; cell!=endc; ++cell)
      {
        // Once we re-initialize our FEValues instantiation to
        // the current cell, we make use of the
        // get_function_values routine to get the values of the
        // "old" data (presumably at $t=t_{n-1}$) and the "new" data
        // (presumably at $t=t_n$) at the nodes of the chosen quadrature
        // formula.
        fe_values.reinit (cell);
        fe_values.get_function_values (old_data, old_data_values);
        fe_values.get_function_values (new_data, new_data_values);

        // Now, we can evaluate $\int_K \sin\left[\theta w_{\mathrm{new}} +
        // (1-\theta) w_{\mathrm{old}}\right] \,\varphi_j\,\mathrm{d}x$ using
        // the desired quadrature formula.
        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            local_nl_term(i) += (std::sin(theta * new_data_values[q_point] +
                                          (1-theta) * old_data_values[q_point]) *
                                 fe_values.shape_value (i, q_point) *
                                 fe_values.JxW (q_point));

        // We conclude by adding up the contributions of the integrals over
        // the cells to the global integral.
        cell->get_dof_indices (local_dof_indices);

        for (unsigned int i=0; i<dofs_per_cell; ++i)
          nl_term(local_dof_indices[i]) += local_nl_term(i);

        local_nl_term = 0;
      }
  }

  // @sect4{SineGordonProblem::compute_nl_matrix}

  // This is the second function dealing with the nonlinear scheme. It
  // computes the matrix $N(\cdot,\cdot)$, whicih appears in the nonlinear
  // term in the Jacobian of $F(\cdot)$. Just as compute_nl_term,
  // we must allow this function to receive as input an "old" and a "new"
  // solution, which we again call $w_{\mathrm{old}}$ and $w_{\mathrm{new}}$
  // below, respectively.
  template <int dim>
  void SineGordonProblem<dim>::compute_nl_matrix (const Vector<double> &old_data,
                                                  const Vector<double> &new_data,
                                                  SparseMatrix<double> &nl_matrix) const
  {
    QGauss<dim>   quadrature_formula (3);
    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values | update_JxW_values | update_quadrature_points);

    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points    = quadrature_formula.size();

    FullMatrix<double> local_nl_matrix (dofs_per_cell, dofs_per_cell);
    std::vector<unsigned int> local_dof_indices (dofs_per_cell);
    std::vector<double> old_data_values (n_q_points);
    std::vector<double> new_data_values (n_q_points);

    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();

    for (; cell!=endc; ++cell)
      {
        // Again, first we re-initialize our FEValues
        // instantiation to the current cell.
        fe_values.reinit (cell);
        fe_values.get_function_values (old_data, old_data_values);
        fe_values.get_function_values (new_data, new_data_values);

        // Then, we evaluate $\int_K \cos\left[\theta w_{\mathrm{new}} +
        // (1-\theta) w_{\mathrm{old}}\right]\, \varphi_i\,
        // \varphi_j\,\mathrm{d}x$ using the desired quadrature formula.
        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)
              local_nl_matrix(i,j) += (std::cos(theta * new_data_values[q_point] +
                                                (1-theta) * old_data_values[q_point]) *
                                       fe_values.shape_value (i, q_point) *
                                       fe_values.shape_value (j, q_point) *
                                       fe_values.JxW (q_point));

        // Finally, we add up the contributions of the integrals over the
        // cells to the global integral.
        cell->get_dof_indices (local_dof_indices);

        for (unsigned int i=0; i<dofs_per_cell; ++i)
          for (unsigned int j=0; j<dofs_per_cell; ++j)
            nl_matrix.add(local_dof_indices[i], local_dof_indices[j],
                          local_nl_matrix(i,j));

        local_nl_matrix = 0;
      }
  }

  // @sect4{SineGordonProblem::solve}

  // As discussed in the Introduction, this function uses the CG iterative
  // solver on the linear system of equations resulting from the finite
  // element spatial discretization of each iteration of Newton's method for
  // the (nonlinear) first equation of the split formulation. The solution to
  // the system is, in fact, $\delta U^{n,l}$ so it is stored in
  // solution_update and used to update solution in
  // the run function.
  //
  // Note that we re-set the solution update to zero before solving for
  // it. This is not necessary: iterative solvers can start from any point and
  // converge to the correct solution. If one has a good estimate about the
  // solution of a linear system, it may be worthwhile to start from that
  // vector, but as a general observation it is a fact that the starting point
  // doesn't matter very much: it has to be a very, very good guess to reduce
  // the number of iterations by more than a few. It turns out that for this
  // problem, using the previous nonlinear update as a starting point actually
  // hurts convergence and increases the number of iterations needed, so we
  // simply set it to zero.
  //
  // The function returns the number of iterations it took to converge to a
  // solution. This number will later be used to generate output on the screen
  // showing how many iterations were needed in each nonlinear iteration.
  template <int dim>
  unsigned int
  SineGordonProblem<dim>::solve ()
  {
    SolverControl solver_control (1000, 1e-12*system_rhs.l2_norm());
    SolverCG<> cg (solver_control);

    PreconditionSSOR<> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);

    solution_update = 0;
    cg.solve (system_matrix, solution_update,
              system_rhs,
              preconditioner);

    return solver_control.last_step();
  }

  // @sect4{SineGordonProblem::output_results}

  // This function outputs the results to a file. It is pretty much identical
  // to the respective functions in step-23 and step-24:
  template <int dim>
  void
  SineGordonProblem<dim>::output_results (const unsigned int timestep_number) const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution, "u");
    data_out.build_patches ();

    const std::string filename =  "solution-" +
                                  Utilities::int_to_string (timestep_number, 3) +
                                  ".vtk";

    std::ofstream output (filename.c_str());
    data_out.write_vtk (output);
  }

  // @sect4{SineGordonProblem::run}

  // This function has the top-level control over everything: it runs the
  // (outer) time-stepping loop, the (inner) nonlinear-solver loop, and
  // outputs the solution after each time step.
  template <int dim>
  void SineGordonProblem<dim>::run ()
  {
    make_grid_and_dofs ();

    // To aknowledge the initial condition, we must use the function $u_0(x)$
    // to compute $U^0$. To this end, below we will create an object of type
    // InitialValues; note that when we create this object (which
    // is derived from the Function class), we set its internal
    // time variable to $t_0$, to indicate that the initial condition is a
    // function of space and time evaluated at $t=t_0$.
    //
    // Then we produce $U^0$ by projecting $u_0(x)$ onto the grid using
    // VectorTools::project. We have to use the same construct
    // using hanging node constraints as in step-21: the VectorTools::project
    // function requires a hanging node constraints object, but to be used we
    // first need to close it:
    {
      ConstraintMatrix constraints;
      constraints.close();
      VectorTools::project (dof_handler,
                            constraints,
                            QGauss<dim>(3),
                            InitialValues<dim> (1, time),
                            solution);
    }

    // For completeness, we output the zeroth time step to a file just like
    // any other other time step.
    output_results (0);

    // Now we perform the time stepping: at every time step we solve the
    // matrix equation(s) corresponding to the finite element discretization
    // of the problem, and then advance our solution according to the time
    // stepping formulas we discussed in the Introduction.
    unsigned int timestep_number = 1;
    for (time+=time_step; time<=final_time; time+=time_step, ++timestep_number)
      {
        old_solution = solution;

        std::cout << std::endl
                  << "Time step #" << timestep_number << "; "
                  << "advancing to t = " << time << "."
                  << std::endl;

        // At the beginning of each time step we must solve the nonlinear
        // equation in the split formulation via Newton's method ---
        // i.e. solve for $\delta U^{n,l}$ then compute $U^{n,l+1}$ and so
        // on. The stopping criterion for this nonlinear iteration is that
        // $\|F_h(U^{n,l})\|_2 \le 10^{-6} \|F_h(U^{n,0})\|_2$. Consequently,
        // we need to record the norm of the residual in the first iteration.
        //
        // At the end of each iteration, we output to the console how many
        // linear solver iterations it took us. When the loop below is done,
        // we have (an approximation of) $U^n$.
        double initial_rhs_norm = 0.;
        bool first_iteration = true;
        do
          {
            assemble_system ();

            if (first_iteration == true)
              initial_rhs_norm = system_rhs.l2_norm();

            const unsigned int n_iterations
              = solve ();

            solution += solution_update;

            if (first_iteration == true)
              std::cout << "    " << n_iterations;
            else
              std::cout << '+' << n_iterations;
            first_iteration = false;
          }
        while (system_rhs.l2_norm() > 1e-6 * initial_rhs_norm);

        std::cout << " CG iterations per nonlinear step."
                  << std::endl;

        // Upon obtaining the solution to the first equation of the problem at
        // $t=t_n$, we must update the auxiliary velocity variable
        // $V^n$. However, we do not compute and store $V^n$ since it is not a
        // quantity we use directly in the problem. Hence, for simplicity, we
        // update $MV^n$ directly:
        Vector<double> tmp_vector (solution.size());
        laplace_matrix.vmult (tmp_vector, solution);
        M_x_velocity.add (-time_step*theta, tmp_vector);

        tmp_vector = 0;
        laplace_matrix.vmult (tmp_vector, old_solution);
        M_x_velocity.add (-time_step*(1-theta), tmp_vector);

        tmp_vector = 0;
        compute_nl_term (old_solution, solution, tmp_vector);
        M_x_velocity.add (-time_step, tmp_vector);

        // Oftentimes, in particular for fine meshes, we must pick the time
        // step to be quite small in order for the scheme to be
        // stable. Therefore, there are a lot of time steps during which
        // "nothing interesting happens" in the solution. To improve overall
        // efficiency -- in particular, speed up the program and save disk
        // space -- we only output the solution every
        // output_timestep_skip time steps:
        if (timestep_number % output_timestep_skip == 0)
          output_results (timestep_number);
      }
  }
}

// @sect3{The main function}

// This is the main function of the program. It creates an object of top-level
// class and calls its principal function. Also, we suppress some of the
// library output by setting deallog.depth_console to
// zero. Furthermore, if exceptions are thrown during the execution of the run
// method of the SineGordonProblem class, we catch and report
// them here. For more information about exceptions the reader should consult
// step-6.
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step25;

      deallog.depth_console (0);

      SineGordonProblem<1> sg_problem;
      sg_problem.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014-2017.
// Modifications copyright (c) 2014-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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 BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/formulas/spherical.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/side.hpp>
//#include <boost/geometry/strategies/concepts/side_concept.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a Great Circle segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class side_by_cross_track
{

public :
    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename promote_floating_point
            <
                typename select_calculation_type_alt
                    <
                        CalculationType,
                        P1, P2, P
                    >::type
            >::type calc_t;

        calc_t d1 = 0.001; // m_strategy.apply(sp1, p);

        calc_t lon1 = geometry::get_as_radian<0>(p1);
        calc_t lat1 = geometry::get_as_radian<1>(p1);
        calc_t lon2 = geometry::get_as_radian<0>(p2);
        calc_t lat2 = geometry::get_as_radian<1>(p2);
        calc_t lon = geometry::get_as_radian<0>(p);
        calc_t lat = geometry::get_as_radian<1>(p);

        calc_t crs_AD = geometry::formula::spherical_azimuth<calc_t, false>
                             (lon1, lat1, lon, lat).azimuth;

        calc_t crs_AB = geometry::formula::spherical_azimuth<calc_t, false>
                             (lon1, lat1, lon2, lat2).azimuth;

        calc_t XTD = asin(sin(d1) * sin(crs_AD - crs_AB));

        return math::equals(XTD, 0) ? 0 : XTD < 0 ? 1 : -1;
    }
};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2013 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_WELL_KNOWN_SRS_HPP
#define MAPNIK_WELL_KNOWN_SRS_HPP

// mapnik
#include <mapnik/global.hpp> // for M_PI on windows
#include <mapnik/enumeration.hpp>

// boost
#include <boost/optional.hpp>

// stl
#include <cmath>

namespace mapnik {

enum well_known_srs_enum {
    WGS_84,
    G_MERC,
    well_known_srs_enum_MAX
};

DEFINE_ENUM( well_known_srs_e, well_known_srs_enum );

static const double EARTH_RADIUS = 6378137.0;
static const double EARTH_DIAMETER = EARTH_RADIUS * 2.0;
static const double EARTH_CIRCUMFERENCE = EARTH_DIAMETER * M_PI;
static const double MAXEXTENT = EARTH_CIRCUMFERENCE / 2.0;
static const double M_PI_by2 = M_PI / 2;
static const double D2R = M_PI / 180;
static const double R2D = 180 / M_PI;
static const double M_PIby360 = M_PI / 360;
static const double MAXEXTENTby180 = MAXEXTENT / 180;
static const double MAX_LATITUDE = R2D * (2 * std::atan(std::exp(180 * D2R)) - M_PI_by2);
static const std::string MAPNIK_LONGLAT_PROJ = "+proj=longlat +ellps=WGS84 +datum=WGS84 +no_defs";
static const std::string MAPNIK_GMERC_PROJ = "+proj=merc +a=6378137 +b=6378137 +lat_ts=0.0 +lon_0=0.0 +x_0=0.0 +y_0=0.0 +k=1.0 +units=m +nadgrids=@null +wktext +no_defs +over";

boost::optional<well_known_srs_e> is_well_known_srs(std::string const& srs);

boost::optional<bool> is_known_geographic(std::string const& srs);

static inline bool lonlat2merc(double * x, double * y , int point_count)
{
    for(int i=0; i<point_count; i++) {
        if (x[i] > 180) x[i] = 180;
        else if (x[i] < -180) x[i] = -180;
        if (y[i] > MAX_LATITUDE) y[i] = MAX_LATITUDE;
        else if (y[i] < -MAX_LATITUDE) y[i] = -MAX_LATITUDE;
        x[i] = x[i] * MAXEXTENTby180;
        y[i] = std::log(std::tan((90 + y[i]) * M_PIby360)) * R2D;
        y[i] = y[i] * MAXEXTENTby180;
    }
    return true;
}

static inline bool merc2lonlat(double * x, double * y , int point_count)
{
    for(int i=0; i<point_count; i++)
    {
        if (x[i] > MAXEXTENT) x[i] = MAXEXTENT;
        else if (x[i] < -MAXEXTENT) x[i] = -MAXEXTENT;
        if (y[i] > MAXEXTENT) y[i] = MAXEXTENT;
        else if (y[i] < -MAXEXTENT) y[i] = -MAXEXTENT;
        x[i] = (x[i] / MAXEXTENT) * 180;
        y[i] = (y[i] / MAXEXTENT) * 180;
        y[i] = R2D * (2 * std::atan(std::exp(y[i] * D2R)) - M_PI_by2);
    }
    return true;
}

}

#endif // MAPNIK_WELL_KNOWN_SRS_HPP


// External libraries
#include <stdio.h>
#include <boost/property_tree/ptree.hpp>

// Local libraries
#include <fmath/constants.h>
#include <fmath/RungeKutta.h>
#include <fparameters/Dimension.h>
#include <fparameters/parameters.h>
#include <fparameters/SpaceIterator.h>

// Local headers
#include "comptonScattMatrix.h"
#include "modelParameters.h"
#include "globalVariables.h"
#include "adafFunctions.h"
#include "write.h"
#include "messages.h"

extern "C" {
	#include <nrMath/random.h>
}
#define RANDOM_GENERATOR gsl_rng_r250 /* R250 random number generator from GNU Scientific Library. */

#define new_max(x,y) ((x) >= (y)) ? (x) : (y)
#define new_min(x,y) ((x) <= (y)) ? (x) : (y)
#define new_abs(x,y) ((x) >= (y)) ?(x-y) : (y-x)

void comptonScattMatrixWrite()
{
	matrixWrite("scattAA.dat", scattAA, nR, nR);
	matrixWrite("scattDA.dat", scattDA, nRcd, nR);
	matrixWrite("reachAD.dat", reachAD, nR, nRcd);
	matrixWrite("reachDA.dat", reachDA, nRcd, nR);
	matrixWrite("reachAA.dat", reachAA, nR, nR);
	vectorWrite("escapeAi.dat", escapeAi, nR);
	vectorWrite("escapeDi.dat", escapeDi, nRcd);
}

void comptonScattMatrix(State& st)
{
	show_message(msgStart,Module_comptonScattMatrix);

	size_t nPhot = GlobalConfig.get<size_t>("scatt.nRandomPhot");
	size_t nTheta = GlobalConfig.get<size_t>("scatt.nTheta");

	double captureRadius = 0.5*sqrt(27.0)*schwRadius;

	Vector rCellsBoundaries(nR+1,0.0), rCellsBoundariesCD(nRcd+1,0.0);
	rCellsBoundaries[0] = st.denf_e.ps[DIM_R][0]/sqrt(paso_r);
	rCellsBoundariesCD[0] = rTr;
    for(size_t iR=1;iR<=nR;iR++) rCellsBoundaries[iR]=rCellsBoundaries[iR-1]*paso_r;
	for(size_t iRcd=1;iRcd<=nRcd;iRcd++) rCellsBoundariesCD[iRcd]=rCellsBoundariesCD[iRcd-1]*paso_rCD;

	double rBound = max(rCellsBoundaries[nR],rCellsBoundariesCD[nRcd]);
	matrixInit(scattAA,nR,nR,0.0);
	matrixInit(scattDA,nRcd,nR,0.0);
	matrixInit(reachAD,nR,nRcd,0.0);
	matrixInit(reachAA,nR,nR,0.0);
	matrixInit(reachDA,nRcd,nR,0.0);
    escapeAi.resize(nR,0.0);
	escapeDi.resize(nRcd,0.0);

    InitialiseRandom(RANDOM_GENERATOR);
	//ADAF SCATTERING MATRIX

	double pasoprim = min(pow(rCellsBoundaries[1]/rCellsBoundaries[0],1.0/10.0),
						1.0+height_fun(st.denf_i.ps[DIM_R][0])/st.denf_i.ps[DIM_R][0]);

	#pragma omp parallel for
	for (int iR=0;iR<nR;iR++) {
		double r0 = st.denf_e.ps[DIM_R][iR];
		double thetaMin;
		if (height_method == 0)
			thetaMin = st.thetaH.get({0,iR,0});
		else
			thetaMin = atan(r0/height_fun(r0));
		double dyaux=(1.0-sin(thetaMin))/nTheta;
        for(size_t kTh=1;kTh<=nTheta;kTh++) {
            double yaux=sin(thetaMin)+kTh*dyaux;  // Theta distributed uniformly in sin(theta).
            double theta0=asin(yaux);
			double y0,z0;
			if (height_method == 0) {
				y0=r0*sin(theta0);
				z0=r0*cos(theta0);
			} else {
				y0 = r0;
				z0 = r0/tan(theta0);
			}
			for(size_t jPh=1;jPh<=nPhot;jPh++) {
				double random_number = gsl_rng_uniform(RandomNumberGenerator);
				double phiprim = 2.0*pi*random_number;
				random_number = gsl_rng_uniform(RandomNumberGenerator);
				double thetaprim = acos(1.0-2.0*random_number);   // Photon directions
																  // distributed
																  // isotropically.
				double pescap = 1.0;
				double z1ant = z0;
				double drprim=r0*(pasoprim-1.0);       // Step.
				double rprim=drprim;
				vector<size_t> countAA(nR,0);
				double r1 = r0;
				double z1 = z0;
				double r1aux = r1;
                do {
					double xprim=rprim*sin(thetaprim)*cos(phiprim);
					double yprim=rprim*sin(thetaprim)*sin(phiprim);
					double zprim=rprim*cos(thetaprim);
					double x1=xprim;
					double y1=y0+yprim;
					z1=z0+zprim;
					r1=sqrt(x1*x1+y1*y1+z1*z1);
					r1aux = (height_method == 0) ? r1 : sqrt(x1*x1+y1*y1);
					double theta1=atan(sqrt(x1*x1+y1*y1)/abs(z1));
					double ne = electronDensityTheta(r1,theta1);
					double exptau = exp(-ne*thomson*drprim);
                    double psc = 1.0-exptau;    // Probability of scattering.
					if (r1aux > rTr && z1*z1ant < 0.0) {
						for(size_t jRcd=0;jRcd<=nRcd;jRcd++) {
							if(r1aux > rCellsBoundariesCD[jRcd] && r1aux < rCellsBoundariesCD[jRcd+1]) {
								reachAD[iR][jRcd] += pescap; 		// Add the probability of
																	// absorption by the ring
																	// jRcd of the thin disk.
								goto LOOP;
							}
						}
                    }
					z1ant = z1;
					for(size_t jR=0;jR<nR;jR++) {

						if(r1aux > rCellsBoundaries[jR] && r1aux < rCellsBoundaries[jR+1]) {

							scattAA[iR][jR] += psc*pescap;
							if (countAA[jR] == 0) {
								reachAA[iR][jR] += pescap;
								countAA[jR]++;
							}
						}
                    }
					pescap *= exptau;
                    drprim=r1*(pasoprim-1.0);
                    rprim += drprim;
                } while(r1aux < rBound && z1 < rBound && r1 > schwRadius);
				LOOP:;
            }
        }
        for(size_t jR=0;jR<nR;jR++) {
            scattAA[iR][jR] /= (nPhot*nTheta);     // Dividing by the number of photons launched.
			reachAA[iR][jR] /= (nPhot*nTheta);
        }
		for (size_t jRcd=0;jRcd<nRcd;jRcd++) {
			reachAD[iR][jRcd] /= (nPhot*nTheta);
		}
	}

	//////////////////////////////////////////////////////////////////////////////////////

	// ADAF ESCAPE PHOTONS
	#pragma omp parallel for
	for (int iR=0;iR<nR;iR++) {
		double r0 = st.thetaH.ps[DIM_R][iR];
		double drprim=r0*(pasoprim-1.0);
		double thetaMin;
		if (height_method == 0)
			thetaMin = st.thetaH.get({0,iR,0});
		else
			thetaMin = atan(r0/height_fun(r0));
		double dyaux=(1.0-sin(thetaMin))/nTheta;
        for(size_t kTh=1;kTh<=nTheta;kTh++) {
            double yaux=sin(thetaMin)+kTh*dyaux;  // Theta distributed uniformly in sin(theta).
            double theta0 = (yaux < 1.0 ? asin(yaux) : pi/2.0);
			double phi0 = 0.0;
			double dPhi = 2.0*pi/nPhot;

			double x0,y0,z0;
			if (height_method == 0) {
				x0 = r0*sin(theta0)*cos(phi0);
				y0 = r0*sin(theta0)*cos(phi0);
				z0 = r0*cos(theta0);
			} else {
				x0 = r0*sin(phi0);
				y0 = r0*cos(phi0);
				z0 = r0/tan(theta0);
			}
			for(size_t jPh=1;jPh<=nPhot;jPh++) {
				double thetaprim = inclination*(pi/180.0);
				double rprim=drprim;
				double r1 = r0;
				double z1 = z0;
				double r1aux = r1;
				double pescap = 1.0;
				do {
					double xprim=rprim*sin(thetaprim);
					double zprim=rprim*cos(thetaprim);
					double x1=x0+xprim;
					z1=z0+zprim;
					r1=sqrt(x1*x1+y0*y0+z1*z1);
					double theta1=atan(sqrt(x1*x1+y0*y0)/abs(z1));
					r1aux = (height_method == 0) ? r1 : sqrt(x1*x1+y0*y0);
					double ne = electronDensityTheta(r1,theta1);
					pescap *= exp(-ne*thomson*drprim);
					drprim=r1*(pasoprim-1.0);
					rprim += drprim;
				} while (r1aux < rCellsBoundaries[nR] && z1 < rCellsBoundaries[nR]);
				escapeAi[iR] += pescap;
				phi0 += dPhi;
            }
        }
		escapeAi[iR] /= (nPhot*nTheta);     // Dividing by the number of photons launched.
	}

	//////////////////////////////////////////////////////////////////////////////////////

	// COLD DISK SCATTERING MATRIX
	#pragma omp parallel for
	for (int iRcd=0;iRcd<nRcd;iRcd++) {
		double r0cd = st.denf_e.ps[DIM_Rcd][iRcd];
		for(size_t jPh=1;jPh<=nPhot;jPh++) {
			double drprim = r0cd*(pasoprim-1.0);             // Step for the photon path.
			double random_number = gsl_rng_uniform(RandomNumberGenerator);
			double phiprim = 2.0*pi*random_number;
			random_number = gsl_rng_uniform(RandomNumberGenerator);
			double thetaprim = 0.5*acos(1.0-2.0*random_number);   // Photon directions
																  // distributed
			double rprim=drprim; 	                           	  // proportional to cos(theta)sin(theta).
			double pescap = 1.0;
			double r1 = r0cd;
			double r1aux = r1;
			double z1 = 0.0;
			vector<size_t> countDA(nR,0);
			do {
				double xprim=rprim*sin(thetaprim)*cos(phiprim);
				double yprim=rprim*sin(thetaprim)*sin(phiprim);
				double zprim=rprim*cos(thetaprim);
				double x1=xprim;
				double y1=r0cd+yprim;
				z1=zprim;
				r1=sqrt(x1*x1+y1*y1+z1*z1);
				double theta1=atan(sqrt(x1*x1+y1*y1)/abs(z1));
				r1aux = (height_method == 0) ? r1 : sqrt(x1*x1+y1*y1);
				double exptau = 1.0;
				if (r1aux < rCellsBoundaries[nR]) {
					double ne = electronDensityTheta(r1aux,theta1);
					exptau = exp(-ne*thomson*drprim);
					double psc = 1.0-exptau;    // Probability of scattering.
					for(size_t jR=0;jR<=nR;jR++) {
						if(r1aux > rCellsBoundaries[jR] && r1aux < rCellsBoundaries[jR+1]) {
							scattDA[iRcd][jR] += psc*pescap;
							if (countDA[jR] == 0) {
								reachDA[iRcd][jR] += pescap;
								countDA[jR]++;
							}
						}
					}
				}
				drprim=r1aux*(pasoprim-1.0);
				rprim += drprim;
				pescap *= exptau;                // Probability that a photon reaches
												 // the previous position.
			} while(r1aux < rBound && z1 < rBound && r1 > schwRadius);
        }
        for(size_t jR=0;jR<nR;jR++) {
            scattDA[iRcd][jR] /= nPhot;     // Dividing by the number of photons launched.
			reachDA[iRcd][jR] /= nPhot;
        }
	}

	//COLD DISK ESCAPE PHOTONS
	#pragma omp parallel for
	for (int iRcd=0;iRcd<nRcd;iRcd++) {
		double r0cd = st.denf_e.ps[DIM_Rcd][iRcd];
		double drprim=r0cd*(pasoprim-1.0);
		for(size_t jPh=1;jPh<=nPhot;jPh++) {
			double random_number = gsl_rng_uniform(RandomNumberGenerator);
			double phiprim = 2.0*pi*random_number;
			double thetaprim = inclination*(pi/180.0);
			double rprim=drprim;
			double r1 = r0cd;
			double z1 = 0.0;
			double r1aux = r1;
			double pescap = 1.0;
			do {
				double xprim=rprim*sin(thetaprim)*cos(phiprim);
				double yprim=rprim*sin(thetaprim)*sin(phiprim);
				double zprim=rprim*cos(thetaprim);
				double x1=xprim;
				double y1=r0cd+yprim;
				z1=zprim;
				r1=sqrt(x1*x1+y1*y1+z1*z1);
				double theta1=atan(sqrt(x1*x1+y1*y1)/abs(z1));
				r1aux = (height_method == 0) ? r1 : sqrt(x1*x1+y1*y1);
				double ne = electronDensityTheta(r1,theta1);
				pescap *= exp(-ne*thomson*drprim);
				drprim=r1*(pasoprim-1.0);
				rprim += drprim;
			} while(r1aux < rBound && z1 < rBound);     // Escape from the region.
			escapeDi[iRcd] += pescap;
        }
        escapeDi[iRcd] /= nPhot;
	}

    FinaliseRandom();
	comptonScattMatrixWrite();

	show_message(msgEnd,Module_comptonScattMatrix);
}

void comptonScattMatrixRead(State& st)
{
	matrixInit(scattAA,nR,nR,0.0);
	matrixInit(scattDA,nRcd,nR,0.0);
	matrixInit(reachAD,nR,nRcd,0.0);
	matrixInit(reachAA,nR,nR,0.0);
	matrixInit(reachDA,nRcd,nR,0.0);
    escapeAi.resize(nR,0.0);
	escapeDi.resize(nRcd,0.0);

	matrixRead("scattAA.dat",scattAA,nR,nR);
	matrixRead("scattDA.dat",scattDA,nRcd,nR);
	matrixRead("reachAD.dat",reachAD,nR,nRcd);
	matrixRead("reachAA.dat",reachAA,nR,nR);
	matrixRead("reachDA.dat",reachDA,nRcd,nR);
	vectorRead("escapeAi.dat",escapeAi,nR);
	vectorRead("escapeDi.dat",escapeDi,nRcd);
}


/*
// Copyright 2018 University of Liege
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Authors:
// - Adrien Crovato
*/

//// Field variables computation
// Compute variables (velocity, speed of sound, density, mach number) in the field
//
// I/O:
// - Minf: freestream Mach number
// - vInf: freestream velocity vector
// - bPan: (network of) body panels (structure)
// - fPan: field panels (structure)
// - sp: sub-panel (structure)
// - b2fAIC: body to field AIC (structure)
// - f2fAIC: field to field AIC (structure)
// - spAIC: body to field AIC for subpanels (structure)

#include <iostream>
#include <Eigen/Dense>
#include "compute_fVars.h"
#include "interp_ctv.h"
#include "interp_sp.h"

#define GAMMA 1.4
#define NV 4 // number of vertices

using namespace std;
using namespace Eigen;

void compute_fVars(double Minf, Vector3d &vInf, Network &bPan, Field &fPan, Subpanel &sp,
                       Body_AIC &b2fAIC, Field2field_AIC &f2fAIC, Subpanel_AIC &spAIC) {

    //// Initialization
    // Temporary variables
    MatrixXd vSing; // corner interpolated singularities
    vSing.resize(NV,2);
    MatrixXd sInterp; // sub-panel interoplated singularities
    sInterp.resize(sp.NS,2);

    //// Potential
    // Perturbation potential
    fPan.phi = b2fAIC.B * bPan.tau + b2fAIC.A * bPan.mu + f2fAIC.C * fPan.sigma;

    // Perturbation potential (with sub-paneling technique)
    // TODO Subpaneling induces small assymmetry. Check why?
    for (int jj = 0; jj < sp.sI.size(); jj++) {
        int j = sp.sI[jj]; // panel global index
        int n = j % bPan.nC_; // panel chordwise index
        int m = j / bPan.nC_; // panel spanwise index
        // Interpolation of singularities from centers to vertices
        vSing = interp_ctv(j, n, m, bPan);
        // Interpolation of singularities from vertices to sub-panel
        sInterp = interp_sp(j, bPan, sp,
                            vSing(0,0), vSing(1,0), vSing(2,0), vSing(3,0),
                            vSing(0,1), vSing(1,1), vSing(2,1), vSing(3,1));

        for (int ii = 0; ii < sp.fI[jj].size(); ii++) {
            int i = sp.fI[jj][ii]; // cell index
            // Velocity induces by each sub-panel
            for (int k = 0; k < sp.NS; k++) {
                fPan.phi(i) += spAIC.A[jj](ii,k) * sInterp(k,0) + spAIC.B[jj](ii,k) * sInterp(k,1);
            }
        }
    }

    //// Velocity
    // Perturbation velocity
    for (int i = 0; i < fPan.nE; i++) {
        int f = fPan.eIdx(i);

        // X-derivative
        double fb=0, ff=0;
        if (fPan.fbdMap(f,0) && fPan.fwdMap(f,0))
            fb = (fPan.phi(f) - fPan.phi(f - 1)) / fPan.deltaX;
        if (fPan.fbdMap(f,1) && fPan.fwdMap(f,1))
            ff = (fPan.phi(f + 1) - fPan.phi(f)) / fPan.deltaX;
        if (fPan.fbdMap(f,0) && fPan.fwdMap(f,0) && fPan.fbdMap(f,1) && fPan.fwdMap(f,1))
            fPan.U(f,0) = 0.5*(fb+ff);
        else
            fPan.U(f,0) = fb+ff;

        // Y-derivative
        fb=0; ff=0;
        if (fPan.fbdMap(f,2) && fPan.fwdMap(f,2))
            fb = (fPan.phi(f) - fPan.phi(f - fPan.nX*fPan.nZ)) / fPan.deltaY;
        if (fPan.fbdMap(f,3) && fPan.fwdMap(f,3))
            ff = (fPan.phi(f + fPan.nX*fPan.nZ) - fPan.phi(f)) / fPan.deltaY;
        if (fPan.fbdMap(f,2) && fPan.fwdMap(f,2) && fPan.fbdMap(f,3) && fPan.fwdMap(f,3))
            fPan.U(f,1) = 0.5*(fb+ff);
        else
            fPan.U(f,1) = fb+ff;

        // Z-derivative
        fb=0; ff=0;
        if (fPan.fbdMap(f,4) && fPan.fwdMap(f,4))
            fb = (fPan.phi(f) - fPan.phi(f - fPan.nX)) / fPan.deltaZ;
        if (fPan.fbdMap(f,5) && fPan.fwdMap(f,5))
            ff = (fPan.phi(f + fPan.nX) - fPan.phi(f)) / fPan.deltaZ;
        if (fPan.fbdMap(f,4) && fPan.fwdMap(f,4) && fPan.fbdMap(f,5) && fPan.fwdMap(f,5))
            fPan.U(f,2) = 0.5*(fb+ff);
        else
            fPan.U(f,2) = fb+ff;
    }
    // Freestream component
    for (int i = 0; i < fPan.nE; ++i) {
        fPan.U.row(fPan.eIdx(i)) += vInf.transpose();
    }

    //// Thermodynamic variables
    for (int i = 0; i < fPan.nE; ++i) {
        int f = fPan.eIdx(i);
        // Speed of sound
        fPan.a(f) = sqrt(
                1/(Minf * Minf) + (GAMMA - 1) / 2 - (GAMMA - 1) / 2 * fPan.U.row(f).dot(fPan.U.row(f)));
        // Mach number
        fPan.M(f) = fPan.U.row(f).norm() / fPan.a(f);
        // Density
        fPan.rho(f) = pow(
                1 + (GAMMA - 1) / 2 * Minf * Minf * (1 - fPan.U.row(f).dot(fPan.U.row(f))),
                1 / (GAMMA - 1));
    }
}


// __BEGIN_LICENSE__
// Copyright (C) 2006-2010 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#ifdef _MSC_VER
#pragma warning(disable:4244)
#pragma warning(disable:4267)
#pragma warning(disable:4996)
#endif

#ifdef NDEBUG
#undef NDEBUG
#endif

#include <cstdlib>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <boost/filesystem/path.hpp>
namespace fs = boost::filesystem;

#include <vw/Math/EulerAngles.h>
#include <vw/Image/ImageView.h>
#include <vw/Image/Algorithms.h>
#include <vw/Image/ImageViewRef.h>
#include <vw/Image/Filter.h>
#include <vw/Image/PixelMask.h>
#include <vw/Image/MaskViews.h>
#include <vw/Image/PerPixelAccessorViews.h>
#include <vw/FileIO/DiskImageView.h>
#include <vw/FileIO/DiskImageResourceGDAL.h>
#include <vw/Cartography/GeoReference.h>
#include <vw/tools/Common.h>

using namespace vw;

// Global Variables
std::string input_file_name, output_file_name = "";
double azimuth, elevation, scale;
double nodata_value;
double blur_sigma;

// Allows FileIO to correctly read/write these pixel types
namespace vw {
  template<> struct PixelFormatID<Vector3>   { static const PixelFormatEnum value = VW_PIXEL_XYZ; };
}

// ---------------------------------------------------------------------------------------------------

//  compute_normals()
//
// Compute a vector normal to the surface of a DEM for each given
// pixel.  The normal is computed by forming a plane with three points
// in the vicinity of the requested pixel, and then finding the vector
// normal to that plane.  The user must specify the scale in the [u,v]
// directions so that the direction of the vector in physical space
// can be properly ascertained.  This is often contained in the (0,0)
// and (1,1) entry of the georeference transform.
class ComputeNormalsFunc : public ReturnFixedType<PixelMask<Vector3> >
{
  double m_u_scale, m_v_scale;

public:
  ComputeNormalsFunc(double u_scale, double v_scale) :
    m_u_scale(u_scale), m_v_scale(v_scale) {}

  BBox2i work_area() const { return BBox2i(Vector2i(0, 0), Vector2i(1, 1)); }

  template <class PixelAccessorT>
  PixelMask<Vector3> operator() (PixelAccessorT const& accessor_loc) const {
    PixelAccessorT acc = accessor_loc;

    // Pick out the three altitude values.
    if (is_transparent(*acc))
      return PixelMask<Vector3>();
    double alt1 = *acc;

    acc.advance(1,0);
    if (is_transparent(*acc))
      return PixelMask<Vector3>();
    double alt2 = *acc;

    acc.advance(-1,1);
    if (is_transparent(*acc))
      return PixelMask<Vector3>();
    double alt3 = *acc;

    // Form two orthogonal vectors in the plane containing the three
    // altitude points
    Vector3 n1(m_u_scale, 0, alt2-alt1);
    Vector3 n2(0, m_v_scale, alt3-alt1);

    // Return the vector normal to the local plane.
    return normalize(cross_prod(n1,n2));
  }
};

template <class ViewT>
UnaryPerPixelAccessorView<EdgeExtensionView<ViewT,ConstantEdgeExtension>, ComputeNormalsFunc> compute_normals(ImageViewBase<ViewT> const& image,
                                                                                double u_scale, double v_scale) {
  return UnaryPerPixelAccessorView<EdgeExtensionView<ViewT,ConstantEdgeExtension>, ComputeNormalsFunc>(edge_extend(image.impl(), ConstantEdgeExtension()),
                                                                                ComputeNormalsFunc (u_scale, v_scale));
}

class DotProdFunc : public ReturnFixedType<PixelMask<PixelGray<double> > > {
  Vector3 m_vec;
public:
  DotProdFunc(Vector3 const& vec) : m_vec(vec) {}
  PixelMask<PixelGray<double> > operator() (PixelMask<Vector3> const& pix) const {
    if (is_transparent(pix))
      return PixelMask<PixelGray<double> >();
    else {
//       std::cout << "Vec1 : " << pix.child() << "   " << norm_2(pix.child()) << "\n";
//       std::cout << "Vec2 : " << m_vec << "   " << norm_2(m_vec) << "\n";
//       std::cout << "OVerall: " << dot_prod(pix.child(),m_vec)/(norm_2(pix.child()) * norm_2(m_vec)) << "\n\n";
      return dot_prod(pix.child(),m_vec)/(norm_2(pix.child()) * norm_2(m_vec));
    }
  }
};

template <class ViewT>
UnaryPerPixelView<ViewT, DotProdFunc> dot_prod(ImageViewBase<ViewT> const& view, Vector3 const& vec) {
  return UnaryPerPixelView<ViewT, DotProdFunc>(view.impl(), DotProdFunc(vec));
}

// ---------------------------------------------------------------------------------------------------

template <class PixelT>
void do_hillshade(po::variables_map const& vm) {

  cartography::GeoReference georef;
  cartography::read_georeference(georef, input_file_name);

  // Select the pixel scale.
  double u_scale, v_scale;
  if (scale == 0) {
    if (georef.is_projected()) {
      u_scale = georef.transform()(0,0);
      v_scale = georef.transform()(1,1);
    } else {
      double meters_per_degree = 2*M_PI*georef.datum().semi_major_axis()/360.0;
      u_scale = georef.transform()(0,0) * meters_per_degree;
      v_scale = georef.transform()(1,1) * meters_per_degree;
    }
  } else {
    u_scale = scale;
    v_scale = -scale;
  }
  // For debugging:
  //  std::cout << "\t--> Scale: " << u_scale << "  " << v_scale << "\n";

  // Set the direction of the light source.
  Vector3 light_0(1,0,0);
  Vector3 light = vw::math::euler_to_rotation_matrix(elevation*M_PI/180, azimuth*M_PI/180, 0, "yzx") * light_0;

  // Compute the surface normals
  std::cout << "Loading: " << input_file_name << ".\n";
  DiskImageView<PixelT> disk_dem_file(input_file_name);
  ImageViewRef<PixelGray<double> > input_image = channel_cast<double>(disk_dem_file);

  ImageViewRef<PixelMask<PixelGray<double> > > dem;
  SrcImageResource *disk_dem_rsrc = DiskImageResource::open(input_file_name);
  if (vm.count("nodata-value")) {
    std::cout << "\t--> Masking pixel value: " << nodata_value << ".\n";
    dem = create_mask(input_image, nodata_value);
  } else if ( disk_dem_rsrc->has_nodata_read() ) {
    nodata_value = disk_dem_rsrc->nodata_read();
    std::cout << "\t--> Extracted nodata value from file: " << nodata_value << ".\n";
    dem = create_mask(input_image, nodata_value);
  } else {
    dem = pixel_cast<PixelMask<PixelGray<double> > >(input_image);
  }
  delete disk_dem_rsrc;

  if (vm.count("blur")) {
    std::cout << "\t--> Blurring pixel with gaussian kernal.  Sigma = " << blur_sigma << "\n";
    dem = gaussian_filter(dem, blur_sigma);
  }

  // The final result is the dot product of the light source with the normals
  ImageViewRef<PixelMask<PixelGray<uint8> > > shaded_image = channel_cast_rescale<uint8>(clamp(dot_prod(compute_normals(dem, u_scale, v_scale), light)));

  // Save the result
  std::cout << "Writing shaded relief image: " << output_file_name << "\n";

  DiskImageResourceGDAL rsrc(output_file_name, shaded_image.format());
  rsrc.set_block_write_size(Vector2i(1024,1024));
  write_georeference(rsrc, georef);
  write_image(rsrc, shaded_image,
              TerminalProgressCallback( "tools.hillshade", "Writing:"));
}

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

  po::options_description desc("Description: Outputs image of a DEM lighted as specified\n\nUsage: hillshade [options] <input file> \n\nOptions");
  desc.add_options()
    ("help,h", "Display this help message")
    ("input-file", po::value<std::string>(&input_file_name), "Explicitly specify the input file")
    ("output-file,o", po::value<std::string>(&output_file_name), "Specify the output file")
    ("azimuth,a", po::value<double>(&azimuth)->default_value(0), "Sets the direction tha the light source is coming from (in degrees).  Zero degrees is to the right, with positive degree counter-clockwise.")
    ("elevation,e", po::value<double>(&elevation)->default_value(45), "Set the elevation of the light source (in degrees).")
    ("scale,s", po::value<double>(&scale)->default_value(0), "Set the scale of a pixel (in the same units as the DTM height values.")
    ("nodata-value", po::value<double>(&nodata_value), "Remap the DEM default value to the min altitude value.")
    ("blur", po::value<double>(&blur_sigma), "Pre-blur the DEM with the specified sigma.");
  po::positional_options_description p;
  p.add("input-file", 1);

  po::variables_map vm;
  try {
    po::store( po::command_line_parser( argc, argv ).options(desc).positional(p).run(), vm );
    po::notify( vm );
  } catch (po::error &e) {
    std::cout << "An error occured while parsing command line arguments.\n";
    std::cout << "\t" << e.what() << "\n\n";
    std::cout << desc << std::endl;
    return 1;
  }

  if( vm.count("help") ) {
    std::cout << desc << std::endl;
    return 1;
  }

  if( vm.count("input-file") != 1 ) {
    std::cout << "Error: Must specify exactly one input file!\n" << std::endl;
    std::cout << desc << std::endl;
    return 1;
  }

  if( output_file_name == "" ) {
    output_file_name = fs::path(input_file_name).replace_extension().string() + "_HILLSHADE.tif";
  }

  try {
    // Get the right pixel/channel type.
    ImageFormat fmt = tools::taste_image(input_file_name);

    switch(fmt.pixel_format) {
    case VW_PIXEL_GRAY:
    case VW_PIXEL_GRAYA:
    case VW_PIXEL_RGB:
    case VW_PIXEL_RGBA:
      switch(fmt.channel_type) {
      case VW_CHANNEL_UINT8:  do_hillshade<PixelGray<uint8>   >(vm); break;
      case VW_CHANNEL_INT16:  do_hillshade<PixelGray<int16>   >(vm); break;
      case VW_CHANNEL_UINT16: do_hillshade<PixelGray<uint16>  >(vm); break;
      case VW_CHANNEL_FLOAT64:do_hillshade<PixelGray<float64> >(vm); break;
      default:                do_hillshade<PixelGray<float32> >(vm); break;
      }
      break;
    default:
      std::cout << "Error: Unsupported pixel format.\n";
      exit(0);
    }
  } catch( Exception& e ) {
    std::cout << "Error: " << e.what() << std::endl;
  }

  return 0;
}


#ifndef SIG_POLAR_SPIN_HPP
#define SIG_POLAR_SPIN_HPP

#include "signlp.hpp"
#include "SigUtil/lib/functional/fold.hpp"
#include "SigUtil/lib/calculation.hpp"
#include <boost/graph/adjacency_list.hpp>

namespace signlp
{

class SpinModel
{
	struct Node
	{
		std::wstring word;
		uint degree;

		bool has_label;
		double label;

		double mean_x;
		double tmp_mean_x;
	};

public:
	using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
		Node,
		boost::property<boost::edge_weight_t, double>
	>;
	using pNode = Graph::vertex_descriptor;
	using pEdge = Graph::edge_descriptor;

	static auto make_node(Graph& g, std::wstring word) ->pNode
	{
		auto v = boost::add_vertex(g);
		g[v].word = word;
		g[v].has_label = false;
		return v;
	}
	static auto make_node(Graph& g, std::wstring word, bool label) ->pNode
	{
		auto v = boost::add_vertex(g);
		g[v].word = word;
		g[v].label = label ? 1 : -1;
		g[v].has_label = true;
		return v;
	}
	static auto make_node(Graph& g, std::wstring word, double label) ->pNode
	{
		auto v = boost::add_vertex(g);
		g[v].word = word;
		g[v].label = label;
		g[v].has_label = true;
		return v;
	}

	static void make_edge(Graph& g, pNode v1, pNode v2, double weight)
	{
		auto e = add_edge(v1, v2, g);
		put(boost::edge_weight, g, e.first, weight);
	};

private:
	Graph graph_;

	const double alpha_;	// ラベル(正解)の反映度
	const double beta_;		// 逆温度

	const sig::array<int, 2> xs_;
	std::unordered_map<pNode, std::vector<pNode>> adj_;
	sig::SimpleRandom<double> rand_d_;

private:
	void init()
	{
		auto nodes = boost::vertices(graph_);

		std::for_each(
			boost::begin(nodes),
			boost::end(nodes),
			[&](pNode n){
				std::vector<pNode> tmp;
				auto adj = adjacent_vertices(n, graph_);
				for (auto it = adj.first, end = adj.second; it != end; ++it){
					tmp.push_back(*it);
				}

				graph_[n].degree = tmp.size();
				adj_.emplace(n, std::move(tmp));

				if (graph_[n].has_label){
					graph_[n].mean_x = graph_[n].label;
					graph_[n].tmp_mean_x = graph_[n].label;
				}
				else{
					double r = rand_d_();
					graph_[n].mean_x = r;
					graph_[n].tmp_mean_x = r;
				}
			}
		);

		auto edges = boost::edges(graph_);

		std::for_each(
			boost::begin(edges),
			boost::end(edges),
			[&](pEdge e){
				auto v1 = boost::source(e, graph_);
				auto v2 = boost::target(e, graph_);
				auto adjn1 = graph_[v1].degree;
				auto adjn2 = graph_[v2].degree;

				double decay = std::sqrt(adjn1 * adjn2);
				auto t = boost::get(boost::get(boost::edge_weight, graph_), e);
				boost::get(boost::get(boost::edge_weight, graph_), e) /= decay;
			}
		);
	}

	void update(uint k)
	{
		const double wx_sum = sig::dotProduct(
			std::plus<double>(),
			[&](uint n){ return graph_[n].mean_x * boost::get(boost::get(boost::edge_weight, graph_), boost::edge(k, n, graph_).first); },
			0, adj_[k]
		);

		const auto exp_wx_sum = sig::map([&](int x_i){	return std::exp(beta_ * x_i * wx_sum); }, xs_);

		auto t = sig::dotProduct(std::plus<double>(), std::multiplies<double>(), 0, exp_wx_sum, xs_) / sig::sum(exp_wx_sum);

		if (!sig::is_number(t)){
			std::cout << "wxsum:" << wx_sum << std::endl;
			std::cout << "exp_wx_sum0:" << exp_wx_sum[0] << std::endl;
			std::cout << "exp_wx_sum1:" << exp_wx_sum[1] << std::endl;
			getchar();
		}

		graph_[k].tmp_mean_x = t;
	}

	void updateL(uint k)
	{
		if (adj_[k].empty()) return;
		/*const double wx_sum = sig::fold_zipWith(
			std::multiply<double>(),
			std::plus<double>(),
			0, w_[k], mean_x_
		);*/
		const double wx_sum = sig::dotProduct(
			std::plus<double>(),
			[&](uint n){ return graph_[n].mean_x * boost::get(boost::get(boost::edge_weight, graph_), boost::edge(k, n, graph_).first); },
			0, adj_[k]
		);

		const auto exp_wx_sum = sig::map([&](int x_i){	return std::exp(beta_ * x_i * wx_sum - alpha_ * std::pow(x_i - graph_[k].label, 2)); }, xs_);

		auto t = sig::dotProduct(std::plus<double>(), std::multiplies<double>(), 0, exp_wx_sum, xs_) / sig::sum(exp_wx_sum);

		if (!sig::is_number(t)){
			std::cout << "wxsum:" << wx_sum << std::endl;
			std::cout << "exp_wx_sum0:" << exp_wx_sum[0] << std::endl;
			std::cout << "exp_wx_sum1:" << exp_wx_sum[1] << std::endl;
			getchar();
		}

		graph_[k].tmp_mean_x = t;
	}

public:
	SpinModel(Graph const& graph, double alpha, double beta) : graph_(graph), alpha_(alpha), beta_(beta), xs_({ -1, 1 }), rand_d_(-1.0, 1.0, true)
	{
		init();
	}

	void train(uint iteration_num, std::function<void(SpinModel const*)> callback)
	{
		for (uint i = 0; i < iteration_num; ++i){
			auto nodes = boost::vertices(graph_);

			std::for_each(
				boost::begin(nodes),
				boost::end(nodes),
				[&](pNode n){ graph_[n].has_label ? updateL(n) : update(n); }
			);

			std::for_each(
				boost::begin(nodes),
				boost::end(nodes),
				[&](pNode n){ graph_[n].mean_x = graph_[n].tmp_mean_x; }
			);

			callback(this);
		}
	}

	auto getScore(std::wstring word) const->sig::Maybe<double>
	{
		auto nodes = boost::vertices(graph_);

		for(auto it = nodes.first, end = nodes.second; it != end; ++it){
			if(graph_[*it].word == word) return graph_[*it].mean_x;
		}
		return boost::none;
	}

	auto getScore() const->std::unordered_map<std::wstring, double>
	{
		std::unordered_map<std::wstring, double> result;
		auto nodes = boost::vertices(graph_);

		for (auto it = nodes.first, end = nodes.second; it != end; ++it){
			result.emplace(graph_[*it].word, graph_[*it].mean_x);
		}
		return result;
	}

	// leave-one-out error
	double getErrorRate() const
	{
		int ct = 0;
		double sum = 0;
		auto nodes = boost::vertices(graph_);

		std::for_each(
			boost::begin(nodes),
			boost::end(nodes),
			[&](pNode n){
				if(graph_[n].has_label){
					sum += (graph_[n].label * graph_[n].mean_x < 0 ? 1 : 0);
					++ct;
				}
			}
		);

		return sum / ct;
	}

	double getMeanPolar() const
	{
		int ct = 0;
		double sum = 0;
		auto nodes = boost::vertices(graph_);

		std::for_each(
			boost::begin(nodes),
			boost::end(nodes),
			[&](pNode n){
				sum += graph_[n].mean_x;
				++ct;
			}
		);

		return sum / ct;
	}
};

}
#endif


#include <armadillo>
#include <boost/program_options.hpp>
#include <json.hpp>
#include <HSMM.hpp>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace robotics;
using namespace std;
namespace po = boost::program_options;

field<mat> fromMatToField(const mat& obs) {
    field<mat> ret(obs.n_cols);
    for(int i = 0; i < obs.n_cols; i++)
        ret(i) = obs.col(i);
    return ret;
}

// Running the Viterbi algorithm.
void ViterbiAlgorithm(HSMM& promp_hsmm, const field<field<mat>>& seq_obs,
        string filename) {
    int nseq = seq_obs.n_elem;
    for(int s = 0; s < nseq; s++) {
        const field<mat>& obs = seq_obs(s);
        int nstates = promp_hsmm.nstates_;
        int nobs = obs.n_elem;
        imat psi_duration(nstates, nobs, fill::zeros);
        imat psi_state(nstates, nobs, fill::zeros);
        mat delta(nstates, nobs, fill::zeros);
        cout << "Before pdf" << endl;
        cube log_pdf = promp_hsmm.computeEmissionsLogLikelihood(obs);
        cout << "After pdf" << endl;
        Viterbi(promp_hsmm.transition_, promp_hsmm.pi_, promp_hsmm.duration_,
                log_pdf, delta, psi_duration, psi_state,
                promp_hsmm.min_duration_, nobs);
        cout << "Delta last column" << endl;
        cout << delta.col(nobs - 1) << endl;
        ivec viterbiStates, viterbiDurations;
        viterbiPath(psi_duration, psi_state, delta, viterbiStates,
                viterbiDurations);
        cout << "Viterbi states and durations" << endl;
        imat states_and_durations = join_horiz(viterbiStates, viterbiDurations);
        cout << states_and_durations << endl;
        cout << "Python states list representation" << endl;
        cout << "[";
        for(int i = 0; i < viterbiStates.n_elem; i++)
            cout << viterbiStates[i] <<
                    ((i + 1 == viterbiStates.n_elem)? "]" : ",");
        cout << endl;
        cout << "Python duration list representation" << endl;
        cout << "[";
        for(int i = 0; i < viterbiDurations.n_elem; i++)
            cout << viterbiDurations[i] <<
                    ((i + 1 == viterbiDurations.n_elem)? "]" : ",");
        cout << endl;

        // Saving the matrix of joint states and durations.
        string viterbi_filename(filename);
        if (nseq > 1)
            viterbi_filename += string(".") + to_string(s);
        states_and_durations.save(viterbi_filename, raw_ascii);
    }
}

int main(int argc, char *argv[]) {
    po::options_description desc("Options");
    desc.add_options()
        ("help,h", "Produce help message")
        ("input,i", po::value<string>(), "Path to the input obs")
        ("output,o", po::value<string>(), "Path to the json output params")
        ("nstates", po::value<int>(), "Number of hidden states for the HSMM")
        ("mindur", po::value<int>(), "Minimum duration of a segment")
        ("ndur", po::value<int>(), "Number of different durations supported")
        ("commitid", po::value<string>(), "Git commit id of the experiment")
        ("labels", po::value<string>(), "Path to the provided labels")
        ("viterbi,v", po::value<string>(), "Path to the output viterbi file")
        ("nfiles", po::value<int>()->default_value(1),
                "Number of files (sequences) to process")
        ("debug", "Flag for activating debug mode in HSMM")
        ("nodur", "Flag to deactivate the learning of durations")
        ("notrans", "Flag to deactivate the learning of transitions")
        ("nopi", "Flag to deactivate the learning of initial pmf")
        ("durmomentmatching", "Flag to active the Gaussian moment matching"
                " for the duration learning")
        ("polybasisfun", po::value<int>()->default_value(1), "Order of the "
                "poly basis functions")
        ("noselftransitions", "Flag to deactive self transitions")
        ("initfraction", po::value<double>()->default_value(0.1), "Fraction "
                "of the least squares estimates for omega kept for init")
        ("wpriorvar", po::value<double>(), "Prior variance for Sigma_w")
        ("alphadurprior", po::value<int>()->default_value(1),
                "Alpha for Dirichlet prior for the duration")
        ("trainingiter", po::value<int>()->default_value(10), "Training "
                "iterations")
        ("rbfbasisfun", po::value<int>()->default_value(3), "Number of radial"
                " basis functions to use between 0 and 1. 0 and 1 are removed.")
        ("delta", po::value<double>(), "If this is given a value, the model "
                "switches to segment agnostic and the samples are generated "
                "according to the provided delta")
        ("leaveoneout", po::value<int>(), "Index of the sequence that will be"
                " left out for validation");
    vector<string> required_fields = {"input", "output", "nstates", "mindur",
            "ndur", "viterbi"};
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    if (vm.count("nodur") && vm.count("durmomentmatching")) {
        cout << "Only one choice for duration learning is allowed" << endl;
        return 0;
    }
    for(auto s: required_fields) {
        if (!vm.count(s)) {
            cerr << "Error: You must provide the argument: " << s << endl;
            return 1;
        }
    }
    string input_filename = vm["input"].as<string>();
    string output_filename = vm["output"].as<string>();
    field<field<mat>> seq_obs(1);
    field<Labels> seq_labels(1);
    int njoints;
    int nseq = vm["nfiles"].as<int>();
    seq_obs.set_size(nseq);
    seq_labels.set_size(nseq);
    for(int i = 0; i < seq_obs.n_elem; i++) {
        string name = input_filename;
        if (nseq != 1)
            name += string(".") + to_string(i);
        mat obs;
        obs.load(name, raw_ascii);
        njoints = obs.n_rows;
        int nobs = obs.n_cols;
        cout << "Time series shape: (" << njoints << ", " << nobs <<
            ")." << endl;
        seq_obs(i) = fromMatToField(obs);

        // Reading labels for different obs.
        if (!vm.count("labels"))
            continue;
        string labels_name = vm["labels"].as<string>();
        if (nseq != 1)
            labels_name += string(".") + to_string(i);
        mat labels_mat;
        labels_mat.load(labels_name);
        for(int j = 0; j < labels_mat.n_rows; j++)
            seq_labels(i).setLabel(labels_mat(j, 0), labels_mat(j, 1),
                    labels_mat(j, 2));
    }

    int min_duration = vm["mindur"].as<int>();
    int nstates = vm["nstates"].as<int>();
    int ndurations = vm["ndur"].as<int>();
    mat transition(nstates, nstates);
    transition.fill(1.0 / nstates );
    if (vm.count("noselftransitions")) {
        transition.fill(1.0 / (nstates - 1));
        transition.diag().zeros();
    }
    vec pi(nstates);
    pi.fill(1.0/nstates);
    mat durations(nstates, ndurations);
    durations.fill(1.0 / ndurations);

    // Setting a combination of polynomial and rbf basis functions.
    auto poly = make_shared<ScalarPolyBasis>(vm["polybasisfun"].as<int>());
    auto comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({poly}));
    int nrbf = vm["rbfbasisfun"].as<int>();
    if (nrbf > 0) {
        vec centers = linspace<vec>(0, 1.0, nrbf + 2);
        centers = centers.subvec(1, centers.n_elem - 2);
        auto rbf = shared_ptr<ScalarGaussBasis>(new ScalarGaussBasis(centers,
                    0.25));
        comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({rbf, poly}));
    }
    int n_basis_functions = comb->dim();

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w(n_basis_functions * njoints);
        mu_w.randn();
        mat Sigma_w = eye<mat>(n_basis_functions * njoints,
                    n_basis_functions * njoints);
        mat Sigma_y = 0.001 * eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP poly(comb, promp, njoints);
        promps.push_back(poly);
    }

    // Creating the ProMP emission.
    shared_ptr<ProMPsEmission> ptr_emission(new ProMPsEmission(promps));

    // Creating a prior for Sigma_w.
    if (vm.count("wpriorvar")) {
        mat Phi = vm["wpriorvar"].as<double>() * eye<mat>(
                n_basis_functions * njoints, n_basis_functions * njoints);
        NormalInverseWishart iw_prior(Phi, Phi.n_rows + 2);
        ptr_emission->set_Sigma_w_Prior(iw_prior);
    }
    if (vm.count("delta"))
        ptr_emission->setDelta(vm["delta"].as<double>());

    // Settings for the initialization algorithm.
    ptr_emission->setParamsForInitialization(vm["initfraction"].as<double>());

    HSMM promp_hsmm(std::static_pointer_cast<AbstractEmission>(ptr_emission),
            transition, pi, durations, min_duration);
    if (vm.count("nodur"))
        promp_hsmm.setDurationLearningChoice("nodur");
    if (vm.count("durmomentmatching"))
        promp_hsmm.setDurationLearningChoice("momentmatching");
    if (vm.count("alphadurprior")) {
        mat alphas = ones<mat>(nstates, ndurations) *
            vm["alphadurprior"].as<int>();
        promp_hsmm.setDurationDirichletPrior(alphas);
    }
    if (vm.count("notrans"))
        promp_hsmm.learning_transitions_ = false;
    if (vm.count("nopi"))
        promp_hsmm.learning_pi_ = false;
    if (vm.count("debug"))
        promp_hsmm.debug_ = true;

    // Initializing the model from data.
    promp_hsmm.init_params_from_data(seq_obs);

    // Saving the model in a json file.
    std::ofstream initial_params(output_filename);
    nlohmann::json initial_model = promp_hsmm.to_stream();
    if (vm.count("commitid"))
        initial_model["git_commit_id"] = vm["commitid"].as<string>();
    initial_params << std::setw(4) << initial_model << std::endl;
    initial_params.close();

    // Leave one out.
    field<field<mat>> t_seq;
    field<Labels> t_labels;
    if (vm.count("leaveoneout")) {
        int omitted = vm["leaveoneout"].as<int>();
        field<field<mat>> left_one_out(seq_obs.n_elem - 1);
        field<Labels> left_labels(seq_labels.n_elem - 1);
        int idx = 0;
        for(int i = 0; i < seq_obs.n_elem; i++)
            if (i != omitted) {
                left_one_out(idx) = seq_obs(i);
                left_labels(idx) = seq_labels(i);
                idx++;
            }
        t_seq = left_one_out;
        t_labels = left_labels;
        cout << "Leaving one out of the training: " << omitted << endl;
    }
    else {
        t_seq = seq_obs;
        t_labels = seq_labels;
    }

    for(int i = 0; i < vm["trainingiter"].as<int>(); i++) {

        // Reading the current parameters.
        std::ifstream current_params_stream(output_filename);
        nlohmann::json current_params;
        current_params_stream >> current_params;
        promp_hsmm.from_stream(current_params);

        bool convergence_reached = promp_hsmm.fit(t_seq, t_labels, 5, 1e-5);

        // Saving again the parameters after one training iteration.
        std::ofstream output_params(output_filename);
        current_params = promp_hsmm.to_stream();
        if (vm.count("commitid"))
            current_params["git_commit_id"] = vm["commitid"].as<string>();
        output_params << std::setw(4) << current_params << std::endl;
        output_params.close();

        ViterbiAlgorithm(promp_hsmm, seq_obs, vm["viterbi"].as<string>());

        if (convergence_reached)
            break;

    }
    cout << "loglikelihood: " << promp_hsmm.loglikelihood(t_seq) << endl;
    if (vm.count("leaveoneout")) {
        int omitted = vm["leaveoneout"].as<int>();
        field<field<mat>> test = {seq_obs(omitted)};
        field<Labels> test_labels = {seq_labels(omitted)};
        cout << "loglikelihoodtest: " << promp_hsmm.loglikelihood(test) << endl;
    }
    return 0;
}


// MIT License

// Copyright (c) 2019 Edward Liu

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "registrators/icp_fast.h"

#include <algorithm>
#include <cstdlib>
#include <ctime>
#include <iostream>
#include <limits>
#include <utility>
#include <vector>

#include <boost/typeof/typeof.hpp>

#include "common/macro_defines.h"
#include "common/math.h"
#include "common/performance/simple_prof.h"

namespace static_map {
namespace registrator {

constexpr int kNormalEstimationKnn = 7;
constexpr int kDim = 3;

using Matrix = Eigen::MatrixXd;
using Vector = Eigen::VectorXd;
using OutlierWeights = Matrix;
using data::EigenPointCloud;

struct Matches {
  //!< Squared distances to closest points, dense matrix
  using Dists = Matrix;
  //!< Identifiers of closest points, dense matrix of integers
  using Ids = Eigen::MatrixXi;

  Matches() = default;
  Matches(const int knn, const int points_count)
      : dists(Dists(knn, points_count)), ids(Ids(knn, points_count)) {}

  //!< squared distances to closest points
  Dists dists;
  //!< identifiers of closest points
  Ids ids;

  double GetDistsQuantile(const double quantile) const {
    // REGISTER_FUNC;
    // build array
    CHECK(quantile >= 0.0 && quantile <= 1.0);
    std::vector<double> values;
    values.reserve(dists.rows() * dists.cols());
    for (int x = 0; x < dists.cols(); ++x) {
      for (int y = 0; y < dists.rows(); ++y) {
        if (dists(y, x) != std::numeric_limits<double>::infinity()) {
          values.push_back(dists(y, x));
        }
      }
    }

    // BUG
    // Sometimes this really happens, which should not.
    CHECK(!values.empty());
    // get quantile
    if (quantile == 1.0) {
      return *std::max_element(values.begin(), values.end());
    }
    const int quantile_index = values.size() * quantile;
    std::nth_element(values.begin(), values.begin() + quantile_index,
                     values.end());
    return values[quantile_index];
  }
};

struct ErrorElements {
  EigenPointCloud reading;    //!< reading point cloud
  EigenPointCloud reference;  //!< reference point cloud
  OutlierWeights weights;     //!< weights for every association
  Matches matches;            //!< associations

  ErrorElements();
  ErrorElements(const EigenPointCloud& source_points,
                const EigenPointCloud& target_points,
                const OutlierWeights& weights, const Matches& matches) {
    REGISTER_FUNC;
    CHECK_GT(matches.ids.rows(), 0);
    CHECK_GT(matches.ids.cols(), 0);
    CHECK(matches.ids.cols() == source_points.points.cols());  // nbpts
    CHECK(weights.rows() == matches.ids.rows());               // knn
    CHECK_EQ(weights.rows(), 1);

    const int dim_points = source_points.points.rows();

    // Count points with no weights
    const int points_count = (weights.array() != 0.0).count();
    CHECK_GT(points_count, 0) << "no point to minimize";

    Matrix kept_points(dim_points, points_count);
    std::vector<double> kept_points_factor(points_count);
    Matches kept_matches(1, points_count);
    OutlierWeights kept_weights(1, points_count);

    int j = 0;
    for (int i = 0; i < source_points.points.cols(); ++i) {
      const auto match_dist = matches.dists(0, i);
      if (match_dist == NNS::InvalidValue) {
        continue;
      }

      if (weights(0, i) != 0.0) {
        kept_points.col(j) = source_points.points.col(i);
        kept_points_factor[j] = source_points.factors[i];
        kept_matches.ids(0, j) = matches.ids(0, i);
        kept_matches.dists(0, j) = match_dist;
        kept_weights(0, j) = weights(0, i);
        ++j;
      }
    }
    CHECK_EQ(j, points_count);
    CHECK_EQ(dim_points, target_points.points.rows());

    const int dim_target_normals = target_points.normals.rows();
    Matrix associated_target_normals;
    if (dim_target_normals > 0) {
      associated_target_normals = Matrix(dim_target_normals, points_count);
    }

    Matrix associated_points(dim_points, points_count);
    // Fetch matched points
    for (int i = 0; i < points_count; ++i) {
      const int ref_index(kept_matches.ids(i));
      associated_points.col(i) =
          target_points.points.block(0, ref_index, dim_points, 1);

      if (dim_target_normals > 0)
        associated_target_normals.col(i) =
            target_points.normals.block(0, ref_index, dim_target_normals, 1);
    }

    // Copy final data to structure
    reading.points = kept_points;
    reading.factors = std::move(kept_points_factor);
    reference.points = associated_points;
    reference.normals = associated_target_normals;

    this->weights = kept_weights;
    this->matches = kept_matches;
  }
};

Matches FindClosests(const std::shared_ptr<NNS>& nns_kdtree,
                     const EigenPointCloud& source_cloud) {
  REGISTER_FUNC;
  const int points_count(source_cloud.points.cols());
  const int search_count = 1;
  const double epsilon = 3.16;
  //! A dense integer matrix
  Matches matches(search_count, points_count);
  nns_kdtree->knn(source_cloud.points, matches.ids, matches.dists, search_count,
                  epsilon, NNS::ALLOW_SELF_MATCH);
  return matches;
}

Eigen::MatrixXd CrossProduct(const Eigen::MatrixXd& A,
                             const Eigen::MatrixXd& B) {
  // Expecting matched points
  CHECK_EQ(A.cols(), B.cols());
  // Expecting homogenous coord X eucl. coord
  CHECK_EQ(A.rows(), B.rows());

  constexpr unsigned int x = 0;
  constexpr unsigned int y = 1;
  constexpr unsigned int z = 2;

  Eigen::MatrixXd cross(B.rows(), B.cols());
  cross.row(x) =
      A.row(y).array() * B.row(z).array() - A.row(z).array() * B.row(y).array();
  cross.row(y) =
      A.row(z).array() * B.row(x).array() - A.row(x).array() * B.row(z).array();
  cross.row(z) =
      A.row(x).array() * B.row(y).array() - A.row(y).array() * B.row(x).array();

  return cross;
}

void SolvePossiblyUnderdeterminedLinearSystem(const Matrix& A, const Vector& b,
                                              Vector& x) {  // NOLINT
  CHECK_EQ(A.cols(), A.rows());
  CHECK_EQ(b.cols(), 1);
  CHECK_EQ(b.rows(), A.rows());
  CHECK_EQ(x.cols(), 1);
  CHECK_EQ(x.rows(), A.cols());

  // using Matrix = typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

  BOOST_AUTO(Aqr, A.fullPivHouseholderQr());
  if (!Aqr.isInvertible()) {
    // Solve reduced problem R1 x = Q1^T b instead of QR x = b, where Q = [Q1
    // Q2] and R = [ R1 ; R2 ] such that ||R2|| is small (or zero) and
    // therefore A = QR ~= Q1 * R1
    const int rank = Aqr.rank();
    const int rows = A.rows();
    const Matrix Q1t = Aqr.matrixQ().transpose().block(0, 0, rank, rows);
    const Matrix R1 = (Q1t * A * Aqr.colsPermutation()).block(0, 0, rank, rows);

    // The under-determined system R1 x = Q1^T b is made unique ..
    // by getting the solution of smallest norm (x = R1^T * (R1 * R1^T)^-1
    // Q1^T b.
    x = R1.triangularView<Eigen::Upper>().transpose() *
        (R1 * R1.transpose()).llt().solve(Q1t * b);
    x = Aqr.colsPermutation() * x;

    BOOST_AUTO(ax, (A * x).eval());
    if (!b.isApprox(ax, 1e-5)) {
      x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
      ax = A * x;

      if ((b - ax).norm() > 1e-5 * std::max(A.norm() * x.norm(), b.norm())) {
        // clang-format off
        LOG(WARNING)
            << "PointMatcher::icp - encountered numerically singular matrix "
               "while minimizing point to plane distance and the current "
               "workaround remained inaccurate."
            << " b=" << b.transpose()
            << " !~ A * x=" << (ax).transpose().eval()
            << ": ||b- ax||=" << (b - ax).norm()
            << ", ||b||=" << b.norm()
            << ", ||ax||=" << ax.norm();
        // clang-format on
      }
    }
  } else {
    // Cholesky decomposition
    x = A.llt().solve(b);
  }
}

Eigen::Matrix4d ComputePointToPlane(const EigenPointCloud& source_cloud,
                                    const EigenPointCloud& target_cloud,
                                    const Eigen::MatrixXd& weights,
                                    const Matches& matches,
                                    bool compensation = false) {
  REGISTER_FUNC;
  const Eigen::MatrixXd target_normals = target_cloud.normals;
  CHECK_EQ(target_normals.rows(), kDim);
  CHECK_EQ(target_normals.cols(), target_cloud.points.cols());
  CHECK_EQ(source_cloud.points.cols(), target_cloud.points.cols());

  // Compute cross product of cross = cross(reading X target_normals)
  const Matrix cross = CrossProduct(source_cloud.points, target_normals);

  // wF = [weights*cross, weights*normals]
  // F  = [cross, normals]
  Matrix wF(target_normals.rows() + cross.rows(), target_normals.cols());
  Matrix F(target_normals.rows() + cross.rows(), target_normals.cols());

  for (int i = 0; i < cross.rows(); i++) {
    wF.row(i) = weights.array() * cross.row(i).array();
    F.row(i) = cross.row(i);
  }
  for (int i = 0; i < target_normals.rows(); i++) {
    wF.row(i + cross.rows()) = weights.array() * target_normals.row(i).array();
    F.row(i + cross.rows()) = target_normals.row(i);
  }

  if (compensation) {
    for (int i = 0; i < target_normals.cols(); ++i) {
      wF.col(i) *= source_cloud.factors[i];
      F.col(i) *= source_cloud.factors[i];
    }
  }

  // Unadjust covariance A = wF * F'
  const Matrix A = wF * F.transpose();
  const Matrix deltas = source_cloud.points - target_cloud.points;

  // dot product of dot = dot(deltas, normals)
  Matrix dotProd = Matrix::Zero(1, target_normals.cols());
  for (int i = 0; i < target_normals.rows(); i++) {
    dotProd += (deltas.row(i).array() * target_normals.row(i).array()).matrix();
  }

  // b = -(wF' * dot)
  const Vector b = -(wF * dotProd.transpose());
  Vector x(A.rows());
  SolvePossiblyUnderdeterminedLinearSystem(A, b, x);

  // Transform parameters to matrix
  Eigen::Matrix4d result;
  Eigen::Transform<double, 3, Eigen::Affine> transform;
  transform =
      Eigen::AngleAxis<double>(x.head(3).norm(), x.head(3).normalized());

  transform.translation() = x.segment(3, 3);
  result = transform.matrix();

  if (result.hasNaN()) {
    // Degenerate situation. This can happen when the source and reading
    // clouds are identical, and then b and x above are 0, and the rotation
    // matrix cannot be determined, it comes out full of NaNs. The correct
    // rotation is the identity.
    result.block(0, 0, kDim, kDim) = Matrix::Identity(kDim, kDim);
  }

  return result;
}

Eigen::Matrix4d ComputePointToPoint(EigenPointCloud& source_cloud,  // NOLINT
                                    EigenPointCloud& target_cloud,  // NOLINT
                                    const Eigen::MatrixXd& weights,
                                    const Matches& matches) {
  CHECK_EQ(source_cloud.points.cols(), target_cloud.points.cols());

  // const int dimCount(mPts.reading.features.rows());
  // const int ptsCount(mPts.reading.features.cols()); //Both point clouds
  // have now the same number of (matched) point

  const Vector w = weights.row(0);
  const double w_sum_inv = 1. / w.sum();
  const Vector meanReading =
      (source_cloud.points.array().rowwise() * w.array().transpose())
          .rowwise()
          .sum() *
      w_sum_inv;
  const Vector meanReference =
      (target_cloud.points.array().rowwise() * w.array().transpose())
          .rowwise()
          .sum() *
      w_sum_inv;

  // Remove the mean from the point clouds
  source_cloud.points.colwise() -= meanReading;
  target_cloud.points.colwise() -= meanReference;

  // Singular Value Decomposition
  const Matrix m(target_cloud.points * w.asDiagonal() *
                 source_cloud.points.transpose());
  const Eigen::JacobiSVD<Matrix> svd(m,
                                     Eigen::ComputeThinU | Eigen::ComputeThinV);
  Matrix rotMatrix(svd.matrixU() * svd.matrixV().transpose());
  // It is possible to get a reflection instead of a rotation. In this case,
  // we take the second best solution, guaranteed to be a rotation. For more
  // details, read the tech report: "Least-Squares Rigid Motion Using SVD",
  // Olga Sorkine http://igl.ethz.ch/projects/ARAP/svd_rot.pdf
  if (rotMatrix.determinant() < 0.) {
    Matrix tmpV = svd.matrixV().transpose();
    tmpV.row(kDim - 1) *= -1.;
    rotMatrix = svd.matrixU() * tmpV;
  }
  const Vector trVector(meanReference - rotMatrix * meanReading);

  Matrix result(Matrix::Identity(4, 4));
  result.topLeftCorner(kDim, kDim) = rotMatrix;
  result.topRightCorner(kDim, 1) = trVector;

  return result;
}

bool CheckConvergence(const std::vector<Eigen::Quaterniond>& rotations,
                      const std::vector<Eigen::Vector3d>& translations) {
  constexpr int kSmoothLength = 4;
  constexpr double kConvergeRotDist = 0.001;
  constexpr double kConvergeTransDist = 0.01;

  double rotation_dist = 0.;
  double translation_dist = 0.;
  bool has_converged = false;
  if (rotations.size() > kSmoothLength) {
    for (size_t i = rotations.size() - 1; i >= rotations.size() - kSmoothLength;
         i--) {
      // Compute the mean derivative
      rotation_dist +=
          std::fabs(rotations[i].angularDistance(rotations[i - 1]));
      translation_dist +=
          std::fabs((translations[i] - translations[i - 1]).norm());
    }

    rotation_dist /= kSmoothLength;
    translation_dist /= kSmoothLength;

    if (rotation_dist < kConvergeRotDist &&
        translation_dist < kConvergeTransDist)
      has_converged = true;
  }

  return has_converged;
}

IcpFast::IcpFast() : Interface() {
  this->type_ = kFastIcp;
  // TODO(edward) This `knn_normal_estimate` is currently of no use, make it
  // useful later.
  REG_REGISTRATOR_INNER_OPTION("knn_normal_estimate",
                               OptionItemDataType::kInt32,
                               options_.knn_for_normal_estimate);
  REG_REGISTRATOR_INNER_OPTION("max_iteration", OptionItemDataType::kInt32,
                               options_.max_iteration);
  REG_REGISTRATOR_INNER_OPTION("dist_outlier_ratio",
                               OptionItemDataType::kFloat32,
                               options_.dist_outlier_ratio);
}

void IcpFast::SetInputSource(InnerCloudPtr cloud) {
  CHECK(cloud);
  CHECK(cloud->GetEigenCloud());
  source_cloud_.reset(new EigenPointCloud(*cloud->GetEigenCloud()));
}

void IcpFast::SetInputTarget(InnerCloudPtr cloud) {
  CHECK(cloud);
  CHECK(cloud->GetEigenCloud());
  CHECK(cloud->GetEigenCloud()->HasNormals());
  target_cloud_.reset(new EigenPointCloud(*cloud->GetEigenCloud()));

  // for debug
  //
  // pcl::PointCloud<pcl::PointXYZINormal> normal_cloud;
  // const int cloud_size = target_cloud_->points.cols();
  // normal_cloud.reserve(cloud_size);
  // for (int i = 0; i < cloud_size; ++i) {
  //   pcl::PointXYZINormal point;
  //   point.x = target_cloud_->points(0, i);
  //   point.y = target_cloud_->points(1, i);
  //   point.z = target_cloud_->points(2, i);
  //   point.normal_x = target_cloud_->normals(0, i);
  //   point.normal_y = target_cloud_->normals(1, i);
  //   point.normal_z = target_cloud_->normals(2, i);
  //   normal_cloud.push_back(point);
  // }
  // pcl::io::savePCDFileBinary("/tmp/normal.pcd", normal_cloud);

  // step2 build KDTREE for closeset point searching
  // nns_kdtree_.reset(
  //     NNS::create(target_cloud_->points, kDim, NNS::KDTREE_LINEAR_HEAP));
}

bool IcpFast::Align(const Eigen::Matrix4d& guess,
                    Eigen::Matrix4d& result) {  // NOLINT
  const int target_points_count = target_cloud_->points.cols();
  const Eigen::Vector3d target_mean =
      target_cloud_->points.rowwise().sum() / target_points_count;
  Eigen::Matrix4d T_target_mean = Eigen::Matrix4d::Identity();
  T_target_mean.block(0, 3, 3, 1) = target_mean;

  target_cloud_->points.colwise() -= target_mean;
  {
    // REGISTER_BLOCK("BuildKdTree");
    nns_kdtree_.reset(
        NNS::create(target_cloud_->points, kDim, NNS::KDTREE_LINEAR_HEAP));
  }
  EigenPointCloud init_source_cloud = *source_cloud_;
  Eigen::Matrix4d T_target_mean_init_guess = T_target_mean.inverse() * guess;
  init_source_cloud.ApplyTransform(T_target_mean_init_guess);

  // initialise all iter status
  Eigen::Matrix4d T_iter = Eigen::Matrix4d::Identity();
  std::vector<Eigen::Quaterniond> rotations_iter;
  std::vector<Eigen::Vector3d> translations_iter;
  rotations_iter.reserve(10);
  translations_iter.reserve(10);
  rotations_iter.push_back(Eigen::Quaterniond::Identity());
  translations_iter.push_back(Eigen::Vector3d::Zero());

  int iterator = 0;
  while (true) {
    REGISTER_BLOCK("Iteration");
    // step1 Find Closest points
    EigenPointCloud step_cloud(init_source_cloud);
    if (this->inner_compensation_) {
      step_cloud.ApplyMotionCompensation(T_iter);
    } else {
      step_cloud.ApplyTransform(T_iter);
    }

    const Matches matches(FindClosests(nns_kdtree_, step_cloud));

    // step2 reject outliers (outliers with weight 0)
    const double limit = matches.GetDistsQuantile(options_.dist_outlier_ratio);
    const Matrix output_weights =
        (matches.dists.array() <= limit).cast<double>();

    // step3 solve point-to-plane and update result
    CHECK_EQ(output_weights.cols(), step_cloud.points.cols());

    ErrorElements error_elements(step_cloud, *target_cloud_, output_weights,
                                 matches);

    T_iter =
        ComputePointToPlane(error_elements.reading, error_elements.reference,
                            error_elements.weights, error_elements.matches,
                            this->inner_compensation_) *
        T_iter;

    // step4 check convergence and jump out
    iterator++;
    rotations_iter.emplace_back(Eigen::Matrix3d(T_iter.block(0, 0, 3, 3)));
    translations_iter.push_back(T_iter.block(0, 3, 3, 1));
    if (CheckConvergence(rotations_iter, translations_iter) ||
        iterator >= options_.max_iteration) {
      const double average_dist =
          error_elements.matches.dists.array().cwiseSqrt().sum() /
          error_elements.matches.dists.cols();
      this->final_score_ = std::exp(-average_dist);
      break;
    }
  }

  target_cloud_->points.colwise() += target_mean;
  result = T_target_mean * T_iter * T_target_mean_init_guess;
  return true;
}

}  // namespace registrator
}  // namespace static_map