/**
 * @file
 * @brief NPDE homework "Handling degrees of freedom (DOFs) in LehrFEM++"
 * @author Julien Gacon
 * @date March 1st, 2019
 * @copyright Developed at ETH Zurich
 */

#include "lfppdofhandling.h"

#include <Eigen/Dense>
#include <array>
#include <memory>

#include "lf/assemble/assemble.h"
#include "lf/base/base.h"
#include "lf/geometry/geometry.h"
#include "lf/mesh/mesh.h"
#include "lf/mesh/utils/utils.h"

namespace LFPPDofHandling {

/* SAM_LISTING_BEGIN_1 */
std::array<std::size_t, 3> countEntityDofs(
    const lf::assemble::DofHandler &dofhandler) {
  std::array<std::size_t, 3> entityDofs;
  //====================
  // Your code goes here

  //STEP 0: Error handling in case we have quadrilateral cell
  const lf::mesh::Entity& ent = dofhandler.Entity(0);

  // STEP 1: obtain the mesh
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();

  // STEP 2: loop over all mesh entities
  for(std::size_t codim = 0; codim <= 2; ++codim){
    // clear the array entry to 0
    entityDofs[codim] = 0;

    for(const lf::mesh::Entity* ent: mesh->Entities(codim)){
      // LF_VERIFY_MSG(ent->RefEl() == lf::base::RefEl::kQuad(), "Unsupported cell type " << ent->RefEl());
      if(ent->RefEl() == lf::base::RefEl::kQuad()){
        throw "Unsupported cell type";
      }

      entityDofs[codim] += dofhandler.NumInteriorDofs(*ent);
    }
  }

  //====================
  return entityDofs;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::size_t countBoundaryDofs(const lf::assemble::DofHandler &dofhandler) {
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  // given an entity, bd\_flags(entity) == true, if the entity is on the
  // boundary
  lf::mesh::utils::AllCodimMeshDataSet<bool> bd_flags(
      lf::mesh::utils::flagEntitiesOnBoundary(mesh));
  std::size_t no_dofs_on_bd = 0;
  //====================
  // Your code goes here

  // using bd_flags to know whether to take the entity into account
  // STEP 1: add up edges at the boundary
  for(const auto* edge: mesh->Entities(1)){

      if(bd_flags(*edge)){
          no_dofs_on_bd += dofhandler.NumInteriorDofs(*edge);
      }
  }

  // STEP 2: add up nodes at the boundary
  for(const auto* node: mesh->Entities(2)){

      if(bd_flags(*node)){
          no_dofs_on_bd += dofhandler.NumInteriorDofs(*node);
      }
  }

  //====================
  return no_dofs_on_bd;
}
/* SAM_LISTING_END_2 */

// clang-format off
/* SAM_LISTING_BEGIN_3 */
double integrateLinearFEFunction(
    const lf::assemble::DofHandler& dofhandler,
    const Eigen::VectorXd& mu) {
  double I = 0;
  //====================
  // Your code goes here

  double vol = 0.;  // store the volume

  // loop over the cells
  std::shared_ptr<const lf::mesh::Mesh> mesh_ptr = dofhandler.Mesh();
  for(const auto* cell: mesh_ptr->Entities(0)){

        // STEP 0: exception handling
        // no quadrilateral cells allowed
        if(cell->RefEl() == lf::base::RefEl::kQuad()){
          throw "Unsupported cell type";
        }

        // dofhandler dimension match
        if (dofhandler.NumLocalDofs(*cell) != 3){
          throw "Dimension mismatch for dofhandler";
        }

        // STEP 2: compute the fixed value of integration using properties of Barycentric basis
        vol = lf::geometry::Volume(*(cell->Geometry())) / 3.;  // calculate the volume for the current cell

        // STEP 3: obtain the array of global indices
        auto indices = dofhandler.GlobalDofIndices(*cell);
        auto end = indices.end();

        for(auto index_ptr = indices.begin(); index_ptr < end; ++index_ptr){
            // update I - dereference the pointer to obtain global index
            I += vol * mu(*index_ptr);
        }
  }
  //====================
  return I;
}
/* SAM_LISTING_END_3 */
// clang-format on

/* SAM_LISTING_BEGIN_4 */
double integrateQuadraticFEFunction(const lf::assemble::DofHandler &dofhandler,
                                    const Eigen::VectorXd &mu) {
  double I = 0;
  //====================
  // Your code goes here

  // loop over the cells
  std::shared_ptr<const lf::mesh::Mesh> mesh_ptr = dofhandler.Mesh();
  for(const auto* cell: mesh_ptr->Entities(0)){

    // STEP 0: exception handling

    // no quadrilateral cells
    if(cell->RefEl() == lf::base::RefEl::kQuad()){
      throw "No quadrilateral cells allowed";
    }

    // dofhandler dimension match
    if(dofhandler.NumLocalDofs(*cell) != 6){
      throw "Dimension mismatch!";
    }

    // STEP 2: compute the fixed value of integration using properties of Barycentric basis
    double vol = lf::geometry::Volume(*(cell->Geometry())) / 3.;  // calculate the volume for the current cell

    // STEP 3: obtain the array of global indices
    auto indices = dofhandler.GlobalDofIndices(*cell);
    auto end = indices.end();

    // start indexing from 0 in C++
    for(int i = 3; i <= 5; ++i){
        I += vol * mu(indices[i]);
    }

  }
  //====================
  return I;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
Eigen::VectorXd convertDOFsLinearQuadratic(
    const lf::assemble::DofHandler &dofh_Linear_FE,
    const lf::assemble::DofHandler &dofh_Quadratic_FE,
    const Eigen::VectorXd &mu) {
  if (dofh_Linear_FE.Mesh() != dofh_Quadratic_FE.Mesh()) {
    throw "Underlying meshes must be the same for both DOF handlers!";
  }
  std::shared_ptr<const lf::mesh::Mesh> mesh =
      dofh_Linear_FE.Mesh();                          // get the mesh
  Eigen::VectorXd zeta(dofh_Quadratic_FE.NumDofs());  // initialise empty zeta
  // safety guard: always set zero if you're not sure to set every entry later
  // on for us this shouldn't be a problem, but just to be sure
  zeta.setZero();

  for (const auto *cell : mesh->Entities(0)) {
    // check if the spaces are actually linear and quadratic
    if(dofh_Linear_FE.NumLocalDofs(*cell) != 3){
      throw "The dimension of linear FE space should be 3!";
    }

    if(dofh_Quadratic_FE.NumLocalDofs(*cell)!= 6){
      throw "The dimension of Quadratic FE space should be 6!";
    }
    //====================
    // Your code goes here
    // STEP 3: obtain the array of global indices
    auto linear_indices = dofh_Linear_FE.GlobalDofIndices(*cell);
    auto quadratic_indices = dofh_Quadratic_FE.GlobalDofIndices(*cell);
    //====================
    // get the global dof indices of the linear and quadratic FE spaces, note
    // that the vectors obey the LehrFEM++ numbering, which we will make use of
    // lin\_dofs will have size 3 for the 3 dofs on the nodes and
    // quad\_dofs will have size 6, the first 3 entries being the nodes and
    // the last 3 the edges
    //====================
    // Your code goes here
    // assign the coefficients of mu to the correct entries of zeta, use
    // the previous subproblem 2-9.a

    // using just one loop from 0-3 but deal with all 6 entries at the time
    for(int i = 0; i < 3; ++i){
          zeta(quadratic_indices[i]) = mu(linear_indices[i]); // the first 3 entries of basis func have coeff 1

          // make use of the pattern of the basis functions used in the linear combination
          zeta(quadratic_indices[i + 3]) = 0.5 * (mu(linear_indices[i]) + mu(linear_indices[(i + 1) % 3]));
    }
    //====================
  }
  return zeta;
}
/* SAM_LISTING_END_5 */

}  // namespace LFPPDofHandling


#include <hmLib/config_vc.h>
#include <array>
#include <cmath>
#include <fstream>
#include <boost/numeric/odeint.hpp>
#include <hmLib/odeint.hpp>
#include <hmLib/odeint/breakable_integrate.hpp>
#include <hmLib/odeint/container_observer.hpp>
#include <hmLib/odeint/stream_observer.hpp>
#include <hmLib/odeint/iterator_observer.hpp>
#include <hmLib/odeint/break_observer.hpp>
#include <hmLib/odeint/eqstate_break_observer.hpp>
#include <hmLib/odeint/breakable_integrate.hpp>
#include<hmLib/odeint/range_stepper.hpp>
#define TEST_METHOD(Name) struct Name{};
namespace hmLib{
	struct test_system{
		static constexpr double region_error(){ return 0.00001; }
		using state = std::array<double, 2>;
		void operator()(const state& x, state& dxdt, double t){
			operator()(x, dxdt, t, region(x,t));
		}
		void operator()(const state& x, state& dxdt, double t, int r){
			double base = std::sqrt(x[0] * x[0] + x[1] * x[1]);
			dxdt[0] = -x[1];
			dxdt[1] = x[0];

			if(r == 2 && dxdt[0] > 0)dxdt[0] = 0.0;
			else if(r == 3 && dxdt[1] < 0)dxdt[1] = 0.0;
			else if(r == 4){
				if(dxdt[0] > 0)dxdt[0] = 0.0;
				if(dxdt[1] < 0)dxdt[1] = 0.0;
			}
		}
		int region(const state& x, double t){
			if(x[0] >= 0.5 - region_error()){
				if(x[1] <= -0.75 - region_error())return 4;
				return 2;
			}
			if(x[1] <= -0.75 - region_error())return 3;

			return 1;
		}
	};
}
int main(){
	using namespace hmLib;
	namespace bodeint = boost::numeric::odeint;
	TEST_METHOD(create){
		bodeint::runge_kutta_dopri5<test_system::state> Stepper;
		test_system System;
		test_system::state State{0.0,1.0};

		odeint::container_observer<test_system::state> Observer;

		bodeint::integrate_adaptive(Stepper, System, State, 0.0, 100.0, 0.1, std::ref(Observer));

		std::ofstream fout("result1.csv");
		for(auto val : Observer){
			fout << val.first << "," << val.second[0] << "," << val.second[1] << std::endl;
		}
	}
	TEST_METHOD(stream_observer){
		bodeint::runge_kutta_dopri5<test_system::state> Stepper;
		test_system System;
		test_system::state State{0.0,1.0};

		odeint::stream_observer Observer(std::cout);

		bodeint::integrate_adaptive(Stepper, System, State, 0.0, 10.0, 0.1, Observer);
	}
	TEST_METHOD(s_iterator_observer){
		bodeint::runge_kutta_dopri5<test_system::state> Stepper;
		test_system System;
		test_system::state State{0.0,1.0};

		std::vector<test_system::state> Log;
		auto Observer = odeint::make_iterator_observer(std::back_inserter(Log));

		bodeint::integrate_adaptive(Stepper, System, State, 0.0, 10.0, 0.1, Observer);
	}
	TEST_METHOD(st_iterator_observer){
		bodeint::runge_kutta_dopri5<test_system::state> Stepper;
		test_system System;
		test_system::state State{0.0,1.0};

		std::vector<test_system::state> Log;
		std::vector<double> TLog;
		auto Observer = odeint::make_iterator_observer(std::back_inserter(Log), std::back_inserter(TLog));

		bodeint::integrate_adaptive(Stepper, System, State, 0.0, 10.0, 0.1, Observer);
	}
	TEST_METHOD(pair_iterator_observer){
		bodeint::runge_kutta_dopri5<test_system::state> Stepper;
		test_system System;
		test_system::state State{0.0,1.0};

		std::vector<std::pair<double, test_system::state> > Log;
		auto Observer = odeint::make_pair_iterator_observer(std::back_inserter(Log));

		bodeint::integrate_adaptive(Stepper, System, State, 0.0, 10.0, 0.1, Observer);
	}
	TEST_METHOD(break_observer){
		bodeint::runge_kutta_dopri5<test_system::state> Stepper;
		test_system System;
		test_system::state State{0.0,1.0};

		auto Observer = odeint::make_break_observer([](const test_system::state& State,double t)->bool{return State[0] > 1.0; });

		odeint::breakable_integrate_adaptive(Stepper, System, State, 0.0, 10.0, 0.1, Observer);
	}
	TEST_METHOD(break_observer_with_container_observer){
		bodeint::runge_kutta_dopri5<test_system::state> Stepper;
		test_system System;
		test_system::state State{0.0,1.0};

		auto Observer = odeint::make_break_observer([](const test_system::state& State, double t)->bool{return State[0] > 1.0; }, odeint::container_observer<test_system::state>());

		odeint::breakable_integrate_adaptive(Stepper, System, State, 0.0, 10.0, 0.1, std::ref(Observer));
	}
	TEST_METHOD(eqstate_break_observer){
		bodeint::runge_kutta_dopri5<test_system::state> Stepper;
		test_system System;
		test_system::state State{0.0,1.0};

		odeint::eqstate_break_observer<test_system::state> Observer(odeint::detail::eqstate_breaker<test_system::state,double>(1));

		//odeint::breakable_integrate_adaptive(Stepper, System, State, 0.0, 100.0, 0.1, std::ref(Observer));
		bodeint::integrate_adaptive(Stepper, System, State, 0.0, 100.0, 0.1, std::ref(Observer));

		std::ofstream fout("result2.csv");
		for(auto val : Observer){
			fout << val.first << "," << val.second[0] << "," << val.second[1] << std::endl;
		}
	}
	TEST_METHOD(range_stepper){
		auto BaseStepper = bodeint::make_dense_output(1.0e-10, 1.0e-6, bodeint::runge_kutta_dopri5<test_system::state>());
		test_system System;
		test_system::state State{0.0,1.0};

		odeint::region_abridged_stepper<decltype(BaseStepper)> Stepper(System.region_error(), std::move(BaseStepper));

//		odeint::container_observer<test_system::state> Observer;

		//odeint::breakable_integrate_adaptive(Stepper, System, State, 0.0, 100.0, 0.1, std::ref(Observer));

		std::ofstream fout("result3b.csv");
		Stepper.initialize(State, 0.0, 0.001);
		for(unsigned int i = 0; i < 100; ++i){
			auto Ans=Stepper.do_step(System);
			fout << Ans.first << "," << Ans.second << "," << Stepper.current_time() << "," << Stepper.current_time_step() << "," << Stepper.current_state()[0] << "," << Stepper.current_state()[1] << "," << Stepper.current_region() << std::endl;
		}

	}
	TEST_METHOD(range_stepper_integrate){
		auto BaseStepper = bodeint::make_dense_output(1.0e-10, 1.0e-6, bodeint::runge_kutta_dopri5<test_system::state>());
		test_system System;
		test_system::state State{0.0,1.0};

		odeint::region_abridged_stepper<decltype(BaseStepper)> Stepper(System.region_error(), std::move(BaseStepper));
		Stepper.initialize(State, 0.0, 0.001);

		//odeint::eqstate_break_observer<test_system::state> Observer(0.001, 1);
		odeint::container_observer<test_system::state> Observer;

		//odeint::breakable_integrate_adaptive(Stepper, System, State, 0.0, 100.0, 0.1, std::ref(Observer));
		bodeint::integrate_adaptive(Stepper, System, State, 0.0, 100.0, 0.1, std::ref(Observer));

		std::ofstream fout("result3.csv");
		for(auto val : Observer){
			fout << val.first << "," << val.second[0] << "," << val.second[1] << std::endl;
		}
	}
	system("pause");

	return 0;
}


#include <Eigen/Core>
#include <fstream>
#include <sequential-line-search/utils.hpp>

using Eigen::MatrixXd;
using Eigen::VectorXd;

Eigen::VectorXd sequential_line_search::utils::GenerateRandomVector(unsigned n)
{
    return 0.5 * (Eigen::VectorXd::Random(n) + Eigen::VectorXd::Ones(n));
}

void sequential_line_search::utils::ExportMatrixToCsv(const std::string& file_path, const Eigen::MatrixXd& X)
{
    std::ofstream   file(file_path);
    Eigen::IOFormat format(Eigen::StreamPrecision, Eigen::DontAlignCols, ",");
    file << X.format(format);
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, if_else) {
  using stan::math::if_else;
  unsigned int c = 5;
  double x = 1.0;
  double y = -1.0;
  EXPECT_FLOAT_EQ(x, if_else(c,x,y));
  c = 0;
  EXPECT_FLOAT_EQ(y, if_else(c,x,y));

  bool d = true;
  int u = 1;
  int v = -1;
  EXPECT_EQ(1.0, stan::math::if_else(d,u,v));
  d = false;
  EXPECT_FLOAT_EQ(-1.0, stan::math::if_else(d,u,v));

  EXPECT_FLOAT_EQ(1.2,if_else(true,1.2,12));
  EXPECT_FLOAT_EQ(12.0,if_else(false,1.2,12));

  EXPECT_FLOAT_EQ(1.0,if_else(true,1,12.3));
  EXPECT_FLOAT_EQ(12.3,if_else(false,1,12.3));
}

TEST(MathFunctions, if_else_promote) {
  using stan::math::if_else;
  double x = 2.5;
  int y = -1;
  EXPECT_FLOAT_EQ(2.5, if_else(true,x,y));
  EXPECT_FLOAT_EQ(-1.0, if_else(false,x,y));
}

TEST(MathFunctions, if_else_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_FLOAT_EQ(1.2, stan::math::if_else(true, 1.2, nan));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::if_else(false, 1.2, nan));

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::if_else(true, nan, 2.4));
  EXPECT_FLOAT_EQ(2.4, stan::math::if_else(false, nan, 2.4));

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::if_else(true, nan, nan));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::if_else(false, nan, nan));
}


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c)      2012 Zach Byerly
//  Copyright (c) 2011-2012 Bryce Adelstein-Lelbach
//
//  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)
////////////////////////////////////////////////////////////////////////////////

//
// This is a program written to evolve in time the equation:
//
// D^2 U / Dt^2 = c^2  D^2 U / Dx^2
//
// The parameter alpha = c*dt/dx must be less than 1 to ensure the stability
//     of the algorithm.
// Discretizing the equation and solving for U(t+dt,x) yields
// alpha^2 * (U(t,x+dx)+U(t,x-dx))+2(1-alpha^2)*U(t,x) - U(t-dt,x)
//
// For the first timestep, we approximate U(t-dt,x) by u(t+dt,x)-2*dt*du/dt(t,x)
//

// Include statements.
#include <hpx/hpx_init.hpp>
#include <hpx/runtime/actions/plain_action.hpp>
#include <hpx/include/async.hpp>
#include <hpx/lcos/future_wait.hpp>
#include <hpx/include/iostreams.hpp>
#include <hpx/util/format.hpp>

#include <boost/math/constants/constants.hpp>

#include <cstddef>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <mutex>
#include <vector>

using boost::program_options::variables_map;
using boost::program_options::options_description;
using boost::program_options::value;

using hpx::naming::id_type;
using hpx::naming::invalid_id;

using hpx::lcos::future;
using hpx::async;
using hpx::lcos::wait;

using hpx::util::high_resolution_timer;

using hpx::init;
using hpx::finalize;
using hpx::find_here;

using hpx::cout;
using hpx::flush;

///////////////////////////////////////////////////////////////////////////////
// Globals.

//double const alpha_squared = 0.25;
double alpha_squared = 0;

// Initialized in hpx_main.
id_type here = invalid_id;
double pi = 0.;
double c = 0.;
double dt = 0.;
double dx = 0.;

// Command line argument.
std::uint64_t nt = 0;
std::uint64_t nx = 0;

struct data{
  // Default constructor: data d1;
  data()
    : mtx()
    , u_value(0.0)
    , computed(false)
  {}

  // Copy constructor: data d1; data d2(d1);
  // We can't copy the mutex, because mutexs are noncopyable.
  data(
       data const& other
       )
    : mtx()
    , u_value(other.u_value)
    , computed(other.computed)
  {}

  data& operator=(
      data const& other
      )
  {
    u_value = other.u_value;
    computed = other.computed;
    return *this;
  }

  hpx::lcos::local::mutex mtx;
  double u_value;
  bool computed;
};

std::vector<std::vector<data> > u;

///////////////////////////////////////////////////////////////////////////////
// Forward declaration of the wave function.
double wave(std::uint64_t t, std::uint64_t x);

// Any global function needs to be wrapped into a plain_action if it should be
// invoked as a HPX-thread.
// This generates the required boilerplate we need for remote invocation.
HPX_PLAIN_ACTION(wave);

double calculate_u_tplus_x(double u_t_xplus, double u_t_x, double u_t_xminus,
                           double u_tminus_x)
{
  double u_tplus_x = alpha_squared*(u_t_xplus + u_t_xminus)
    + 2.0*(1-alpha_squared)*u_t_x - u_tminus_x;
  return u_tplus_x;
}

double calculate_u_tplus_x_1st(double u_t_xplus, double u_t_x,
                               double u_t_xminus, double u_dot)
{
  double u_tplus_x = 0.5*alpha_squared*(u_t_xplus + u_t_xminus)
    + (1-alpha_squared)*u_t_x + dt*u_dot;
  return u_tplus_x;
}

double wave(std::uint64_t t, std::uint64_t x)
{
  std::lock_guard<hpx::lcos::local::mutex> l(u[t][x].mtx);
  //  hpx::util::format_to(cout, "calling wave... t={1} x={2}\n", t, x) << flush;
  if (u[t][x].computed)
    {
      //cout << ("already computed!\n") << flush;
      return u[t][x].u_value;
    }
  u[t][x].computed = true;

  if (t == 0) //first timestep are initial values
    {
      //        hpx::util::format_to(cout, "first timestep\n") << flush;
      u[t][x].u_value = std::sin(2.*pi*x*dx); // initial u(x) value
      return u[t][x].u_value;
    }
  future<double> n1;

  // NOT using ghost zones here... just letting the stencil cross the periodic
  // boundary.
  if (x == 0)
    n1 = async<wave_action>(here,t-1,nx-1);
  else
    n1 = async<wave_action>(here,t-1,x-1);

  future<double> n2 = async<wave_action>(here,t-1,x);

  future<double> n3;

  if (x == (nx-1))
    n3 = async<wave_action>(here,t-1,0);
  else
    n3 = async<wave_action>(here,t-1,x+1);

  double u_t_xminus = n1.get(); //get the futures
  double u_t_x = n2.get();
  double u_t_xplus = n3.get();

  if (t == 1) //second time coordinate handled differently
    {
      double u_dot = 0;// initial du/dt(x)
      u[t][x].u_value = calculate_u_tplus_x_1st(u_t_xplus,u_t_x,u_t_xminus,u_dot);
      return u[t][x].u_value;
    } else {
    double u_tminus_x = async<wave_action>(here,t-2,x).get();
    u[t][x].u_value =  calculate_u_tplus_x(u_t_xplus,u_t_x,u_t_xminus,u_tminus_x);
    return u[t][x].u_value;
  }

}

///////////////////////////////////////////////////////////////////////////////
int hpx_main(variables_map& vm)
{
  here = find_here();
  pi = boost::math::constants::pi<double>();

  //    dt = vm["dt-value"].as<double>();
  //    dx = vm["dx-value"].as<double>();
  //    c = vm["c-value"].as<double>();
  nx = vm["nx-value"].as<std::uint64_t>();
  nt = vm["nt-value"].as<std::uint64_t>();

  c = 1.0;

  dt = 1.0/(nt-1);
  dx = 1.0/(nx-1);
  alpha_squared = (c*dt/dx)*(c*dt/dx);

  // check that alpha_squared satisfies the stability condition
  if (0.25 < alpha_squared)
    {
      cout << (("alpha^2 = (c*dt/dx)^2 should be less than 0.25 for stability!\n"))
          << flush;
    }

  u = std::vector<std::vector<data> >(nt, std::vector<data>(nx));

  hpx::util::format_to(cout, "dt = {1}\n", dt) << flush;
  hpx::util::format_to(cout, "dx = {1}\n", dx) << flush;
  hpx::util::format_to(cout, "alpha^2 = {1}\n", alpha_squared) << flush;

  {
    // Keep track of the time required to execute.
    high_resolution_timer t;

    std::vector<future<double> > futures;
    for (std::uint64_t i=0;i<nx;i++)
      futures.push_back(async<wave_action>(here,nt-1,i));

    // open file for output
    std::ofstream outfile;
    outfile.open ("output.dat");

    wait(futures, [&](std::size_t i, double n)
         { double x_here = i*dx;
           hpx::util::format_to(outfile, "{1} {2}\n", x_here, n) << flush; });

    outfile.close();

    char const* fmt = "elapsed time: {1} [s]\n";
    hpx::util::format_to(std::cout, fmt, t.elapsed());
  }

  finalize();
  return 0;
}

///////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[])
{
  // Configure application-specific options.
  options_description
    desc_commandline("Usage: " HPX_APPLICATION_STRING " [options]");

  desc_commandline.add_options()
    ( "dt-value"
      , value<double>()->default_value(0.05)
      , "dt parameter of the wave equation")

    ( "dx-value"
      , value<double>()->default_value(0.1)
      , "dx parameter of the wave equation")

    ( "c-value"
      , value<double>()->default_value(1.0)
      , "c parameter of the wave equation")

    ( "nx-value"
      , value<std::uint64_t>()->default_value(100)
      , "nx parameter of the wave equation")

    ( "nt-value"
      , value<std::uint64_t>()->default_value(400)
      , "nt parameter of the wave equation")
    ;

  // Initialize and run HPX.
  return init(desc_commandline, argc, argv);
}


/*
 *  Zp.hpp
 *
 *  Created by Andrea Bedini on 19/09/08.
 *  Copyright (c) 2008-2014, Andrea Bedini <andrea.bedini@gmail.com>.
 *
 *  Distributed under the terms of the Modified BSD License.
 *  The full license is in the file COPYING, distributed as part of
 *  this software.
 *
 */

#ifndef ZP_HPP
#define ZP_HPP

#include <boost/cstdint.hpp>
#include <boost/operators.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_signed.hpp>
#include <boost/utility/enable_if.hpp>

#include <iosfwd>

namespace modular {
  class Zp;
}

namespace boost {
  template<> struct is_integral <modular::Zp> : public true_type {};
  template<> struct is_signed   <modular::Zp> : public false_type {};
  template<> struct is_unsigned   <modular::Zp> : public true_type {};
}

namespace modular {
  using boost::enable_if;
  using boost::disable_if;

  using boost::uint64_t;

  typedef long double ldouble;

  inline uint64_t mul_mod(uint64_t a, uint64_t b, uint64_t m)
  {
    uint64_t x = a * b;
    uint64_t y = m * (uint64_t)( (ldouble)a * (ldouble)b/m + (ldouble)1/2 );
    uint64_t r = x - y;
    if ( (int64_t)r < 0 )  r += m;
    return  r;
  }

  class Zp : boost::ring_operators< Zp
	   , boost::equality_comparable< Zp
	   > >
  {
    static uint64_t M;
    uint64_t rep_;

  public:
    Zp() : rep_(0) { }
    Zp(Zp const& x) : rep_(x.rep_) { }

    template<class T>
    explicit Zp(T n, typename boost::enable_if<boost::is_signed<T> >::type* = 0)
      : rep_( n < 0 ?  M - ((- n) % M) : n % M)
    { }

    template<class T>
    explicit Zp(T n, typename boost::disable_if<boost::is_signed<T> >::type* = 0)
      : rep_(n % M)
    { }

    operator unsigned long() const { return rep_; }

    static uint64_t get_modulus() {
      return M;
    }

    static void set_modulus(uint64_t p) {
      M = p;
    }

    // addable
    Zp& operator+=(Zp const& x)
    {
      if (rep_ >= M - x.rep_)
	rep_ -= M - x.rep_;
      else
	rep_ += x.rep_;
      return *this;
    }

    // subtractable
    Zp& operator-=(Zp const& x)
    {
      if (rep_ >= x.rep_)
	rep_ -= x.rep_;
      else
	rep_ = M - x.rep_ + rep_;
      return *this;
    }

    // multipliable
    Zp& operator*=(Zp const& x)
    {
      rep_ = mul_mod(rep_, x.rep_, M);
      // rep_ *= x.rep_;
      // rep_ %= M;
      return *this;
    }

    Zp operator-() const
    {
      return Zp(M - rep_);
    }

    // equality_comparable
    bool operator==(Zp const& rhs) const
    {
      return rep_ == rhs.rep_;
    }

    friend std::ostream& operator<<(std::ostream& o, Zp const& x)
    {
      return o << x.rep_ << " (" << M << ")";
    }
  };

  uint64_t Zp::M;
}

#endif


/* ----------------------------------------------------------------------------
 * Copyright 2021, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#pragma once

#include <Eigen/Dense>
#include <unsupported/Eigen/Splines>
#include "panther_types.hpp"

void CPs2TrajAndPwp(std::vector<Eigen::Vector3d> &qp, std::vector<double> &qy, std::vector<mt::state> &traj,
                    mt::PieceWisePol &pwp, int param_pp, int param_py, Eigen::RowVectorXd &knots_p, double dc);

void CPs2TrajAndPwp_old(std::vector<Eigen::Vector3d> &q, std::vector<mt::state> &traj, mt::PieceWisePol &solution_,
                        int N, int p, int num_seg, Eigen::RowVectorXd &knots, double dc);

Eigen::Spline3d findInterpolatingBsplineNormalized(const std::vector<double> &times,
                                                   const std::vector<Eigen::Vector3d> &positions);


#ifndef CSVFUNCTION_H
#define CSVFUNCTION_H

#include <iostream>
#include <fstream>
#include <sstream>
#include "IntrogressionSimulations.h"
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/filesystem.hpp>
#include <stdio.h>
#include <iomanip>
#include <chrono>
#include "random.h"

#ifdef _OPENMP
    #include <omp.h>
#endif

enum nameofstream {Parametersoff, Dataoff, AlleleFrequencyoff_mean, AlleleFrequencyoff_var};

void CSV_WriteOutput(std::ofstream arrayofstream[4], const Parameters &pars, SimData &SimulationData){

    using namespace boost::accumulators;
    // Parameters output
    for (int i = 0; i < 2; ++i)
    {
        arrayofstream[Parametersoff] << "NINIT" << i << arrayofstream[Parametersoff].fill() << pars.NINIT[i] << std::endl;
    }
    arrayofstream[Parametersoff] << "NGEN" << arrayofstream[Parametersoff].fill() << pars.NGEN << std::endl;
    arrayofstream[Parametersoff] << "NLOCI" << arrayofstream[Parametersoff].fill() << pars.NLOCI << std::endl;
    arrayofstream[Parametersoff] << "NREP" << arrayofstream[Parametersoff].fill() << pars.NREP << std::endl;
    arrayofstream[Parametersoff] << "MUTATIONRATE" << arrayofstream[Parametersoff].fill() << pars.MUTATIONRATE << std::endl;
    arrayofstream[Parametersoff] << "RECOMBINATIONRATE" << arrayofstream[Parametersoff].fill() << pars.RECOMBINATIONRATE << std::endl;
    arrayofstream[Parametersoff] << "CARRYINGCAPACITY" << arrayofstream[Parametersoff].fill() << pars.K << std::endl;
    arrayofstream[Parametersoff] << "BIRTHRATE" << arrayofstream[Parametersoff].fill() << pars.BIRTHRATE << std::endl;
    arrayofstream[Parametersoff] << "DEATHRATEA" << arrayofstream[Parametersoff].fill() << pars.DEATHRATEA << std::endl;
    arrayofstream[Parametersoff] << "DEATHRATEa" << arrayofstream[Parametersoff].fill() << pars.DEATHRATEa << std::endl;
    arrayofstream[Parametersoff] << std::endl;

    //Simulation output
    arrayofstream[Dataoff]
    << "Generation" << arrayofstream[Dataoff].fill()
    << "AVG_popsize" << arrayofstream[Dataoff].fill()
    << "AVG_major0" << arrayofstream[Dataoff].fill()
    << "AVG_major1" << arrayofstream[Dataoff].fill()
    << "AVG_introgressed0" << arrayofstream[Dataoff].fill()
    << "AVG_introgressed1" << arrayofstream[Dataoff].fill()
    << "VAR_popsize" << arrayofstream[Dataoff].fill()
    << "VAR_major0" << arrayofstream[Dataoff].fill()
    << "VAR_major1" << arrayofstream[Dataoff].fill()
    << "VAR_introgressed0" << arrayofstream[Dataoff].fill()
    << "VAR_introgressed1" << std::endl;

    for(int i = 0; i < pars.NGEN; ++i){
        accumulator_set<int, stats<tag::mean, tag::variance > > popsize;
        accumulator_set<int, stats<tag::mean, tag::variance > > major0;
        accumulator_set<int, stats<tag::mean, tag::variance > > major1;
        accumulator_set<double, stats<tag::mean, tag::variance > > introgressed0;
        accumulator_set<double, stats<tag::mean, tag::variance > > introgressed1;

        ///FINISH ANALYSIS OF THE GENOTYPE RESCUE PER LOCUS PART.
        for(int j = 0; j < pars.NREP; ++j){
            popsize(SimulationData.DataSet[j]->popsize[i]);
            major0(SimulationData.DataSet[j]->major0[i]);
            major1(SimulationData.DataSet[j]->major1[i]);
            introgressed0(SimulationData.DataSet[j]->introgressed0[i]);
            introgressed1(SimulationData.DataSet[j]->introgressed1[i]);
        }

        arrayofstream[Dataoff]
        << i << arrayofstream[Dataoff].fill()
        << mean(popsize) << arrayofstream[Dataoff].fill()
        << (double)mean(major0) << arrayofstream[Dataoff].fill()
        << (double)mean(major1) << arrayofstream[Dataoff].fill()
        << mean(introgressed0) << arrayofstream[Dataoff].fill()
        << mean(introgressed1) << arrayofstream[Dataoff].fill()

        << variance(popsize) << arrayofstream[Dataoff].fill()
        << (double)variance(major0) << arrayofstream[Dataoff].fill()
        << (double)variance(major1) << arrayofstream[Dataoff].fill()
        << variance(introgressed0)<< arrayofstream[Dataoff].fill()
        << variance(introgressed1) << arrayofstream[Dataoff].fill() << std::endl;
    };

    for(int i = 0; i < pars.NGEN; ++i){
        arrayofstream[AlleleFrequencyoff_mean] << i << arrayofstream[AlleleFrequencyoff_mean].fill();
        arrayofstream[AlleleFrequencyoff_var] << i << arrayofstream[AlleleFrequencyoff_var].fill();
        for(int l = 0; l < pars.NLOCI; ++l)
        {
            accumulator_set<double, stats<tag::mean, tag::variance > > locus;
            for(int r = 0; r < pars.NREP; ++r)
            {
                locus((double)SimulationData.DataSet[r]->allele0[i][l] / ((double)SimulationData.DataSet[r]->popsize[i]));
            }
            arrayofstream[AlleleFrequencyoff_mean] << mean(locus)  << arrayofstream[AlleleFrequencyoff_mean].fill();
            arrayofstream[AlleleFrequencyoff_var] << variance(locus) << arrayofstream[AlleleFrequencyoff_var].fill();
        }
        arrayofstream[AlleleFrequencyoff_mean] << std::endl;
        arrayofstream[AlleleFrequencyoff_var] << std::endl;
    }

    // Cleanup
    for(int i = 0; i < pars.NREP; ++i){
        delete SimulationData.DataSet[i];
    }
}

std::string CreateOutputStreams(std::ofstream arrayofstream[4]){

    std::string CurrentWorkingDirectory = boost::filesystem::current_path().c_str();
    CurrentWorkingDirectory.append("/data");

    auto t = std::time(nullptr);
    auto tm = *std::localtime(&t);
    std::ostringstream oss;
    oss << std::put_time(&tm, "%d-%m-%Y-%H-%M-%S");
    std::string str = oss.str();

    std::string MainOutputFolder = CurrentWorkingDirectory;
    MainOutputFolder.append("/");
    MainOutputFolder.append(str);

    boost::filesystem::create_directories(MainOutputFolder.c_str());

    std::string ParameterOutput = MainOutputFolder;
    ParameterOutput.append("/Parameters.csv");

    arrayofstream[Parametersoff].open(ParameterOutput);
    arrayofstream[Parametersoff].fill(',');

    std::string DataOutput = MainOutputFolder;
    DataOutput.append("/Data.csv");

    std::string AlleleFOutput_mean = MainOutputFolder;
    std::string AlleleFOutput_var = MainOutputFolder;

    AlleleFOutput_mean.append("/AlleleF_mean.csv");
    AlleleFOutput_var.append("/AlleleF_var.csv");

    arrayofstream[Dataoff].open(DataOutput);
    arrayofstream[AlleleFrequencyoff_mean].open(AlleleFOutput_mean);
    arrayofstream[AlleleFrequencyoff_var].open(AlleleFOutput_var);
    arrayofstream[Dataoff].fill(',');
    arrayofstream[AlleleFrequencyoff_mean].fill(',');
    arrayofstream[AlleleFrequencyoff_var].fill(',');

    return MainOutputFolder;
}

int main(int argc, char *argv[]){
    // Initialize simulation
    rnd::set_seed();
    const Parameters GlobalPars(argc, argv);

    SimData SimulationData;
    // Run nrep successful simulations
    // auto start = std::chrono::high_resolution_clock::now();
    #ifdef _OPENMP
        printf("Parallel activated : Number of threads=%i\n",omp_get_max_threads());
    #endif

    #pragma omp parallel for schedule(static)
    for(int task = 0; task < GlobalPars.NREP; ++task){
        while(RunSimulation(GlobalPars, SimulationData)==false);
    }
    // auto finish = std::chrono::high_resolution_clock::now();

    // Write outputfiles
    std::ofstream arrayofstream[4];
    CreateOutputStreams(arrayofstream);
	CSV_WriteOutput(arrayofstream, GlobalPars, SimulationData);

    /*
    // Cleanup
    for(int i = 0; i < GlobalPars.NREP; ++i){
        delete SimulationData.DataSet[i];
    }
    */

    /*
    std::chrono::duration<double> elapsed = finish-start;
    std::cout << "Elapsed time: " << elapsed.count() << " s" << std::endl;
    */
    return 0;
}

#endif


#include "../../include/IntrinsicFormula/AmpSolver.h"
#include <igl/cotmatrix_entries.h>
#include <igl/massmatrix.h>
#include <Eigen/Sparse>
#include <SymGEigsShiftSolver.h>
#include <MatOp/SparseCholesky.h>
#include <MatOp/SparseSymShiftSolve.h>
#include <cassert>

// Baseline implementation; can be used if Spectra not available

/*
static void inversePowerIteration(const Eigen::SparseMatrix<double>& A, const Eigen::SparseMatrix<double>& B, Eigen::VectorXd& sol)
{
    sol.resize(A.rows());
    sol.setRandom();
    double eps = 1e-6;
    Eigen::SparseMatrix<double> M = A + eps * B;
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver(M);
    sol.setRandom();
    for (int i = 0; i < 10000; i++)
    {
        sol = solver.solve(B * sol);
        sol /= std::sqrt(sol.transpose() * B * sol);
    }
}
*/

void ampSolver(const Eigen::MatrixXd& V, const MeshConnectivity& mesh, const Eigen::MatrixXd& omegas, Eigen::VectorXd& amplitudes)
{
    Eigen::MatrixXd C;
    igl::cotmatrix_entries(V, mesh.faces(), C);

    int nedges = mesh.nEdges();
    Eigen::VectorXd onestar(nedges);

    for (int i = 0; i < nedges; i++)
    {
        onestar[i] = 0;
        for (int j = 0; j < 2; j++)
        {
            int fidx = mesh.edgeFace(i, j);
            if (fidx != -1)
            {
                int opp = mesh.oppositeVertexIndex(i, j);
                onestar[i] += C(fidx, opp);
            }
        }
    }

    std::vector<Eigen::Triplet<double> > Mcoeffs;
    for (int i = 0; i < nedges; i++)
    {
        int v0 = mesh.edgeVertex(i, 0);
        int v1 = mesh.edgeVertex(i, 1);

        // Laplacian coeffs
        Mcoeffs.push_back({ v0, v0, onestar[i] });
        Mcoeffs.push_back({ v1, v1, onestar[i] });
        Mcoeffs.push_back({ v0, v1, -onestar[i] });
        Mcoeffs.push_back({ v1, v0, -onestar[i] });

        // frequency coeffs
        // frequency coeffs
        double freqweight1 = 0.5 * omegas(i, 0) * omegas(i, 0);
        double freqweight2 = 0.5 * omegas(i, 1) * omegas(i, 1);
        Mcoeffs.push_back({ v0, v0, freqweight1 * onestar[i] });
        Mcoeffs.push_back({ v1, v1, freqweight2 * onestar[i] });
    }

    int nverts = V.rows();
    Eigen::SparseMatrix<double> M(nverts, nverts);
    M.setFromTriplets(Mcoeffs.begin(), Mcoeffs.end());

    Eigen::SparseMatrix<double> massM;
    igl::massmatrix(V, mesh.faces(), igl::MASSMATRIX_TYPE_BARYCENTRIC, massM);

    Spectra::SymShiftInvert<double> op(M, massM);
    Spectra::SparseSymMatProd<double> Bop(massM);
    Spectra::SymGEigsShiftSolver<Spectra::SymShiftInvert<double>, Spectra::SparseSymMatProd<double>, Spectra::GEigsMode::ShiftInvert> geigs(op, Bop, 1, 6, -1e-6);
    geigs.init();
    int nconv = geigs.compute(Spectra::SortRule::LargestMagn, 1e6);

    Eigen::VectorXd evalues;
    Eigen::MatrixXd evecs;

    evalues = geigs.eigenvalues();
    evecs = geigs.eigenvectors();
    if (nconv != 1 || geigs.info() != Spectra::CompInfo::Successful)
    {
        std::cout << "Eigensolver failed to converge!!" << std::endl;
    }
    amplitudes = evecs.col(0);
    std::cout << "Eigenvalue is " << evalues[0] << std::endl;

    //inversePowerIteration(M, massM, amplitudes);

    // try to fix sign
    int posvotes = 0;
    int negvotes = 0;
    for (int i = 0; i < nverts; i++)
    {
        if (amplitudes[i] < 0)
            negvotes++;
        else
            posvotes++;
    }
    if (negvotes > posvotes)
        amplitudes *= -1;
}


#include <boost/compute/random/mersenne_twister_engine.hpp>


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

#include <pmp/algorithms/SurfaceSmoothing.h>
#include <pmp/algorithms/DifferentialGeometry.h>

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

//=============================================================================

namespace pmp {

//=============================================================================

using SparseMatrix = Eigen::SparseMatrix<double>;
using Triplet = Eigen::Triplet<double>;

//=============================================================================

void SurfaceSmoothing::explicitSmoothing(unsigned int iters,
                                         bool useUniformLaplace)
{
    auto points = m_mesh.vertexProperty<Point>("v:point");
    auto eweight = m_mesh.addEdgeProperty<Scalar>("e:cotan");
    auto laplace = m_mesh.addVertexProperty<Point>("v:laplace");

    // compute Laplace weight per edge: cotan or uniform
    if (useUniformLaplace)
    {
        for (auto e : m_mesh.edges())
            eweight[e] = 1.0;
    }
    else
    {
        for (auto e : m_mesh.edges())
            eweight[e] = std::max(0.0, cotanWeight(m_mesh, e));
    }

    // smoothing iterations
    SurfaceMesh::Vertex vv;
    SurfaceMesh::Edge e;
    for (unsigned int i = 0; i < iters; ++i)
    {
        // step 1: compute Laplace for each vertex
        for (auto v : m_mesh.vertices())
        {
            Point l(0, 0, 0);

            if (!m_mesh.isSurfaceBoundary(v))
            {
                Scalar w(0);

                for (auto h : m_mesh.halfedges(v))
                {
                    vv = m_mesh.toVertex(h);
                    e = m_mesh.edge(h);
                    l += eweight[e] * (points[vv] - points[v]);
                    w += eweight[e];
                }

                l /= w;
            }

            laplace[v] = l;
        }

        // step 2: move each vertex by its (damped) Laplacian
        for (auto v : m_mesh.vertices())
        {
            points[v] += 0.5f * laplace[v];
        }
    }

    // clean-up custom properties
    m_mesh.removeVertexProperty(laplace);
    m_mesh.removeEdgeProperty(eweight);
}

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

void SurfaceSmoothing::implicitSmoothing(Scalar timestep,
                                         bool useUniformLaplace)
{
    if (!m_mesh.nVertices())
        return;

    // properties
    auto points = m_mesh.vertexProperty<Point>("v:point");
    auto vweight = m_mesh.addVertexProperty<Scalar>("v:area");
    auto eweight = m_mesh.addEdgeProperty<Scalar>("e:cotan");
    auto idx = m_mesh.addVertexProperty<int>("v:idx", -1);

    // compute weights: cotan or uniform
    if (useUniformLaplace)
    {
        for (auto v : m_mesh.vertices())
            vweight[v] = 1.0 / m_mesh.valence(v);
        for (auto e : m_mesh.edges())
            eweight[e] = 1.0;
    }
    else
    {
        for (auto v : m_mesh.vertices())
            vweight[v] = 0.5 / voronoiArea(m_mesh, v);
        for (auto e : m_mesh.edges())
            eweight[e] = std::max(0.0, cotanWeight(m_mesh, e));
    }

    // collect free (non-boundary) vertices in array free_vertices[]
    // assign indices such that idx[ free_vertices[i] ] == i
    unsigned i = 0;
    std::vector<SurfaceMesh::Vertex> free_vertices;
    free_vertices.reserve(m_mesh.nVertices());
    for (auto v : m_mesh.vertices())
    {
        if (!m_mesh.isSurfaceBoundary(v))
        {
            idx[v] = i++;
            free_vertices.push_back(v);
        }
    }
    const unsigned int n = free_vertices.size();

    // A*X = B
    SparseMatrix A(n, n);
    Eigen::MatrixXd B(n, 3);

    // nonzero elements of A as triplets: (row, column, value)
    std::vector<Triplet> triplets;

    // setup matrix A and rhs B
    double ww;
    SurfaceMesh::Vertex v, vv;
    SurfaceMesh::Edge e;
    for (unsigned int i = 0; i < n; ++i)
    {
        v = free_vertices[i];

        // rhs row
        B(i, 0) = points[v][0] / vweight[v];
        B(i, 1) = points[v][1] / vweight[v];
        B(i, 2) = points[v][2] / vweight[v];

        // lhs row
        ww = 0.0;
        for (auto h : m_mesh.halfedges(v))
        {
            vv = m_mesh.toVertex(h);
            e = m_mesh.edge(h);
            ww += eweight[e];

            // fixed boundary vertex -> right hand side
            if (m_mesh.isSurfaceBoundary(vv))
            {
                B(i, 0) -= -timestep * eweight[e] * points[vv][0];
                B(i, 1) -= -timestep * eweight[e] * points[vv][1];
                B(i, 2) -= -timestep * eweight[e] * points[vv][2];
            }
            // free interior vertex -> matrix
            else
            {
                triplets.emplace_back(i, idx[vv], -timestep * eweight[e]);
            }
        }

        // center vertex -> matrix
        triplets.emplace_back(i, i, 1.0 / vweight[v] + timestep * ww);
    }

    // build sparse matrix from triplets
    A.setFromTriplets(triplets.begin(), triplets.end());

    // solve A*X = B
    Eigen::SimplicialLDLT<SparseMatrix> solver(A);
    Eigen::MatrixXd X = solver.solve(B);
    if (solver.info() != Eigen::Success)
    {
        std::cerr << "SurfaceSmoothing: Could not solve linear system\n";
    }
    else
    {
        // copy solution
        for (unsigned int i = 0; i < n; ++i)
        {
            v = free_vertices[i];
            points[v][0] = X(i, 0);
            points[v][1] = X(i, 1);
            points[v][2] = X(i, 2);
        }
    }

    // clean-up
    m_mesh.removeVertexProperty(idx);
    m_mesh.removeVertexProperty(vweight);
    m_mesh.removeEdgeProperty(eweight);
}

//=============================================================================
} // namespace pmp
//=============================================================================


/*************************************************************************
Copyright (c) 2019 Cognitics, Inc.

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

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

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
****************************************************************************/
#include "sfa/ConvexHull.h"
#include "sfa/Point.h"
#include "sfa/Polygon.h"
#include "sfa/PointMath.h"
#include "sfa/RingMath.h"
#include <float.h>
#include <cmath>
#include <boost/foreach.hpp>
namespace sfa {

//MELKMAN ========================================================================================
    int MelkmanHull::isLeft(const Point& p, const Point& p1, const Point& p2)
    {
        double a = (p1.X() - p.X())*(p2.Y() - p.Y()) - (p2.X() - p.X())*(p1.Y() - p.Y());
        if (a < -SFA_EPSILON) return -1;
        else if (a > SFA_EPSILON) return 1;
        else return 0;
    }

    Geometry* MelkmanHull::apply(const LineString* a)
    {
        //check if the linestring is long enough
        if (a->getNumPoints() < 3)
            new LineString(a);

        std::vector<Point> points;
        for (int i = 0; i < a->getNumPoints(); i++)
        {
            points.push_back(*a->getPointN(i));
        }

        //remove any redundant points
        if (a->isClosed()) points.pop_back();

        //create the dequeue
        Point* D = new Point[2*points.size() + 1];
        int bottom = int(points.size()) - 2;
        int top = bottom + 3;

        //special case, the first three points are collinear
        //in that case keep moving down
        int first = 0;
        int second = 1;
        int third = 2;

        while(third < int(points.size()))
        {
            if (Collinear(points[first],points[second],points[third]))
            {
                second++;
                third++;
            }
            else break;
        }
        if (third >= int(points.size()))
            return new LineString(a);

        //set up the first three points so that they are in CCW order
        if (isLeft(points[first], points[second], points[third]) > 0)
        {
            D[bottom+1] = points[first];
            D[bottom+2] = points[second];
        }
        else
        {
            D[bottom+1] = points[second];
            D[bottom+2] = points[first];
        }
        D[bottom] = points[third];
        D[top] = points[third];

        for (int i = third++; i < int(points.size()); i++)
        {
            // test if next vertex is inside the deque hull
            if ((isLeft(D[bottom], D[bottom+1], points[i]) > 0) &&
                (isLeft(D[top-1], D[top], points[i]) > 0) )
                    continue;

            // Find the rightmost vertex to connect to
            while (isLeft(D[bottom], D[bottom+1], points[i]) <= 0)
                ++bottom;

            D[--bottom] = points[i];    // insert V[i] at bot of deque

            // find the leftmost vertex to connect to
            while (isLeft(D[top-1], D[top], points[i]) <= 0)
                --top;

            D[++top] = points[i];        // push V[i] onto top of deque
        }

        LineString* result = new LineString;
        for (int i = 0; i <= (top-bottom); i++)
        {
            D[bottom + i].setZ(0);
            result->addPoint(D[bottom + i]);
        }
        if (result->isEmpty())
        {
            delete result;
            return NULL;
        }
        else
        {
            Polygon* returnValue = new Polygon;
            returnValue->addRing(result);
            return returnValue;
        }
    }

//GRAHAM =========================================================================================

    void GrahamHull::findLowest(void)
    {
        int n = 0;
        for (int i = 0; i < num; i++)
        {
            if ( (P[n]->p.Y() > P[i]->p.Y()) || (P[n]->p.Y() == P[i]->p.Y() && P[n]->p.X() > P[i]->p.X()) )
                n = i;
        }

        if (n != 0) swap(0,n);
    }

    int GrahamHull::comparePoints(GrahamPoint* a, GrahamPoint* b)
    {
        int comp = CrossProduct(P[0]->p,a->p,P[0]->p,b->p);
        if (comp > 0) return -1;
        else if (comp < 0) return 1;
        else
        {
            double x = abs(a->p.X() - P[0]->p.X()) - abs(b->p.X() - P[0]->p.X());
            double y = abs(a->p.Y() - P[0]->p.Y()) - abs(b->p.Y() - P[0]->p.Y());

            if (x<0 || y<0)
            {
                a->toDelete = true;
                return -1;
            }
            else if (x>0 || y>0)
            {
                b->toDelete = true;
                return 1;
            }
            else
            {
                return 0;
            }
        }
    }

    void GrahamHull::swap(int i, int j)
    {
        GrahamPoint* temp = P[i];
        P[i] = P[j];
        P[j] = temp;
    }

    void GrahamHull::quickSort(int left, int right)
    {
        int i = left, j = right;
        GrahamPoint* pivot = P[(left + right) / 2];

        while (i <= j) {
            while (comparePoints(P[i],pivot) < 0)
                i++;
            while (comparePoints(P[j],pivot) > 0)
                j--;
            if (i <= j) {
                swap(i,j);
                i++;
                j--;
            }
        };

        if (left < j)
            quickSort(left, j);
        if (i < right)
            quickSort(i, right);
    }

    void GrahamHull::sort(void)
    {
        quickSort(1,num-1);
    }

    void GrahamHull::compress(void)
    {
    /*    This method doesn't actually delete anything, it just pushes the "deleted" points to the end of
        the vector and ignores them for the rest of the computation.
    */
        int j = 0;
        int i = 0;
        int nextGroup = 0;

        //pre compress first group to ensure that the first point is not deleted
        for (i = 1; i < num; i++)
        {
            if (P[0]->p.equals(&P[i]->p)) nextGroup++;
            else break;
        }
        j = 1;

        //compress the rest of the array
        for (i = nextGroup+1; i < num; i++)
        {
            bool deleteGroup = false;
            nextGroup = i;

            //Find the end of this duplicate group
            for (int k = i+1; k < num; k++)
            {
                if (P[k]->p.equals(&P[i]->p))
                {
                    if (P[k]->toDelete) deleteGroup = true;
                    nextGroup++;
                }
                else break;
            }

            if (!P[i]->toDelete && !deleteGroup)
            {
                P[j] = P[i];
                j++;
            }
            i = nextGroup;
        }

        num = j;
    }

    void GrahamHull::compute(void)
    {
        if (num == 0)
        {
            return;
        }
        else if (num == 1)
        {
            hull.push_back(_P[0].p);
        }
        else if (num == 2)
        {
            hull.push_back(_P[0].p);
            hull.push_back(_P[1].p);
        }
        else if (num == 3)
        {
            hull.push_back(_P[0].p);
            hull.push_back(_P[1].p);
            hull.push_back(_P[2].p);
            hull.push_back(_P[0].p);

            if (    (hull[1].X() - hull[0].X())*(hull[2].Y() - hull[0].Y()) <
                    (hull[2].X() - hull[0].X())*(hull[1].Y() - hull[0].Y()) )
                    std::reverse(hull.begin(),hull.end());
        }
        else
        {
            findLowest();
            sort();
            compress();

        /*
            Quick test to ensure there is a hull..if there are only 2 points in the queue, then that means
            the rest were deleted because of collinearity, meaning the result is simple a line between the two
            points.
        */
            if (num == 2)
            {
                hull.push_back(P[0]->p);
                hull.push_back(P[1]->p);
                hull.push_back(hull.front());
                return;
            }

        //    Create Resulting Hull
            hull.push_back(P[0]->p);
            hull.push_back(P[1]->p);
            hull.push_back(P[2]->p);

            int i = 3;

            while (i < num)
            {
                Point p1 = hull[hull.size() - 2];
                Point p2 = hull.back();
                if (CrossProduct(p1,p2,p1,P[i]->p) > 0)
                {
                    hull.push_back(P[i]->p);
                    i++;
                }
                else
                {
                    hull.pop_back();
                }

            }

        //    Close hull
            hull.push_back(hull.front());
        }

    //    cleanup
        _P.clear();
        P.clear();
        num = int(hull.size());
    }

    GrahamHull::GrahamHull(const std::vector<Point>& points)
    {
        // First snap all points to a grid using SFA_EPSILON
        // This Graham Hull algorithm falls apart when points are too
        // close together because the left/right detection fails.
        // There is a bug in this code that causes both points to be deleted
        // in some cases where two points are within SFA_EPSILON distance.
        // For now this is an effective workaround to that problem
        const double epsilon = SFA_EPSILON;
        std::set<sfa::Point> grid;//All unique points here
        BOOST_FOREACH(sfa::Point pt,points)
        {
            double closest_dist = DBL_MAX;
            BOOST_FOREACH(const sfa::Point &gridpt,grid)
            {
                closest_dist = std::min<double>(closest_dist,gridpt.distance(&pt));
            }
            // If pt is not within epsilon distance from any grid point, insert pt into the grid
            if(closest_dist>epsilon)
            {
                grid.insert(pt);
            }
        }
        std::vector<sfa::Point> snappedPoints;
        snappedPoints.insert(snappedPoints.begin(),grid.begin(),grid.end());

        num = int(snappedPoints.size());
        _P.reserve(num);
        for (int j = 0; j < num; j++)
        {
            _P.push_back(GrahamPoint(snappedPoints[j]));
            _P.back().p.setZ(0);
            P.push_back(&_P.back());
        }

        compute();
    }

    GrahamHull::GrahamHull(const MultiPoint* a)
    {
        num = a->getNumGeometries();
        _P.reserve(num);
        for (int j = 0; j < num; j++)
        {
            _P.push_back(GrahamPoint(*static_cast<Point*>(a->getGeometryN(j+1))));
            _P.back().p.setZ(0);
            P.push_back(&_P.back());
        }

        compute();
    }

    Geometry* GrahamHull::getHullGeometry(void)
    {
        if (num == 0)
            return NULL;
        else if (num == 1)
            return new Point(hull[0]);
        else
        {
            LineString* line = new LineString;
            for (std::vector<Point>::iterator it = hull.begin(), end = hull.end(); it != end; it++)
                line->addPoint(*it);
            if (num == 2)
                return line;
            else
            {
                Polygon* polygon = new Polygon;
                polygon->addRing(line);
                return polygon;
            }
        }
    }

    std::vector<Point> GrahamHull::getHullPoints(void)
    {
        return hull;
    }

    Geometry* GrahamHull::apply(const MultiPoint* a)
    {
        GrahamHull hull(a);
        return hull.getHullGeometry();
    }

    std::vector<Point> GrahamHull::apply(const std::vector<Point>& points)
    {
        GrahamHull hull(points);
        return hull.getHullPoints();
    }

}


#define EIGEN_USE_MKL_ALL
#define MLT_VERBOSE

#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <chrono>

#include <Eigen/Core>

#include "utils/optimizers/stochastic_gradient_descent.hpp"
#include "utils/loss_functions.hpp"
#include "utils/activation_functions.hpp"
#include "../misc.hpp"
#include "models/transformers/sparse_autoencoder.hpp"
#include "models/classifiers/optimizable_linear_classifier.hpp"
#include "models/classifiers/perceptron.hpp"

using namespace std;

using namespace Eigen;

using namespace mlt::models::transformers;
using namespace mlt::models::classifiers;
using namespace mlt::utils;

vector<vector<double>> parseCsv(string file, bool skipFirstLine, char delim = ',') {
	vector<vector<double>> result;

	ifstream str(file.c_str());
		string line;

	if (skipFirstLine) {
		getline(str, line);
	}

	while (getline(str, line)) {
		vector<double> currentLine;
		stringstream lineStream(line);
		string cell;

		while (getline(lineStream, cell, delim)) {
			currentLine.push_back(stod(cell));
		}

		result.push_back(currentLine);
	}

	return result;
}

tuple<MatrixXd, VectorXi> load_training_data(string file) {
	auto data = parseCsv(file, true);
	MatrixXd features(data[0].size(), data.size());
	VectorXi classes(data.size());

	for(auto i = 0; i < data.size(); i++) {
		classes(i) = data[i][0];
		for (auto j = 1; j < data[i].size(); j++) {
			features(j - 1, i) = data[i][j] / 255.0;
		}
	}

	return{ features, classes };
}

MatrixXd load_test_data(string file) {
	auto data = parseCsv(file, true);
	MatrixXd features(data[0].size(), data.size());

	for (auto i = 0; i < data.size(); i++) {
		for (auto j = 0; j < data[i].size(); j++) {
			features(j, i) = data[i][j] / 255.0;
		}
	}

	return features;
}

void output_result(const VectorXi& result, string filename) {
	ofstream outputFile(filename, ofstream::app);

	outputFile << "ImageId,Label" << endl;

	for (size_t i = 0; i < result.rows(); i++) {
		outputFile << i + 1 << "," << result(i) << endl;
	}

	outputFile.close();

}

string current_date_time() {
	auto now = chrono::system_clock::now();
	auto in_time_t = chrono::system_clock::to_time_t(now);

	stringstream ss;
	ss << put_time(localtime(&in_time_t), "%Y%m%d%H%M%S");
	return ss.str();
}

template <class Model, class TargetType>
double split_crossvalidation(Model& model, const MatrixXd& features, const TargetType& target, double training_percentage) {
	assert(features.cols() == target.cols() || (target.cols() == 1 && features.cols() == target.rows()));
	assert(training_percentage > 0 && training_percentage < 1);
	auto training = static_cast<size_t>(round(features.cols() * training_percentage));
	auto validation = features.cols() - training;
	assert(training > 0 && validation > 0);

	auto training_features = features.leftCols(training);
	auto training_target = target.cols() == 1 ? target.block(0, 0, training, 1) : target.block(0, 0, target.rows(), training);
	auto validation_features = features.rightCols(validation);
	auto validation_target = target.cols() == 1 ? target.block(training, 0, validation, 1) : target.block(0, training, target.rows(), validation);

	model.fit(training_features, training_target);

	return model.score(validation_features, validation_target);
}

template <class Transformation, class Model, class TargetType>
double split_crossvalidation(Transformation& transformation, Model& model, const MatrixXd& features, const TargetType& target, double training_percentage) {
	assert(features.cols() == target.cols() || (target.cols() == 1 && features.cols() == target.rows()));
	assert(training_percentage > 0 && training_percentage < 1);
	auto training = static_cast<size_t>(round(features.cols() * training_percentage));
	auto validation = features.cols() - training;
	assert(training > 0 && validation > 0);

	auto training_features = features.leftCols(training);
	auto training_target = target.cols() == 1 ? target.block(0, 0, training, 1) : target.block(0, 0, target.rows(), training);
	auto validation_features = features.rightCols(validation);
	auto validation_target = target.cols() == 1 ? target.block(training, 0, validation, 1) : target.block(0, training, target.rows(), validation);

	transformation.fit(training_features, training_target);
	model.fit(transformation.transform(training_features), training_target);

	return model.score(transformation.transform(validation_features), validation_target);
}

int main() {
	print_info();
	cout << endl;

	MatrixXd features;
	VectorXi classes;

	tie(features, classes) = load_training_data("train.csv");

	//auto output_filename = "digit_recognizer_cross_val_" + current_date_time() + ".csv";
	//ofstream output_file(output_filename);

	//output_file << "Type;Batch Size;Learning Rate;Weight Decay;L2 Regularization;Score" << endl;

	for (auto batch_size : { 512, 256, 1024, 2048 }) {
		for (auto learning_rate : { 0.1, 0.01, 0.001, 0.0001 }) {
			for (auto decay : { 0.99, 0.95 }) {
				for (auto regularization : { 0.005, 0.0005 }) {

					using loss_t = loss_functions::SoftmaxLoss;
					using act_t = activation_functions::SigmoidActivation;
					using opt_t = optimizers::StochasticGradientDescent<>;

					loss_t loss;
					act_t act;
					opt_t opt1(batch_size, 10, 0.001, decay);
					opt_t opt2(batch_size, 200, learning_rate, decay);

					//auto model1 = create_sparse_autoencoder(196, act, act, opt1, 3e-3, 0.1, 3);
					auto model1 = create_perceptron(200, true, learning_rate, true);
					auto model2 = OptimizableLinearClassifier<loss_t, opt_t>(loss, opt2, regularization, true);

					//auto score = split_crossvalidation(model1, model2, features, classes, 0.8);
					//cout << "Score for SparseAutoencoder -> Softmax " << batch_size << " " << learning_rate << " " << decay << " " << regularization << ": " << score << endl;
					//output_file << "SparseAutoencoder -> Softmax;" << batch_size << ";" << learning_rate << ";" << decay << ";" << regularization << ";" << score << endl;

					auto score = split_crossvalidation(model1, features, classes, 0.8);
					cout << "Score for Perceptron " << batch_size << " " << learning_rate << " " << decay << " " << regularization << ": " << score << endl;
					// output_file << "SparseAutoencoder -> Softmax;" << batch_size << ";" << learning_rate << ";" << decay << ";" << regularization << ";" << score << endl;

					auto score2 = split_crossvalidation(model2, features, classes, 0.8);
					cout << "Score for Softmax " << batch_size << " " << learning_rate << " " << decay << " " << regularization << ": " << score2 << endl;
					//output_file << "Softmax;" << batch_size << ";" << learning_rate << ";" << decay << ";" << regularization << ";" << score2 << endl;
				}
			}
		}
	}

	cin.get();

	return 0;
}


//
//  su3_x.hpp
//  Lattice
//
//  Created by Evan Owen on 10/19/18.
//  Copyright © 2018 Evan Owen. All rights reserved.
//

#ifndef su3_x_hpp
#define su3_x_hpp

#include <random>
#include <complex>
#include <Eigen/Dense>

class su3_x_lattice;
typedef Eigen::Matrix<std::complex<double>, 3, 3> su3_link;
typedef Eigen::Matrix<std::complex<double>, 1, 3> su3_vector;
#define su3_identity su3_link::Identity()
#define su3_zero su3_link::Zero()

typedef Eigen::Matrix<std::complex<double>, 2, 2> su2_link;
#define su2_identity su2_link::Identity()
#define su2_zero su2_link::Zero()

#define I complex<double>(0,1)
#define Q(s) s->link[0]
#define D_MAX 6
#define SQRT3   1.73205080756887729352744634151  // sqrt(3)
#define SQRT1_3 0.577350269189625764509148780502 // 1 / sqrt(3)

class su3_x_site {
public:
    // variables
    su3_x_lattice* lattice; // parent lattice
    su3_link link[D_MAX]; // link values in each direction
    su3_link link_inverse[D_MAX]; // link inverse values
    su3_link p_link[D_MAX]; // conjugate momenta in each direction
    su3_x_site* forward[D_MAX]; // adjacent sites in the forward direction
    su3_x_site* backward[D_MAX]; // adjacent sites in the backward direction
    std::mt19937* gen; // random number generator for this time-slice
    bool forward_edge; // site is at the forward edge of the extra dimension
    bool backward_edge; // site is at the backward edge of the extra dimension
    bool is_locked;
    double eps;
    su3_link wf_z[D_MAX]; // exponent for wilson flow

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // methods
    void init(su3_x_lattice* lattice, su3_x_site* lattice_sites, int s);
    su3_link make_unitary(const su3_link& g);
    su3_link cayley_ham(const su3_link& Q);
    bool lock();
    void unlock();
    double rand_double(double min = 0.0, double max = 1.0);
    int rand_int(int min, int max);
    double rand_normal(double mean = 0.0, double stdev = 1.0);
    void reset_links(bool cold);
    void copy_links(su3_x_site* site);
    void read_links(std::ifstream& ckptFile, bool bigEndian);
    void set_link(int d, const su3_link& value);
    void init_momenta();
    su2_link create_su2(bool random);
    su3_link create_link(bool random);
    double action();
    double hamiltonian();
    void hmc_step_p(double frac);
    void hmc_step_link();
    su3_link p_link_dot(int d);
    double link_trace();
    double plaq();
    su3_link plaquette(int d1, int d2);
    su3_link staple(int d1);
    su3_link reverse_staple(int d1);
    su3_link staple_x(int d1);
    su3_link reverse_staple_x(int d1);
    su3_link cloverleaf(int d1, int d2);
    double wilson_loop(int a, int b);
    std::complex<double> polyakov_loop(int r);
    double correlator(int T);
    double field_strength();
//    double field_strength_x();
    double topological_charge();
    double mag_U();
    double abs_U();
    double four_point(int T, int R);
    su3_link overrelax(su3_link g);
    void relax(bool coulomb);
    double sum_landau();
    double sum_coulomb();
    su3_link sum_G(bool coulomb);
    double heat_bath();
    double heat_bath_link(int d1);
    void cool();
    void cool_link(int d1);
    void wilson_flow(su3_x_site* target, int step);
    void wilson_flow_link(su3_x_site* target, int step, int d);
    void stout_smear(su3_x_site* target, double rho);
    void stout_smear_link(su3_x_site* target, double rho, int d1);
};

class su3_x_lattice {
public:
    // variables
    int N; // array size (spacial)
    int T; // array size (time)
    int N5; // 5th dimension size
    int D; // number of dimensions
    double beta; // coupling factor
    double eps5; // ratio of coupling in extra dimension to normal coupling
    int n_sites; // number of sites in the entire lattice
    int n_sites_5; // number of sites in a 5d sub-lattice
    int n_slice; // number of sites in each time slice of the entire lattice
    int n5_center; // center lattice
    std::vector<su3_x_site> site; // site values
    std::vector<std::mt19937> gen; // array of random number generators (one for each time-slice)
    std::vector<su2_link> sigma; // pauli spin matrices
    std::vector<su3_link> lambda; // gell-mann matrices
    double z; // heat bath metropolis factor
    int verbose;
    bool parallel;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // hmc
    std::vector<su3_x_site> site_1; // site values for hmc
    double dt; // hmc step size
    int n_steps; // hmc step count
    int hmc_accept; // number of accepted hmc configurations
    int hmc_count; // number of attempted hmc configurations

    // time step for wilson flow algorithm
    double wf_dt;

    // methods
    su3_x_lattice(int N, int T, int N5, int D, double beta, double eps5, bool cold = false);
    su3_x_lattice(su3_x_lattice* lattice);
    su3_x_lattice(int N, int T, int N5, int D, double beta, double eps5, std::ifstream& ckptFile, bool isNersc = false);
    ~su3_x_lattice();
    void init();
    double rand_double(double min = 0.0, double max = 1.0);
    int rand_int(int min, int max);
    double rand_normal(double mean = 0.0, double stdev = 1.0);
    int get_site(int s, int d, int n);
    double link_trace();
    double plaq(int n5);
    double action(int n5);
    double wilson_loop(int a, int b, int n5);
    double polyakov_loop(int R, int n5);
    double correlator(int T, int n5);
    double four_point(int T, int R, int n5);
    double hamiltonian();
    void hmc(int n_sweeps = 0, bool update_dt = false, bool no_metropolis = false);
    void heat_bath(int n_sweeps = 0);
    void cool(int n5, int n_sweeps = 0);
//    void wilson_flow(int n_sweeps = 0);
    void wilson_flow(int n5, int n_sweeps = 0);
    void stout_smear(int n5, double rho = 0.1, int n_sweeps = 0);
    double field_strength(int n5);
//    double field_strength_x(int n5);
    double topological_charge(int n5);
    int thermalize(int n_min = 0, int n_max = 0);
    double ave_link_t(int n5);
    double theta(bool coulomb, int n5);
    long double relax(long double error_target, bool coulomb, int n5);
    void write_four_point(const char* filename, bool coulomb, int n5);
    void write_correlator(const char* filename, bool coulomb, int n5);
};

#endif /* su3_x_hpp */


#include <test_common.h>
#include <igl/triangle_triangle_adjacency.h>
#include <Eigen/Geometry>

TEST_CASE("triangle_triangle_adjacency: dot", "[igl]")
{
  const auto test_case = [](const std::string &param)
  {
    Eigen::MatrixXd V;
    Eigen::MatrixXi F,TT,TTi;
    // Load example mesh: GetParam() will be name of mesh file
    test_common::load_mesh(param, V, F);
    igl::triangle_triangle_adjacency(F,TT,TTi);
    REQUIRE (TT.rows() == F.rows());
    REQUIRE (TTi.rows() == F.rows());
    REQUIRE (TT.cols() == F.cols());
    REQUIRE (TTi.cols() == F.cols());
    for(int f = 0;f<F.rows();f++)
    {
      for(int c = 0;c<3;c++)
      {
        if(TT(f,c) >= 0)
        {
          REQUIRE (F.rows() > TT(f,c));
          REQUIRE (0 <= TTi(f,c));
          REQUIRE (3 > TTi(f,c));
          REQUIRE (f == TT(TT(f,c),TTi(f,c)));
        }
      }
    }
    // REQUIRE (b == a);
    // REQUIRE (a==b);
    // REQUIRE(a == Approx(b).margin(1e-15))
    // REQUIRE (1e-12 > a);
  };

  test_common::run_test_cases(test_common::manifold_meshes(), test_case);
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 5> test_int_6_5(const Eigen::Matrix<int, 6, 5> & M)
{
	return M;
}
void export_int_6_5()
{
	boost::python::def("test_int_6_5",test_int_6_5);
}


#include <doctest/doctest.h>

#include <Eigen/Dense>
#include <robotics/linear_control/lqr.hpp>
#include <string>

TEST_CASE("Robotics")
{
    static constexpr int N = 2;
    static constexpr int M = 1;

    const double dt = 0.1;

    // State matrix
    Robotics::SquareMatrix<N> A;
    A << dt, 1.0, 0, dt;

    // Control matrix
    Robotics::Matrix<N, M> B;
    B << 0, 1;

    // Weights
    Robotics::SquareMatrix<N> Q = Robotics::SquareMatrix<N>::Identity();
    Robotics::SquareMatrix<M> R = Robotics::SquareMatrix<M>::Identity();

    Robotics::LinearControl::LQR<N, M> planner(A, B, Q, R);

    CHECK(true == true);
}


#include <boost/random/lognormal_distribution.hpp>


#define EIGEN_DEFAULT_DENSE_INDEX_TYPE int32_t

#include "../../../repos/stl_reader/stl_reader.h"
#include <map>
#include <array>
#include <deque>
#include <chrono>
#include <random>
#include <vector>
#include <fstream>
#include <iostream>
#include <unordered_set>
#include <Eigen/Dense>

using Vertex = Eigen::Vector3f;
using Transform = Eigen::Transform<float, 3, Eigen::Affine>;
using Triangle = std::array<Vertex, 3u>;
using CudaTriangle = Vertex*;
using CudaConstTriangle = Vertex const*;
using Mesh = std::vector<Triangle>;

// usage ./IntersectionReference example23.in

constexpr float    cgPi                            =    3.1415926539f;
constexpr int32_t  cgSphereSectors                 =    11;
constexpr int32_t  cgSphereBelts                   =    5;
constexpr float    cgMaxSphereRadius               =   17.0f;
constexpr float    cgMaxTriangleSide               = cgMaxSphereRadius * 2.0f;
constexpr float    cgCalculateSpheresInflate       =    0.51f;
constexpr int32_t  cgCalculateSpheresNeighbours    =    3;
constexpr float    cgApproximateLeaveInPlaceFactor =    0.01f;
constexpr uint32_t cgApproximateResultSize         =   64u;       // TODO remove and use total surface to compute this value T = sqrt(s*(s-c)*(s-b)*(s-a))
constexpr int32_t  cgApproximateIterations         = 4444u;       // TODO perhaps depends on the result size
constexpr float    cgApproximateTemperatureFactor  =    0.001f;

Eigen::Matrix3f randomTransform() {
  std::default_random_engine generator;
  generator.seed((std::chrono::high_resolution_clock::now() - std::chrono::high_resolution_clock::time_point::min()).count());
  std::uniform_real_distribution<float> distribution(0.1f, 1.0f);
  Eigen::Matrix3f result;
  for(int32_t i = 0; i < 9; ++i) {
    result(i / 3, i % 3) = distribution(generator);
  }
  return result;
}

std::deque<Triangle> divideLargeTriangles(std::deque<Triangle> &aMesh) {
  std::deque<Triangle> result;
  for(auto const &triangle : aMesh) {
    float maxSide = (triangle[0] - triangle[1]).norm();
    maxSide = std::max(maxSide, (triangle[0] - triangle[2]).norm());
    maxSide = std::max(maxSide, (triangle[1] - triangle[2]).norm());
    int32_t divisor = static_cast<int32_t>(std::ceil(maxSide / cgMaxTriangleSide));
    auto vector01 = (triangle[1] - triangle[0]) / divisor;
    auto vector02 = (triangle[2] - triangle[0]) / divisor;
    auto lineBase = triangle[0];
    auto base0 = lineBase;
    auto base1 = (divisor > 1) ? (base0 + vector01) : triangle[1];
    auto base2 = (divisor > 1) ? (base0 + vector02) : triangle[2];
    for(int i = 0; i < divisor - 1; ++i) {
      for(int j = 0; j < divisor - i - 1; j++) {
        result.push_back({base0, base1, base2});
        auto base1next = base1 + vector02;
        result.push_back({base1, base1next, base2});
        base1 = base1next;
        base0 = base2;
        base2 += vector02;
      }
      result.push_back({base0, base1, base2});
      lineBase += vector01;
      base0 = lineBase;
      base1 = base0 + vector01;
      base2 = base0 + vector02;
    }
    result.push_back({base0, triangle[1], base2});
  }
  return result;
}

auto readMesh(std::string const &aFilename, Eigen::Vector3f aTranslation, Eigen::Matrix3f const &aTransform) {
  std::deque<Triangle> work;
  stl_reader::StlMesh<float, int32_t> mesh(aFilename);
  for(int32_t indexTriangle = 0; indexTriangle < mesh.num_tris(); ++indexTriangle) {
    Triangle triangle;
    for(int32_t indexCorner = 0; indexCorner < 3; ++indexCorner) {
      float const * const coords = mesh.tri_corner_coords(indexTriangle, indexCorner);
      Eigen::Vector3f in;
      for(int32_t i = 0; i < 3; ++i) {
        in(i) = coords[i];
      }
      triangle[indexCorner] = /*aTransform **/ (in + aTranslation);
    }
    work.push_back(triangle);
  }
  std::cout << "bef: " << work.size();
  work = divideLargeTriangles(work);
  std::cout << " aft: " << work.size() << '\n';
  return Mesh(work.cbegin(), work.cend());
}

auto readMesh(char const * const aFilename, Eigen::Matrix3f const &aTransform) {
  std::ifstream in(aFilename);
  std::string filename1, filename2;
  Eigen::Vector3f translation;
  in >> filename1 >> filename2 >> translation(0) >> translation(1) >> translation(2);
  std::cout << filename1 << '\n' << filename2 << '\n';
  return std::pair(readMesh(filename1, {0.0f, 0.0f, 0.0f}, aTransform), readMesh(filename2, translation, aTransform));
}

void writeMesh(Mesh const &aMesh1, Mesh const &aMesh2, char const * const aFilename) {
  std::ofstream out(aFilename);
  out << "solid Exported from Blender-2.82 (sub 7)\n";
  for(auto const & triangle : aMesh1) {
    out << "facet normal 0.000000 0.000000 0.000000\nouter loop\n";
    for(auto const & vertex : triangle) {
      out << "vertex " << vertex(0) << ' ' << vertex(1) << ' ' << vertex(2) << '\n';
    }
    out << "endloop\nendfacet\n";
  }
  for(auto const & triangle : aMesh2) {
    out << "facet normal 0.000000 0.000000 0.000000\nouter loop\n";
    for(auto const & vertex : triangle) {
      out << "vertex " << vertex(0) << ' ' << vertex(1) << ' ' << vertex(2) << '\n';
    }
    out << "endloop\nendfacet\n";
  }
  out << "endsolid Exported from Blender-2.82 (sub 7)\n";
}

float calculateDistanceSum(std::unordered_set<uint32_t> const &aIndices, std::deque<Vertex> const &aVertices) {
  std::vector<Vertex> selection;
  selection.reserve(cgApproximateResultSize);
  for(auto const &i : aIndices) {
    selection.push_back(aVertices[i]);
  }
  float sum;
  for(uint32_t i = 0u; i < cgApproximateResultSize; ++i) {
    for(uint32_t j = 0u; j < i; ++j) {
      auto diffSquared = (selection[i] - selection[j]).squaredNorm();
      sum += -1.0f / diffSquared;
    }
  }
  return sum;
}

std::vector<Vertex> approximate(Mesh const aMesh, float const aMedianSideSizeHarmonic) {
  std::deque<Vertex> all;
  std::deque<Vertex> reference;
  float limit = aMedianSideSizeHarmonic * cgApproximateLeaveInPlaceFactor;
  for(auto const &triangle : aMesh) {
    for(auto const &vertex : triangle) {
      bool was = false;
      for(auto const &ref : reference) {
        if((ref - vertex).norm() < limit) {
          was = true;
          break;
        }
        else { // nothing to do
        }
      }
      if(!was) {
        reference.push_back(vertex);
      }
      else { // nothing to do
      }
    }
  }
  std::default_random_engine generator;
  generator.seed((std::chrono::high_resolution_clock::now() - std::chrono::high_resolution_clock::time_point::min()).count());
  std::uniform_int_distribution<uint32_t> distributionAll(0, reference.size() - 1);
  std::uniform_int_distribution<uint32_t> distributionSubset(0, cgApproximateResultSize - 1);
  std::uniform_real_distribution<float> distributionFloat(0.0f, 1.0f);
  std::unordered_set<uint32_t> actualIndices;
  std::cout << "mash size (tri): " << aMesh.size() << "  ref size (vert): " << reference.size();
  size_t iterationsNeeded = 0u;                            // TODO slow and fails if reference.size() <~ cgApproximateResultSize
  while(actualIndices.size() < cgApproximateResultSize) {
    uint32_t randomIndex = distributionAll(generator);
    if(actualIndices.find(randomIndex) == actualIndices.end()) {
      actualIndices.insert(randomIndex);
    }
    else { // nothing to do
    }
    ++iterationsNeeded;
  }
  std::cout << " iter needed: " << iterationsNeeded << '\n';
  std::unordered_set<uint32_t> bestIndices = actualIndices;
  float actualDistancesSum = calculateDistanceSum(actualIndices, reference);
  float bestDistancesSum = actualDistancesSum;
  float initialTemperature = cgApproximateTemperatureFactor / aMedianSideSizeHarmonic / aMedianSideSizeHarmonic * reference.size() * reference.size();
  for(int32_t i = 0u; i < cgApproximateIterations; ++i) {
    auto candidateIndices = actualIndices;
    uint32_t toRemoveIndex = distributionSubset(generator);
    auto thisOne = candidateIndices.begin();
    for(uint32_t j = 0u; j < toRemoveIndex; ++j) {
      ++thisOne;
    }
    candidateIndices.erase(thisOne);
    while(true) {
      uint32_t toInsert = distributionAll(generator);
      if(candidateIndices.find(toInsert) == candidateIndices.end()) {
        candidateIndices.insert(toInsert);
        break;
      }
      else { // nothing to do
      }
    }
    float candidateDistancesSum = calculateDistanceSum(candidateIndices, reference);
    if(candidateDistancesSum > bestDistancesSum) {
      bestDistancesSum = candidateDistancesSum;
      bestIndices = candidateIndices;
    }
    else { // nothing to do
    }
    float diff = actualDistancesSum - candidateDistancesSum;
    float temperature = initialTemperature / (i + 1);
    if(diff < 0.0f || distributionFloat(generator) < ::expf(-diff / temperature)) {
      actualIndices = candidateIndices;
      actualDistancesSum = candidateDistancesSum;
    }
    else { // nothing to do
    }
  }
  std::vector<Vertex> result;
  result.reserve(cgApproximateResultSize);
  for(auto const &i : bestIndices) {
    result.push_back(reference[i]);
  }
  return result;
}

std::vector<float> calculateSpheres(std::vector<Vertex> const &aPoints) {
  Vertex aabb1{std::numeric_limits<float>::max(), std::numeric_limits<float>::max(), std::numeric_limits<float>::max()};
  Vertex aabb2{-std::numeric_limits<float>::max(), -std::numeric_limits<float>::max(), -std::numeric_limits<float>::max()};
  for(auto const &point : aPoints) {
    for(size_t i = 0u; i < 3u; ++i) {
      aabb1[i] = std::min(point[i], aabb1[i]);
      aabb2[i] = std::max(point[i], aabb2[i]);
    }
  }
  float max = (aabb2 - aabb1).norm();

  std::vector<float> result;
  result.reserve(aPoints.size());
  std::vector<float> distances(aPoints.size() - 1u);
  for(auto const &point: aPoints) {
    distances.clear();
    for(auto const &other: aPoints) {
      if(other != point) {
        distances.push_back(-(point - other).norm());
      }
      else { // nothing to do
      }
    }
    std::make_heap(distances.begin(), distances.end());
    float nth;
    for(int32_t i = 0; i < cgCalculateSpheresNeighbours; ++i) {
      std::pop_heap(distances.begin(), distances.end());
      nth = -distances.back();
      distances.pop_back();
    }
    result.push_back(nth * cgCalculateSpheresInflate);
  }
  return result;
}

auto harmonic(Triangle const &aTriangle) {
  float a = (aTriangle[0] - aTriangle[1]).norm();
  float b = (aTriangle[1] - aTriangle[2]).norm();
  float c = (aTriangle[2] - aTriangle[0]).norm();
  return std::pair{ 3.0f / (1.0f / a + 1.0f / b + 1.0f / c), std::max({a, b, c})};
}

auto calculateMedianSideSizeHarmonicAndMaxSide(Mesh const &aMesh1, Mesh const &aMesh2) {
  std::deque<float> harmonics;
  float maxSide1 = 0.0f;
  for(auto const &item1 : aMesh1) {
    auto [harm, max] = harmonic(item1);
    harmonics.push_back(harm);
    maxSide1 = std::max(maxSide1, max);
  }
  float maxSide2 = 0.0f;
  for(auto const &item2 : aMesh2) {
    auto [harm, max] = harmonic(item2);
    harmonics.push_back(harm);
    maxSide2 = std::max(maxSide2, max);
  }
  std::sort(harmonics.begin(), harmonics.end());
  float median = harmonics[harmonics.size() / 2u];
  std::cout << "1: " << aMesh1.size() << " 2: " << aMesh2.size() << " med: " << median << " max: " << maxSide1 << ' ' << maxSide2 << '\n';
  return std::pair(median, std::min(maxSide1, maxSide2)); // In general case, if the meshes are farther apart than the less of the maximum sides, they definitely don't intersect.
}

Mesh toTetras(std::vector<Vertex> const& aPoints, float const aMedianSizeHarmonic) {
  Mesh result;
  float size = aMedianSizeHarmonic / 5.0f;
  float cogShift = size / 4.0f;
  for(auto const &point : aPoints) {
    Vertex corner0(-cogShift, -cogShift, -cogShift);
    Vertex corner1(size - cogShift, -cogShift, -cogShift);
    Vertex corner2(-cogShift, size - cogShift, -cogShift);
    Vertex corner3(-cogShift, -cogShift, size - cogShift);
    result.push_back({point + corner0, point + corner1, point + corner2});
    result.push_back({point + corner0, point + corner1, point + corner3});
    result.push_back({point + corner0, point + corner2, point + corner3});
    result.push_back({point + corner1, point + corner2, point + corner3});
  }
  return result;
}

Mesh getUnitSphere() {
  Mesh result;
  result.reserve(cgSphereBelts * 2 * cgSphereSectors);
  float sectorAngleHalf = cgPi / cgSphereSectors;
  float sectorAngleFull = sectorAngleHalf * 2.0f;
  float beltAngle       = cgPi / (cgSphereBelts + 1.0f);
  float bias = 0.0f;
  float beltAngleUp = 0.0f;
  float beltAngleMiddle = beltAngle;
  float beltAngleDown = 2.0f * beltAngle;
  float beltRadiusUp = 0.0f;
  float beltRadiusMiddle = std::sin(beltAngleMiddle);
  float beltRadiusDown = std::sin(beltAngleDown);
  float beltZup = 1.0f;
  float beltZmiddle = std::cos(beltAngleMiddle);
  float beltZdown = std::cos(beltAngleDown);
  for(int32_t belt = 0; belt < cgSphereBelts; ++belt) {
    float sectorAngleUpDown = bias + sectorAngleHalf;
    float sectorAngleMiddle1 = bias + 0.0f;
    float sectorAngleMiddle2 = bias + sectorAngleFull;
    for(int32_t sector = 0; sector < cgSphereSectors; ++sector) {
      Vertex corner1(beltRadiusUp * std::sin(sectorAngleUpDown), beltRadiusUp * std::cos(sectorAngleUpDown), beltZup);
      Vertex corner2(beltRadiusMiddle * std::sin(sectorAngleMiddle1), beltRadiusMiddle * std::cos(sectorAngleMiddle1), beltZmiddle);
      Vertex corner3(beltRadiusMiddle * std::sin(sectorAngleMiddle2), beltRadiusMiddle * std::cos(sectorAngleMiddle2), beltZmiddle);
      result.push_back({corner1, corner2, corner3});
      corner1 = {beltRadiusDown * std::sin(sectorAngleUpDown), beltRadiusDown * std::cos(sectorAngleUpDown), beltZdown};
      result.push_back({corner2, corner3, corner1});
      sectorAngleUpDown += sectorAngleFull;
      sectorAngleMiddle1 = sectorAngleMiddle2;
      sectorAngleMiddle2 += sectorAngleFull;
    }
    beltAngleUp = beltAngleMiddle;
    beltAngleMiddle = beltAngleDown;
    beltAngleDown += beltAngle;
    beltRadiusUp = beltRadiusMiddle;
    beltRadiusMiddle = beltRadiusDown;
    beltRadiusDown = std::sin(beltAngleDown);
    beltZup = beltZmiddle;
    beltZmiddle = beltZdown;
    beltZdown = std::cos(beltAngleDown);
    bias += sectorAngleHalf;
  }
  return result;
}

Mesh toSpheres(std::vector<Vertex> const& aPoints, std::vector<float> const& aRadii) {
  static auto unitSphere = getUnitSphere();
  Mesh result;
  result.reserve(unitSphere.size() * aPoints.size());
  for(size_t i = 0u; i < aPoints.size(); ++i) {
    auto &point = aPoints[i];
    auto &radius = aRadii[i];
    for(auto const &face : unitSphere) {
      result.push_back({face[0] * radius + point, face[1] * radius + point, face[2] * radius + point});
    }
  }
  return result;
}

auto calculateDistance(std::vector<Vertex> const &aApproximate1, std::vector<Vertex> const &aApproximate2) {
  float distance = std::numeric_limits<float>::max();
  for(auto const &vertex1 : aApproximate1) {
    for(auto const &vertex2 : aApproximate2) {
      distance = std::min(distance, (vertex1 - vertex2).norm());
    }
  }
  return distance;
}

void check(Mesh const &aMesh1, Mesh const &aMesh2) {
  auto [medianSideSizeHarmonic, maxSide] = calculateMedianSideSizeHarmonicAndMaxSide(aMesh1, aMesh2);
  auto approximate1 = approximate(aMesh1, medianSideSizeHarmonic);
  auto approximate2 = approximate(aMesh2, medianSideSizeHarmonic);
  auto sphereSizes1 = calculateSpheres(approximate1);
  auto sphereSizes2 = calculateSpheres(approximate2);
  for(auto i : sphereSizes1) {
    std::cout << i << ' ';
  }
  for(auto i : sphereSizes2) {
    std::cout << i << ' ';
  }
  std::cout << '\n';
  auto distance = calculateDistance(approximate1, approximate2);
  std::cout << "dist: " << distance << '\n';
  writeMesh(toTetras(approximate1, medianSideSizeHarmonic), toTetras(approximate2, medianSideSizeHarmonic), "points.stl");
  writeMesh(toSpheres(approximate1, sphereSizes1), toSpheres(approximate2, sphereSizes2), "spheres.stl");
}

int main(int argc, char **argv) {
  int ret = 0;
  if(argc < 2) {
    std::cerr << "Usage: " << argv[0] << " <filenameIn>\n";
    ret = 1;
  }
  else {
    try {
      auto transform = randomTransform();
      auto [mesh1, mesh2] = readMesh(argv[1], transform);
      writeMesh(mesh1, mesh2, "out.stl");
      check(mesh1, mesh2);
    }
    catch(std::exception &e) {
      ret = 2;
    }
  }
  return ret;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Regression

#include <mpi.h>

#include <boost/test/unit_test.hpp>

#include <xolotl/core/Constants.h>
#include <xolotl/core/temperature/HeatEquationHandler.h>
#include <xolotl/options/Options.h>
#include <xolotl/test/config.h>

using namespace std;
using namespace xolotl;
using namespace core;
using namespace temperature;

/**
 * This suite is responsible for testing the HeatEquationHandler.
 */
BOOST_AUTO_TEST_SUITE(HeatEquationHandler_testSuite)

/**
 * Method checking the initialization of the off-diagonal and diagonal part of
 * the Jacobian, and the compute temperature methods.
 */
BOOST_AUTO_TEST_CASE(checkHeat1D)
{
	MPI_Init(NULL, NULL);
	// Set the DOF
	const int dof = 9;

	// Create the heat handler
	auto heatHandler = HeatEquationHandler(5.0e-12, 1000.0, 1);
	heatHandler.setHeatCoefficient(tungstenHeatCoefficient);
	heatHandler.setHeatConductivity(tungstenHeatConductivity);

	// Check the initial temperatures
	BOOST_REQUIRE_CLOSE(
		heatHandler.getTemperature({0.0, 0.0, 0.0}, 0.0), 1000.0, 0.01);
	BOOST_REQUIRE_CLOSE(
		heatHandler.getTemperature({1.0, 0.0, 0.0}, 0.0), 1000.0, 0.01);

	// Create ofill
	network::IReactionNetwork::SparseFillMap ofill;
	// Create dfill
	network::IReactionNetwork::SparseFillMap dfill;

	// Initialize it
	heatHandler.initializeTemperature(dof, ofill, dfill);

	// Check that the temperature "diffusion" is well set
	BOOST_REQUIRE_EQUAL(ofill[9][0], 9);
	BOOST_REQUIRE_EQUAL(dfill[9][0], 9);

	// The size parameter in the x direction
	double hx = 1.0;

	// The arrays of concentration
	double concentration[3 * (dof + 1)];
	double newConcentration[3 * (dof + 1)];

	// Initialize their values
	for (int i = 0; i < 3 * (dof + 1); i++) {
		concentration[i] = (double)i * i;
		newConcentration[i] = 0.0;
	}

	// Get pointers
	double* conc = &concentration[0];
	double* updatedConc = &newConcentration[0];

	// Get the offset for the grid point in the middle
	// Supposing the 3 grid points are laid-out as follow:
	// 0 | 1 | 2
	double* concOffset = conc + (dof + 1);
	double* updatedConcOffset = updatedConc + (dof + 1);

	// Fill the concVector with the pointer to the middle, left, and right grid
	// points
	double* concVector[3]{};
	concVector[0] = concOffset; // middle
	concVector[1] = conc; // left
	concVector[2] = conc + 2 * (dof + 1); // right

	// Compute the heat equation at this grid point
	heatHandler.computeTemperature(concVector, updatedConcOffset, hx, hx, hx);

	// Check the new values of updatedConcOffset
	BOOST_REQUIRE_CLOSE(updatedConcOffset[9], 1.367e+16, 0.01);

	// Set the temperature in the handler
	heatHandler.setTemperature(concOffset);
	// Check the updated temperature
	plsm::SpaceVector<double, 3> pos{1.0, 0.0, 0.0};
	BOOST_REQUIRE_CLOSE(heatHandler.getTemperature(pos, 1.0), 361.0, 0.01);

	// Initialize the indices and values to set in the Jacobian
	IdType indices[1];
	double val[3];
	// Get the pointer on them for the compute diffusion method
	IdType* indicesPointer = &indices[0];
	double* valPointer = &val[0];

	// Compute the partial derivatives for the heat equation a the grid point
	heatHandler.computePartialsForTemperature(
		valPointer, indicesPointer, hx, hx, hx);

	// Check the values for the indices
	BOOST_REQUIRE_EQUAL(indices[0], 9);

	// Check the values
	BOOST_REQUIRE_CLOSE(val[0], -1.367e+14, 0.01);
	BOOST_REQUIRE_CLOSE(val[1], 6.835e+13, 0.01);
	BOOST_REQUIRE_CLOSE(val[2], 6.835e+13, 0.01);
}

BOOST_AUTO_TEST_CASE(checkHeat2D)
{
	// Set the DOF
	const int dof = 9;

	// Create the heat handler
	auto heatHandler = HeatEquationHandler(5.0e-12, 1000.0, 2);
	heatHandler.setHeatCoefficient(tungstenHeatCoefficient);
	heatHandler.setHeatConductivity(tungstenHeatConductivity);

	// Check the initial temperatures
	BOOST_REQUIRE_CLOSE(
		heatHandler.getTemperature({0.0, 0.0, 0.0}, 0.0), 1000.0, 0.01);
	BOOST_REQUIRE_CLOSE(
		heatHandler.getTemperature({1.0, 0.0, 0.0}, 0.0), 1000.0, 0.01);

	// Create ofill
	network::IReactionNetwork::SparseFillMap ofill;
	// Create dfill
	network::IReactionNetwork::SparseFillMap dfill;

	// Initialize it
	heatHandler.initializeTemperature(dof, ofill, dfill);

	// Check that the temperature "diffusion" is well set
	BOOST_REQUIRE_EQUAL(ofill[9][0], 9);
	BOOST_REQUIRE_EQUAL(dfill[9][0], 9);

	// The step size in the x direction
	double hx = 1.0;
	// The size parameter in the y direction
	double sy = 1.0;

	// The arrays of concentration
	double concentration[9 * (dof + 1)];
	double newConcentration[9 * (dof + 1)];

	// Initialize their values
	for (int i = 0; i < 9 * (dof + 1); i++) {
		concentration[i] = (double)i * i;
		newConcentration[i] = 0.0;
	}

	// Get pointers
	double* conc = &concentration[0];
	double* updatedConc = &newConcentration[0];

	// Get the offset for the grid point in the middle
	// Supposing the 9 grid points are laid-out as follow:
	// 6 | 7 | 8
	// 3 | 4 | 5
	// 0 | 1 | 2
	double* concOffset = conc + 4 * (dof + 1);
	double* updatedConcOffset = updatedConc + 4 * (dof + 1);

	// Fill the concVector with the pointer to the middle, left, right, bottom,
	// and top grid points
	double* concVector[5]{};
	concVector[0] = concOffset; // middle
	concVector[1] = conc + 3 * (dof + 1); // left
	concVector[2] = conc + 5 * (dof + 1); // right
	concVector[3] = conc + 1 * (dof + 1); // bottom
	concVector[4] = conc + 7 * (dof + 1); // top

	// Compute the heat equation at this grid point
	heatHandler.computeTemperature(
		concVector, updatedConcOffset, hx, hx, hx, sy, 1);

	// Check the new values of updatedConcOffset
	BOOST_REQUIRE_CLOSE(updatedConcOffset[9], 1.367e+17, 0.01);

	// Set the temperature in the handler
	heatHandler.setTemperature(concOffset);
	// Check the updated temperature
	plsm::SpaceVector<double, 3> pos{1.0, 0.0, 0.0};
	BOOST_REQUIRE_CLOSE(heatHandler.getTemperature(pos, 1.0), 2401, 0.01);

	// Initialize the indices and values to set in the Jacobian
	IdType indices[1];
	double val[5];
	// Get the pointer on them for the compute diffusion method
	IdType* indicesPointer = &indices[0];
	double* valPointer = &val[0];

	// Compute the partial derivatives for the heat equation a the grid point
	heatHandler.computePartialsForTemperature(
		valPointer, indicesPointer, hx, hx, hx, sy, 1);

	// Check the values for the indices
	BOOST_REQUIRE_EQUAL(indices[0], 9);

	// Check the values
	BOOST_REQUIRE_CLOSE(val[0], -2.734e+14, 0.01);
	BOOST_REQUIRE_CLOSE(val[1], 6.835e+13, 0.01);
	BOOST_REQUIRE_CLOSE(val[2], 6.835e+13, 0.01);
	BOOST_REQUIRE_CLOSE(val[3], 6.835e+13, 0.01);
	BOOST_REQUIRE_CLOSE(val[4], 6.835e+13, 0.01);
}

BOOST_AUTO_TEST_CASE(checkHeat3D)
{
	// Set the DOF
	const int dof = 9;

	// Create the heat handler
	auto heatHandler = HeatEquationHandler(5.0e-12, 1000.0, 3);
	heatHandler.setHeatCoefficient(tungstenHeatCoefficient);
	heatHandler.setHeatConductivity(tungstenHeatConductivity);

	// Check the initial temperatures
	BOOST_REQUIRE_CLOSE(
		heatHandler.getTemperature({0.0, 0.0, 0.0}, 0.0), 1000.0, 0.01);
	BOOST_REQUIRE_CLOSE(
		heatHandler.getTemperature({1.0, 0.0, 0.0}, 0.0), 1000.0, 0.01);

	// Create ofill
	network::IReactionNetwork::SparseFillMap ofill;
	// Create dfill
	network::IReactionNetwork::SparseFillMap dfill;

	// Initialize it
	heatHandler.initializeTemperature(dof, ofill, dfill);

	// Check that the temperature "diffusion" is well set
	BOOST_REQUIRE_EQUAL(ofill[9][0], 9);
	BOOST_REQUIRE_EQUAL(dfill[9][0], 9);

	// The step size in the x direction
	double hx = 1.0;
	// The size parameter in the y direction
	double sy = 1.0;
	// The size parameter in the z direction
	double sz = 1.0;

	// The arrays of concentration
	double concentration[27 * (dof + 1)];
	double newConcentration[27 * (dof + 1)];

	// Initialize their values
	for (int i = 0; i < 27 * (dof + 1); i++) {
		concentration[i] = (double)i * i / 10.0;
		newConcentration[i] = 0.0;
	}

	// Get pointers
	double* conc = &concentration[0];
	double* updatedConc = &newConcentration[0];

	// Get the offset for the grid point in the middle
	// Supposing the 27 grid points are laid-out as follow (a cube!):
	// 6 | 7 | 8    15 | 16 | 17    24 | 25 | 26
	// 3 | 4 | 5    12 | 13 | 14    21 | 22 | 23
	// 0 | 1 | 2    9  | 10 | 11    18 | 19 | 20
	//   front         middle           back
	double* concOffset = conc + 13 * (dof + 1);
	double* updatedConcOffset = updatedConc + 13 * (dof + 1);

	// Fill the concVector with the pointer to the middle, left, right, bottom,
	// top, front, and back grid points
	double* concVector[7]{};
	concVector[0] = concOffset; // middle
	concVector[1] = conc + 12 * (dof + 1); // left
	concVector[2] = conc + 14 * (dof + 1); // right
	concVector[3] = conc + 10 * (dof + 1); // bottom
	concVector[4] = conc + 16 * (dof + 1); // top
	concVector[5] = conc + 4 * (dof + 1); // front
	concVector[6] = conc + 22 * (dof + 1); // back

	// Compute the heat equation at this grid point
	heatHandler.computeTemperature(
		concVector, updatedConcOffset, hx, hx, hx, sy, 1, sz, 1);

	// Check the new values of updatedConcOffset
	BOOST_REQUIRE_CLOSE(updatedConcOffset[9], 1.24397e+17, 0.01);

	// Set the temperature in the handler
	heatHandler.setTemperature(concOffset);
	// Check the updated temperature
	plsm::SpaceVector<double, 3> pos{1.0, 0.0, 0.0};
	BOOST_REQUIRE_CLOSE(heatHandler.getTemperature(pos, 1.0), 1932.1, 0.01);

	// Initialize the indices and values to set in the Jacobian
	IdType indices[1];
	double val[7];
	// Get the pointer on them for the compute diffusion method
	IdType* indicesPointer = &indices[0];
	double* valPointer = &val[0];

	// Compute the partial derivatives for the heat equation a the grid point
	heatHandler.computePartialsForTemperature(
		valPointer, indicesPointer, hx, hx, hx, sy, 1, sz, 1);

	// Check the values for the indices
	BOOST_REQUIRE_EQUAL(indices[0], 9);

	// Check the values
	BOOST_REQUIRE_CLOSE(val[0], -4.101e+14, 0.01);
	BOOST_REQUIRE_CLOSE(val[1], 6.835e+13, 0.01);
	BOOST_REQUIRE_CLOSE(val[2], 6.835e+13, 0.01);
	BOOST_REQUIRE_CLOSE(val[3], 6.835e+13, 0.01);
	BOOST_REQUIRE_CLOSE(val[4], 6.835e+13, 0.01);
	BOOST_REQUIRE_CLOSE(val[5], 6.835e+13, 0.01);
	BOOST_REQUIRE_CLOSE(val[6], 6.835e+13, 0.01);

	// Finalize MPI
	MPI_Finalize();
}

BOOST_AUTO_TEST_SUITE_END()