////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2021 Andreas Wagner.
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
////////////////////////////////////////////////////////////////////////////////

#ifndef TUMORMODELS_IMPLICIT_LINEAR_FLOW_SOLVER_HPP
#define TUMORMODELS_IMPLICIT_LINEAR_FLOW_SOLVER_HPP

#include "mpi.h"
#include <Eigen/Dense>
#include <memory>
#include <vector>

namespace macrocirculation {

// forward declarations
class GraphStorage;
class DofMap;
class PetscVec;
class PetscMat;
class PetscKsp;
class FETypeNetwork;
class Point;
class LocalEdgeDofMap;
class Edge;
class Vertex;

void assemble_mass(MPI_Comm comm, const GraphStorage &graph, const DofMap &dof_map, PetscVec &mass_vec);

void assemble_mass(MPI_Comm comm,
                   const std::vector< std::shared_ptr< GraphStorage > > &graph,
                   const std::vector< std::shared_ptr< DofMap  > > &dof_map,
                   PetscVec &mass_vec);

// TODO: Move somewhere else!!!
// TODO: Clean up from implicit advection solver
void interpolate_to_vertices(const MPI_Comm comm,
                             const GraphStorage &graph,
                             const DofMap &map,
                             const std::size_t component,
                             const PetscVec &dof_vector,
                             std::vector<Point> &points,
                             std::vector<double> &interpolated);

/*! @brief An implicit euler for the  linear flow equations. */
class ImplicitLinearFlowSolver {
public:
  ImplicitLinearFlowSolver(MPI_Comm comm, std::shared_ptr<GraphStorage> graph, std::shared_ptr<DofMap> dof_map, size_t degree);

  /*! @brief The component index of the pressure p inside the dof map.
   *         This can be used inside a local dof map to get the dof indices for a single component. */
  static const size_t p_component = 0;

  /*! @brief The component index of the flow q inside the dof map.
   *         This can be used inside a local dof map to get the dof indices for a single component. */
  static const size_t q_component = 1;

  const DofMap &get_dof_map() const;

  const PetscVec &get_solution() const { return *u; }

  /*! @brief Requests the Jacobi preconditioner for the linear solver, which yields (more) reproducible results for unit-testing. */
  void use_pc_jacobi();

  /*! @brief Requests the Jacobi preconditioner for the linear solver, which yields (more) reproducible results for unit-testing. */
  void use_named_solver(const std::string & name);

  /*! @brief Sets the initial constant value of p and q. */
  void set_initial_value(double p, double q);

  /*! @brief Sets up the matrices for the given time step size tau. */
  void setup(double tau);

  /*! @brief Solves the system. For the _next_ time step t.
   *
   * @param tau The current time step width.
   * @param t The (future) time for the next time step. */
  void solve(double tau, double t);

  /*! @brief Assembles the left-hand-side matrix for the given time step. */
  void assemble_matrix(double tau);

  /*! @brief Assembles the right-hand-side vectors for the given time step at the given time. */
  void assemble_rhs(double tau, double t);

  /*! @brief Assembles matrix and right-hand-side. */
  void assemble(double tau, double t);

  static double get_C(const Edge &e);

  static double get_L(const Edge &e);

  static double get_R(const Edge &e);

  /*! @brief Returns the current p and q values at a given vertex. */
  void get_1d_pq_values_at_vertex(const Vertex &v, double &p, double &q) const;

  /*! @brief Evaluates p and q of the current solution on the edge e parametrized on [0, 1] at \f$ s \in [0,1] \f$. */
  void evaluate_1d_pq_values(const Edge &e, double s, double &p, double &q) const;

  /*! @brief Returns the degree of the finite element basis functions. */
  size_t get_degree() const;

  void applySlopeLimiter();

  PetscKsp& get_solver();

private:
  void assemble_matrix_cells(double tau);

  void assemble_matrix_inner_boundaries(double tau);

  void assemble_rhs_inflow(double tau, double t);

  void assemble_matrix_inflow(double tau);

  void assemble_matrix_free_outflow(double tau);

  void assemble_matrix_nfurcations(double tau);

  void assemble_rhs_cells();

  void assemble_matrix_0d_model(double tau);

  void assemble_matrix_rcl_model(double tau);

  void assemble_rhs_0d_model(double tau);

  void assemble_rhs_rcl_model(double tau);

  void assemble_matrix_characteristic(double tau);

  void assemble_rhs_characteristic(double tau);

private:
  MPI_Comm d_comm;

  std::shared_ptr<GraphStorage> d_graph;

  std::shared_ptr<DofMap> d_dof_map;

  size_t degree;

  double d_tau;

  std::shared_ptr<PetscVec> u;
  std::shared_ptr<PetscVec> rhs;
  std::shared_ptr<PetscMat> A;

  std::shared_ptr<PetscVec> mass;

  std::shared_ptr<PetscKsp> linear_solver;
};

} // namespace macrocirculation

#endif //TUMORMODELS_IMPLICIT_LINEAR_FLOW_SOLVER_HPP


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/detail/enforce_precision.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( round2even_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      detail::enforce_precision<A0> enforcer;

      const A0 v = bs::abs(a0);
      const A0 t2n = Twotonmb<A0>();
      A0 d0 = (v+t2n);
      A0 d = (d0-t2n);
      d = (v < t2n)?d:v;
      return a0 < Zero<A0>() ? -d : d;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( round2even_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };
} } }

#endif


#include <iostream>
#include <SFML/Graphics.hpp>
#include <boost/random.hpp>
#include <boost/random/beta_distribution.hpp>
#include <thread>
#include <chrono>
using namespace std::chrono;

typedef boost::random::mt19937 gen_type;
typedef boost::random::beta_distribution<> beta_dist_type;
typedef boost::random::uniform_real_distribution<> uniform_dist_type;
typedef boost::random::variate_generator<gen_type&, beta_dist_type> beta_gen_type;
typedef boost::random::variate_generator<gen_type&, uniform_dist_type> uniform_gen_type;

// values for median = 0.33 for beta dist
const float alpha = 0.797341459119764;
const float beta = 2;

const int max = 100;
const int min = 10;
int rotation_counter = 0;
int time_to_run = 0;
// Hz
double angular_speed;

milliseconds ms = duration_cast<milliseconds>(system_clock::now().time_since_epoch());
gen_type random_generator(ms.count());
uniform_dist_type uniform_dist(5, 20);
uniform_gen_type random_uniform_generator(random_generator, uniform_dist);
beta_dist_type beta_dist(alpha, beta);
beta_gen_type random_beta_generator(random_generator, beta_dist);

double get_random_beta()
{
    return (random_beta_generator() * (max - min)) + min;
}

double get_random_uniform()
{
    return random_uniform_generator();
}

void print_help()
{
    std::cout << "Usage: -t [seconds to run]" << std::endl;
}

bool parse(int argc, char* argv[])
{
    if (argc == 1)
    {
        print_help();
        return false;
    }

    for (int i = 1; i < argc; i++)
    {
        if (std::string(argv[i]) == "-t")
        {
            time_to_run = std::atoi(argv[++i]);
        }
        if (std::string(argv[i]) == "-h")
        {
            print_help();
            return false;
        }
    }

    return true;
}

void  crash()
{
    uniform_dist_type uniform_dist_crash(1, 100);
    uniform_gen_type crash_gen(random_generator, uniform_dist_crash);
    int val = crash_gen();
    if (val <= 20)
    {
        std::cout << "It's a feature!" << std::endl;
        *((int*)0xABCD000000000000) = 5;
    }
}

int main(int argc, char* argv[])
{
    if (!parse(argc, argv))
    {
        return 1;
    }

    crash();
    angular_speed = get_random_uniform();
    sf::RenderWindow window(sf::VideoMode(200, 200), "Rectangle");
    sf::RectangleShape shape(sf::Vector2f(50, 100));
    shape.setFillColor(sf::Color::Green);
    shape.setOrigin(25, 50);
    shape.setPosition(100, 100);
    sf::Clock frame_clock;
    sf::Clock program_clock;

    while (program_clock.getElapsedTime().asSeconds() < time_to_run)
    {
        float delay = get_random_beta();
        sf::Event event;
        while (window.pollEvent(event))
        {
            if (event.type == sf::Event::Closed)
                window.close();
        }
        window.clear();
        while (frame_clock.getElapsedTime().asSeconds() < delay/1000)
        {
            std::this_thread::sleep_for(std::chrono::microseconds(10));
        }
        std::cout << "dt: " << delay << " r: "<< shape.getRotation() << std::endl;
        if (shape.getRotation() + angular_speed >= 360)
        {
            rotation_counter++;
        }
        shape.rotate(angular_speed);
        window.draw(shape);
        window.display();
        frame_clock.restart();
    }

    window.close();
    std::cout << "Rotations: " << rotation_counter << std::endl;
    return 0;
}


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

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

namespace model {

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

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

} // namespace model


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

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

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

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

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

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

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

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

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

      D = D + (fi * internal_vector);
    }

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

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

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

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

OptimalUPnPFunctionInfo::~OptimalUPnPFunctionInfo(){};

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

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

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


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

  @copyright 2015 NumScale SAS

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

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

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

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

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

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

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

#endif


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

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

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

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

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

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

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

#include <iostream>

#include <fstream>

#include <time.h>

#include <sys/resource.h>

#include <math.h>

#include <float.h>

#include <NTL/ZZ.h>

#include <NTL/RR.h>

#include <NTL/mat_ZZ.h>

#include <NTL/LLL.h>

using namespace std;
using namespace NTL;

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

ZZ lambda, modul, seed;

struct {

  ZZ lambda;

  double min_norm_x_x;

} best;

#define MAX_DIMS 24

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

double fact[MAX_DIMS];

RR fact_RR[MAX_DIMS];

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

const ZZ n_min=to_ZZ(1);

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

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

   Call the function with:

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

   Example:

        lll_esearch "output-file" 340282366920938463463374607431768211456 3

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

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

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

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

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

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

    modul = to_ZZ(argv[2]);

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

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

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

    out_file << modul;

    //out_file << seed;

    out_file << " " << blen;

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

    out_file << endl;

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

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

#if 0

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

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

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

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

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

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

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

#else

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

  fact_RR[1] = to_RR(0.2886751345948128822545744);

  fact_RR[2] = to_RR(0.1767766952966368811002111);

  fact_RR[3] = to_RR(0.125);

  fact_RR[4] = to_RR(0.0883883476483184405501055);

  fact_RR[5] = to_RR(0.0721687836487032205636436);

  fact_RR[6] = to_RR(0.0625);

  fact_RR[7] = to_RR(0.0625);

  fact_RR[8] = to_RR(0.06007);

  fact_RR[9] = to_RR(0.05953);

  fact_RR[10] = to_RR(0.06136);

  fact_RR[11] = to_RR(0.06559);

  fact_RR[12] = to_RR(0.07253);

  fact_RR[13] = to_RR(0.08278);

  fact_RR[14] = to_RR(0.09735);

  fact_RR[15] = to_RR(0.11774);

  fact_RR[16] = to_RR(0.14624);

  fact_RR[17] = to_RR(0.18629);

  fact_RR[18] = to_RR(0.24308);

  fact_RR[19] = to_RR(0.32454);

  fact_RR[20] = to_RR(0.44289);

  fact_RR[21] = to_RR(0.61722);

  fact_RR[22] = to_RR(0.87767);

  fact_RR[23] = to_RR(1.27241);

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

#endif

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

    best.lambda = to_ZZ(0);

    best.min_norm_x_x = 0.0;

    ZZ t_h = to_ZZ(10);

    struct rusage cur_ru;

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

        lambda = RandomBits_ZZ(blen) * 4 + 1;

        mat_ZZ x;

        x.SetDims(x_xmax, x_xmax);

        min_norm_x_x = 1.0;

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

            x.SetDims(j,j);

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

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

                x[i][i] = 1;

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

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

            ZZ det, rg;

            rg = LLL(det, x, 0);

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

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

                if (min_x_x > x_x)

                    min_x_x = x_x;
            }

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

            if (min_norm_x_x > norm_x_x)

                min_norm_x_x = norm_x_x;
        }

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

            best.lambda = lambda;

            getrusage(RUSAGE_SELF, &cur_ru);

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

        ZZ n_ = n - n_min;

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

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

            t_h *= 10;
        }
    }

    getrusage(RUSAGE_SELF, &cur_ru);

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

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

    out_file.close();

}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_FUNCTIONAL_MATH_HPP
#define BOOST_COMPUTE_FUNCTIONAL_MATH_HPP

#include <boost/compute/functional/detail/macros.hpp>

namespace boost {
namespace compute {

BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(acos, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(acosh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(acospi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(asin, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(asinh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(asinpi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(atan, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(atan2, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(atanh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(atanpi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(atan2pi, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(cbrt, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(ceil, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(copysign, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(cos, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(cosh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(cospi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(erf, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(erfc, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(exp, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(exp2, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(exp10, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(expm1, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fabs, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fdim, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(floor, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fma, T (T, T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fmax, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fmin, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fmod, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(hypot, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(ilogb, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(lgamma, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(log, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(log2, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(log10, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(log1p, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(logb, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(mad, T (T, T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(nextafter, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(pow, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(pown, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(powr, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(remainder, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(rint, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(rootn, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(round, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(rsqrt, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(sin, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(sinh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(sinpi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(sqrt, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(tan, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(tanh, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(tanpi, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(tgamma, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(trunc, T (T), class T)

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_FUNCTIONAL_MATH_HPP


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

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

namespace tudat
{
namespace gravitation
{

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

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

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

        break;

    case mercury:

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

        break;

    case venus:

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

        break;

    case earth:

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

        break;

    case moon:

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

        break;

    case mars:

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

        break;

    case jupiter:

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

        break;

    case saturn:

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

        break;

    case uranus:

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

        break;

    case neptune:

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

        break;

    default:

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

} // namespace gravitation
} // namespace tudat


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

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

#include <functional>

namespace wavepi {
namespace forward {

using namespace dealii;

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

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

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

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

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

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

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

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);
}

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);

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

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

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

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

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);

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

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

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

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);

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

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

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

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);

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

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

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

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);
}

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

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

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);
}

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);

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

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

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

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

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

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);

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

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

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

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);

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

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

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

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);

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

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

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

      val_next = 1.0 / val_next;

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

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

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

   cell->get_dof_indices(copy_data.local_dof_indices);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


#ifndef PYTHONIC_INCLUDE_NUMPY_TANH_HPP
#define PYTHONIC_INCLUDE_NUMPY_TANH_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/tanh.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    auto tanh(T const &v) -> decltype(boost::simd::tanh(v))
    {
      return boost::simd::tanh(v);
    }
    template <class T>
    std::complex<T> tanh(std::complex<T> const &v)
    {
      return std::tanh(v);
    }
  }
#define NUMPY_NARY_FUNC_NAME tanh
#define NUMPY_NARY_FUNC_SYM wrapper::tanh
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_DDP_HPP_
#define CROCODDYL_CORE_SOLVERS_DDP_HPP_

#include <Eigen/Cholesky>
#include <vector>
#include "crocoddyl/core/solver-base.hpp"

namespace crocoddyl {

class SolverDDP : public SolverAbstract {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  explicit SolverDDP(ShootingProblem& problem);
  ~SolverDDP();

  bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
             const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, unsigned int const& maxiter = 100,
             const bool& is_feasible = false, const double& regInit = 1e-9);
  void computeDirection(const bool& recalc = true);
  double tryStep(const double& steplength = 1);
  double stoppingCriteria();
  const Eigen::Vector2d& expectedImprovement();
  double calc();
  void backwardPass();
  void forwardPass(const double& stepLength);

  const std::vector<Eigen::MatrixXd>& get_Vxx() const;
  const std::vector<Eigen::VectorXd>& get_Vx() const;
  const std::vector<Eigen::MatrixXd>& get_Qxx() const;
  const std::vector<Eigen::MatrixXd>& get_Qxu() const;
  const std::vector<Eigen::MatrixXd>& get_Quu() const;
  const std::vector<Eigen::VectorXd>& get_Qx() const;
  const std::vector<Eigen::VectorXd>& get_Qu() const;
  const std::vector<Eigen::MatrixXd>& get_K() const;
  const std::vector<Eigen::VectorXd>& get_k() const;
  const std::vector<Eigen::VectorXd>& get_gaps() const;

 private:
  void computeGains(unsigned int const& t);
  void increaseRegularization();
  void decreaseRegularization();
  void allocateData();

 protected:
  double regfactor_;
  double regmin_;
  double regmax_;

  double cost_try_;
  std::vector<Eigen::VectorXd> xs_try_;
  std::vector<Eigen::VectorXd> us_try_;
  std::vector<Eigen::VectorXd> dx_;

  // allocate data
  std::vector<Eigen::MatrixXd> Vxx_;
  std::vector<Eigen::VectorXd> Vx_;
  std::vector<Eigen::MatrixXd> Qxx_;
  std::vector<Eigen::MatrixXd> Qxu_;
  std::vector<Eigen::MatrixXd> Quu_;
  std::vector<Eigen::VectorXd> Qx_;
  std::vector<Eigen::VectorXd> Qu_;
  std::vector<Eigen::MatrixXd> K_;
  std::vector<Eigen::VectorXd> k_;
  std::vector<Eigen::VectorXd> gaps_;

 private:
  Eigen::VectorXd xnext_;
  Eigen::VectorXd x_reg_;
  Eigen::MatrixXd FxTVxx_p_;
  std::vector<Eigen::MatrixXd> FuTVxx_p_;
  Eigen::VectorXd fTVxx_p_;
  std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;
  std::vector<Eigen::VectorXd> Quuk_;
  std::vector<double> alphas_;
  double th_grad_;
  double th_step_;
  bool was_feasible_;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_DDP_HPP_


#include <tiny_precision.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(tiny_precision);

BOOST_AUTO_TEST_CASE(value_tests)
{
  double value1  = tiny::machine_precision<double>();
  BOOST_CHECK_CLOSE( value1, 2.2204460492503131e-16, 0.01 );

  float  value2  = tiny::machine_precision<float>();
  BOOST_CHECK_CLOSE( value2, 1.1920929e-07f, 0.01f );
}

BOOST_AUTO_TEST_SUITE_END();


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

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/assign.hpp>
#include <boost/test/unit_test.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/volatility/optionlet/strippedoptionlet.hpp>
#include <ql/time/calendars/unitedstates.hpp>
#include <qle/math/flatextrapolation.hpp>
#include <qle/termstructures/strippedoptionletadapter.hpp>
#include <test/toplevelfixture.hpp>

using namespace QuantExt;
using namespace QuantLib;
using namespace boost::unit_test_framework;

using boost::assign::list_of;
using qle::test::TopLevelFixture;
using std::vector;

namespace {

// Fixture to create an optionlet surface that is used in the tests
class F : public TopLevelFixture {
public:
    Date asof;
    vector<Date> expiries;
    vector<Rate> strikes;
    DayCounter dc;
    vector<vector<Handle<Quote> > > vols;
    boost::shared_ptr<StrippedOptionlet> optionletSurface;

    F() : expiries(2), strikes(2), dc(Actual365Fixed()), vols(2) {

        // Set the evaluation date
        asof = Date(17, Apr, 2019);
        Settings::instance().evaluationDate() = asof;

        // Inputs for the optionlet surface
        Natural settlementDays = 2;
        UnitedStates calendar;
        BusinessDayConvention bdc = Following;
        boost::shared_ptr<IborIndex> dummyIborIndex;
        VolatilityType type = Normal;

        expiries[0] = Date(17, Apr, 2020);
        expiries[1] = Date(19, Apr, 2021);

        strikes[0] = 0.02;
        strikes[1] = 0.04;

        // clang-format off
        vols[0].push_back(Handle<Quote>(boost::make_shared<SimpleQuote>(0.0091)));
        vols[0].push_back(Handle<Quote>(boost::make_shared<SimpleQuote>(0.0092)));
        vols[1].push_back(Handle<Quote>(boost::make_shared<SimpleQuote>(0.0070)));
        vols[1].push_back(Handle<Quote>(boost::make_shared<SimpleQuote>(0.0088)));
        // clang-format on

        // Create the optionlet surface
        optionletSurface = boost::make_shared<StrippedOptionlet>(settlementDays, calendar, bdc, dummyIborIndex,
                                                                 expiries, strikes, vols, dc, type);
    }

    ~F() {}
};

} // namespace
BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, TopLevelFixture)

BOOST_AUTO_TEST_SUITE(StrippedOptionletAdapterTests)

BOOST_FIXTURE_TEST_CASE(testFlatExtrapAfterLastExpiry, F) {

    // Set up optionlet adapter with flat extrapolation
    boost::shared_ptr<StrippedOptionletAdapter<LinearFlat, LinearFlat> > adapter =
        boost::make_shared<StrippedOptionletAdapter<LinearFlat, LinearFlat> >(optionletSurface);

    // Pick a date 1Y after the max date
    Date testDate = expiries.back() + 1 * Years;

    // Check flat extrapolation on the pillar strikes
    for (Size i = 0; i < strikes.size(); i++) {
        Volatility testVol = adapter->volatility(testDate, strikes[i], true);
        BOOST_CHECK_CLOSE(testVol, vols.back()[i]->value(), 1e-12);
    }

    // Check flat extrapolation below first strike
    Volatility testVol = adapter->volatility(testDate, strikes.front() / 2.0, true);
    BOOST_CHECK_CLOSE(testVol, vols.back().front()->value(), 1e-12);

    // Check flat extrapolation above last strike
    testVol = adapter->volatility(testDate, 2.0 * strikes.back(), true);
    BOOST_CHECK_CLOSE(testVol, vols.back().back()->value(), 1e-12);

    // Check flat extrapolation between two strikes
    Rate avgStrike = (strikes[0] + strikes[1]) / 2.0;
    Volatility expectedVol = (vols.back()[0]->value() + vols.back()[1]->value()) / 2.0;
    testVol = adapter->volatility(testDate, avgStrike, true);
    BOOST_CHECK_CLOSE(testVol, expectedVol, 1e-12);
}

BOOST_FIXTURE_TEST_CASE(testFlatExtrapBetweenFirstLastExpiry, F) {

    // Set up optionlet adapter with flat extrapolation
    boost::shared_ptr<StrippedOptionletAdapter<LinearFlat, LinearFlat> > adapter =
        boost::make_shared<StrippedOptionletAdapter<LinearFlat, LinearFlat> >(optionletSurface);

    // Check flat extrapolation on expiry pillars
    for (Size i = 0; i < expiries.size(); i++) {
        // Below first strike
        Volatility testVol = adapter->volatility(expiries[i], strikes.front() / 2.0, true);
        BOOST_CHECK_CLOSE(testVol, vols[i].front()->value(), 1e-12);
        // Above last strike
        testVol = adapter->volatility(expiries[i], 2.0 * strikes.back(), true);
        BOOST_CHECK_CLOSE(testVol, vols[i].back()->value(), 1e-12);
    }

    // Pick a date between first expiry and last expiry
    Size numDays = dc.dayCount(expiries.front(), expiries.back()) / 2;
    Date testDate = expiries.front() + numDays * Days;

    // Check flat extrapolation below first strike
    Volatility testVol = adapter->volatility(testDate, strikes.front() / 2.0, true);
    Volatility expectedVol = adapter->volatility(testDate, strikes.front(), true);
    BOOST_CHECK_CLOSE(testVol, expectedVol, 1e-12);

    // Check flat extrapolation above last strike
    testVol = adapter->volatility(testDate, 2.0 * strikes.back(), true);
    expectedVol = adapter->volatility(testDate, strikes.back(), true);
    BOOST_CHECK_CLOSE(testVol, expectedVol, 1e-12);
}

BOOST_FIXTURE_TEST_CASE(testFlatExtrapBeforeFirstExpiry, F) {

    // Set up optionlet adapter with flat extrapolation
    boost::shared_ptr<StrippedOptionletAdapter<LinearFlat, LinearFlat> > adapter =
        boost::make_shared<StrippedOptionletAdapter<LinearFlat, LinearFlat> >(optionletSurface);

    // Pick a date between evaluation date and first expiry
    Size numDays = dc.dayCount(asof, expiries.front()) / 2;
    Date testDate = asof + numDays * Days;

    // Check flat extrapolation on the pillar strikes
    for (Size i = 0; i < strikes.size(); i++) {
        Volatility testVol = adapter->volatility(testDate, strikes[i], true);
        BOOST_CHECK_CLOSE(testVol, vols.front()[i]->value(), 1e-12);
    }

    // Check flat extrapolation below first strike
    Volatility testVol = adapter->volatility(testDate, strikes.front() / 2.0, true);
    BOOST_CHECK_CLOSE(testVol, vols.front().front()->value(), 1e-12);

    // Check flat extrapolation above last strike
    testVol = adapter->volatility(testDate, 2.0 * strikes.back(), true);
    BOOST_CHECK_CLOSE(testVol, vols.front().back()->value(), 1e-12);

    // Check flat extrapolation between two strikes
    Rate avgStrike = (strikes[0] + strikes[1]) / 2.0;
    Volatility expectedVol = (vols.front()[0]->value() + vols.front()[1]->value()) / 2.0;
    testVol = adapter->volatility(testDate, avgStrike, true);
    BOOST_CHECK_CLOSE(testVol, expectedVol, 1e-12);
}

BOOST_AUTO_TEST_CASE(testOneStrikeColumn) {

    BOOST_TEST_MESSAGE("Testing StrippedOptionletAdapter when StrippedOptionlet has one strike column");

    // As of date
    Date asof(17, Apr, 2019);
    Settings::instance().evaluationDate() = asof;

    // Set up the stripped optionlet surface with one strike column
    Natural settlementDays = 2;
    UnitedStates calendar;
    BusinessDayConvention bdc = Following;
    Actual365Fixed dc;
    boost::shared_ptr<IborIndex> dummyIborIndex;
    VolatilityType type = Normal;
    vector<Date> expiries = list_of(Date(17, Jul, 2019))(Date(17, Oct, 2019));
    vector<Rate> strikes = list_of(0.0);

    vector<vector<Handle<Quote> > > vols(2);
    vols[0].push_back(Handle<Quote>(boost::make_shared<SimpleQuote>(0.0091)));
    vols[1].push_back(Handle<Quote>(boost::make_shared<SimpleQuote>(0.0070)));

    // Create the optionlet surface
    boost::shared_ptr<StrippedOptionlet> optionletSurface = boost::make_shared<StrippedOptionlet>(
        settlementDays, calendar, bdc, dummyIborIndex, expiries, strikes, vols, dc, type);

    // Set up optionlet adapter with flat extrapolation
    boost::shared_ptr<StrippedOptionletAdapter<LinearFlat, LinearFlat> > adapter =
        boost::make_shared<StrippedOptionletAdapter<LinearFlat, LinearFlat> >(optionletSurface);

    // Ask for vols on expiry dates at any strikes and should get back inputs
    Volatility strikeShift = 0.10;
    Volatility testVol = adapter->volatility(expiries[0], 0.0, true);
    Volatility expectedVol = vols[0][0]->value();
    BOOST_CHECK_CLOSE(testVol, expectedVol, 1e-12);

    testVol = adapter->volatility(expiries[0], 0.0 + strikeShift, true);
    BOOST_CHECK_CLOSE(testVol, expectedVol, 1e-12);

    testVol = adapter->volatility(expiries[0], 0.0 - strikeShift, true);
    BOOST_CHECK_CLOSE(testVol, expectedVol, 1e-12);

    testVol = adapter->volatility(expiries[0] - 1 * Weeks, 0.0, true);
    BOOST_CHECK_CLOSE(testVol, expectedVol, 1e-12);

    testVol = adapter->volatility(expiries[1], 0.0, true);
    expectedVol = vols[1][0]->value();
    BOOST_CHECK_CLOSE(testVol, expectedVol, 1e-12);

    testVol = adapter->volatility(expiries[1] + 1 * Weeks, 0.0, true);
    BOOST_CHECK_CLOSE(testVol, expectedVol, 1e-12);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate  a value equal to 3 times @ref Eps of the chosen type

    @par Header <boost/simd/constant/threeeps.hpp>

    @par Semantic:

    @code
    T r = Threeeps<T>();
    @endcode

    @code
    if T is integral
      r = 3
    else if T is double
      r =   6.661338147750939e-16;
    else if T is float
      r =  3.5762787e-07;
    @endcode

    @return The Threeeps constant for the proper type
  **/
  template<typename T> T Threeeps();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant threeeps.

      @return The Threeeps constant for the proper type
    **/
    Value Threeeps();
  }
} }
#endif

#include <boost/simd/constant/scalar/threeeps.hpp>
#include <boost/simd/constant/simd/threeeps.hpp>

#endif


#include <fstream>
#include <iostream>

#include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>

#include <a_star.hpp>

using libMultiRobotPlanning::AStar;
using libMultiRobotPlanning::Neighbor;
using libMultiRobotPlanning::PlanResult;

struct State {
  State(int x, int y) : x(x), y(y) {}

  State(const State&) = default;
  State(State&&) = default;
  State& operator=(const State&) = default;
  State& operator=(State&&) = default;

  bool operator==(const State& other) const {
    return std::tie(x, y) == std::tie(other.x, other.y);
  }

  friend std::ostream& operator<<(std::ostream& os, const State& s) {
    return os << "(" << s.x << "," << s.y << ")";
  }

  int x;
  int y;
};

namespace std {
template <>
struct hash<State> {
  size_t operator()(const State& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    return seed;
  }
};
}  // namespace std

enum class Action {
  Up,
  Down,
  Left,
  Right,
};

std::ostream& operator<<(std::ostream& os, const Action& a) {
  switch (a) {
    case Action::Up:
      os << "Up";
      break;
    case Action::Down:
      os << "Down";
      break;
    case Action::Left:
      os << "Left";
      break;
    case Action::Right:
      os << "Right";
      break;
  }
  return os;
}

class Environment {
 public:
  Environment(size_t dimx, size_t dimy, std::unordered_set<State> obstacles,
              State goal)
      : m_dimx(dimx),
        m_dimy(dimy),
        m_obstacles(std::move(obstacles)),
        m_goal(std::move(goal))  // NOLINT
  {}

  int admissibleHeuristic(const State& s) {
    return std::abs(s.x - m_goal.x) + std::abs(s.y - m_goal.y);
  }

  bool isSolution(const State& s) { return s == m_goal; }

  void getNeighbors(const State& s,
                    std::vector<Neighbor<State, Action, int> >& neighbors) {
    neighbors.clear();

    State up(s.x, s.y + 1);
    if (stateValid(up)) {
      neighbors.emplace_back(Neighbor<State, Action, int>(up, Action::Up, 1));
    }
    State down(s.x, s.y - 1);
    if (stateValid(down)) {
      neighbors.emplace_back(
          Neighbor<State, Action, int>(down, Action::Down, 1));
    }
    State left(s.x - 1, s.y);
    if (stateValid(left)) {
      neighbors.emplace_back(
          Neighbor<State, Action, int>(left, Action::Left, 1));
    }
    State right(s.x + 1, s.y);
    if (stateValid(right)) {
      neighbors.emplace_back(
          Neighbor<State, Action, int>(right, Action::Right, 1));
    }
  }

  void onExpandNode(const State& /*s*/, int /*fScore*/, int /*gScore*/) {}

  void onDiscover(const State& /*s*/, int /*fScore*/, int /*gScore*/) {}

 public:
  bool stateValid(const State& s) {
    return s.x >= 0 && s.x < m_dimx && s.y >= 0 && s.y < m_dimy &&
           m_obstacles.find(s) == m_obstacles.end();
  }

 private:
  int m_dimx;
  int m_dimy;
  std::unordered_set<State> m_obstacles;
  State m_goal;
};

int main(int argc, char* argv[]) {
  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");
  int startX, startY, goalX, goalY;
  std::string mapFile;
  std::string outputFile;
  desc.add_options()("help", "produce help message")(
      "startX", po::value<int>(&startX)->required(),
      "start position x-component")("startY",
                                    po::value<int>(&startY)->required(),
                                    "start position y-component")(
      "goalX", po::value<int>(&goalX)->required(), "goal position x-component")(
      "goalY", po::value<int>(&goalY)->required(), "goal position y-component")(
      "map,m", po::value<std::string>(&mapFile)->required(), "input map (txt)")(
      "output,o", po::value<std::string>(&outputFile)->required(),
      "output file (YAML)");

  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") != 0u) {
      std::cout << desc << "\n";
      return 0;
    }
  } catch (po::error& e) {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  std::unordered_set<State> obstacles;

  std::ifstream map(mapFile);
  int dimX = 0;
  int y = 0;
  while (map.good()) {
    std::string line;
    std::getline(map, line);
    int x = 0;
    for (char c : line) {
      if (c == '#') {
        obstacles.insert(State(x, y));
      }
      ++x;
    }
    dimX = std::max(dimX, x);
    ++y;
  }
  std::cout << dimX << " " << y << std::endl;

  bool success = false;

  State goal(goalX, goalY);
  State start(startX, startY);
  Environment env(dimX, y - 1, obstacles, goal);

  AStar<State, Action, int, Environment> astar(env);

  PlanResult<State, Action, int> solution;

  if (env.stateValid(start)) {
    success = astar.search(start, solution);
  }

  std::ofstream out(outputFile);
  if (success) {
    std::cout << "Planning successful! Total cost: " << solution.cost
              << std::endl;
    for (size_t i = 0; i < solution.actions.size(); ++i) {
      std::cout << solution.states[i].second << ": " << solution.states[i].first
                << "->" << solution.actions[i].first
                << "(cost: " << solution.actions[i].second << ")" << std::endl;
    }
    std::cout << solution.states.back().second << ": "
              << solution.states.back().first << std::endl;

    out << "schedule:" << std::endl;
    out << "  agent1:" << std::endl;
    for (size_t i = 0; i < solution.states.size(); ++i) {
      out << "    - x: " << solution.states[i].first.x << std::endl
          << "      y: " << solution.states[i].first.y << std::endl
          << "      t: " << i << std::endl;
    }
  } else {
    std::cout << "Planning NOT successful!" << std::endl;
  }

  return 0;
}


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

#ifndef SIGNING_HPP_
#define SIGNING_HPP_

#include "total_unimodularity.hpp"

#include <set>

#include <boost/type_traits/is_const.hpp>

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

namespace unimod
{

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

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

      }
    }
    return false;
  }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

            result = false;
          }
        }

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

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

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

            break;
          }
        }
      }
    }

    return result;
  }

}

#endif /* SIGNING_HPP_ */


#ifndef ThresholdCalculator_hpp
#define ThresholdCalculator_hpp

#include <utility>

#include <boost/math/tools/roots.hpp>
using boost::math::tools::bracket_and_solve_root;
using boost::math::tools::eps_tolerance;

/** @file ThresholdCalculator.hpp
  * @brief
  * @author C.D. Clark III
  * @date 06/26/17
  */

namespace libArrhenius {

/** @class ThresholdCalculator
  * @brief
  * @author C.D. Clark III
  */
// this allows the specialization below to detuce Real and Method types for integrators.
template< class Integrator >
class ThresholdCalculator {};

template< template<typename,typename> class Integrator, typename Real, typename Method>
class ThresholdCalculator<Integrator<Real,Method>> : public Integrator<Real,Method>
  // by inheriting from the integrator, we can configure it directly through
  // the threshold calculator without writing wrapper functions.
{
  protected:
    Real ThresholdOmega = 1;

  public:
    template<typename ...Args>
    ThresholdCalculator(Args&&... args)
    :Integrator<Real,Method>(std::forward<Args>(args)...)
    {
    }

    virtual ~ThresholdCalculator () {};

    void setThresholdOmega(Real O){ ThresholdOmega = O; }
    Real getThresholdOmega() const {return ThresholdOmega;}
    Real operator()(size_t N, Real const *t, Real const *T) const
    {
      Real *dT = new Real[N];
      Real *TT = new Real[N];

      for(size_t i = 0; i < N; i++)
        dT[i] = T[i] - T[0];

      // the function that will be rooted
      // should take a single argument and return
      // something.
      auto f = [&](Real x){

        // create the temperature profile that will be evaluated.
        for(size_t i = 0; i < N; i++)
          TT[i] = T[0] + x*dT[i];

        // calculate damage parameter
        Real Omega = Integrator<Real,Method>::operator()(N,t,TT);

        // damage will be between zero and infinity. we are looking for
        // the value of x that will give Omega = ThresholdOmega, so return log of Omega/ThresholdOmega.

        // I don't know what log(Omega/ThresholdOmega) returns, but
        // if this static cast isn't in place, we get a segmentation fault
        // when trying to use it with the boost.multiprecision float types.
        return static_cast<Real>(log(Omega/ThresholdOmega));
      };

      eps_tolerance<Real> tol( std::numeric_limits<Real>::digits - 3 ); // maximum precision we can reasonably expect to achieve.
      boost::uintmax_t it = 100;  // maximum number of iterations to all the algorithm to try.

      // CAREFULE: make sure initial guess is not an integer!
      Real guess = 1.0;
      Real factor = 2.0;

      if( f(0) > 0 )
        guess = -1.0;

      auto min_max = bracket_and_solve_root(f, guess, factor, true, tol, it);

      // clean up our memory
      delete[] dT;
      delete[] TT;

      return (min_max.first + min_max.second)/2;

    }

    Real Omega(size_t N, Real const *t, Real const *T) const
    {
      return Integrator<Real,Method>::operator()(N,t,T);
    }

  protected:
};

}
#endif // include protector