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

/*    $Id: step-23.cc 27657 2012-11-21 13:19:08Z bangerth $       */
/*    Version: $Name:  $                                          */
/*                                                                */
/*    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}

// We start with the usual assortment of include files that we've seen in so
// many of the previous tests:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.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/data_out.h>

#include <fstream>
#include <iostream>

// Here are the only three include files of some new interest: The first one
// is already used, for example, for the
// VectorTools::interpolate_boundary_values and
// VectorTools::apply_boundary_values functions. However, we here use another
// function in that class, VectorTools::project to compute our initial values
// as the $L^2$ projection of the continuous initial values. Furthermore, we
// use VectorTools::create_right_hand_side to generate the integrals
// $(f^n,\phi^n_i)$. These were previously always generated by hand in
// assemble_system or similar functions in application
// code. However, we're too lazy to do that here, so simply use a library
// function:
#include <deal.II/numerics/vector_tools.h>

// In a very similar vein, we are also too lazy to write the code to assemble
// mass and Laplace matrices, although it would have only taken copying the
// relevant code from any number of previous tutorial programs. Rather, we
// want to focus on the things that are truly new to this program and
// therefore use the MatrixTools::create_mass_matrix and
// MatrixTools::create_laplace_matrix functions. They are declared here:
#include <deal.II/numerics/matrix_tools.h>

// Finally, here is an include file that contains all sorts of tool functions
// that one sometimes needs. In particular, we need the
// Utilities::int_to_string class that, given an integer argument, returns a
// string representation of it. It is particularly useful since it allows for
// a second parameter indicating the number of digits to which we want the
// result padded with leading zeros. We will use this to write output files
// that have the form solution-XXX.gnuplot where XXX
// denotes the number of the time step and always consists of three digits
// even if we are still in the single or double digit time steps.
#include <deal.II/base/utilities.h>

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

  // @sect3{The WaveEquation class}

  // Next comes the declaration of the main class. It's public interface of
  // functions is like in most of the other tutorial programs. Worth
  // mentioning is that we now have to store four matrices instead of one: the
  // mass matrix $M$, the Laplace matrix $A$, the matrix $M+k^2\theta^2A$ used
  // for solving for $U^n$, and a copy of the mass matrix with boundary
  // conditions applied used for solving for $V^n$. Note that it is a bit
  // wasteful to have an additional copy of the mass matrix around. We will
  // discuss strategies for how to avoid this in the section on possible
  // improvements.
  //
  // Likewise, we need solution vectors for $U^n,V^n$ as well as for the
  // corresponding vectors at the previous time step, $U^{n-1},V^{n-1}$. The
  // system_rhs will be used for whatever right hand side vector
  // we have when solving one of the two linear systems in each time
  // step. These will be solved in the two functions solve_u and
  // solve_v.
  //
  // Finally, the variable theta is used to indicate the
  // parameter $\theta$ that is used to define which time stepping scheme to
  // use, as explained in the introduction. The rest is self-explanatory.
  template <int dim>
  class WaveEquation
  {
  public:
    WaveEquation ();
    void run ();

  private:
    void setup_system ();
    void solve_u ();
    void solve_v ();
    void output_results () const;

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

    ConstraintMatrix constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;
    SparseMatrix<double> matrix_u;
    SparseMatrix<double> matrix_v;

    Vector<double>       solution_u, solution_v;
    Vector<double>       old_solution_u, old_solution_v;
    Vector<double>       system_rhs;

    double time, time_step;
    unsigned int timestep_number;
    const double theta;
  };

  // @sect3{Equation data}

  // Before we go on filling in the details of the main class, let us define
  // the equation data corresponding to the problem, i.e. initial and boundary
  // values for both the solution $u$ and its time derivative $v$, as well as
  // a right hand side class. We do so using classes derived from the Function
  // class template that has been used many times before, so the following
  // should not be a surprise.
  //
  // Let's start with initial values and choose zero for both the value $u$ as
  // well as its time derivative, the velocity $v$:
  template <int dim>
  class InitialValuesU : public Function<dim>
  {
  public:
    InitialValuesU () : Function<dim>() {}

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

  template <int dim>
  class InitialValuesV : public Function<dim>
  {
  public:
    InitialValuesV () : Function<dim>() {}

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

  template <int dim>
  double InitialValuesU<dim>::value (const Point<dim>  & /*p*/,
                                     const unsigned int component) const
  {
    Assert (component == 0, ExcInternalError());
    return 0;
  }

  template <int dim>
  double InitialValuesV<dim>::value (const Point<dim>  & /*p*/,
                                     const unsigned int component) const
  {
    Assert (component == 0, ExcInternalError());
    return 0;
  }

  // Secondly, we have the right hand side forcing term. Boring as we are, we
  // choose zero here as well:
  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide () : Function<dim>() {}

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

  template <int dim>
  double RightHandSide<dim>::value (const Point<dim>  & /*p*/,
                                    const unsigned int component) const
  {
    Assert (component == 0, ExcInternalError());
    return 0;
  }

  // Finally, we have boundary values for $u$ and $v$. They are as described
  // in the introduction, one being the time derivative of the other:
  template <int dim>
  class BoundaryValuesU : public Function<dim>
  {
  public:
    BoundaryValuesU () : Function<dim>() {}

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

  template <int dim>
  class BoundaryValuesV : public Function<dim>
  {
  public:
    BoundaryValuesV () : Function<dim>() {}

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

  template <int dim>
  double BoundaryValuesU<dim>::value (const Point<dim> &p,
                                      const unsigned int component) const
  {
    Assert (component == 0, ExcInternalError());

    if ((this->get_time() <= 0.5) &&
        (p[0] < 0) &&
        (p[1] < 1./3) &&
        (p[1] > -1./3))
      return std::sin (this->get_time() * 4 * numbers::PI);
    else
      return 0;
  }

  template <int dim>
  double BoundaryValuesV<dim>::value (const Point<dim> &p,
                                      const unsigned int component) const
  {
    Assert (component == 0, ExcInternalError());

    if ((this->get_time() <= 0.5) &&
        (p[0] < 0) &&
        (p[1] < 1./3) &&
        (p[1] > -1./3))
      return (std::cos (this->get_time() * 4 * numbers::PI) *
              4 * numbers::PI);
    else
      return 0;
  }

  // @sect3{Implementation of the WaveEquation class}

  // The implementation of the actual logic is actually fairly short, since we
  // relegate things like assembling the matrices and right hand side vectors
  // to the library. The rest boils down to not much more than 130 lines of
  // actual code, a significant fraction of which is boilerplate code that can
  // be taken from previous example programs (e.g. the functions that solve
  // linear systems, or that generate output).
  //
  // Let's start with the constructor (for an explanation of the choice of
  // time step, see the section on Courant, Friedrichs, and Lewy in the
  // introduction):
  template <int dim>
  WaveEquation<dim>::WaveEquation () :
    fe (1),
    dof_handler (triangulation),
    time_step (1./64),
    theta (0.5)
  {}

  // @sect4{WaveEquation::setup_system}

  // The next function is the one that sets up the mesh, DoFHandler, and
  // matrices and vectors at the beginning of the program, i.e. before the
  // first time step. The first few lines are pretty much standard if you've
  // read through the tutorial programs at least up to step-6:
  template <int dim>
  void WaveEquation<dim>::setup_system ()
  {
    GridGenerator::hyper_cube (triangulation, -1, 1);
    triangulation.refine_global (7);

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

    dof_handler.distribute_dofs (fe);

    std::cout << "Number of degrees of freedom: "
              << dof_handler.n_dofs()
              << std::endl
              << 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();

    // Then comes a block where we have to initialize the 3 matrices we need
    // in the course of the program: the mass matrix, the laplace matrix, and
    // the matrix $M+k^2\theta^2A$ used when solving for $U^n$ in each time
    // step.
    //
    // When setting up these matrices, note that they all make use of the same
    // sparsity pattern object. Finally, the reason why matrices and sparsity
    // patterns are separate objects in deal.II (unlike in many other finite
    // element or linear algebra classes) becomes clear: in a significant
    // fraction of applications, one has to hold several matrices that happen
    // to have the same sparsity pattern, and there is no reason for them not
    // to share this information, rather than re-building and wasting memory
    // on it several times.
    //
    // After initializing all of these matrices, we call library functions
    // that build the Laplace and mass matrices. All they need is a DoFHandler
    // object and a quadrature formula object that is to be used for numerical
    // integration. Note that in many respects these functions are better than
    // what we would usually do in application programs, for example because
    // they automatically parallelize building the matrices if multiple
    // processors are available in a machine. The matrices for solving linear
    // systems will be filled in the run() method because we need to re-apply
    // boundary conditions every time step.
    mass_matrix.reinit (sparsity_pattern);
    laplace_matrix.reinit (sparsity_pattern);
    matrix_u.reinit (sparsity_pattern);
    matrix_v.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);

    // The rest of the function is spent on setting vector sizes to the
    // correct value. The final line closes the hanging node constraints
    // object. Since we work on a uniformly refined mesh, no constraints exist
    // or have been computed (i.e. there was no need to call
    // DoFTools::make_hanging_node_constraints as in other programs), but we
    // need a constraints object in one place further down below anyway.
    solution_u.reinit (dof_handler.n_dofs());
    solution_v.reinit (dof_handler.n_dofs());
    old_solution_u.reinit (dof_handler.n_dofs());
    old_solution_v.reinit (dof_handler.n_dofs());
    system_rhs.reinit (dof_handler.n_dofs());

    constraints.close ();
  }

  // @sect4{WaveEquation::solve_u and WaveEquation::solve_v}

  // The next two functions deal with solving the linear systems associated
  // with the equations for $U^n$ and $V^n$. Both are not particularly
  // interesting as they pretty much follow the scheme used in all the
  // previous tutorial programs.
  //
  // One can make little experiments with preconditioners for the two matrices
  // we have to invert. As it turns out, however, for the matrices at hand
  // here, using Jacobi or SSOR preconditioners reduces the number of
  // iterations necessary to solve the linear system slightly, but due to the
  // cost of applying the preconditioner it is no win in terms of run-time. It
  // is not much of a loss either, but let's keep it simple and just do
  // without:
  template <int dim>
  void WaveEquation<dim>::solve_u ()
  {
    SolverControl           solver_control (1000, 1e-8*system_rhs.l2_norm());
    SolverCG<>              cg (solver_control);

    cg.solve (matrix_u, solution_u, system_rhs,
              PreconditionIdentity());

    std::cout << "   u-equation: " << solver_control.last_step()
              << " CG iterations."
              << std::endl;
  }

  template <int dim>
  void WaveEquation<dim>::solve_v ()
  {
    SolverControl           solver_control (1000, 1e-8*system_rhs.l2_norm());
    SolverCG<>              cg (solver_control);

    cg.solve (matrix_v, solution_v, system_rhs,
              PreconditionIdentity());

    std::cout << "   v-equation: " << solver_control.last_step()
              << " CG iterations."
              << std::endl;
  }

  // @sect4{WaveEquation::output_results}

  // Likewise, the following function is pretty much what we've done
  // before. The only thing worth mentioning is how here we generate a string
  // representation of the time step number padded with leading zeros to 3
  // character length using the Utilities::int_to_string function's second
  // argument.
  template <int dim>
  void WaveEquation<dim>::output_results () const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution_u, "U");
    data_out.add_data_vector (solution_v, "V");

    data_out.build_patches ();

    const std::string filename = "solution-" +
                                 Utilities::int_to_string (timestep_number, 3) +
                                 ".gnuplot";
    std::ofstream output (filename.c_str());
    data_out.write_gnuplot (output);
  }

  // @sect4{WaveEquation::run}

  // The following is really the only interesting function of the program. It
  // contains the loop over all time steps, but before we get to that we have
  // to set up the grid, DoFHandler, and matrices. In addition, we have to
  // somehow get started with initial values. To this end, we use the
  // VectorTools::project function that takes an object that describes a
  // continuous function and computes the $L^2$ projection of this function
  // onto the finite element space described by the DoFHandler object. Can't
  // be any simpler than that:
  template <int dim>
  void WaveEquation<dim>::run ()
  {
    setup_system();

    VectorTools::project (dof_handler, constraints, QGauss<dim>(3),
                          InitialValuesU<dim>(),
                          old_solution_u);
    VectorTools::project (dof_handler, constraints, QGauss<dim>(3),
                          InitialValuesV<dim>(),
                          old_solution_v);

    // The next thing is to loop over all the time steps until we reach the
    // end time ($T=5$ in this case). In each time step, we first have to
    // solve for $U^n$, using the equation $(M^n + k^2\theta^2 A^n)U^n =$
    // $(M^{n,n-1} - k^2\theta(1-\theta) A^{n,n-1})U^{n-1} + kM^{n,n-1}V^{n-1}
    // +$ $k\theta \left[k \theta F^n + k(1-\theta) F^{n-1} \right]$. Note
    // that we use the same mesh for all time steps, so that $M^n=M^{n,n-1}=M$
    // and $A^n=A^{n,n-1}=A$. What we therefore have to do first is to add up
    // $MU^{n-1} - k^2\theta(1-\theta) AU^{n-1} + kMV^{n-1}$ and the forcing
    // terms, and put the result into the system_rhs vector. (For
    // these additions, we need a temporary vector that we declare before the
    // loop to avoid repeated memory allocations in each time step.)
    //
    // The one thing to realize here is how we communicate the time variable
    // to the object describing the right hand side: each object derived from
    // the Function class has a time field that can be set using the
    // Function::set_time and read by Function::get_time. In essence, using
    // this mechanism, all functions of space and time are therefore
    // considered functions of space evaluated at a particular time. This
    // matches well what we typically need in finite element programs, where
    // we almost always work on a single time step at a time, and where it
    // never happens that, for example, one would like to evaluate a
    // space-time function for all times at any given spatial location.
    Vector<double> tmp (solution_u.size());
    Vector<double> forcing_terms (solution_u.size());

    for (timestep_number=1, time=time_step;
         time<=5;
         time+=time_step, ++timestep_number)
      {
        std::cout << "Time step " << timestep_number
                  << " at t=" << time
                  << std::endl;

        mass_matrix.vmult (system_rhs, old_solution_u);

        mass_matrix.vmult (tmp, old_solution_v);
        system_rhs.add (time_step, tmp);

        laplace_matrix.vmult (tmp, old_solution_u);
        system_rhs.add (-theta * (1-theta) * time_step * time_step, tmp);

        RightHandSide<dim> rhs_function;
        rhs_function.set_time (time);
        VectorTools::create_right_hand_side (dof_handler, QGauss<dim>(2),
                                             rhs_function, tmp);
        forcing_terms = tmp;
        forcing_terms *= theta * time_step;

        rhs_function.set_time (time-time_step);
        VectorTools::create_right_hand_side (dof_handler, QGauss<dim>(2),
                                             rhs_function, tmp);

        forcing_terms.add ((1-theta) * time_step, tmp);

        system_rhs.add (theta * time_step, forcing_terms);

        // After so constructing the right hand side vector of the first
        // equation, all we have to do is apply the correct boundary
        // values. As for the right hand side, this is a space-time function
        // evaluated at a particular time, which we interpolate at boundary
        // nodes and then use the result to apply boundary values as we
        // usually do. The result is then handed off to the solve_u()
        // function:
        {
          BoundaryValuesU<dim> boundary_values_u_function;
          boundary_values_u_function.set_time (time);

          std::map<unsigned int,double> boundary_values;
          VectorTools::interpolate_boundary_values (dof_handler,
                                                    0,
                                                    boundary_values_u_function,
                                                    boundary_values);

          // The matrix for solve_u() is the same in every time steps, so one
          // could think that it is enough to do this only once at the
          // beginning of the simulation. However, since we need to apply
          // boundary values to the linear system (which eliminate some matrix
          // rows and columns and give contributions to the right hand side),
          // we have to refill the matrix in every time steps before we
          // actually apply boundary data. The actual content is very simple:
          // it is the sum of the mass matrix and a weighted Laplace matrix:
          matrix_u.copy_from (mass_matrix);
          matrix_u.add (theta * theta * time_step * time_step, laplace_matrix);
          MatrixTools::apply_boundary_values (boundary_values,
                                              matrix_u,
                                              solution_u,
                                              system_rhs);
        }
        solve_u ();

        // The second step, i.e. solving for $V^n$, works similarly, except
        // that this time the matrix on the left is the mass matrix (which we
        // copy again in order to be able to apply boundary conditions, and
        // the right hand side is $MV^{n-1} - k\left[ \theta A U^n +
        // (1-\theta) AU^{n-1}\right]$ plus forcing terms. %Boundary values
        // are applied in the same way as before, except that now we have to
        // use the BoundaryValuesV class:
        laplace_matrix.vmult (system_rhs, solution_u);
        system_rhs *= -theta * time_step;

        mass_matrix.vmult (tmp, old_solution_v);
        system_rhs += tmp;

        laplace_matrix.vmult (tmp, old_solution_u);
        system_rhs.add (-time_step * (1-theta), tmp);

        system_rhs += forcing_terms;

        {
          BoundaryValuesV<dim> boundary_values_v_function;
          boundary_values_v_function.set_time (time);

          std::map<unsigned int,double> boundary_values;
          VectorTools::interpolate_boundary_values (dof_handler,
                                                    0,
                                                    boundary_values_v_function,
                                                    boundary_values);
          matrix_v.copy_from (mass_matrix);
          MatrixTools::apply_boundary_values (boundary_values,
                                              matrix_v,
                                              solution_v,
                                              system_rhs);
        }
        solve_v ();

        // Finally, after both solution components have been computed, we
        // output the result, compute the energy in the solution, and go on to
        // the next time step after shifting the present solution into the
        // vectors that hold the solution at the previous time step. Note the
        // function SparseMatrix::matrix_norm_square that can compute
        // $\left<V^n,MV^n\right>$ and $\left$ in one step,
        // saving us the expense of a temporary vector and several lines of
        // code:
        output_results ();

        std::cout << "   Total energy: "
                  << (mass_matrix.matrix_norm_square (solution_v) +
                      laplace_matrix.matrix_norm_square (solution_u)) / 2
                  << std::endl;

        old_solution_u = solution_u;
        old_solution_v = solution_v;
      }
  }
}

// @sect3{The main function}

// What remains is the main function of the program. There is nothing here
// that hasn't been shown in several of the previous programs:
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step23;

      deallog.depth_console (0);

      WaveEquation<2> wave_equation_solver;
      wave_equation_solver.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;
}


/* Copyright 2022 Zuru Tech HK Limited.
 *
 * 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.
 */

#ifndef SOLVERS_SPARSESYSTEM_HPP
#define SOLVERS_SPARSESYSTEM_HPP

#include <filesystem>
#include <tuple>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <petscmat.h>
#include <petscvec.h>

namespace solvers {

/**
 * \brief SparseSystem Represent a solved sparse symmetric linear system.
 */
class SparseSystem {
private:
    /**
     * \brief _upper_A Triangular upper part of the sparse symmetric matrix of the system.
     */
    Eigen::SparseMatrix<double, Eigen::RowMajor> _upper_A;

    /**
     * \brief _b Constant term of the system.
     */
    Eigen::VectorXd _b;

    /**
     * \brief _x Solution of the system.
     */
    Eigen::VectorXd _x;

public:
    SparseSystem(const Eigen::SparseMatrix<double, Eigen::RowMajor>& upper_A,
                 Eigen::VectorXd b,
                 Eigen::VectorXd x);

    /**
     * \brief loadNpz Load a sparse system from a .npz file in StAnD format.
     *
     * \param[in] npz_path Path to the input .npz file.
     * \return Solved sparse symmetric linear system.
     */
    [[nodiscard]] static SparseSystem loadNpz(
        const std::filesystem::path& npz_path);

    /**
     * \brief saveNps Save a sparse system in a .npz file in StAnD format.
     *
     * \param[in] npz_path Path to the output .npz file.
     */
    void saveNpz(const std::filesystem::path& npz_path) const;

    /**
     * \brief removeNullEntries Remove null entries from the sparse matrix.
     *
     * \param[in] tolerance Maximum absolute value for a matrix coefficient to be considered null.
     */
    void removeNullEntries(double tolerance = 0.);

    /**
     * \brief dim Get the number of rows and columns of the sparse matrix.
     *
     * \return Number of rows and columns of the sparse matrix.
     */
    [[nodiscard]] size_t dim() const;

    /**
     * \brief nnz Get the number of non-null coefficients of the sparse matrix.
     *
     * \return Number of non-null coefficients of the sparse matrix.
     */
    [[nodiscard]] size_t nnz() const;

    /**
     * \brief getEigenSolution Get the solution vector in Eigen format.
     *
     * \return Solution vector.
     */
    [[nodiscard]] Eigen::VectorXd getEigenSolution() const;

    /**
     * \brief getPetscSolution Get the solution vector in PETSc format.
     *
     * \return Solution vector.
     */
    [[nodiscard]] Vec getPetscSolution() const;

    /**
     * \brief toEigenCSC Get the coefficient matrix and the constant term in Eigen format.
     *        The sparse coefficient matrix is stored in CSC format.
     *
     * \return Pair of coefficient matrix and constant term in Eigen format.
     */
    [[nodiscard]] std::tuple<Eigen::SparseMatrix<double, Eigen::ColMajor>,
                             Eigen::VectorXd>
    toEigenCSC() const;

    /**
     * \brief toEigenCSR Get the coefficient matrix and the constant term in Eigen format.
     *        The sparse coefficient matrix is stored in CSR format.
     *
     * \return Pair of coefficient matrix and constant term in Eigen format.
     */
    [[nodiscard]] std::tuple<Eigen::SparseMatrix<double, Eigen::RowMajor>,
                             Eigen::VectorXd>
    toEigenCSR() const;

    /**
     * \brief toEigenUpperCSR Get the upper part of coefficient matrix and
     *        the constant term in Eigen format.
     *        The upper part of the sparse coefficient matrix is stored in CSR format.
     *
     * \return Pair of upper part of coefficient matrix and constant term in Eigen format.
     */
    [[nodiscard]] std::tuple<Eigen::SparseMatrix<double, Eigen::RowMajor>,
                             Eigen::VectorXd>
    toEigenUpperCSR() const;

    /**
     * \brief toPetscCSR Get the coefficient matrix and the constant term in PETSc format.
     *        The sparse coefficient matrix is stored in CSR format.
     *
     * \return Pair of coefficient matrix and constant term in Eigen format.
     */
    [[nodiscard]] std::tuple<Mat, Vec> toPetscCSR() const;

    /**
     * \brief toStdCSR Get the coefficient matrix and the constant term in std::vector format.
     *        The sparse coefficient matrix is stored in CSR format and it is
     *        represented by three vectors: compressed row indices, column indices and values.
     *
     * \return Tuple of vectors representing:
     *         - compressed row indices of the coefficient matrix;
     *         - column indices of the coefficient matrix;
     *         - values of the coefficients;
     *         - constant term.
     */
    [[nodiscard]] std::tuple<std::vector<int>,
                             std::vector<int>,
                             std::vector<double>,
                             std::vector<double>>
    toStdCSR() const;
};

}    // namespace solvers

#endif    // SOLVERS_SPARSESYSTEM_HPP


#include <iostream>
#include <string>
#include <armadillo>

using namespace std;
using namespace arma;

mat loadData( string arxeio)
{
  int DIM = 1;
  ifstream myFile;

  // choose data
  try
  {
    myFile.open(arxeio.c_str());
  }
  catch(const std::exception& e)
  {
    cout<< "Failed to open file data "<<arxeio<< endl;
  }

  //get size
  double num = 0.0;
  int lenght =0;
  while (myFile >> num)
  {
    lenght++;
  }
  myFile.close();

  // fix size
  mat m(DIM,lenght);

  // open again to get data
  myFile.open(arxeio.c_str());
  num = 0.0;
  int l = 0;

  // for (int d = 0 ; d++; d < DIM)
  // {
    while (myFile >> num)
    {
      m(0,l) = num;
      l++;
    }
  // }

  myFile.close();
  return m;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/firstbitset.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>

STF_CASE_TPL (" firstbitset real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::firstbitset;
  using r_t = decltype(firstbitset(T()));

  // return type conformity test
  STF_TYPE_IS(r_t,(bd::as_integer_t<T, unsigned>));

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(firstbitset(bs::Inf<T>()), r_t(1ull<<bs::Nbmantissabits<T>()));
  STF_EQUAL(firstbitset(bs::Minf<T>()), r_t(1ull<<bs::Nbmantissabits<T>()));
  STF_EQUAL(firstbitset(bs::Nan<T>()), bs::One<r_t>());
#endif
  STF_EQUAL(firstbitset(bs::Signmask<T>()), bs::One<r_t>()+bs::Valmax<r_t>()/2);
  STF_EQUAL(firstbitset(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for real_

STF_CASE_TPL (" firstbitset signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::firstbitset;
  using r_t = decltype(firstbitset(T()));

  // return type conformity test
  STF_TYPE_IS(r_t,(bd::as_integer_t<T, unsigned>));

  // specific values tests
  STF_EQUAL(firstbitset(bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(firstbitset(bs::Signmask<T>()), bs::One<r_t>()+bs::Valmax<r_t>()/2);
  STF_EQUAL(firstbitset(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for signed_int_

STF_CASE_TPL (" firstbitset unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::firstbitset;
  using r_t = decltype(firstbitset(T()));

  // return type conformity test
  STF_TYPE_IS(r_t,(bd::as_integer_t<T, unsigned>));

  // specific values tests
  STF_EQUAL(firstbitset(bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(firstbitset(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for unsigned_int_


#include "k52/optimization/steepest_descent_method.h"

#ifdef BUILD_WITH_MPI

#include <boost/mpi.hpp>
#include <k52/parallel/mpi/constants.h>

#endif

#include <iostream>
#include <cmath>

#include <k52/optimization/params/i_continuous_parameters.h>

namespace
{
/// @brief Golden ratio value for GoldenSectionSearch method
/// @link https://en.wikipedia.org/wiki/Golden_ratio
	const double kAlpha = (1.0 + sqrt(5.0)) / 2.0;
}

using ::std::vector;

namespace k52
{
namespace optimization
{

SteepestDescentMethod::SteepestDescentMethod(
    double increment_of_the_argument,
    size_t max_iteration_number,
    double precision)
    : increment_of_the_argument_(increment_of_the_argument)
    , max_iteration_number_(max_iteration_number)
    , precision_(precision)
{
}

SteepestDescentMethod* SteepestDescentMethod::Clone() const
{
    return new SteepestDescentMethod(
        increment_of_the_argument_,
        max_iteration_number_,
        precision_);
}

std::string SteepestDescentMethod::get_name() const
{
    return "Steepest Descent Method";
}

#ifdef BUILD_WITH_MPI
void SteepestDescentMethod::Send(boost::mpi::communicator* communicator, int target) const
 {
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, increment_of_the_argument_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, precision_);
}

void SteepestDescentMethod::Receive(boost::mpi::communicator* communicator, int source)
{
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, increment_of_the_argument_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, precision_);
}
#endif

vector<double> SteepestDescentMethod::FindOptimalParameters(const vector<double>& initial_parameters)
{
    vector<double> parameters(initial_parameters);
    size_t counter = 0;
    double lambda = 1;

    vector<double> next_step_parameters(parameters);
    do
    {
        parameters = next_step_parameters;
        counter++;
        vector<double> gradient = CalculateGradient(parameters);
        vector<double> invert_gradiend = InvertVector(gradient);

        lambda = GoldenSectionSearch(parameters, lambda, invert_gradiend);

        for (size_t i = 0; i < next_step_parameters.size(); ++i)
        {
            next_step_parameters[i] = parameters[i] + lambda * invert_gradiend[i];
        }

    } while (!IsExitCriteriaFulfilled(next_step_parameters)
        && !(counter == max_iteration_number_));

    return parameters;
}

bool SteepestDescentMethod::IsExitCriteriaFulfilled(
    const vector<double>& steps_array) const
{
    double temp = 0;
    vector<double> gradient = CalculateGradient(steps_array);
    for (size_t i = 0; i < steps_array.size(); ++i)
    {
        temp += gradient[i] * gradient[i];
    }
    if (sqrt(temp) > precision_)
    {
        return false;
    }
    return true;
}
/// @TODO: needs to replaced copy-pasted method to the method from Math
/// @Source: conjugate_gradient_method.cpp
/// @link https://github.com/PavelKovalets/k52/blob/develop/src/optimization/conjugate_gradient_method.cpp#L159
vector<double> SteepestDescentMethod::CalculateGradient(
    const vector<double>& parameters) const
{
    vector<double> gradient(parameters.size());

    for (size_t i = 0; i<parameters.size(); ++i)
    {
        gradient[i] = CalculateDerivative(parameters, i);
    }

    return gradient;
}

/// @TODO: needs to replaced copy-pasted method to the method from Math
/// @Source: conjugate_gradient_method.cpp
/// @link https://github.com/PavelKovalets/k52/blob/develop/src/optimization/conjugate_gradient_method.cpp#L126
double SteepestDescentMethod::CalculateDerivative(
    const vector<double>& parameters,
    const size_t& index) const
{
    double half = 0.5;
    vector<double> decrement_function(parameters);
    vector<double> increment_function(parameters);
    decrement_function[index] = parameters[index] - increment_of_the_argument_ * half;
    increment_function[index] = parameters[index] + increment_of_the_argument_ * half;
    double increment_function_value = CountObjectiveFunctionValueToMinimize(increment_function);
    double decrement_function_value = CountObjectiveFunctionValueToMinimize(decrement_function);
    return (increment_function_value - decrement_function_value) / increment_of_the_argument_;
}

/// @TODO: needs to replaced to the method from Math
vector<double> SteepestDescentMethod::InvertVector(
    const vector<double>& parameters) const
{
    vector<double> new_parameters(parameters);
    for (size_t i = 0; i < new_parameters.size(); ++i)
    {
        new_parameters[i] *= -1;
    }
    return new_parameters;
}

double SteepestDescentMethod::GoldenSectionSearch(
    const vector<double>& init_point,
    const double& step,
    const vector<double>& direction) const
{
    vector<double> next_point(init_point.size());
    double lambda = 0;
    double step_for_borders = 0;

    step_for_borders = Localization(init_point, step, direction);

    lambda = FindStepLength(init_point, step_for_borders, direction);

    return lambda;
}

double SteepestDescentMethod::FindStepLength(
    const vector<double>& init_parameters,
    const double& step_for_borders,
    const vector<double>& direction) const
{
    vector<double> right_border_parameters(init_parameters.size());
    vector<double> next_left_border_point(init_parameters.size());
    vector<double> next_right_border_point(init_parameters.size());
    double left_border = 0;
    double next_left_border = 0;
    double next_right_border = 0;
    double constriction_number = 0.382;
    double right_border = step_for_borders;

    for (size_t i = 0; i < init_parameters.size(); ++i)
    {
        right_border_parameters[i] = init_parameters[i] + step_for_borders*direction[i];
    }

    next_left_border = left_border + constriction_number*(right_border - left_border);
    next_right_border = right_border - constriction_number*(right_border - left_border);

    double last_left_border = 0;
    double last_right_border = 0;
    double next_left_border_value = 0;
    double next_right_border_value = 0;
    do
    {
        for (size_t i = 0; i < init_parameters.size(); ++i)
        {
            next_left_border_point[i] = init_parameters[i] + next_left_border*direction[i];
            next_right_border_point[i] = init_parameters[i] + next_right_border*direction[i];
        }
        next_left_border_value = CountObjectiveFunctionValueToMinimize(next_left_border_point);
        next_right_border_value = CountObjectiveFunctionValueToMinimize(next_right_border_point);

        last_left_border = left_border;
        last_right_border = right_border;

        if (next_left_border < next_right_border)
        {
            if (next_left_border_value > next_right_border_value)
            {
                left_border = next_left_border;
                next_left_border = next_right_border;
                next_right_border = right_border - constriction_number * (right_border - left_border);
            }
            else
            {
                right_border = next_right_border;
                next_right_border = next_left_border;
                next_left_border = left_border + constriction_number * (right_border - left_border);
            }
        }
        else
        {
            if (next_right_border_value > next_left_border_value)
            {
                left_border = next_right_border;
                next_right_border = next_left_border;
                next_left_border = right_border - constriction_number * (right_border - left_border);
            }
            else
            {
                right_border = next_left_border;
                next_left_border = next_right_border;
                next_right_border = left_border + constriction_number * (right_border - left_border);
            }
        }
        if ((left_border == last_left_border && right_border == last_right_border)) {
            break;
        }

    } while ((abs(left_border - right_border) > precision_));

    return (left_border + right_border) / 2;
}

double SteepestDescentMethod::Localization(
    const vector<double>& init_parameters,
    const double& init_step,
    const vector<double>& direction) const
{
    vector<double> point(init_parameters.size());
    double step = init_step*2;

    do {
		step /= 2;
        for (size_t i = 0; i < point.size(); ++i)
        {
            point[i] = init_parameters[i] + step*direction[i];
        }
    } while ((CountObjectiveFunctionValueToMinimize(point) > CountObjectiveFunctionValueToMinimize(init_parameters))
        && (abs(step) > precision_));

    vector<double> next_step_point(point);
    do
    {
        point = next_step_point;
        step *= kAlpha;
        for (size_t i = 0; i < point.size(); ++i)
        {
        next_step_point[i] = init_parameters[i] + step*direction[i];
        }

    } while (CountObjectiveFunctionValueToMinimize(next_step_point) <= CountObjectiveFunctionValueToMinimize(point));

    double final_step = 0;
    for (size_t i = 0; i < next_step_point.size(); ++i)
    {
        final_step += (next_step_point[i] - init_parameters[i])*(next_step_point[i] - init_parameters[i]);
    }

    return sqrt(final_step);
}

}//optimization
}//k52


#include <cmath>
#include <boost/math/special_functions/fpclassify.hpp> // isnan, isinf
#include <ecf/ECF.h>
#include "SymbRegEvalOp.h"
#include "ReadData.h"
#include "infixTree.h"

// from WriteBest.h
extern bool evaluateVerbose;

void SymbRegEvalOp::registerParameters(StateP state)
{
	state->getRegistry()->registerEntry("input_file", (voidP)(new std::string), ECF::STRING);
	state->getRegistry()->registerEntry("linear_scaling", (voidP) (new uint(0)), ECF::UINT);
	state->getRegistry()->registerEntry("error_weights_file", (voidP) (new std::string), ECF::STRING);
	state->getRegistry()->registerEntry("error_metric", (voidP) (new std::string), ECF::STRING);
	state->getRegistry()->registerEntry("data_skip", (voidP) (new uint(1)), ECF::UINT);
	state->getRegistry()->registerEntry("data_offset", (voidP) (new uint(0)), ECF::UINT);
}

void SymbRegEvalOp::createTerminals(StateP state)
{
	static std::string configTerminals = *((std::string*) state->getGenotypes()[0]->getParameterValue(state, "terminalset").get());
	//configTerminals = *((std::string*) state->getRegistry()->getEntry("Tree.terminalset").get());

	varNames.clear();
	std::string terminalSet;
	for(uint i = 0; i < nVariables; i++) {
		varNames.push_back ("x" + uint2str(i + 1));
		terminalSet += varNames[i] + " ";
	}

	// set terminal names
	terminalSet = configTerminals + " " + terminalSet;
	state->getGenotypes()[0]->setParameterValue(state, "terminalset", (voidP) new std::string(terminalSet));
	//state->getRegistry()->modifyEntry("Tree.terminalset", (voidP) new std::string(terminalSet));

	// reinitialize population with updated terminals
	Tree::Tree* hometree = (Tree::Tree*) state->getGenotypes()[0].get();
	hometree->primitiveSet_ = Tree::PrimitiveSetP();
	state->getPopulation()->initialize(state);
}

bool SymbRegEvalOp::initialize(StateP state)
{
	state_ = state;
	recordResults = false;
	results.clear();

	voidP sptr = state->getRegistry()->getEntry("data_skip");
	uint skip = *((uint*)sptr.get());
	sptr = state->getRegistry()->getEntry("data_offset");
	uint offset = *((uint*)sptr.get());
	sptr = state->getRegistry()->getEntry("linear_scaling");
	uint scaling = *((uint*)sptr.get());
	linearScaling = false;
	if(scaling != 0)
		linearScaling = true;

	std::string dataFile = *((std::string*) state->getRegistry()->getEntry("input_file").get());
	// read from file into Evaluator data
	if (!readDataFromFile(data, dataFile, offset, skip)) {
		return false;
	}

	nSamples = data.size();
	nVariables = data[0].size() - 1;
	results.resize(nSamples);

	// a) create terminal names for use with canonical GP
	createTerminals(state);

	// b) read terminal names from config
	//varNames.clear();
	//std::string terminals = *((std::string*) state->getGenotypes()[0]->getParameterValue(state, "terminalset").get());
	//std::stringstream ss(terminals);
	//std::string terminal;
	//for(uint i = 0; i < nVariables; i++) {
	//	ss >> terminal;
	//	varNames.push_back (terminal);
	//}

	// initialize evaluator
	eval.data = &data;
	eval.initialize();
	for(uint i = 0; i < nVariables; i++) {
		eval.addTerminal(varNames[i]);
	}

	return true;
}

FitnessP SymbRegEvalOp::evaluate(IndividualP individual)
{
	if(linearScaling == true)
		return evaluateUsingLinearScaling(individual);

	// we try to minimize the function value, so we use FitnessMin fitness (for minimization problems)
	FitnessP fitness (new FitnessMin);

	// get the genotype we defined in the configuration file
	Tree::Tree* tree = (Tree::Tree*) individual->getGenotype().get();

	// output tree expression to string
	std::stringstream sValue;
	for(uint i = 0; i < tree->size(); i++) {
		sValue << (*tree)[i]->primitive_->getName() << " ";
	}
	// send expression to evaluator
	eval.parseExpression(sValue.str(), tree->size());

	double value = 0;
	double result;
	// evaluating data from input file
	for (uint i = 0; i < nSamples; i++) {
		//// set only defined variables (x1, x2, ...)
		//for(uint term = 0; term < nVariables; term++) {
		//	tree->setTerminalValue(varNames[term], &data[i][term]);
		//}

		//// get the f value of the current tree
		//tree->execute(&result);

		result = eval.executeParsedExpression(i);

		// clip the value to [0, 255]
		result = (int) abs(result);
		if(result > 255)
			result = 255;

		if(recordResults)
			results[i] = result;

		// add the absolute difference
		//value += fabs(eval.data[i][nVariables] - result);
		// or squared error
		double error = fabs(data[i][nVariables] - result) * fabs(data[i][nVariables] - result);

		value += error;
	}

	value /= nSamples;	   // MSE
	value = sqrt(value);   // RMSE

	fitness->setValue(value);

	if(evaluateVerbose) {
		stringstream ss;
		string infix;
		showTree(infix, tree);
		ss << infix << endl;
		ECF_LOG(state_, 1, ss.str());
	}

	return fitness;
}

FitnessP SymbRegEvalOp::evaluateUsingLinearScaling(IndividualP individual)
{
	FitnessP fitness (new FitnessMin);

	// get the genotype we defined in the configuration file
	Tree::Tree* tree = (Tree::Tree*) individual->getGenotype().get();

	// output tree expression to string
	std::stringstream sValue;
	for(uint i = 0; i < tree->size(); i++) {
		sValue << (*tree)[i]->primitive_->getName() << " ";
	}
	// send expression to evaluator
	eval.parseExpression(sValue.str(), tree->size());

	//average value of y and t
	double mean_y = 0, mean_t = 0, sum_yt = 0, sum_sqr_y = 0;

	for (uint i = 0 ; i < nSamples ; i++)
	{
		//// set only defined variables (x1, x2, ...)
		//for(uint term = 0; term < nVariables; term++) {
		//	tree->setTerminalValue(varNames[term], &data[i][term]);
		//}

		// get the y value of the current tree
		double y;
		//tree->execute(&y);
		y = eval.executeParsedExpression(i);

		// clip the value to [0, 255]
		y = (int) abs(y);
		if(y > 255)
			y = 255;

		mean_y += y;
		double t = data[i][nVariables];
		mean_t += t;
		sum_yt += t*y;
		sum_sqr_y += y*y;
	}

	mean_y /= nSamples;
	mean_t /= nSamples;

	//coefficients for linear regression
	double b = (sum_yt - nSamples*mean_y*mean_t)/(sum_sqr_y - nSamples*mean_y*mean_y);
	double a = mean_t - b*mean_y;

	bool isNaN = boost::math::isnan(a) || boost::math::isnan(b);
	bool isInf = boost::math::isinf(a) || boost::math::isinf(b);
	if (isNaN || isInf) {
		a = 0;
		b = 1;
//		fitness->setValue(1e14);
//		return fitness;
	}

	double value = 0;
	double var_of_y = 0; //variance of variable result

	for(uint i = 0; i < nSamples; i++) {
		//// set only defined variables (x1, x2, ...)
		//for(uint term = 0; term < nVariables; term++) {
		//	tree->setTerminalValue(varNames[term], &data[i][term]);
		//}

		double y;
		//tree->execute(&y);
		y = eval.executeParsedExpression(i);

		// clip the value to [0, 255]
		y = (int) abs(y);
		if(y > 255)
			y = 255;

		var_of_y += (y-mean_y)*(y-mean_y);
		double result = a + b*y;
		result = (int) result;

		if(recordResults)
			results[i] = result;

		// add the absolute difference
		//value += fabs(eval.data[i][nVariables] - result);
		// or squared error
		double error = fabs(data[i][nVariables] - result) * fabs(data[i][nVariables] - result);
		value += error;
	}

	value /= nSamples;	   // MSE
	value = sqrt(value);   // RMSE

	var_of_y /= nSamples-1;

	if (evaluateVerbose) {
		stringstream ss;
		string infix;
		showTree(infix, tree);
		ss << infix << endl;
		ss << "Linear scaling parameters: scale=" << b << " offset=" << a << std::endl;
		ECF_LOG(state_, 1, ss.str());
	}

	fitness->setValue(value);

	return fitness;
}


#include <cylbot_mcl/pose_cloud2d.h>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/bind.hpp>
#include <cmath>
#include <vector>
#include <utility>
#include <algorithm>
#include <tf/transform_datatypes.h>
#include <limits>
#include <set>

namespace cylbot_mcl
{
	PoseCloud2D::PoseCloud2D(const RobotModel& model, const int num_samples, const cylbot_map_creator::LikelihoodField& field) :
		PoseCloud(model, num_samples)
	{
		this->likelihood_field = field;

		last_sensor_update = 0;
		w_slow = w_fast = 0;
	}

	PoseCloud2D::PoseCloud2D(const RobotModel& model,
							 const geometry_msgs::PoseWithCovarianceStamped& initial_pose,
							 const int num_particles,
							 const cylbot_map_creator::LikelihoodField& field) :
		PoseCloud(model, initial_pose, num_particles)
	{
		this->likelihood_field = field;

		last_sensor_update = 0;
		w_slow = w_fast = 0;
	}

	void PoseCloud2D::sensorUpdate(const geometry_msgs::Pose map_pose,
								   const pcl::PointCloud<pcl::PointXYZ>& beam_ends,
								   double curr_time)
	{
		// this type is used as a temporary datastructure in this function only
		// this typedef just saves some typing and makes the later code more readable
		typedef geometry_msgs::PoseArray::_poses_type::iterator WeightPairFirst;
		typedef double WeightPairSecond;
		typedef std::pair<WeightPairFirst, WeightPairSecond> WeightPair;
		typedef std::vector<WeightPair> Weights;

		// if we haven't set a map yet then don't do anything
		if(likelihood_field.data.size() == 0)
			return;

		// don't perform sensor updates unless moving
		// this is to prevent the variance from trending to 0 with probability 1
		// see page 108 Resampling section of Probabilistic Robotics Textbook
		if(fabs(last_cmd.linear.y) < 0.1 && fabs(last_cmd.angular.z) < 0.1)
			return;

		// update at most every 5 seconds
		if(curr_time - last_sensor_update < 1)
			return;

		last_sensor_update = curr_time;

		// store an iterator to a pose along with its computed weight
		Weights pose_weights;
		double total_weight = 0;
		double max_weight = 0;

		//ROS_INFO("Starting calculation of true pose probability");
		double true_prob = getMeasurementProbability(map_pose, map_pose, beam_ends);
		ROS_INFO_STREAM("Finished calculation of true pose probability " << true_prob);

		// for(pcl::PointCloud<pcl::PointXYZ>::const_iterator beam_end = beam_ends.points.begin();
		// 	beam_end != beam_ends.points.end();
		// 	beam_end++)
		// {
		// 	if(beam_end->x != beam_end->x || beam_end->y != beam_end->y || beam_end->z != beam_end->z)
		// 		ROS_WARN("sensorUpdate: beam_end has NaN value");
		// }

		// calculate the weight of each pose particle
		//ROS_INFO("Starting calculation of pose cloud probabilities");
		for(geometry_msgs::PoseArray::_poses_type::iterator pose = pose_array.poses.begin();
			pose != pose_array.poses.end();
			pose++)
		{
			double prob = getMeasurementProbability(map_pose, *pose, beam_ends);
			if(prob < 0)
				continue; // ignore points which had no beam
			pose_weights.push_back(std::make_pair(pose, prob));
			total_weight += prob;

			if(prob > max_weight)
				max_weight = prob;
		}

		// if there was no valid sensor data
		// then we cannot perform an update
		if(pose_weights.size() == 0)
			return;

		//ROS_INFO_STREAM("Finished calculation of pose cloud probabilities");
		double avg_weight = total_weight/pose_array.poses.size();

		w_slow = w_slow + model.alpha_slow*(avg_weight - w_slow);
		w_fast = w_fast + model.alpha_fast*(avg_weight - w_fast);
		ROS_INFO_STREAM("total weight: " << total_weight);
		ROS_INFO_STREAM("Average prob: " << total_weight/pose_array.poses.size());
		ROS_INFO_STREAM("Max prob: " << max_weight);

		// normalize
		for(Weights::iterator weight_pair = pose_weights.begin();
			weight_pair != pose_weights.end();
			weight_pair++)
		{
			weight_pair->second /= total_weight;
			//ROS_INFO_STREAM(weight_pair->second);
		}

		double M = (double)(pose_weights.size());

		// create prob generator
		boost::random::uniform_real_distribution<> dist(0.0, 1.0/M);
		boost::random::uniform_real_distribution<> new_sample_dist(0.0, 1.0);
		boost::variate_generator<boost::mt19937, boost::random::uniform_real_distribution<> > rand(rng, dist);
		boost::variate_generator<boost::mt19937, boost::random::uniform_real_distribution<> > new_sample_rand(rng, new_sample_dist);

		// resample using low variance sampling from table 4.4 on page 110 of the Probabilistic robotics book
		std::set<geometry_msgs::Pose*> pose_set;
		geometry_msgs::PoseArray::_poses_type poses;

		double r = rand();
		double c = pose_weights.begin()->second;
		int i=0;
		for(int m=0; m<M; m++)
		{
			double U = r + m/M;
			while(U > c)
			{
				i++;
				c = c + pose_weights[i].second;
			}

			// if(new_sample_rand() < 1.0 - w_fast/w_slow)
			// {
			// 	// geometry_msgs::PoseArray randomPoses = generateUniformPoses(std::make_pair(-20.0, 20.0),
			// 	// 															std::make_pair(-20.0, 20.0),
			// 	// 															100);
			// 	// for(int i=0; i<randomPoses.poses.size(); i++)
			// 	// 	poses.push_back(randomPoses.poses[i]);

			// 	geometry_msgs::PoseWithCovarianceStamped map_pose_with_cov;
			// 	map_pose_with_cov.pose.pose = map_pose;
			// 	map_pose_with_cov.pose.covariance[0] = map_pose_with_cov.pose.covariance[7] = map_pose_with_cov.pose.covariance[35] = .001;

			// 	geometry_msgs::PoseArray new_samples = generatePoses(map_pose_with_cov, 1);
			// 	for(geometry_msgs::PoseArray::_poses_type::const_iterator pose = new_samples.poses.begin();
			// 		pose != new_samples.poses.end();
			// 		pose++)
			// 	{
			// 		poses.push_back(*pose);
			// 	}

			// }
			// else
			{
				pose_set.insert(&(*(pose_weights[i].first)));
				// poses.push_back(*(pose_weights[i].first));
			}
		}

		for(std::set<geometry_msgs::Pose*>::const_iterator pose = pose_set.begin();
			pose != pose_set.end();
			pose++)
		{
			poses.push_back(*(*pose));
		}

		// repeat some of the points if there are only a few
		i=0;
		while(poses.size() < 100)
		{
			poses.push_back(poses[i++%poses.size()]);
		}

		pose_array.poses = poses;
	}

	void PoseCloud2D::fieldUpdate(const cylbot_map_creator::LikelihoodField& field)
	{
		this->likelihood_field = field;
	}

	double PoseCloud2D::getMeasurementProbability(const geometry_msgs::Pose& map_pose,
												  const geometry_msgs::Pose& pose,
												  const pcl::PointCloud<pcl::PointXYZ>& beam_ends)
	{
		double probability = -1.0;

		int x_origin = (int)fabs(likelihood_field.info.origin.position.x/likelihood_field.info.resolution);
		int y_origin = (int)fabs(likelihood_field.info.origin.position.y/likelihood_field.info.resolution);

		// construct transfrom from map to estimated pose
		tf::Transform pose_transform;
		pose_transform.setOrigin(tf::Vector3(pose.position.x, pose.position.y, pose.position.z));
		// the pose estimates align along the y-axis, but the tf package uses the x-axis as a reference
		// so need to rotate the pose estimates by -90 degrees back to the x-axis
		tf::Quaternion pose_rotation = tf::createQuaternionFromRPY(0, 0, tf::getYaw(map_pose.orientation) - 3.14159/2.0);
		pose_transform.setRotation(pose_rotation);

		//ROS_INFO("Starting scan probability calculation");
		for(pcl::PointCloud<pcl::PointXYZ>::const_iterator beam_end = beam_ends.points.begin();
			beam_end != beam_ends.points.end();
			beam_end++)
		{
			tf::Vector3 map_beam_end = pose_transform(tf::Vector3(beam_end->x, beam_end->y, beam_end->z));

			// convert to grid coordinates
			int x = round(map_beam_end.getX()/likelihood_field.info.resolution);
			int y = round(map_beam_end.getY()/likelihood_field.info.resolution);
			double distance = getCellDistance(x + x_origin, y + y_origin);

			//ROS_INFO_STREAM("distance:" << distance);

			// lookup failed so skip this beam
			if(distance == -1)
			{
				//ROS_INFO("Failed to lookup point (%d, %d) -- (%d, %d)", x, y, x + x_origin, y + y_origin);

				// assume every point not in the map has an equal probability of hit
				// in this case inversely proportional to the probability of hitting a maximum
				if(probability < 0)
					probability = model.sensor_params.zmax;
				else
					probability *= model.sensor_params.zmax;

				continue;
			}

			double beam_prob = model.sensor_params.likelihoodProbability(distance);
			if(probability < 0)
				probability = beam_prob;
			else
				probability *= beam_prob;

		}

		//ROS_INFO_STREAM("Finished calculating probability of " << probability);
		return probability;
	}

	int PoseCloud2D::getCellDistance(const int x, const int y)
	{
		if(x < likelihood_field.info.width && x >= 0 &&
		   y < likelihood_field.info.height && y >= 0)
		{
			return likelihood_field.data[y*likelihood_field.info.width + x];
		}

		return -1;
	}
}


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.
*/

#include <boost/make_shared.hpp>
#include <ql/math/distributions/chisquaredistribution.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/processes/eulerdiscretization.hpp>
#include <qle/models/crcirpp.hpp>
#include <qle/processes/crcirppstateprocess.hpp>

namespace QuantExt {

CrCirppStateProcess::CrCirppStateProcess(CrCirpp* const model,
                                         CrCirppStateProcess::Discretization disc)
    : StochasticProcess(boost::shared_ptr<StochasticProcess::discretization>(new EulerDiscretization)), model_(model),
      discretization_(disc) {}

Size CrCirppStateProcess::size() const { return 2; }

Disposable<Array> CrCirppStateProcess::initialValues() const {
    Array res(size(), 0.0);
    res[0] = model_->parametrization()->y0(0); // y0
    res[1] = 1.0; // S(0,0) = 1
    return res;
}

Disposable<Array> CrCirppStateProcess::drift(Time t, const Array& x) const {
    QL_FAIL("not implemented");
}

Disposable<Matrix> CrCirppStateProcess::diffusion(Time t, const Array& x) const {
    QL_FAIL("not implemented");
}

Disposable<Array> CrCirppStateProcess::evolve(Time t0, const Array& x0, Time dt, const Array& dw) const {
    Array retVal(size());
    Real kappa, theta, sigma, y0;
    kappa = model_->parametrization()->kappa(t0);
    theta = model_->parametrization()->theta(t0);
    sigma = model_->parametrization()->sigma(t0);
    y0 = model_->parametrization()->y0(t0);

    const Real sdt = std::sqrt(dt);
    switch (discretization_) {

    case BrigoAlfonsi: {
        // see D. Brigo and F. Mercurio. Interest Rate Models: Theory and Practice, 2nd
        // Edition. Springer, 2006.
        // Ensures non-negative values for \sigma^2 \leq 2*\kappa*\theta
        Real temp = (1 - kappa / 2.0 * dt);
        Real temp2 = temp * std::sqrt(x0[0]) + sigma * sdt * dw[0] / (2.0 * temp);
        retVal[0] = temp2 * temp2 + (kappa * theta - sigma * sigma / 4) * dt;
        break;
    }
    default:
        QL_FAIL("unknown discretization schema");
    }

    // second element is S(0,i)
    Real SM_ti = model_->defaultCurve()->survivalProbability(t0 + dt);
    Real SM_ti_pre = model_->defaultCurve()->survivalProbability(t0);
    Real Pcir_ti = model_->zeroBond(0, t0 + dt, y0);
    Real Pcir_ti_pre = model_->zeroBond(0, t0, y0);
    retVal[1] = x0[1] * SM_ti / SM_ti_pre * Pcir_ti_pre / Pcir_ti * exp(-x0[0] * dt);

    return retVal;
}

} // namespace QuantExt


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

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "tudat/basics/testMacros.h"
#include "tudat/astro/basic_astro/unitConversions.h"
#include "tudat/astro/basic_astro/sphericalStateConversions.h"
#include "tudat/astro/reference_frames/referenceFrameTransformations.h"

namespace tudat
{
namespace unit_tests
{

using namespace orbital_element_conversions;
using namespace unit_conversions;
using namespace reference_frames;

BOOST_AUTO_TEST_SUITE( test_spherical_state_conversions )

//! Test Cartesian to spherical orbital state transformations, using Matlab script of Erwin Mooij to generate reference data.
//! See test_aerodynamic_angle_calculator unit test
BOOST_AUTO_TEST_CASE( testSphericalStateConversions )
{
    // Test case 1: arbitrary rotation
    {
        // Define Cartesian state
        Eigen::Vector6d cartesianState;
        cartesianState << -1656517.23153109, -5790058.28764025, -2440584.88186829,
                6526.30784888051, -2661.34558272018, 2377.09572383163;

        // Define associated spherical orbital state
        double testHeadingAngle = 1.229357188236127;
        double testFlightPathAngle = -0.024894033070522;
        double testLatitude = -0.385027359562548;
        double testLongitude = -1.849449608688977;
        double radius = cartesianState.segment( 0, 3 ).norm( );
        double speed = cartesianState.segment( 3, 3 ).norm( );

        // Convert pack and forth
        Eigen::Vector6d sphericalOrbitState  = convertCartesianToSphericalOrbitalState(
                    cartesianState );
        Eigen::Vector6d reconvertedCartesianState  = convertSphericalOrbitalToCartesianState(
                    sphericalOrbitState );

        // Check computed spherical orbital state against reference data
        BOOST_CHECK_SMALL(
                    std::fabs( radius - sphericalOrbitState( radiusIndex ) ),
                    radius * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testLatitude - sphericalOrbitState( latitudeIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testLongitude - sphericalOrbitState( longitudeIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( speed - sphericalOrbitState( speedIndex ) ),
                    speed * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testFlightPathAngle - sphericalOrbitState( flightPathIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testHeadingAngle - sphericalOrbitState( headingAngleIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );

        // Check consistency of back-and-forth conversion
        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i ) - cartesianState( i ) ),
                        radius * 2.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i  + 3 ) - cartesianState( i + 3 ) ),
                        speed * 2.0 * std::numeric_limits< double >::epsilon( ) );
        }
    }

    // Test case 2: rotation with zero (heading, flight path, latitude) and half pi (longitude) angles.
    {
        // Define Cartesian state
        Eigen::Vector6d cartesianState;
        cartesianState << 0.0, 6498098.09700000, 0.0, 0.0, 0.0, 7.438147520000000e+03;

        // Define associated spherical orbital state
        double testHeadingAngle = 0.0;
        double testFlightPathAngle = 0.0;
        double testLatitude = 0.0;
        double testLongitude = mathematical_constants::PI / 2.0;
        double radius = cartesianState.segment( 0, 3 ).norm( );
        double speed = cartesianState.segment( 3, 3 ).norm( );

        // Convert pack and forth
        Eigen::Vector6d sphericalOrbitState  = convertCartesianToSphericalOrbitalState(
                    cartesianState );
        Eigen::Vector6d reconvertedCartesianState  = convertSphericalOrbitalToCartesianState(
                    sphericalOrbitState );

        // Check computed spherical orbital state against reference data
        BOOST_CHECK_SMALL(
                    std::fabs( radius - sphericalOrbitState( radiusIndex ) ),
                    radius * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testLatitude - sphericalOrbitState( latitudeIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testLongitude - sphericalOrbitState( longitudeIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( speed - sphericalOrbitState( speedIndex ) ),
                    4.0 * speed * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testFlightPathAngle - sphericalOrbitState( flightPathIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testHeadingAngle - sphericalOrbitState( headingAngleIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );

        // Check consistency of back-and-forth conversion
        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i ) - cartesianState( i ) ),
                        radius * 2.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i  + 3 ) - cartesianState( i + 3 ) ),
                        speed * 4.0 * std::numeric_limits< double >::epsilon( ) );
        }
    }

    // Test case 3: rotation with zero (heading, flight path, longitude) and half pi (latitude) angles.
    {
        // Define Cartesian state
        Eigen::Vector6d cartesianState;
        cartesianState << 0.0, 0.0, 6.498098097000000e3, -7.438147520000000e3, 0.0, 0.0;

        // Define associated spherical orbital state
        double testHeadingAngle = 0.0;
        double testFlightPathAngle = 0.0;
        double testLatitude = mathematical_constants::PI / 2.0;
        double testLongitude = 0.0;
        double radius = cartesianState.segment( 0, 3 ).norm( );
        double speed = cartesianState.segment( 3, 3 ).norm( );

        // Convert pack and forth
        Eigen::Vector6d sphericalOrbitState  = convertCartesianToSphericalOrbitalState(
                    cartesianState );
        Eigen::Vector6d reconvertedCartesianState  = convertSphericalOrbitalToCartesianState(
                    sphericalOrbitState );

        // Check computed spherical orbital state against reference data
        BOOST_CHECK_SMALL(
                    std::fabs( radius - sphericalOrbitState( radiusIndex ) ),
                    radius * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testLatitude - sphericalOrbitState( latitudeIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testLongitude - sphericalOrbitState( longitudeIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( speed - sphericalOrbitState( speedIndex ) ),
                    speed * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testFlightPathAngle - sphericalOrbitState( flightPathIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testHeadingAngle - sphericalOrbitState( headingAngleIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );

        // Check consistency of back-and-forth conversion
        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i ) - cartesianState( i ) ),
                        radius * 2.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i  + 3 ) - cartesianState( i + 3 ) ),
                        speed * 2.0 * std::numeric_limits< double >::epsilon( ) );
        }
    }

    // Test case 4: rotation with zero (heading, longitude) and half pi (latitude, flight path) angles.
    {
        Eigen::Vector6d cartesianState;
        cartesianState << 0.0, 0.0, 6.498098097000000e3, 0.0, 0.0, -7.438147520000000e3;

        // Define associated spherical orbital state
        double testFlightPathAngle = -mathematical_constants::PI / 2.0;
        double testLatitude = mathematical_constants::PI / 2.0;
        double testLongitude = 0.0;
        double radius = cartesianState.segment( 0, 3 ).norm( );
        double speed = cartesianState.segment( 3, 3 ).norm( );

        // Convert pack and forth
        Eigen::Vector6d sphericalOrbitState  = convertCartesianToSphericalOrbitalState(
                    cartesianState );
        Eigen::Vector6d reconvertedCartesianState  = convertSphericalOrbitalToCartesianState(
                    sphericalOrbitState );

        // Check computed spherical orbital state against reference data (heading angle not tested, because close to
        // undefined; value is numerically irrelevant for physical state).
        BOOST_CHECK_SMALL(
                    std::fabs( radius - sphericalOrbitState( radiusIndex ) ),
                    radius * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testLatitude - sphericalOrbitState( latitudeIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testLongitude - sphericalOrbitState( longitudeIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( speed - sphericalOrbitState( speedIndex ) ),
                    speed * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL(
                    std::fabs( testFlightPathAngle - sphericalOrbitState( flightPathIndex ) ),
                    2.0 * mathematical_constants::PI * std::numeric_limits< double >::epsilon( ) );

        // Check consistency of back-and-forth conversion
        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i ) - cartesianState( i ) ),
                        radius * 2.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i  + 3 ) - cartesianState( i + 3 ) ),
                        speed * 2.0 * std::numeric_limits< double >::epsilon( ) );
        }
    }

    // Test case 5: rotation with undefined heading angle.
    {
        Eigen::Vector6d cartesianState;
        cartesianState << 0.0, 0.0, 6.498098097000000e3, 0.0, 0.0, -7.438147520000000e3;

        // Define associated spherical orbital state
        Eigen::Vector6d sphericalOrbitalState;
        sphericalOrbitalState( headingAngleIndex )  = TUDAT_NAN;
        sphericalOrbitalState( flightPathIndex ) = -mathematical_constants::PI / 2.0;
        sphericalOrbitalState( latitudeIndex ) = mathematical_constants::PI / 2.0;
        sphericalOrbitalState( longitudeIndex ) = 0.0;
        sphericalOrbitalState( radiusIndex )  = cartesianState.segment( 0, 3 ).norm( );
        sphericalOrbitalState( speedIndex )  = cartesianState.segment( 3, 3 ).norm( );

        Eigen::Vector6d reconvertedCartesianState  = convertSphericalOrbitalToCartesianState(
                    sphericalOrbitalState );

        // Check consistency of back-and-forth conversion
        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i ) - cartesianState( i ) ),
                        sphericalOrbitalState( radiusIndex ) * 2.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i  + 3 ) - cartesianState( i + 3 ) ),
                        sphericalOrbitalState( speedIndex ) * 2.0 * std::numeric_limits< double >::epsilon( ) );
        }
    }

    // Test case 5: rotation with undefined flight path angle.
    {
        Eigen::Vector6d cartesianState;
        cartesianState << 0.0, 0.0, 6.498098097000000e3, 0.0, 0.0, 0.0;

        // Define associated spherical orbital state
        Eigen::Vector6d sphericalOrbitalState;
        sphericalOrbitalState( headingAngleIndex )  = TUDAT_NAN;
        sphericalOrbitalState( flightPathIndex ) = TUDAT_NAN;
        sphericalOrbitalState( latitudeIndex ) = mathematical_constants::PI / 2.0;
        sphericalOrbitalState( longitudeIndex ) = 0.0;
        sphericalOrbitalState( radiusIndex )  = cartesianState.segment( 0, 3 ).norm( );
        sphericalOrbitalState( speedIndex )  = cartesianState.segment( 3, 3 ).norm( );

        Eigen::Vector6d reconvertedCartesianState  = convertSphericalOrbitalToCartesianState(
                    sphericalOrbitalState );

        // Check consistency of back-and-forth conversion
        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i ) - cartesianState( i ) ),
                        sphericalOrbitalState( radiusIndex ) * 2.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL(
                        std::fabs( reconvertedCartesianState( i  + 3 ) - cartesianState( i + 3 ) ),
                        2.0 * std::numeric_limits< double >::epsilon( ) );
        }
    }

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2013, The University of Texas at Dallas
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of The University of Texas at Dallas nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

/*
 * Filename: connectivity.cpp
 *
 * Description: This file contains the ROS node that calculates the control variables
 * for a collection of iRobot Creates.
 *
 * Log
 * ----
 * 2013-09-15 File created by Hazen Eckert
 *
 */

// ROS includes
#include "ros/ros.h"
#include "ros/assert.h"
#include "dynamic_reconfigure/server.h"
#include "create_driver/vicon_driver.h"

// Library includes
#include <string>
#include <vector>
#include <map>
#include <set>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>

// Local Package includes
#include "connectivity_controller/EigenConnectivityMsgs.h"
#include "connectivity_controller/ConnectivityVariablesConfig.h"

using namespace std;
using namespace Eigen;

#define RHO1 0.7
#define RHO2 2.3

// Collision avoidance variables
double R = 0.7;
double r = 0.33;

double psi( double x, double rho1, double rho2 )
{
	if ( x <= rho1 ) {
		return 1;
	} else if ( rho1 < x && x < rho2 ) {
		return ( exp(-1/(rho2-x)) / ( exp(-1/(rho2-x)) + exp(1/(rho1-x)) ) );
	} else if ( rho2 <= x ) {
		return 0;
	}
	return 0;
}

double psiPrime( double d )
{
	if ( d <= RHO1 ) {
		return 0;
	} else if ( RHO1 < d && d < RHO2 ) {
		return (-0.5)*( pow((d-RHO1),(-2))+pow((d-RHO2),(-2)) ) / (1+  cosh(  (1/(-d+RHO1))+(1/(-d+RHO2)) )   );
	} else if ( RHO2 <= d ) {
		return 0;
	}
	return 0;
}

void reconfigureCallback(connectivity_controller::ConnectivityVariablesConfig &config, uint32_t level) {
	R = config.r_max;
	r = config.r_min;

}

int main(int argc, char **argv)
{
	// ROS Initalization
	ros::init(argc, argv, "connectivity");

	ros::NodeHandle n;
	ros::NodeHandle private_n("~");

	dynamic_reconfigure::Server<connectivity_controller::ConnectivityVariablesConfig> server;
  	dynamic_reconfigure::Server<connectivity_controller::ConnectivityVariablesConfig>::CallbackType f;
  	f = boost::bind(&reconfigureCallback, _1, _2);
 	server.setCallback(f);

	// ROS Parameters

	// List of robot names
	vector<string> robot_names;
	XmlRpc::XmlRpcValue robot_list;
	private_n.getParam("robot_list", robot_list);
	ROS_ASSERT(robot_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < robot_list.size(); i++)
	{
		ROS_ASSERT(robot_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
		robot_names.push_back(static_cast<string>(robot_list[i]));
	}

	// List of formation x positions
	vector<double> formation_x;
	XmlRpc::XmlRpcValue x_list;
	private_n.getParam("formation_x_list", x_list);
	ROS_ASSERT(x_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < x_list.size(); i++)
	{
		ROS_ASSERT(x_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
		formation_x.push_back(static_cast<double>(x_list[i]));
	}

	// List of formation y positions
	vector<double> formation_y;
	XmlRpc::XmlRpcValue y_list;
	private_n.getParam("formation_y_list", y_list);
	ROS_ASSERT(y_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < y_list.size(); i++)
	{
		ROS_ASSERT(y_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
		formation_y.push_back(static_cast<double>(y_list[i]));
	}

	// Number of robots
	const int num_robots = robot_names.size();

	// ROS Subscribers
	map<string, create_driver::ViconStream> vicon;
	vector<string>::iterator name_it;
	vector<ros::Subscriber> sub;

	for (name_it = robot_names.begin(); name_it != robot_names.end(); name_it++)
	{
		vicon.insert(pair<string, create_driver::ViconStream>(*name_it, create_driver::ViconStream()));
		sub.push_back(n.subscribe(*name_it + "/tf", 10, &create_driver::ViconStream::callback, &vicon[*name_it]));
	}

	// ROS Publishers
	ros::Publisher pub = n.advertise<connectivity_controller::EigenConnectivityMsgs>("/connectivity", 10);

	// ROS loop
	ros::Rate loop_rate(250); // 250 Hz

	double rho2[] = { 2.3, 2.2, 1.9, 1.5, 1.7, 2.0 };

	while (ros::ok())
	{
		// Retrieve Vicon Data
		ros::spinOnce();

		// Calculations
		MatrixXd adjacency_matrix = MatrixXd::Zero(num_robots, num_robots);
		MatrixXd P = MatrixXd::Zero(num_robots, num_robots - 1);
		MatrixXd L = MatrixXd::Zero(num_robots, num_robots);
		MatrixXd S = MatrixXd::Zero(num_robots, num_robots);

		// Adjacency Matrix
		for (int i = 0; i < num_robots; i++)
		{
			for (int j = 0; j < num_robots; j++)
			{
				if ( i == j )
				{
					adjacency_matrix(i,j) = 0.0;
				} else {
					adjacency_matrix(i,j) = psi(sqrt(
					pow(vicon[robot_names[j]].x() - vicon[robot_names[i]].x(), 2) +
					pow(vicon[robot_names[j]].y() - vicon[robot_names[i]].y(), 2)
					), RHO1, rho2[i]);
				}
			}
		}
		ROS_WARN_STREAM("Adj matrix:"  << adjacency_matrix );

		// L Matrix
		for (int i = 0; i < num_robots; i++)
		{
			for (int j = 0; j < num_robots; j++)
			{
				if ( i == j ) {
					double rowSum = 0;
					for (int k = 0; k < num_robots; k++) {
						rowSum += adjacency_matrix(i,k);
					}
					L(i,j) = rowSum - adjacency_matrix(i,j);
				} else {
					L(i,j) = -adjacency_matrix(i,j);
				}
			}
		}

		ROS_WARN_STREAM("L matrix:"  << L );

		// S Matrix
		S = MatrixXd::Identity(num_robots, num_robots) - L/((double)num_robots); // S = I - L/n

		ROS_WARN_STREAM("S matrix:"  << S );

		// Get eigenvalues and eigen vectors
		EigenSolver<MatrixXd> es(S.transpose());
		VectorXcd eigenvalues = es.eigenvalues();
		MatrixXcd eigenvectors = es.eigenvectors();

		ROS_WARN_STREAM("Eigenvalues"  << eigenvalues );
		ROS_WARN_STREAM("Eigenvectors:"  << eigenvectors );

		std::complex<double> max_eigenvalue(0.0,0.0);
		VectorXcd gamma = VectorXcd::Zero(num_robots);

		for (int i = 0; i < num_robots; i++)
		{
			if (abs(eigenvalues(i)) > abs(max_eigenvalue))
			{
				max_eigenvalue = eigenvalues(i);
				gamma = eigenvectors.col(i);
			}
		}

		//ROS_WARN_STREAM("Max Eigenvalue:"  << max_eigenvalue );
		ROS_WARN_STREAM("Gamma:"  << gamma );

		MatrixXcd inv_gamma1 = MatrixXcd::Zero(num_robots, num_robots);
		MatrixXcd inv_gamma2 = MatrixXcd::Zero(num_robots, num_robots);
		MatrixXcd inv_gamma1m = MatrixXcd::Zero(num_robots, num_robots);;
		MatrixXcd inv_gamma2m = MatrixXcd::Zero(num_robots, num_robots);;

		double control_input_CC_x[num_robots];
		double control_input_CC_y[num_robots];

		for (int i = 0; i < num_robots; i++)
		{
			VectorXcd gammai = VectorXcd::Zero(num_robots);
			for (int m = 0; m < num_robots; m++)
			{
				gammai(m) = abs(gamma(i));
			}

			//ROS_WARN_STREAM("gammai:"  << gammai );

			inv_gamma1.row(i) = gamma.transpose()/gamma(i);

			//ROS_WARN_STREAM("inv_gamma1.row(" << i << "):"  << inv_gamma1.row(i) );

			VectorXcd temp_inv_gamma2 = VectorXcd::Zero(num_robots);
			for (int n = 0; n < num_robots; n++)
			{
				temp_inv_gamma2(n) = 1/gamma(n).real();
			}
			inv_gamma2.row(i) = gamma(i)*temp_inv_gamma2.transpose();

			//ROS_WARN_STREAM("inv_gamma2.row(" << i << "):"  << inv_gamma2.row(i) );

			double upper_bound_g = 1000.0;
			double lower_bound_g = 1.1;
			for (int k = 0; k < num_robots; k++)
			{
				double gain_param = inv_gamma1(i,k).real();
				if (gain_param > lower_bound_g && gain_param < upper_bound_g)
				{
					inv_gamma1m(i,k) = pow(pow(gain_param,2)-pow(lower_bound_g,2), 2)/pow(pow(gain_param,2) - pow(upper_bound_g,2),2);
				}

				gain_param = inv_gamma2(i,k).real();
				if (gain_param > lower_bound_g && gain_param < upper_bound_g)
				{
					inv_gamma2m(i,k) = pow(pow(gain_param,2)-pow(lower_bound_g,2), 2)/pow(pow(gain_param,2) - pow(upper_bound_g,2),2);
				}
			}

			//ROS_WARN_STREAM("inv_gamma1m.row(" << i << "):"  << inv_gamma1m.row(i) );
			//ROS_WARN_STREAM("inv_gamma2m.row(" << i << "):"  << inv_gamma2m.row(i) );

			set<int> neighbors_out;
			set<int> neighbors_in;
			for (int j = 0; j < num_robots; j++)
			{
				if (adjacency_matrix(i,j) > 0)
					neighbors_out.insert(j);
				if (adjacency_matrix(j,i) > 0)
					neighbors_in.insert(j);
			}
			vector<int> neighbors(num_robots*2);
			vector<int>::iterator it = set_union(neighbors_out.begin(), neighbors_out.end(),
													neighbors_in.begin(), neighbors_in.end(),
														neighbors.begin());
			neighbors.resize(it - neighbors.begin());

			int Ni = neighbors.size();
			////////////////////////////////////////////
			std::stringstream ss1;
			for (std::set<int>::iterator it = neighbors_out.begin(); it != neighbors_out.end(); ++it)
			{
				ss1 << ' ' << *it;
			}
			//ROS_WARN_STREAM("neighbors_out:"  << ss1.str() );

			std::stringstream ss2;
			for (std::set<int>::iterator it = neighbors_in.begin(); it != neighbors_in.end(); ++it)
			{
				ss2 << ' ' << *it;
			}
			//ROS_WARN_STREAM("neighbors_in:"  << ss2.str() );

			std::stringstream ss3;
			for (std::vector<int>::iterator it = neighbors.begin(); it != neighbors.end(); ++it)
			{
				ss3 << ' ' << *it;
			}
			//ROS_WARN_STREAM("neighbors:"  << ss3.str() );
			/////////////////////////////////////
			double sumx = 0.0;
			double sumy = 0.0;

			for (int k = 0; k < Ni; k++)
			{
				sumx = sumx + (-1)*( abs(inv_gamma1m(i,neighbors[k]) ) + abs(inv_gamma2m(i,neighbors[k]) )  ) * (vicon[robot_names[i]].x() - vicon[robot_names[neighbors[k]]].x());
				sumy = sumy + (-1)*( abs(inv_gamma1m(i,neighbors[k]) ) + abs(inv_gamma2m(i,neighbors[k]) )  ) * (vicon[robot_names[i]].y() - vicon[robot_names[neighbors[k]]].y());
			}

			control_input_CC_x[i] = sumx;
			control_input_CC_y[i] = sumy;

		}

		// P Matrix
		if (num_robots == 6)
		{
			P << 	1/sqrt(2), 		-1/sqrt(6.0),		-1/2.0/sqrt(3), 	-1/2.0/sqrt(5), 	-1/sqrt(30),
					0.0,       		sqrt(2.0/3), 		-1/2.0/sqrt(3), 	-1/2.0/sqrt(5), 	-1/sqrt(30),
					0.0, 		 	0.0, 				sqrt(3.0)/2,    	-1/(2*sqrt(5)),		-1/sqrt(30),
					0.0, 			0.0, 				0.0, 				2/sqrt(5), 			-1/sqrt(30),
					0.0, 			0.0, 				0.0,				0.0, 				sqrt(5.0/6),
					-1/sqrt(2), 	-1/sqrt(6),			-1/2.0/sqrt(3), 	-1/2.0/sqrt(5), 	-1/sqrt(30);
		} else if (num_robots == 4) {
			P << 	-0.5,        		-0.5,              	-0.5,
					 0.833333333333333, -0.166666666666667, -0.166666666666667,
					-0.166666666666667,  0.833333333333333, -0.166666666666667,
					-0.166666666666667, -0.166666666666667,  0.833333333333333;
		}
		// M Matrix
		MatrixXd M = P.transpose() * L * P;
		//std::cerr << "adjacency_matrix"  << adjacency_matrix  ;
		// dLdx and dLdy for each robot
		MatrixXd dLdx[num_robots];
		MatrixXd dLdy[num_robots];

		for (int i = 0; i < num_robots; i++)
		{
			dLdx[i] = MatrixXd::Zero(num_robots, num_robots);
			dLdy[i] = MatrixXd::Zero(num_robots, num_robots);
		}

		for(int k = 0; k < num_robots; k++)
		{
			for(int i = 0; i < num_robots; i++)
			{
				for (int j = i + 1; j < num_robots; j++)
				{
					double D = sqrt(
					pow(vicon[robot_names[i]].x() - vicon[robot_names[j]].x(), 2) +
					pow(vicon[robot_names[i]].y() - vicon[robot_names[j]].y(), 2)
					);

					double dAdD = psiPrime(D);

					if( k == i ) {
						double dDdx = (vicon[robot_names[i]].x()-vicon[robot_names[j]].x()) / D;
						double dDdy = (vicon[robot_names[i]].y()-vicon[robot_names[j]].y()) / D;

						dLdx[k](i,j) = dAdD*dDdx;
						dLdy[k](i,j) = dAdD*dDdy;

					}

					if( k == j ) {
						double dDdx = -(vicon[robot_names[i]].x()-vicon[robot_names[j]].x()) /D;
						double dDdy = -(vicon[robot_names[i]].y()-vicon[robot_names[j]].y()) / D;

						dLdx[k](i,j) = dAdD*dDdx;
						dLdy[k](i,j) = dAdD*dDdy;

					}

					dLdx[k](j,i) = dLdx[k](i,j);
					dLdy[k](j,i) = dLdy[k](i,j);

				}
			}

			for(int i = 0; i < num_robots; i++)
			{
				double sumx = 0;
				double sumy = 0;
				for(int j = 0; j<num_robots; j++)
				{
					sumx += dLdx[k](i,j);
					sumy += dLdy[k](i,j);
				}
				dLdx[k](i,i) = -sumx;
	            dLdy[k](i,i) = -sumy;
			}
		}

		// dMdx and dMdy for each robot
		MatrixXd dMdx[num_robots];
		MatrixXd dMdy[num_robots];

		for (int i = 0; i < num_robots; i++)
		{
			dMdx[i] = P.transpose() * dLdx[i] * P;
			dMdy[i] = P.transpose() * dLdy[i] * P;
			//std:cerr << dMdx[i]  ;
		}

		// Collision Avoidance for each robot starts here:
		MatrixXd CAMatrix_x = MatrixXd::Zero(num_robots,num_robots);
		MatrixXd CAMatrix_y = MatrixXd::Zero(num_robots,num_robots);

		for(int i = 0; i < num_robots; i++)
		{
			for (int j = i + 1; j < num_robots; j++)
			{
				double D = sqrt(
				pow(vicon[robot_names[i]].x() - vicon[robot_names[j]].x(), 2) +
				pow(vicon[robot_names[i]].y() - vicon[robot_names[j]].y(), 2)
				);
				if( D < R && D > r){
					CAMatrix_x(i,j) = -4*(pow(R,2)-pow(r,2))
										*(pow(D,2)-pow(R,2))
										/pow((pow(D,2)-pow(r,2)),3)
										*(vicon[robot_names[i]].x()
										- vicon[robot_names[j]].x());

		        	CAMatrix_y(i,j) = -4*(pow(R,2)-pow(r,2))
		        						*(pow(D,2)-pow(R,2))
		        						/pow((pow(D,2)-pow(r,2)),3)
		        						*(vicon[robot_names[i]].y()
		        						- vicon[robot_names[j]].y());
            	} else {
            		CAMatrix_x(i,j) = 0;
            		CAMatrix_y(i,j) = 0;
            	}

            	CAMatrix_x(j,i) = -CAMatrix_x(i,j);
	       		CAMatrix_y(j,i) = -CAMatrix_y(i,j);
			}
		}

		double control_input_CA_x[num_robots];
		double control_input_CA_y[num_robots];

		for(int i = 0; i < num_robots; i++)
		{
			double sumx = 0;
			double sumy = 0;
			for (int j = 0; j < num_robots; j++)
			{
				sumx = sumx + CAMatrix_x(i,j);
				sumy = sumy + CAMatrix_y(i,j);
			}
			control_input_CA_x[i] = sumx;
			control_input_CA_y[i] = sumy;
		}

		double control_input_FC_x[num_robots];
		double control_input_FC_y[num_robots];

		for (int i = 0; i < num_robots; i++)
		{
			control_input_FC_x[i] = formation_x[i] - vicon[robot_names[i]].x();
			control_input_FC_y[i] = formation_y[i] - vicon[robot_names[i]].y();
		}

		// Trace of Mx and My for each robot
		double trace_Mx[num_robots];
		double trace_My[num_robots];

		for (int i = 0; i < num_robots; i++)
		{
			//std::cerr << (M.inverse()*dMdx[i])  ;
			trace_Mx[i] = (M.inverse()*dMdx[i]).trace();
			trace_My[i] = (M.inverse()*dMdy[i]).trace();
		}

		double determinant_M = M.determinant();

		// Send msg

		connectivity_controller::EigenConnectivityMsgs msg;

		msg.trMx.assign(trace_Mx, trace_Mx + num_robots);
		msg.trMy.assign(trace_My, trace_My + num_robots);

		msg.u_cc_x.assign(control_input_CC_x, control_input_CC_x + num_robots);
		msg.u_cc_y.assign(control_input_CC_y, control_input_CC_y + num_robots);

		msg.u_ca_x.assign(control_input_CA_x, control_input_CA_x + num_robots);
		msg.u_ca_y.assign(control_input_CA_y, control_input_CA_y + num_robots);

		msg.u_fc_x.assign(control_input_FC_x, control_input_FC_x + num_robots);
		msg.u_fc_y.assign(control_input_FC_y, control_input_FC_y + num_robots);
		msg.detM = determinant_M;

		pub.publish(msg);

		// Sleep
		loop_rate.sleep();
	}

	return 0;
}