#include <cmath>
#include <limits>
#include <memory>

#include <Eigen/Dense>
#include <boost/filesystem.hpp>

#include "catch.hpp"
// MPI
#ifdef USE_MPI
#include "mpi.h"
#endif

#include "constraints.h"
#include "element.h"
#include "function_base.h"
#include "hexahedron_element.h"
#include "linear_function.h"
#include "mesh.h"
#include "node.h"
#include "partio_writer.h"
#include "quadrilateral_element.h"

//! Check mesh class for 2D case
TEST_CASE("Mesh is checked for 2D case", "[mesh][2D]") {
  // Dimension
  const unsigned Dim = 2;
  // Degrees of freedom
  const unsigned Dof = 2;
  // Number of phases
  const unsigned Nphases = 1;
  // Number of nodes per cell
  const unsigned Nnodes = 4;
  // Tolerance
  const double Tolerance = 1.E-9;
  // Json property
  Json jfunctionproperties;
  jfunctionproperties["id"] = 0;
  std::vector<double> x_values{{0.0, 0.5, 1.0}};
  std::vector<double> fx_values{{0.0, 1.0, 1.0}};
  jfunctionproperties["xvalues"] = x_values;
  jfunctionproperties["fxvalues"] = fx_values;

  // math function
  std::shared_ptr<mpm::FunctionBase> mfunction =
      std::make_shared<mpm::LinearFunction>(0, jfunctionproperties);

  // 4-noded quadrilateral element
  std::shared_ptr<mpm::Element<Dim>> element =
      Factory<mpm::Element<Dim>>::instance()->create("ED2Q4");

  // Assign material
  unsigned mid = 0;
  std::vector<unsigned> mids(1, mid);
  // Initialise material
  Json jmaterial;
  jmaterial["density"] = 1000.;
  jmaterial["youngs_modulus"] = 1.0E+7;
  jmaterial["poisson_ratio"] = 0.3;

  auto le_material =
      Factory<mpm::Material<Dim>, unsigned, const Json&>::instance()->create(
          "LinearElastic2D", std::move(0), jmaterial);

  std::map<unsigned, std::shared_ptr<mpm::Material<Dim>>> materials;
  materials[mid] = le_material;

  //! Check Mesh IDs
  SECTION("Check mesh ids") {
    //! Check for id = 0
    SECTION("Mesh id is zero") {
      unsigned id = 0;
      auto mesh = std::make_shared<mpm::Mesh<Dim>>(id);
      REQUIRE(mesh->id() == 0);
      REQUIRE(mesh->is_isoparametric() == true);
    }

    SECTION("Mesh id is zero and cartesian") {
      unsigned id = 0;
      auto mesh = std::make_shared<mpm::Mesh<Dim>>(id, false);
      REQUIRE(mesh->id() == 0);
      REQUIRE(mesh->is_isoparametric() == false);
    }

    SECTION("Mesh id is positive") {
      //! Check for id is a positive value
      unsigned id = std::numeric_limits<unsigned>::max();
      auto mesh = std::make_shared<mpm::Mesh<Dim>>(id);
      REQUIRE(mesh->id() == std::numeric_limits<unsigned>::max());
    }
  }

  SECTION("Add neighbours") {
    auto mesh = std::make_shared<mpm::Mesh<Dim>>(0);
    auto neighbourmesh = std::make_shared<mpm::Mesh<Dim>>(1);
    REQUIRE(mesh->nneighbours() == 0);
    mesh->add_neighbour(0, neighbourmesh);
    REQUIRE(mesh->nneighbours() == 1);
  }

  // Check add / remove particle
  SECTION("Check add / remove particle functionality") {
    // Particle 1
    mpm::Index id1 = 0;
    Eigen::Vector2d coords;
    coords.setZero();
    std::shared_ptr<mpm::ParticleBase<Dim>> particle1 =
        std::make_shared<mpm::Particle<Dim>>(id1, coords);

    // Particle 2
    mpm::Index id2 = 1;
    coords << 2., 2.;
    std::shared_ptr<mpm::ParticleBase<Dim>> particle2 =
        std::make_shared<mpm::Particle<Dim>>(id2, coords);

    auto mesh = std::make_shared<mpm::Mesh<Dim>>(0);
    // Check mesh is active
    REQUIRE(mesh->status() == false);

    // Check nodal coordinates size
    REQUIRE(mesh->nodal_coordinates().size() == 0);
    // Check node pairs size
    REQUIRE(mesh->node_pairs().size() == 0);

    // Define nodes
    coords << 0., 0.;
    std::shared_ptr<mpm::NodeBase<Dim>> node0 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(0, coords);

    // Add node 0 and check
    REQUIRE(mesh->add_node(node0) == true);

    coords << 2., 0.;
    std::shared_ptr<mpm::NodeBase<Dim>> node1 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(1, coords);

    // Add node 1 and check
    REQUIRE(mesh->add_node(node1) == true);

    coords << 2., 2.;
    std::shared_ptr<mpm::NodeBase<Dim>> node2 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(2, coords);

    // Add node 2 and check
    REQUIRE(mesh->add_node(node2) == true);

    coords << 0., 2.;
    std::shared_ptr<mpm::NodeBase<Dim>> node3 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(3, coords);

    // Add node 3 and check
    REQUIRE(mesh->add_node(node3) == true);

    // Create cell1
    coords.setZero();
    auto cell1 = std::make_shared<mpm::Cell<Dim>>(id1, Nnodes, element);

    // Add nodes to cell
    cell1->add_node(0, node0);
    cell1->add_node(1, node1);
    cell1->add_node(2, node2);
    cell1->add_node(3, node3);

    int mpi_size;
    MPI_Comm_size(MPI_COMM_WORLD, &mpi_size);
    if (mpi_size == 1) cell1->rank(1);

    // Initialize cell
    REQUIRE(cell1->initialise() == true);

    // Add cell 1 and check
    REQUIRE(mesh->add_cell(cell1) == true);

    // Check nodal coordinates size
    REQUIRE(mesh->nodal_coordinates().size() == 4);
    // Check node pairs size
    REQUIRE(mesh->node_pairs().size() == 4);

    // Add particle 1 and check
    REQUIRE(mesh->add_particle(particle1) == true);
    // Add particle 2 and check
    REQUIRE(mesh->add_particle(particle2) == true);
    // Add particle 2 again and check
    REQUIRE(mesh->add_particle(particle2) == false);

    // Check mesh is active
    REQUIRE(mesh->status() == true);
    // Check number of particles in mesh
    REQUIRE(mesh->nparticles() == 2);

    // Update coordinates
    Eigen::Vector2d coordinates;
    coordinates << 1., 1.;
    // Check iterate over functionality
    mesh->iterate_over_particles(
        std::bind(&mpm::ParticleBase<Dim>::assign_coordinates,
                  std::placeholders::_1, coordinates));

    // Particle 1
    {
      // Check if nodal coordinate update has gone through
      auto check_coords = particle1->coordinates();
      // Check if coordinates for each particle is zero
      for (unsigned i = 0; i < check_coords.size(); ++i)
        REQUIRE(check_coords[i] == Approx(1.).epsilon(Tolerance));
    }
    // Particle 2
    {
      // Check if nodal coordinate update has gone through
      auto check_coords = particle2->coordinates();
      // Check if coordinates for each particle is zero
      for (unsigned i = 0; i < check_coords.size(); ++i)
        REQUIRE(check_coords[i] == Approx(1.).epsilon(Tolerance));
    }

    // Remove particle 2 and check
    REQUIRE(mesh->remove_particle(particle2) == true);
    // Check number of particles in mesh
    REQUIRE(mesh->nparticles() == 1);

    // Remove all non-rank particles in mesh
    mesh->remove_all_nonrank_particles();
    // Check number of particles in mesh
    REQUIRE(mesh->nparticles() == 0);

    // Add and use remove all particles
    REQUIRE(mesh->add_particle(particle1) == true);
    REQUIRE(mesh->add_particle(particle2) == true);

    // Check number of particles in mesh
    REQUIRE(mesh->nparticles() == 2);
    std::vector<mpm::Index> remove_pids = {{0, 1}};
    // Remove all particles
    mesh->remove_particles(remove_pids);
    // Check number of particles in mesh
    REQUIRE(mesh->nparticles() == 0);

    // Test assign node concentrated force
    SECTION("Check assign node concentrated force") {
      unsigned Nphase = 0;
      // Set external force to zero
      Eigen::Matrix<double, Dim, 1> force;
      force.setZero();
      REQUIRE_NOTHROW(node0->update_external_force(false, Nphase, force));
      REQUIRE_NOTHROW(node1->update_external_force(false, Nphase, force));

      const unsigned Direction = 0;
      // Check external force
      for (unsigned i = 0; i < Dim; ++i) {
        REQUIRE(node0->external_force(Nphase)(i) ==
                Approx(0.).epsilon(Tolerance));
        REQUIRE(node1->external_force(Nphase)(i) ==
                Approx(0.).epsilon(Tolerance));
      }

      tsl::robin_map<mpm::Index, std::vector<mpm::Index>> node_sets;
      node_sets[0] = std::vector<mpm::Index>{0, 1};

      REQUIRE(mesh->create_node_sets(node_sets, true) == true);

      REQUIRE(mesh->assign_nodal_concentrated_forces(mfunction, 0, 0, 10.5) ==
              true);
      REQUIRE(mesh->assign_nodal_concentrated_forces(mfunction, -1, 0, 0.5) ==
              true);
      REQUIRE(mesh->assign_nodal_concentrated_forces(mfunction, 5, 0, 0.5) ==
              false);
      REQUIRE(mesh->assign_nodal_concentrated_forces(mfunction, -5, 1, 0.5) ==
              false);

      double current_time = 0.0;
      node0->apply_concentrated_force(Nphase, current_time);
      node1->apply_concentrated_force(Nphase, current_time);
      // Check external force
      for (unsigned i = 0; i < Dim; ++i) {
        REQUIRE(node0->external_force(Nphase)(i) ==
                Approx(0.).epsilon(Tolerance));
        REQUIRE(node1->external_force(Nphase)(i) ==
                Approx(0.).epsilon(Tolerance));
      }

      current_time = 0.25;
      node0->apply_concentrated_force(Nphase, current_time);
      node1->apply_concentrated_force(Nphase, current_time);
      std::vector<double> ext_forces = {0.25, 0., 0.};
      // Check external force
      for (unsigned i = 0; i < Dim; ++i) {
        REQUIRE(node0->external_force(Nphase)(i) ==
                Approx(ext_forces.at(i)).epsilon(Tolerance));
        REQUIRE(node1->external_force(Nphase)(i) ==
                Approx(ext_forces.at(i)).epsilon(Tolerance));
      }

      current_time = 5.0;
      node0->apply_concentrated_force(Nphase, current_time);
      node1->apply_concentrated_force(Nphase, current_time);
      ext_forces = {0.75, 0., 0.};
      // Check external force
      for (unsigned i = 0; i < Dim; ++i) {
        REQUIRE(node0->external_force(Nphase)(i) ==
                Approx(ext_forces.at(i)).epsilon(Tolerance));
        REQUIRE(node1->external_force(Nphase)(i) ==
                Approx(ext_forces.at(i)).epsilon(Tolerance));
      }
    }
  }

  // Check add / remove node
  SECTION("Check add / remove node functionality") {
    // Node 1
    mpm::Index id1 = 0;
    Eigen::Vector2d coords;
    coords.setZero();
    std::shared_ptr<mpm::NodeBase<Dim>> node1 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(id1, coords);

    // Node 2
    mpm::Index id2 = 1;
    std::shared_ptr<mpm::NodeBase<Dim>> node2 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(id2, coords);

    // Create a mesh
    auto mesh = std::make_shared<mpm::Mesh<Dim>>(0);
    // Check mesh is active
    REQUIRE(mesh->status() == false);

    // Check nodal coordinates size
    REQUIRE(mesh->nodal_coordinates().size() == 0);
    // Check node pairs size
    REQUIRE(mesh->node_pairs().size() == 0);

    // Add node 1 and check
    REQUIRE(mesh->add_node(node1) == true);
    // Add node 2 and check
    REQUIRE(mesh->add_node(node2) == true);
    // Add node 2 again and check
    REQUIRE(mesh->add_node(node2) == false);

    // Check number of nodes in mesh
    REQUIRE(mesh->nnodes() == 2);

    // Check nodal coordinates size
    REQUIRE(mesh->nodal_coordinates().size() == 2);

    // Update coordinates
    Eigen::Vector2d coordinates;
    coordinates << 1., 1.;

    // Set only node2 to be active
    node2->assign_status(true);

    // Check iterate over functionality if nodes are active
    mesh->iterate_over_nodes_predicate(
        std::bind(&mpm::NodeBase<Dim>::assign_coordinates,
                  std::placeholders::_1, coordinates),
        std::bind(&mpm::NodeBase<Dim>::status, std::placeholders::_1));

    // Node 1
    {
      // Check if nodal coordinate update has gone through
      auto check_coords = node1->coordinates();
      // Check if coordinates for each node is zero
      for (unsigned i = 0; i < check_coords.size(); ++i)
        REQUIRE(check_coords[i] == Approx(0.).epsilon(Tolerance));
    }
    // Node 2
    {
      // Check if nodal coordinate update has gone through
      auto check_coords = node2->coordinates();
      // Check if coordinates for each node is zero
      for (unsigned i = 0; i < check_coords.size(); ++i)
        REQUIRE(check_coords[i] == Approx(1.).epsilon(Tolerance));
    }

    coordinates.setZero();
    node1->assign_coordinates(coordinates);
    node2->assign_coordinates(coordinates);
    for (unsigned i = 0; i < coordinates.size(); ++i) {
      REQUIRE(node1->coordinates()[i] == Approx(0.).epsilon(Tolerance));
      REQUIRE(node2->coordinates()[i] == Approx(0.).epsilon(Tolerance));
    }

    REQUIRE(node1->status() == false);
    REQUIRE(node2->status() == true);

    mesh->find_active_nodes();

    // Check iterate over functionality if nodes are active
    coordinates.fill(5.3);

    mesh->iterate_over_active_nodes(
        std::bind(&mpm::NodeBase<Dim>::assign_coordinates,
                  std::placeholders::_1, coordinates));

    // Node 1
    {
      // Check if nodal coordinate update has gone through
      auto check_coords = node1->coordinates();
      for (unsigned i = 0; i < check_coords.size(); ++i)
        REQUIRE(check_coords[i] == Approx(0.).epsilon(Tolerance));
    }
    // Node 2
    {
      // Check if nodal coordinate update has gone through
      auto check_coords = node2->coordinates();
      for (unsigned i = 0; i < check_coords.size(); ++i)
        REQUIRE(check_coords[i] == Approx(5.3).epsilon(Tolerance));
    }

    coordinates.fill(1.0);
    // Check iterate over functionality
    mesh->iterate_over_nodes(std::bind(&mpm::NodeBase<Dim>::assign_coordinates,
                                       std::placeholders::_1, coordinates));

    // Node 1
    {
      // Check if nodal coordinate update has gone through
      auto check_coords = node1->coordinates();
      // Check if coordinates for each node is zero
      for (unsigned i = 0; i < check_coords.size(); ++i)
        REQUIRE(check_coords[i] == Approx(1.).epsilon(Tolerance));
    }
    // Node 2
    {
      // Check if nodal coordinate update has gone through
      auto check_coords = node2->coordinates();
      // Check if coordinates for each node is zero
      for (unsigned i = 0; i < check_coords.size(); ++i)
        REQUIRE(check_coords[i] == Approx(1.).epsilon(Tolerance));
    }

    // Remove node 2 and check
    REQUIRE(mesh->remove_node(node2) == true);
    // Check number of nodes in mesh
    REQUIRE(mesh->nnodes() == 1);
  }

  // Check add / remove cell
  SECTION("Check add / remove cell functionality") {

    // Cell 1
    mpm::Index id1 = 0;
    Eigen::Vector2d coords;
    coords.setZero();
    auto cell1 = std::make_shared<mpm::Cell<Dim>>(id1, Nnodes, element);

    // Cell 2
    mpm::Index id2 = 1;
    auto cell2 = std::make_shared<mpm::Cell<Dim>>(id2, Nnodes, element);

    auto mesh = std::make_shared<mpm::Mesh<Dim>>(0);
    // Check mesh is active
    REQUIRE(mesh->status() == false);

    // Add cell 1 and check
    REQUIRE(mesh->add_cell(cell1) == true);
    // Add cell 2 and check
    REQUIRE(mesh->add_cell(cell2) == true);
    // Add cell 2 again and check
    REQUIRE(mesh->add_cell(cell2) == false);

    // Check number of cells in mesh
    REQUIRE(mesh->ncells() == 2);

    // Check iterate over functionality
    mesh->iterate_over_cells(
        std::bind(&mpm::Cell<Dim>::nnodes, std::placeholders::_1));

    // Remove cell 2 and check
    REQUIRE(mesh->remove_cell(cell2) == true);
    // Check number of cells in mesh
    REQUIRE(mesh->ncells() == 1);
  }

  SECTION("Check particle is in cell") {
    // Cell 1
    mpm::Index id1 = 0;
    Eigen::Vector2d coords;
    coords.setZero();

    // Mesh
    auto mesh = std::make_shared<mpm::Mesh<Dim>>(0);
    // Check mesh is inactive (false)
    REQUIRE(mesh->status() == false);

    // Define nodes
    std::shared_ptr<mpm::NodeBase<Dim>> node0 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(0, coords);

    // Add node 0 and check
    REQUIRE(mesh->add_node(node0) == true);

    coords << 2., 0.;
    std::shared_ptr<mpm::NodeBase<Dim>> node1 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(1, coords);

    // Add node 1 and check
    REQUIRE(mesh->add_node(node1) == true);

    coords << 2., 2.;
    std::shared_ptr<mpm::NodeBase<Dim>> node2 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(2, coords);

    // Add node 2 and check
    REQUIRE(mesh->add_node(node2) == true);

    coords << 0., 2.;
    std::shared_ptr<mpm::NodeBase<Dim>> node3 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(3, coords);

    // Add node 3 and check
    REQUIRE(mesh->add_node(node3) == true);

    // Create cell1
    coords.setZero();
    auto cell1 = std::make_shared<mpm::Cell<Dim>>(id1, Nnodes, element);

    // Add nodes to cell
    cell1->add_node(0, node0);
    cell1->add_node(1, node1);
    cell1->add_node(2, node2);
    cell1->add_node(3, node3);

    // Initialize cell
    REQUIRE(cell1->initialise() == true);
    // Particle type 2D
    const std::string particle_type = "P2D";

    // Initialise material models
    mesh->initialise_material_models(materials);

    // Generate material points in cell
    REQUIRE(mesh->nparticles() == 0);

    REQUIRE(mesh->generate_material_points(1, particle_type, mids, -1, 0) ==
            false);
    REQUIRE(mesh->nparticles() == 0);

    // Add cell 1 and check
    REQUIRE(mesh->add_cell(cell1) == true);

    SECTION("Check generating 1 particle / cell") {
      // Generate material points in cell
      REQUIRE(mesh->generate_material_points(1, particle_type, mids, -1, 0) ==
              true);
      REQUIRE(mesh->nparticles() == 1);
    }

    SECTION("Check generating 2 particle / cell") {
      REQUIRE(mesh->generate_material_points(2, particle_type, mids, -1, 0) ==
              true);
      REQUIRE(mesh->nparticles() == 4);
    }

    SECTION("Check generating 3 particle / cell") {
      REQUIRE(mesh->generate_material_points(3, particle_type, mids, -1, 0) ==
              true);
      REQUIRE(mesh->nparticles() == 9);
    }

    SECTION("Check material point generation") {
      // Assign argc and argv to nput arguments of MPM
      int argc = 7;
      char* argv[] = {(char*)"./mpm",   (char*)"-f", (char*)"./",
                      (char*)"-p",      (char*)"8",  (char*)"-i",
                      (char*)"mpm.json"};

      // Create an IO object
      auto io = std::make_shared<mpm::IO>(argc, argv);

      tsl::robin_map<mpm::Index, std::vector<mpm::Index>> cell_sets;
      cell_sets[1] = std::vector<mpm::Index>{0};

      REQUIRE(mesh->create_cell_sets(cell_sets, true) == true);

      REQUIRE(mesh->nparticles() == 0);

      SECTION("Gauss point generation") {
        // Gauss point generation
        Json jgen;
        jgen["type"] = "gauss";
        jgen["material_id"] = {0};
        jgen["cset_id"] = 1;
        jgen["particle_type"] = "P2D";
        jgen["check_duplicates"] = false;
        jgen["nparticles_per_dir"] = 2;
        jgen["pset_id"] = 2;

        // Generate
        REQUIRE(mesh->generate_particles(io, jgen) == true);
        // Number of particles
        REQUIRE(mesh->nparticles() == 4);
      }

      SECTION("Inject points") {
        // Gauss point generation
        Json jgen;
        jgen["type"] = "inject";
        jgen["material_id"] = {0};
        jgen["cset_id"] = 1;
        jgen["particle_type"] = "P2D";
        jgen["check_duplicates"] = false;
        jgen["nparticles_per_dir"] = 2;
        jgen["velocity"] = {0., 0.};
        jgen["duration"] = {0.1, 0.2};

        // Generate
        REQUIRE(mesh->generate_particles(io, jgen) == true);
        // Inject particles
        REQUIRE_NOTHROW(mesh->inject_particles(0.05));
        // Number of particles
        REQUIRE(mesh->nparticles() == 0);
        // Inject particles
        REQUIRE_NOTHROW(mesh->inject_particles(0.25));
        // Number of particles
        REQUIRE(mesh->nparticles() == 0);
        // Inject particles
        REQUIRE_NOTHROW(mesh->inject_particles(0.15));
        // Number of particles
        REQUIRE(mesh->nparticles() == 4);
      }
    }

    // Particle 1
    coords << 1.0, 1.0;
    std::shared_ptr<mpm::ParticleBase<Dim>> particle1 =
        std::make_shared<mpm::Particle<Dim>>(100, coords);

    // Particle 2
    coords << 1.5, 1.5;
    std::shared_ptr<mpm::ParticleBase<Dim>> particle2 =
        std::make_shared<mpm::Particle<Dim>>(101, coords);

    // Add particle 1 and check
    REQUIRE(mesh->add_particle(particle1) == true);
    // Add particle 2 and check
    REQUIRE(mesh->add_particle(particle2) == true);

    // Check mesh is active
    REQUIRE(mesh->status() == true);

    // Locate particles in a mesh
    auto particles = mesh->locate_particles_mesh();

    // Should find all particles in mesh
    REQUIRE(particles.size() == 0);

    // Check location of particle 1
    REQUIRE(particle1->cell_id() == 0);
    // Check location of particle 2
    REQUIRE(particle2->cell_id() == 0);
  }

  //! Check create nodes and cells in a mesh
  SECTION("Check create nodes and cells") {
    // Vector of nodal coordinates
    std::vector<Eigen::Matrix<double, Dim, 1>> coordinates;

    // Nodal coordinates
    Eigen::Matrix<double, Dim, 1> node;

    // Cell 0
    // Node 0
    node << 0., 0.;
    coordinates.emplace_back(node);
    // Node 1
    node << 0.5, 0.;
    coordinates.emplace_back(node);
    // Node 2
    node << 0.5, 0.5;
    coordinates.emplace_back(node);
    // Node 3
    node << 0., 0.5;
    coordinates.emplace_back(node);

    // Cell 1
    // Node 4
    node << 1.0, 0.;
    coordinates.emplace_back(node);
    // Node 5
    node << 1.0, 0.5;
    coordinates.emplace_back(node);

    // Create a new mesh
    unsigned meshid = 0;
    auto mesh = std::make_shared<mpm::Mesh<Dim>>(meshid);

    SECTION("Check creation of nodes") {
      // Node type 2D
      const std::string node_type = "N2D";
      // Global node index
      mpm::Index gnid = 0;
      mesh->create_nodes(gnid, node_type, coordinates, false);
      // Check if mesh has added nodes
      REQUIRE(mesh->nnodes() == coordinates.size());
      // Try again this shouldn't add more coordinates
      mesh->create_nodes(gnid, node_type, coordinates);
      // Check if mesh has added nodes
      REQUIRE(mesh->nnodes() == coordinates.size());
      // Clear coordinates and try creating a list of nodes with an empty list
      unsigned nnodes = coordinates.size();
      coordinates.clear();
      // This fails with empty list error in node creation
      mesh->create_nodes(gnid, node_type, coordinates);
      REQUIRE(mesh->nnodes() == nnodes);

      SECTION("Check creation of cells") {
        // Cell with node ids
        std::vector<std::vector<mpm::Index>> cells{// cell #0
                                                   {0, 1, 2, 3},
                                                   // cell #1
                                                   {1, 4, 5, 2}};
        // Assign 4-noded quadrilateral element to cell
        std::shared_ptr<mpm::Element<Dim>> element =
            Factory<mpm::Element<Dim>>::instance()->create("ED2Q4");

        // Global cell index
        mpm::Index gcid = 0;
        mesh->create_cells(gcid, element, cells, false);
        // Check if mesh has added cells
        REQUIRE(mesh->ncells() == cells.size());
        // Try again this shouldn't add more cells
        mesh->create_cells(gcid, element, cells);
        // Check if mesh has added cells
        REQUIRE(mesh->ncells() == cells.size());
        // Clear cells and try creating a list of empty cells
        unsigned ncells = cells.size();
        cells.clear();
        // This fails with empty list error in node creation
        gcid = 10;
        mesh->create_cells(gcid, element, cells);
        REQUIRE(mesh->ncells() == ncells);

        // Try with invalid node ids
        cells = {// cell #0
                 {10, 11, 12, 13},
                 // cell #1
                 {11, 14, 15, 12}};
        gcid = 20;
        mesh->create_cells(gcid, element, cells);
        REQUIRE(mesh->ncells() == ncells);

        SECTION("Check creation of particles") {
          // Vector of particle coordinates
          std::vector<Eigen::Matrix<double, Dim, 1>> coordinates;
          coordinates.clear();

          // Particle coordinates
          Eigen::Matrix<double, Dim, 1> particle;

          // Cell 0
          // Particle 0
          particle << 0.125, 0.125;
          coordinates.emplace_back(particle);
          // Particle 1
          particle << 0.25, 0.125;
          coordinates.emplace_back(particle);
          // Particle 2
          particle << 0.25, 0.25;
          coordinates.emplace_back(particle);
          // Particle 3
          particle << 0.125, 0.25;
          coordinates.emplace_back(particle);

          // Cell 1
          // Particle 4
          particle << 0.675, 0.125;
          coordinates.emplace_back(particle);
          // Particle 5
          particle << 0.85, 0.125;
          coordinates.emplace_back(particle);
          // Particle 6
          particle << 0.85, 0.25;
          coordinates.emplace_back(particle);
          // Particle 7
          particle << 0.675, 0.25;
          coordinates.emplace_back(particle);

          // Initialise material models in mesh
          mesh->initialise_material_models(materials);

          SECTION("Check addition of particles to mesh") {
            // Particle type 2D
            const std::string particle_type = "P2D";
            // Create particles from file
            mesh->create_particles(particle_type, coordinates, mids, 0, false);
            // Check if mesh has added particles
            REQUIRE(mesh->nparticles() == coordinates.size());

            // Test assign particles cells
            SECTION("Check assign particles cells") {
              // Vector of particle cells
              std::vector<std::array<mpm::Index, 2>> particles_cells;
              // Particle cells
              particles_cells.emplace_back(std::array<mpm::Index, 2>({0, 0}));
              particles_cells.emplace_back(std::array<mpm::Index, 2>({1, 0}));
              particles_cells.emplace_back(std::array<mpm::Index, 2>({2, 0}));
              particles_cells.emplace_back(std::array<mpm::Index, 2>({3, 0}));

              particles_cells.emplace_back(std::array<mpm::Index, 2>({4, 1}));
              particles_cells.emplace_back(std::array<mpm::Index, 2>({5, 1}));
              particles_cells.emplace_back(std::array<mpm::Index, 2>({6, 1}));
              particles_cells.emplace_back(std::array<mpm::Index, 2>({7, 1}));

              REQUIRE(mesh->assign_particles_cells(particles_cells) == true);

              // Locate particles
              auto missing_particles = mesh->locate_particles_mesh();
              REQUIRE(missing_particles.size() == 0);

              auto check_particles_cells = mesh->particles_cells();

              REQUIRE(check_particles_cells.size() == mesh->nparticles());

              for (unsigned i = 0; i < particles_cells.size(); ++i)
                for (unsigned j = 0; j < 2; ++j)
                  REQUIRE(check_particles_cells.at(i).at(j) ==
                          particles_cells.at(i).at(j));
            }

            // Locate particles
            auto missing_particles = mesh->locate_particles_mesh();
            REQUIRE(missing_particles.size() == 0);

            // Test assign particles cells again should fail
            SECTION("Check assign particles cells") {
              // Vector of particle cells
              std::vector<std::array<mpm::Index, 2>> particles_cells;
              // Particle cells
              particles_cells.emplace_back(std::array<mpm::Index, 2>({0, 0}));
              particles_cells.emplace_back(std::array<mpm::Index, 2>({1, 0}));
              particles_cells.emplace_back(std::array<mpm::Index, 2>({2, 0}));
              particles_cells.emplace_back(std::array<mpm::Index, 2>(
                  {3, std::numeric_limits<mpm::Index>::max()}));
              particles_cells.emplace_back(std::array<mpm::Index, 2>({50, 0}));

              REQUIRE(mesh->assign_particles_cells(particles_cells) == false);
            }

            // Clear coordinates and try creating a list of particles with
            // an empty list
            unsigned nparticles = coordinates.size();
            coordinates.clear();
            // This fails with empty list error in particle creation
            mesh->create_particles(particle_type, coordinates, mids, 0, false);
            REQUIRE(mesh->nparticles() == nparticles);

            const unsigned phase = 0;
            // Particles coordinates
            REQUIRE(mesh->particle_coordinates().size() == mesh->nparticles());
            // Particle stresses
            std::string attribute = "stresses";
            REQUIRE(mesh->template particles_tensor_data<6>(attribute).size() ==
                    mesh->nparticles());
            // Particle strains
            attribute = "strains";
            REQUIRE(mesh->template particles_tensor_data<6>(attribute).size() ==
                    mesh->nparticles());
            // Particle velocities
            attribute = "velocities";
            REQUIRE(mesh->template particles_tensor_data<3>(attribute).size() ==
                    mesh->nparticles());
            // Particle invalid data
            attribute = "invalid";
            REQUIRE(mesh->template particles_tensor_data<3>(attribute).size() ==
                    0);

            // State variable
            attribute = "pdstrain";
            REQUIRE(mesh->particles_statevars_data(attribute).size() ==
                    mesh->nparticles());

            // Locate particles in mesh
            SECTION("Locate particles in mesh") {
              // Locate particles in a mesh
              auto particles = mesh->locate_particles_mesh();

              // Should find all particles in mesh
              REQUIRE(particles.size() == 0);

              // Create particle 100
              Eigen::Vector2d coords;
              coords << 100., 100.;

              mpm::Index pid = 100;
              std::shared_ptr<mpm::ParticleBase<Dim>> particle100 =
                  std::make_shared<mpm::Particle<Dim>>(pid, coords);

              // Add particle100 and check
              REQUIRE(mesh->add_particle(particle100) == false);

              // Locate particles in a mesh
              particles = mesh->locate_particles_mesh();
              // Should miss particle100
              REQUIRE(particles.size() == 0);
            }

            // Test HDF5
            SECTION("Write particles HDF5") {
              REQUIRE(mesh->write_particles_hdf5(0, "particles-2d.h5") == true);

              auto phdf5 = mesh->particles_hdf5();
              REQUIRE(phdf5.size() == mesh->nparticles());

#ifdef USE_PARTIO
              REQUIRE_NOTHROW(mpm::partio::write_particles(
                  "partio-2d.bgeo", mesh->particles_hdf5()));
              // Check if .bgeo exists
              REQUIRE(boost::filesystem::exists("./partio-2d.bgeo") == true);
#endif
            }

            // Test assign particles volumes
            SECTION("Check assign particles volumes") {
              // Vector of particle coordinates
              std::vector<std::tuple<mpm::Index, double>> particles_volumes;
              // Volumes
              particles_volumes.emplace_back(std::make_tuple(0, 10.5));
              particles_volumes.emplace_back(std::make_tuple(1, 10.5));

              REQUIRE(mesh->nparticles() == 8);

              REQUIRE(mesh->assign_particles_volumes(particles_volumes) ==
                      true);

              // When volume assignment fails
              particles_volumes.emplace_back(std::make_tuple(2, 0.0));
              particles_volumes.emplace_back(std::make_tuple(3, -10.0));

              REQUIRE(mesh->assign_particles_volumes(particles_volumes) ==
                      false);
            }

            // Test assign particles tractions
            SECTION("Check assign particles tractions") {
              tsl::robin_map<mpm::Index, std::vector<mpm::Index>> particle_sets;
              particle_sets[0] = std::vector<mpm::Index>{0};
              particle_sets[1] = std::vector<mpm::Index>{1};
              particle_sets[2] = std::vector<mpm::Index>{2};
              particle_sets[3] = std::vector<mpm::Index>{3};

              REQUIRE(mesh->create_particle_sets(particle_sets, true) == true);

              REQUIRE(mesh->nparticles() == 8);

              REQUIRE(mesh->create_particles_tractions(mfunction, 0, 0, 10.5) ==
                      true);
              REQUIRE(mesh->create_particles_tractions(mfunction, 1, 1,
                                                       -10.5) == true);
              REQUIRE(mesh->create_particles_tractions(mfunction, 2, 0,
                                                       -12.5) == true);
              REQUIRE(mesh->create_particles_tractions(mfunction, 3, 1, 0.5) ==
                      true);

              REQUIRE(mesh->create_particles_tractions(mfunction, -1, 1, 0.5) ==
                      true);
              REQUIRE(mesh->create_particles_tractions(mfunction, 5, 0, 0.5) ==
                      false);
              REQUIRE(mesh->create_particles_tractions(mfunction, -5, 1, 0.5) ==
                      false);

              // Locate particles in a mesh
              auto particles = mesh->locate_particles_mesh();
              REQUIRE(particles.size() == 0);
              mesh->iterate_over_particles(
                  std::bind(&mpm::ParticleBase<Dim>::compute_shapefn,
                            std::placeholders::_1));

              // Compute volume
              mesh->iterate_over_particles(
                  std::bind(&mpm::ParticleBase<Dim>::compute_volume,
                            std::placeholders::_1));

              mesh->apply_traction_on_particles(10);
            }

            // Test assign particles stresses
            SECTION("Check assign particles stresses") {
              // Vector of particle stresses
              std::vector<Eigen::Matrix<double, 6, 1>> particles_stresses;

              REQUIRE(mesh->nparticles() == 8);

              // Stresses
              particles_stresses.emplace_back(
                  Eigen::Matrix<double, 6, 1>::Constant(0.0));
              particles_stresses.emplace_back(
                  Eigen::Matrix<double, 6, 1>::Constant(0.1));
              particles_stresses.emplace_back(
                  Eigen::Matrix<double, 6, 1>::Constant(-0.2));
              particles_stresses.emplace_back(
                  Eigen::Matrix<double, 6, 1>::Constant(0.3));
              particles_stresses.emplace_back(
                  Eigen::Matrix<double, 6, 1>::Constant(-0.4));
              particles_stresses.emplace_back(
                  Eigen::Matrix<double, 6, 1>::Constant(0.5));
              particles_stresses.emplace_back(
                  Eigen::Matrix<double, 6, 1>::Constant(-0.6));
              particles_stresses.emplace_back(
                  Eigen::Matrix<double, 6, 1>::Constant(-0.7));

              REQUIRE(mesh->assign_particles_stresses(particles_stresses) ==
                      true);
              // When stresses fail
              particles_stresses.emplace_back(
                  Eigen::Matrix<double, 6, 1>::Constant(0.8));
              REQUIRE(mesh->assign_particles_stresses(particles_stresses) ==
                      false);
              unsigned id = 1;
              auto mesh_fail = std::make_shared<mpm::Mesh<Dim>>(id);
              REQUIRE(mesh_fail->assign_particles_stresses(
                          particles_stresses) == false);
            }

            // Test assign particles velocity constraints
            SECTION("Check assign particles velocity constraints") {
              tsl::robin_map<mpm::Index, std::vector<mpm::Index>> particle_sets;
              particle_sets[0] = std::vector<mpm::Index>{0};
              particle_sets[1] = std::vector<mpm::Index>{1};
              particle_sets[2] = std::vector<mpm::Index>{2};
              particle_sets[3] = std::vector<mpm::Index>{3};

              REQUIRE(mesh->create_particle_sets(particle_sets, true) == true);

              REQUIRE(mesh->nparticles() == 8);

              int set_id = 0;
              int dir = 0;
              double constraint = 10.5;
              // Add velocity constraint to mesh
              auto velocity_constraint =
                  std::make_shared<mpm::VelocityConstraint>(set_id, dir,
                                                            constraint);
              REQUIRE(mesh->create_particle_velocity_constraint(
                          set_id, velocity_constraint) == true);

              // Add velocity constraint to all nodes in mesh
              velocity_constraint = std::make_shared<mpm::VelocityConstraint>(
                  -1, dir, constraint);
              REQUIRE(mesh->create_particle_velocity_constraint(
                          set_id, velocity_constraint) == true);

              // When constraints fail
              dir = 2;
              // Add velocity constraint to mesh
              velocity_constraint = std::make_shared<mpm::VelocityConstraint>(
                  set_id, dir, constraint);
              REQUIRE(mesh->create_particle_velocity_constraint(
                          set_id, velocity_constraint) == false);

              mesh->apply_particle_velocity_constraints();
            }
          }
        }
        // Test assign velocity constraints to nodes
        SECTION("Check assign velocity constraints to nodes") {
          tsl::robin_map<mpm::Index, std::vector<mpm::Index>> node_sets;
          node_sets[0] = std::vector<mpm::Index>{0, 2};
          node_sets[1] = std::vector<mpm::Index>{1, 3};

          REQUIRE(mesh->create_node_sets(node_sets, true) == true);

          //! Constraints object
          auto constraints = std::make_shared<mpm::Constraints<Dim>>(mesh);

          int set_id = 0;
          int dir = 0;
          double constraint = 10.5;
          // Add velocity constraint to mesh
          auto velocity_constraint = std::make_shared<mpm::VelocityConstraint>(
              set_id, dir, constraint);
          REQUIRE(constraints->assign_nodal_velocity_constraint(
                      set_id, velocity_constraint) == true);

          set_id = 1;
          dir = 1;
          constraint = -12.5;
          // Add velocity constraint to mesh
          velocity_constraint = std::make_shared<mpm::VelocityConstraint>(
              set_id, dir, constraint);
          REQUIRE(constraints->assign_nodal_velocity_constraint(
                      set_id, velocity_constraint) == true);

          // Add velocity constraint to all nodes in mesh
          velocity_constraint =
              std::make_shared<mpm::VelocityConstraint>(-1, dir, constraint);
          REQUIRE(constraints->assign_nodal_velocity_constraint(
                      set_id, velocity_constraint) == true);

          // When constraints fail
          dir = 2;
          // Add velocity constraint to mesh
          velocity_constraint = std::make_shared<mpm::VelocityConstraint>(
              set_id, dir, constraint);
          REQUIRE(constraints->assign_nodal_velocity_constraint(
                      set_id, velocity_constraint) == false);
        }

        SECTION("Check assign friction constraints to nodes") {
          tsl::robin_map<mpm::Index, std::vector<mpm::Index>> node_sets;
          node_sets[0] = std::vector<mpm::Index>{0, 2};
          node_sets[1] = std::vector<mpm::Index>{1, 3};

          REQUIRE(mesh->create_node_sets(node_sets, true) == true);

          //! Constraints object
          auto constraints = std::make_shared<mpm::Constraints<Dim>>(mesh);

          int set_id = 0;
          int dir = 0;
          int sign_n = 1;
          double friction = 0.5;
          // Add friction constraint to mesh
          auto friction_constraint = std::make_shared<mpm::FrictionConstraint>(
              set_id, dir, sign_n, friction);
          REQUIRE(constraints->assign_nodal_frictional_constraint(
                      set_id, friction_constraint) == true);

          set_id = 1;
          dir = 1;
          sign_n = -1;
          friction = -0.25;
          // Add friction constraint to mesh
          friction_constraint = std::make_shared<mpm::FrictionConstraint>(
              set_id, dir, sign_n, friction);
          REQUIRE(constraints->assign_nodal_frictional_constraint(
                      set_id, friction_constraint) == true);

          // Add friction constraint to all nodes in mesh
          friction_constraint = std::make_shared<mpm::FrictionConstraint>(
              -1, dir, sign_n, friction);
          REQUIRE(constraints->assign_nodal_frictional_constraint(
                      set_id, friction_constraint) == true);

          // When constraints fail
          dir = 2;
          // Add friction constraint to mesh
          friction_constraint = std::make_shared<mpm::FrictionConstraint>(
              set_id, dir, sign_n, friction);
          REQUIRE(constraints->assign_nodal_frictional_constraint(
                      set_id, friction_constraint) == false);
        }

        // Test assign velocity constraints to nodes
        SECTION("Check assign velocity constraints to nodes") {
          // Vector of particle coordinates
          std::vector<std::tuple<mpm::Index, unsigned, double>>
              velocity_constraints;
          //! Constraints object
          auto constraints = std::make_shared<mpm::Constraints<Dim>>(mesh);
          // Constraint
          velocity_constraints.emplace_back(std::make_tuple(0, 0, 10.5));
          velocity_constraints.emplace_back(std::make_tuple(1, 1, -10.5));
          velocity_constraints.emplace_back(std::make_tuple(2, 0, -12.5));
          velocity_constraints.emplace_back(std::make_tuple(3, 1, 0.0));

          REQUIRE(constraints->assign_nodal_velocity_constraints(
                      velocity_constraints) == true);
          // When constraints fail
          velocity_constraints.emplace_back(std::make_tuple(3, 2, 0.0));
          REQUIRE(constraints->assign_nodal_velocity_constraints(
                      velocity_constraints) == false);
        }

        // Test assign friction constraints to nodes
        SECTION("Check assign friction constraints to nodes") {
          // Vector of particle coordinates
          std::vector<std::tuple<mpm::Index, unsigned, int, double>>
              friction_constraints;
          //! Constraints object
          auto constraints = std::make_shared<mpm::Constraints<Dim>>(mesh);
          // Constraint
          friction_constraints.emplace_back(std::make_tuple(0, 0, 1, 0.5));
          friction_constraints.emplace_back(std::make_tuple(1, 1, -1, 0.5));
          friction_constraints.emplace_back(std::make_tuple(2, 0, 1, 0.25));
          friction_constraints.emplace_back(std::make_tuple(3, 1, -1, 0.0));

          REQUIRE(constraints->assign_nodal_friction_constraints(
                      friction_constraints) == true);
          // When constraints fail
          friction_constraints.emplace_back(std::make_tuple(3, 2, -1, 0.0));
          REQUIRE(constraints->assign_nodal_friction_constraints(
                      friction_constraints) == false);
        }

        // Test assign nodes concentrated_forces
        SECTION("Check assign nodes concentrated_forces") {
          // Vector of node coordinates
          std::vector<std::tuple<mpm::Index, unsigned, double>>
              nodes_concentrated_forces;
          // Concentrated_Forces
          nodes_concentrated_forces.emplace_back(std::make_tuple(0, 0, 10.5));
          nodes_concentrated_forces.emplace_back(std::make_tuple(1, 1, -10.5));
          nodes_concentrated_forces.emplace_back(std::make_tuple(2, 0, -12.5));
          nodes_concentrated_forces.emplace_back(std::make_tuple(3, 1, 0.0));

          REQUIRE(mesh->nnodes() == 6);

          REQUIRE(mesh->assign_nodal_concentrated_forces(
                      nodes_concentrated_forces) == true);
          // When concentrated_forces fail
          nodes_concentrated_forces.emplace_back(std::make_tuple(3, 2, 0.0));
          REQUIRE(mesh->assign_nodal_concentrated_forces(
                      nodes_concentrated_forces) == false);
          nodes_concentrated_forces.emplace_back(std::make_tuple(300, 0, 0.0));
          REQUIRE(mesh->assign_nodal_concentrated_forces(
                      nodes_concentrated_forces) == false);
        }

        // Test assign rotation matrices to nodes
        SECTION("Check assign rotation matrices to nodes") {
          // Map of nodal id and euler angles
          std::map<mpm::Index, Eigen::Matrix<double, Dim, 1>> euler_angles;
          // Node 0 with Euler angles of 10 and 20 deg
          euler_angles.emplace(std::make_pair(
              0, (Eigen::Matrix<double, Dim, 1>() << 10. * M_PI / 180,
                  20. * M_PI / 180)
                     .finished()));
          // Node 1 with Euler angles of 30 and 40 deg
          euler_angles.emplace(std::make_pair(
              1, (Eigen::Matrix<double, Dim, 1>() << 30. * M_PI / 180,
                  40. * M_PI / 180)
                     .finished()));
          // Node 2 with Euler angles of 50 and 60 deg
          euler_angles.emplace(std::make_pair(
              2, (Eigen::Matrix<double, Dim, 1>() << 50. * M_PI / 180,
                  60. * M_PI / 180)
                     .finished()));
          // Node 3 with Euler angles of 70 and 80 deg
          euler_angles.emplace(std::make_pair(
              3, (Eigen::Matrix<double, Dim, 1>() << 70. * M_PI / 180,
                  80. * M_PI / 180)
                     .finished()));

          // Check compute and assign rotation matrix
          REQUIRE(mesh->compute_nodal_rotation_matrices(euler_angles) == true);

          // Check for failure when missing node id
          // Node 100 (non-existent) with Euler angles of 90 and 100 deg
          euler_angles.emplace(std::make_pair(
              100, (Eigen::Matrix<double, Dim, 1>() << 90. * M_PI / 180,
                    100. * M_PI / 180)
                       .finished()));
          REQUIRE(mesh->compute_nodal_rotation_matrices(euler_angles) == false);

          // Check for failure of empty input
          std::map<mpm::Index, Eigen::Matrix<double, Dim, 1>>
              empty_euler_angles;
          REQUIRE(mesh->compute_nodal_rotation_matrices(empty_euler_angles) ==
                  false);

          // Check for failure when no nodes are assigned
          auto mesh_fail = std::make_shared<mpm::Mesh<Dim>>(1);
          REQUIRE(mesh_fail->compute_nodal_rotation_matrices(euler_angles) ==
                  false);
        }
      }
    }
  }

  //! Check if nodal properties is initialised
  SECTION("Check nodal properties initialisation") {
    // Create the different meshes
    std::shared_ptr<mpm::Mesh<Dim>> mesh = std::make_shared<mpm::Mesh<Dim>>(0);

    // Define nodes
    Eigen::Vector2d coords;
    coords << 0., 0.;
    std::shared_ptr<mpm::NodeBase<Dim>> node0 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(0, coords);

    coords << 2., 0.;
    std::shared_ptr<mpm::NodeBase<Dim>> node1 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(1, coords);

    coords << 2., 2.;
    std::shared_ptr<mpm::NodeBase<Dim>> node2 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(2, coords);

    coords << 0., 2.;
    std::shared_ptr<mpm::NodeBase<Dim>> node3 =
        std::make_shared<mpm::Node<Dim, Dof, Nphases>>(3, coords);

    // Add nodes 0 to 3 to the mesh
    REQUIRE(mesh->add_node(node0) == true);
    REQUIRE(mesh->add_node(node1) == true);
    REQUIRE(mesh->add_node(node2) == true);
    REQUIRE(mesh->add_node(node3) == true);

    // Initialise material models
    mesh->initialise_material_models(materials);

    // Check nodal properties creation
    REQUIRE_NOTHROW(mesh->create_nodal_properties());

    // Check nodal properties initialisation
    REQUIRE_NOTHROW(mesh->initialise_nodal_properties());
  }
}


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

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

//local
#include "parameters.hpp"

namespace tsdf{

template<typename Generator, typename ScalarContainer>
class GeneratorCRTP {
public:
	GeneratorCRTP(const Parameters<ScalarContainer>& parameters);

	const Parameters<ScalarContainer> parameters;
	typedef typename ScalarContainer::Scalar ContainerScalar;

	//TODO: make this generate method protected & hidden+called by corresponding methods in 2D & 3D versions
	//(not overloads), since then we can make the parameters different and remove the "image_y_coordinate" parameter for
	//the 3D case.
	/**
	 * @brief Generate a discrete implicit TSDF (Truncated Signed Distance Function) from the give depth image presumed to
	 * have been taken at the specified camera pose.
	 * @details Each voxel will contain the distance to the nearest surface, in voxels,
	 * truncated to +/- 1.0. Uses voxel size, truncation bounds, and other parameters that the generator was initialized
	 * with. For 2D versions of the algorithm, a coordinate is specified to limit usage of the depth image to a single pixel
	 * row.
	 * @param depth_image a 16-bit depth image
	 * @param camera_pose pose of the camera relative to world
	 * @param image_y_coordinate - (2D case only) row of the depth image to use
	 * @return the generated TSDF voxel grid.
	 */
	ScalarContainer generate(
			const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
			const Eigen::Matrix<ContainerScalar, 4, 4, Eigen::ColMajor>& camera_pose =
					Eigen::Matrix<ContainerScalar, 4, 4, Eigen::ColMajor>::Identity(),
			int image_y_coordinate = 0) const;
};

}  // namespace tsdf


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2006 Cristina Duminuco
Copyright (C) 2006, 2008 Ferdinando Ametrano
Copyright (C) 2006 Katiuscia Manzoni
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

// Based on swaptionvolatilitycube.cpp file from test-suite.
#ifndef cl_adjoint_swaption_volatility_cube_impl_hpp
#define cl_adjoint_swaption_volatility_cube_impl_hpp
#pragma once

#include "adjointswaptionvolatilitycubetest.hpp"
#include "utilities.hpp"
#include "adjointtestutilities.hpp"
#include "adjointtestbase.hpp"
#include <test-suite/swaptionvolstructuresutilities.hpp>
#include <ql/termstructures/volatility/swaption/swaptionvolcube2.hpp>
#include <ql/indexes/swap/euriborswap.hpp>
#include <boost/make_shared.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

#define OUTPUT_FOLDER_NAME "SwaptionVolatilityCube"

/*! The swaption vol cube is made up of ordered swaption vol surface
layers, each layer refers to a swap index of a given length
(in years), all indexes belong to the same family. In order
to identify the family (and its market conventions) an index of
whatever length from that family must be passed in as
swapIndexBase.

Tested functions are volatility and blackvariance.

Output of Volatility function is implied volatility.
Input components are strike rate, option time, and swaption period.
Implied volatility was obtained using interpolation from volatility cube values.
Volatility is a piecewise linear function of the strike rate. And the Volatility function is differentiated
with respect to strike.
Also Volatility is differentiated with respect to volatilities from volatility cube obtained using interpolation.

Output of BlackVariance function is Black variance.
Input components are strike rate, option time, and swaption period.
Black variance was derived from the formula:
volatility * volatility * optionTime.
The BlackVariance function is differentiated with respect to strike.
Derivative of blackVariance with respect to strike:
2 * volatility * optionTime * (volatility)'
Also blackVariance is differentiated with respect to volatilities from volatility cube
because volatility is differentiated  with respect to them.
*/

namespace
{
    enum
    {
#if defined CL_GRAPH_GEN
        // Number of points for dependency plots.
        pointNo = 100,
        // Number of points for performance plot.
        iterNo = 20,
        // Step for portfolio size for performance testing .
        step = 50,
#else
        // Number of points for dependency plots.
        pointNo = 1,
        // Number of points for performance plot.
        iterNo = 1,
        // Step for portfolio size for performance testing .
        step = 1,
#endif
        // Defines performance accuracy. Its value is a minimum number
        // of calling of O(1) complexity methods per one performance test.
        iterNumFactor = 1000,
    };

    enum TestType { VolAtmVol, StrikeAtmVol, VolBlackVariance, StrikeBlackVariance };

    std::string toString(TestType type)
    {
        switch (type)
        {
        case VolAtmVol:
            return "ImpliedVolatilitiesDiffVols";
        case StrikeAtmVol:
            return "ImpliedVolatilitiesDiffStrike";
        case VolBlackVariance:
            return "BlackVariancesDiffVols";
        case StrikeBlackVariance:
            return "BlackVariancesDiffStrike";
        default:
            return "";
        }
    }

    // Output variable which is differentiated.
    std::string variableType(TestType type)
    {
        switch (type)
        {
        case VolAtmVol:
            return "Volatility";
        case StrikeAtmVol:
            return "Volatility";
        case VolBlackVariance:
            return "BlackVariance";
        case StrikeBlackVariance:
            return "BlackVariance";
        default:
            return "";
        }
    }

    struct CommonVars1
    {
        CommonVars1()
        : backup_()
        , conventions_()
        , vegaWeighedSmileFit_(false)
        , atm_()
        , cube_()
        , termStructure_(flatRate(0.05, Actual365Fixed()))
        {
            conventions_.setConventions();

            // ATM swaptionvolmatrix.
            atm_.setMarketData();

            // Swaptionvolcube.
            cube_.setMarketData();
        }

        // Cleanup.
        SavedSettings backup_;
        // Global data.
        SwaptionMarketConventions conventions_;
        bool vegaWeighedSmileFit_;
        AtmVolatility atm_;
        VolatilityCube cube_;
        RelinkableHandle<YieldTermStructure> termStructure_;
    };

    struct CommonVars : public CommonVars1
    {
        CommonVars()
        :CommonVars1()
        , atmVolMatrix_(boost::make_shared<SwaptionVolatilityMatrix>(conventions_.calendar,
        conventions_.optionBdc,
        atm_.tenors.options,
        atm_.tenors.swaps,
        atm_.volsHandle,
        conventions_.dayCounter))
        , swapIndexBase_(boost::make_shared<EuriborSwapIsdaFixA>(2 * Years, termStructure_))
        , shortSwapIndexBase_(boost::make_shared<EuriborSwapIsdaFixA>(1 * Years, termStructure_))
        {
        }

        RelinkableHandle<SwaptionVolatilityStructure> atmVolMatrix_;
        boost::shared_ptr<SwapIndex> swapIndexBase_;
        boost::shared_ptr<SwapIndex> shortSwapIndexBase_;
    };

    // Struct for graphics recording.
    struct StrikeVariance
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "Strike rate", ""
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type&
            operator << (stream_type& stm, StrikeVariance& v)
        {
                stm << v.strike_
                    << ";" << v.blackVariance_
                    << std::endl;
                return stm;
            }

        Real strike_;
        Real blackVariance_;
    };

    struct SwaptionVolatilityCubeTestData : public CommonVars
    {
        explicit SwaptionVolatilityCubeTestData(TestType type, Real shift = 1e-10)
        : CommonVars()
        , test_type_(type)
        , generalVar_()
        , shift_(shift)
        , optionSize_(atm_.tenors.options.size())
        , swapsSize_(atm_.tenors.swaps.size())
        {}

        // Calculate volatility in case of test of function Volatility
        // and blackVariance in case of test of function blackVariance.
        Volatility calcVol(const std::vector<std::vector<Handle<Quote> > >& vols,
            const Period& optionTenor, const Period& swapTenor)
        {
            RelinkableHandle<SwaptionVolatilityStructure> atmVolMatrix = RelinkableHandle<SwaptionVolatilityStructure>(
                boost::make_shared<SwaptionVolatilityMatrix>(conventions_.calendar,
                conventions_.optionBdc,
                atm_.tenors.options,
                atm_.tenors.swaps,
                vols,
                conventions_.dayCounter));
            return calcVol(atmVolMatrix, optionTenor, swapTenor);
        }

        // Calculate volatility in case of test of function Volatility
        // and blackVariance in case of test of function blackVariance.
        Volatility calcVol(const RelinkableHandle<SwaptionVolatilityStructure>& atmVolMatrix,
            const Period& optionTenor, const Period& swapTenor)
        {
            SwaptionVolCube2 volCube(atmVolMatrix,
                cube_.tenors.options,
                cube_.tenors.swaps,
                cube_.strikeSpreads,
                cube_.volSpreadsHandle,
                swapIndexBase_,
                shortSwapIndexBase_,
                vegaWeighedSmileFit_);

            return calcVol(volCube, optionTenor, swapTenor);
        }

        // Calculate volatility in case of test of function Volatility
        // and blackVariance in case of test of function blackVariance.
        Volatility calcVol(const SwaptionVolCube2& volCube, const Period& optionTenor, const Period& swapTenor)
        {
            Rate strike = volCube.atmStrike(optionTenor,
                swapTenor);

            if (test_type_ == VolAtmVol)
                return volCube.volatility(optionTenor, swapTenor, strike, true);
            if (test_type_ == VolBlackVariance)
                return volCube.blackVariance(optionTenor, swapTenor, strike, true);

            return Volatility(0.0);
        }

        TestType test_type_;
        std::vector<Real> generalVar_;
        Real shift_;
        unsigned optionSize_, swapsSize_;
    };

    struct SwaptionVolatilityCubeTestDiffVols
        : public cl::AdjointTest<SwaptionVolatilityCubeTestDiffVols>
    {
        static const CalcMethod default_method = other;

        // Constructor parameters:
        // data to construct swaption volatility cube
        // logger - pointer to output stream that contains log stream.
        explicit SwaptionVolatilityCubeTestDiffVols(SwaptionVolatilityCubeTestData* data, cl::tape_empty_test_output* logger = nullptr)
        : AdjointTest()
        , data_(data)
        , vols_()
        , indepDouble_()
        , BlackVariance_()
        , swapSize_(data_->atm_.tenors.swaps.size())
        , optionSize_(data_->atm_.tenors.options.size())
        , optionTenorNumber_(optionSize_ / 2)
        , optionTenor_(data_->atm_.tenors.options[optionTenorNumber_])
        {
            setUniformData();
        }

        Size indepVarNumber() { return swapSize_; }

        Size depVarNumber() { return swapSize_; }

        Size minPerfIteration() { return 0; }

        double absTol() const { return 1e-4; }

        void recordTape()
        {
            cl::Independent(vols_[optionTenorNumber_]);
            calculateBlackVar();
            f_ = std::make_unique<cl::tape_function<double>>(vols_[optionTenorNumber_], BlackVariance_);
        }

        // Calculate volatility or blackVariance depending on test type
        // for different volatility cubes based on different input volatilities.
        void calculateBlackVar()
        {
            std::vector<std::vector<Handle<Quote> > > vols;
            vols.resize(optionSize_);
            for (Size i = 0; i < optionSize_; i++) {
                vols[i].resize(swapSize_);
                for (Size j = 0; j < swapSize_; j++)
                {
                    vols[i][j] = Handle<Quote>(boost::make_shared<SimpleQuote>(vols_[i][j]));
                }
            }

            BlackVariance_.resize(swapSize_);
            for (int i = 0; i < swapSize_; i++)
            {
                BlackVariance_[i] = data_->calcVol(vols, optionTenor_, data_->atm_.tenors.swaps[i]);
            }
        }

        // Calculates derivatives using finite difference method.
        void calcAnalytical()
        {
            analyticalResults_.resize(swapSize_ * swapSize_, 0.0);
            Real h = data_->shift_;
            std::vector<std::vector<cl::tape_double>> volsLeft, volsRight;
            volsLeft = vols_;
            volsRight = vols_;
            for (Size k = 0; k < swapSize_; k++)
            {
                volsRight[optionTenorNumber_][k] += h;
                volsLeft[optionTenorNumber_][k] -= h;

                std::vector<std::vector<Handle<Quote> > > volsShiftedRight;
                volsShiftedRight.resize(optionSize_);
                std::vector<std::vector<Handle<Quote> > > volsShiftedLeft;
                volsShiftedLeft.resize(optionSize_);
                for (Size i = 0; i < optionSize_; i++) {
                    volsShiftedRight[i].resize(swapSize_);
                    volsShiftedLeft[i].resize(swapSize_);
                    for (Size j = 0; j < swapSize_; j++)
                    {
                        volsShiftedRight[i][j] = Handle<Quote>(boost::make_shared<SimpleQuote>(volsRight[i][j]));
                        volsShiftedLeft[i][j] = Handle<Quote>(boost::make_shared<SimpleQuote>(volsLeft[i][j]));
                    }
                }

                analyticalResults_[k*swapSize_ + k] = (data_->calcVol(volsShiftedRight, optionTenor_, data_->atm_.tenors.swaps[k]) -
                    data_->calcVol(volsShiftedLeft, optionTenor_, data_->atm_.tenors.swaps[k])) / (2 * h);

                volsRight[optionTenorNumber_][k] -= h;
                volsLeft[optionTenorNumber_][k] += h;
            }
        }

        // Calculates derivatives using adjoint.
        void calcAdjoint()
        {
            adjointResults_ = f_->Jacobian(indepDouble_);
        }

        void setUniformData()
        {
            vols_.resize(optionSize_);
            for (Size i = 0; i < data_->optionSize_; i++) {
                vols_[i].resize(swapSize_);
                for (Size j = 0; j < swapSize_; j++)
                {
                    vols_[i][j] = data_->atm_.vols[i][j];
                }
            }

            indepDouble_.resize(swapSize_);
            for (Size i = 0; i < swapSize_; i++)
            {
                indepDouble_[i] = double(data_->atm_.vols[optionTenorNumber_][i]);
            }
        }

        SwaptionVolatilityCubeTestData* data_;
        std::vector<std::vector<cl::tape_double>> vols_;
        std::vector<double> indepDouble_;
        std::vector<Real> BlackVariance_;
        Size swapSize_;
        Size optionSize_;
        Size optionTenorNumber_;
        Period optionTenor_;
    };

    struct SwaptionVolatilityCubeTest
        : public cl::AdjointTest<SwaptionVolatilityCubeTest>
    {
        // Constructor parameters:
        // size - size of strike rates vector for perfomance plot
        // data to construct swaption volatility cube
        // logger - pointer to output stream which contains log stream.
        explicit SwaptionVolatilityCubeTest(Size size, SwaptionVolatilityCubeTestData* data, cl::tape_empty_test_output* logger = nullptr)
        : AdjointTest()
        , size_(size)
        , strikes_(size)
        , data_(data)
        , BlackVariance_()
        , SumBlackVariance_()
        , optPer_(data_->optionSize_ / 2)
        , swPer_(data_->swapsSize_ / 2)
        {
            assert(size_);
            setLogger(logger);
            if (size_ != 1)
            {
                for (Size i = 0; i < size_; i++)
                {
                    strikes_[i] = (0.4 + (0.2 * i) / (size_ - 1));
                }
            }
            else
            {
                strikes_[0] = 0.5;
            }
        }

        Size indepVarNumber() { return size_; }

        Size depVarNumber() { return 1; }

        Size minPerfIteration() { return iterNumFactor; }

        double absTol() const { return 1e-4; }

        void recordTape()
        {
            Independent(strikes_);
            calculateBlackVariance();
            f_ = std::make_unique<cl::tape_function<double>>(strikes_, SumBlackVariance_);
        }

        // Calculate volatility in case of test of function Volatility
        // and blackVariance in case of test of function blackVariance.
        void calculateBlackVariance()
        {
            SwaptionVolCube2 volCube(data_->atmVolMatrix_,
                data_->cube_.tenors.options,
                data_->cube_.tenors.swaps,
                data_->cube_.strikeSpreads,
                data_->cube_.volSpreadsHandle,
                data_->swapIndexBase_,
                data_->shortSwapIndexBase_,
                data_->vegaWeighedSmileFit_);

            SumBlackVariance_.resize(1, 0);
            BlackVariance_.resize(size_);
            for (int i = 0; i < size_; i++)
            {
                if (data_->test_type_ == StrikeAtmVol)
                {
                    // Calculate volatility.
                    BlackVariance_[i] = volCube.volatility(data_->atm_.tenors.options[optPer_],
                        data_->atm_.tenors.swaps[swPer_],
                        strikes_[i], true);
                    SumBlackVariance_[0] += BlackVariance_[i];
                }

                if (data_->test_type_ == StrikeBlackVariance)
                {
                    // Calculate black variance.
                    BlackVariance_[i] += volCube.blackVariance(data_->atm_.tenors.options[optPer_],
                        data_->atm_.tenors.swaps[swPer_],
                        strikes_[i], true);
                    SumBlackVariance_[0] += BlackVariance_[i];
                }
            }
        }

        // Calculates derivatives using finite difference method.
        void calcAnalytical()
        {
            SwaptionVolCube2 volCube(data_->atmVolMatrix_,
                data_->cube_.tenors.options,
                data_->cube_.tenors.swaps,
                data_->cube_.strikeSpreads,
                data_->cube_.volSpreadsHandle,
                data_->swapIndexBase_,
                data_->shortSwapIndexBase_,
                data_->vegaWeighedSmileFit_);

            analyticalResults_.resize(size_);
            for (int i = 0; i < size_; i++)
            {
                if (data_->test_type_ == StrikeAtmVol)
                {
                    // Calculate volatility.
                    Real blackDown = volCube.volatility(data_->atm_.tenors.options[optPer_],
                        data_->atm_.tenors.swaps[swPer_],
                        strikes_[i] - data_->shift_, true);

                    Real blackShifted = volCube.volatility(data_->atm_.tenors.options[optPer_],
                        data_->atm_.tenors.swaps[swPer_],
                        strikes_[i] + data_->shift_, true);

                    analyticalResults_[i] = (blackShifted - blackDown) / (2 * data_->shift_);
                }

                if (data_->test_type_ == StrikeBlackVariance)
                {
                    // Calculate black variance.
                    Real black = volCube.blackVariance(data_->atm_.tenors.options[optPer_],
                        data_->atm_.tenors.swaps[swPer_],
                        strikes_[i], true);

                    Real blackShifted = volCube.blackVariance(data_->atm_.tenors.options[optPer_],
                        data_->atm_.tenors.swaps[swPer_],
                        strikes_[i] + data_->shift_, true);

                    analyticalResults_[i] = (blackShifted - black) / data_->shift_;
                }
            }
        }

        SwaptionVolatilityCubeTestData* data_;
        Size size_;
        std::vector<cl::tape_double> strikes_;
        std::vector<Real> BlackVariance_;
        std::vector<Real> SumBlackVariance_;
        int optPer_;
        int swPer_;
    };

    struct TestData
        : public SwaptionVolatilityCubeTestData
    {
        explicit TestData(TestType type, Real shift = 1e-10)
        : SwaptionVolatilityCubeTestData(type, shift)
        , outPerform_title_("Swaption  " + variableType(type) + " differentiation performance with respect to strike rate")
        , outAdjoint_title_("Swaption " + variableType(type) + " adjoint differentiation with respect to fixed strike rate")
        , out_title_(variableType(type) + " dependence on strike rate")
        , out_ylabel_(variableType(type))
        , outPerform_(OUTPUT_FOLDER_NAME "//" + toString(type), {
            { "title", outPerform_title_ }
            , { "not_clear", "Not" }
            , { "ylabel", "Time (s)" }
            , { "xlabel", "Number of strike rates" }
            , { "line_box_width", "-5" }
            , { "cleanlog", "true" }
            , { "smooth", "default" }
        })
            , outAdjoint_(OUTPUT_FOLDER_NAME "//" + toString(type), {
                { "title", outAdjoint_title_ }
            , { "filename", "Adjoint" }
            , { "not_clear", "Not" }
            , { "ylabel", "Time (s)" }
            , { "xlabel", "Number of strikes" }
            , { "cleanlog", "false" }
            , { "smooth", "default" }
        })
            , outSize_(OUTPUT_FOLDER_NAME "//" + toString(type), {
                { "title", "Tape size dependence on number of strike rates" }
            , { "filename", "TapeSize" }
            , { "not_clear", "Not" }
            , { "ylabel", "Memory (MB)" }
            , { "cleanlog", "false" }
        })

            , out_(OUTPUT_FOLDER_NAME "//" + toString(type) + "//output", {
                { "filename", "StrikeDependence" }
            , { "not_clear", "Not" }
            , { "title", out_title_ }
            , { "ylabel", out_ylabel_ }
            , { "xlabel", "Strike rate" }
            , { "cleanlog", "false" }
        })
        { }

        bool makeOutput()
        {
            bool ok = true;

            if (pointNo > 0)
            {
                ok &= recordDependencePlot();
            }
            ok &= cl::recordPerformance(*this, iterNo, step);

            return ok;
        }

        std::shared_ptr<SwaptionVolatilityCubeTest> getTest(size_t size)
        {
            return std::make_shared<SwaptionVolatilityCubeTest>(size, this, &outPerform_);
        }

        // Makes plots for dependence BlackVariance on strikes rates.
        bool recordDependencePlot()
        {
            std::vector<StrikeVariance> outData(pointNo);
            auto test = getTest(pointNo);
            bool ok = test->testReverse();
            for (Size i = 0; i < pointNo; i++)
            {
                outData[i] = { test->strikes_[i], test->BlackVariance_[i] };
            }

            out_ << outData;
            return ok;

            return true;
        }

        std::string outPerform_title_;
        std::string outAdjoint_title_;
        std::string out_title_;
        std::string out_ylabel_;
        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;
    };

}

#endif


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-reduction
    Function object implementing sum capabilities

    Returns the sum of the elements of the SIMD vector

    @par Semantic:

    For every parameter of pack type T

    @code
    scalar_of_t<T> r = sum(x);
    @endcode

    is similar to:

    @code
    scalar_of_t<T> r = Zero;
    for(result_type i = 0; i != T::static_size; ++i)
      r += x[i];
    @endcode

  **/
  scalar_of_t<Value> sum(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/sum.hpp>
#include <boost/simd/function/simd/sum.hpp>

#endif


/*
 * ScalarCurveConfig.hpp
 *
 *  Created on: Mar 5, 2015
 *      Author: Paul Furgale, Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include <Eigen/Core>
#include <kindr/Core>

namespace curves {

typedef Eigen::Matrix<double, 6, 1> Vector6d;

struct SE3Config {
  typedef kindr::HomTransformQuatD ValueType;
  typedef kindr::TwistGlobalD DerivativeType;
};

}  // namespace curves


#include "ros/ros.h"
#include <rlss_ros/OccupancyGrid.h>
#include <rlss/OccupancyGrid.hpp>
#include <fstream>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem.hpp>

namespace fs = boost::filesystem;
constexpr unsigned int DIM = DIMENSION;

using OccupancyGrid = rlss::OccupancyGrid<double, DIM>;
using OccIndex = OccupancyGrid::Index;
using OccCoordinate = OccupancyGrid::Coordinate;
using StdVectorVectorDIM = OccupancyGrid::StdVectorVectorDIM;
using VectorDIM = OccupancyGrid::VectorDIM;
using Ellipsoid = rlss::Ellipsoid<double, DIM>;
using MatrixDIMDIM = rlss::internal::MatrixRC<double, DIM, DIM>;

int main(int argc, char **argv) {
    ros::init(argc, argv, "static_map_feeder");
    ros::NodeHandle nh;

    std::string obstacles_directory;
    nh.getParam("obstacles_directory", obstacles_directory);

    std::vector<double> step_size;
    nh.getParam("occupancy_grid_step_size", step_size);
    if(step_size.size() != DIM) {
        ROS_FATAL_STREAM("occupancy grid step size dimension" + std::to_string(step_size.size()) + " does not match dimension " + std::to_string(DIM));
        return 0;
    }
    OccCoordinate occ_step_size;
    for(std::size_t i = 0; i < DIM; i++) {
        occ_step_size(i) = step_size[i];
    }

    ROS_INFO_STREAM(occ_step_size.transpose());
    OccupancyGrid occupancy_grid(occ_step_size);
    boost::filesystem::path p(obstacles_directory);
    std::cout << std::endl;
    for(auto& p: fs::directory_iterator(obstacles_directory)) {
        ROS_INFO_STREAM(p.path().string());
        std::fstream obstacle_file(p.path().string(), std::ios_base::in);
        std::string type;
        obstacle_file >> type;
        if(type == "cvxhull") {
            StdVectorVectorDIM obstacle_pts;
            VectorDIM vec;
            while (obstacle_file >> vec(0)) {
                for(unsigned int d = 1; d < DIM; d++)
                    obstacle_file >> vec(d);
                obstacle_pts.push_back(vec);
            }
            occupancy_grid.addObstacle(obstacle_pts);
        } else if(type == "ellipsoid") {
            VectorDIM center;
            MatrixDIMDIM mtr;

            for(unsigned int i = 0; i < DIM; i++) {
                obstacle_file >> center(i);
            }

            for(unsigned int r = 0; r < DIM; r++) {
                for(unsigned int c = 0; c < DIM; c++) {
                    obstacle_file >> mtr(r, c);
                }
            }
            Ellipsoid ell(center, mtr);
            occupancy_grid.addObstacle(ell);
        }
    }

    ros::Publisher pub = nh.advertise<rlss_ros::OccupancyGrid>("occupancy_grid", 1);

    ros::Rate rate(1);
    while(ros::ok()) {
        rlss_ros::OccupancyGrid msg;

        for(unsigned int i = 0; i < DIM; i++) {
            msg.step_size.push_back(occ_step_size(i));
        }

        const auto& index_set = occupancy_grid.getIndexSet();

        for(const auto& idx: index_set) {
            for(unsigned int i = 0; i < DIM; i++) {
                msg.occupied_indexes.push_back(idx(i));
            }
        }

        pub.publish(msg);
        rate.sleep();
    }

    return 0;
}


/******************************************************************************
Copyright (c) 2020, Johannes Pankert. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

 * Neither the name of the 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 <gtest/gtest.h>

#include <ocs2_core/automatic_differentiation/CppAdInterface.h>
#include <ocs2_core/automatic_differentiation/Types.h>
#include <ocs2_robotic_tools/common/RotationTransforms.h>

#include <Eigen/Dense>

using namespace ocs2;
using vector3_t = Eigen::Matrix<scalar_t, 3, 1>;
using vector3_ad_t = Eigen::Matrix<ad_scalar_t, 3, 1>;
using matrix3_t = Eigen::Matrix<scalar_t, 3, 3>;
using Quaternion_t = Eigen::Quaternion<scalar_t>;
using Quaternion_ad_t = Eigen::Quaternion<ad_scalar_t>;
using AngleAxis_t = Eigen::AngleAxis<scalar_t>;

TEST(RotationTransforms, quaternionDifferenceJacobian) {
  auto adFunction = [](const ad_vector_t& x, const ad_vector_t& p, ad_vector_t& y) {
    Quaternion_ad_t q(x.head<4>());
    Quaternion_ad_t qRef(p.head<4>());
    y.resize(3);
    y = quaternionDistance(q, qRef);
  };

  CppAdInterface adInterface(adFunction, 4, 4, "quaternion_error");
  adInterface.createModels();

  for (int i = 0; i < 1000; i++) {
    Quaternion_t q = Quaternion_t::UnitRandom();
    Quaternion_t qRef = Quaternion_t::UnitRandom();

    vector_t qDyn = q.coeffs();
    vector_t qRefDyn = qRef.coeffs();
    ASSERT_TRUE(quaternionDistance(q, qRef).isApprox(adInterface.getFunctionValue(qDyn, qRefDyn), 1e-5));

    matrix_t jacManual = quaternionDistanceJacobian(q, qRef);
    matrix_t jacAuto = adInterface.getJacobian(qDyn, qRefDyn);
    ASSERT_TRUE(jacManual.isApprox(jacAuto, 1e-5));
  }
}

// Asserts that the quaterion performs the same rotation as the rotation matrix
void testQuaternionAgainstRotationMatrix(const Quaternion_t& quat, const matrix3_t& rotMatrix) {
  ASSERT_TRUE(quat.toRotationMatrix().isApprox(rotMatrix, 1e-5)) << "rotMatrix = \n"
                                                                 << rotMatrix << "\n quat = " << quat.coeffs().transpose();
}

// Attempts to test all the branches in the cpp ad function
TEST(RotationTransforms, matrixToQuaternionCppAd) {
  auto adFunction = [](const ad_vector_t& x, ad_vector_t& y) {
    Eigen::Map<const ad_matrix_t> rotMatrix(x.data(), 3, 3);
    y.resize(4);
    y = matrixToQuaternion(rotMatrix).coeffs();
  };

  CppAdInterface adInterface(adFunction, 9, "matrix_to_quaternion");
  adInterface.createModels();

  auto testRotationMatrix = [&](const matrix3_t& rotMatrix) -> Quaternion_t {
    Eigen::Map<const vector_t> serializedRotMatrix(rotMatrix.data(), 9, 1);
    const vector_t result = adInterface.getFunctionValue(serializedRotMatrix);
    const Quaternion_t resultQuaternion(result[3], result[0], result[1], result[2]);
    testQuaternionAgainstRotationMatrix(resultQuaternion, rotMatrix);
    return resultQuaternion;
  };

  // No rotation
  testRotationMatrix(matrix3_t::Identity());

  // 90 deg rotations
  testRotationMatrix(AngleAxis_t(0.5 * M_PI, vector3_t::UnitX()).toRotationMatrix());
  testRotationMatrix(AngleAxis_t(0.5 * M_PI, vector3_t::UnitY()).toRotationMatrix());
  testRotationMatrix(AngleAxis_t(0.5 * M_PI, vector3_t::UnitZ()).toRotationMatrix());

  // 180 deg rotations
  testRotationMatrix(AngleAxis_t(M_PI, vector3_t::UnitX()).toRotationMatrix());
  testRotationMatrix(AngleAxis_t(M_PI, vector3_t::UnitY()).toRotationMatrix());
  testRotationMatrix(AngleAxis_t(M_PI, vector3_t::UnitZ()).toRotationMatrix());

  // Random
  for (int i = 0; i < 1000; i++) {
    const Quaternion_t q = Quaternion_t::UnitRandom();
    auto resultQuaternion = testRotationMatrix(q.toRotationMatrix());

    // Assert that our original generating quaternion matches the resulting one
    ASSERT_TRUE(quaternionDistance(q, resultQuaternion).isZero(1e-5))
        << "q = " << q.coeffs().transpose() << ", resultQuaternion = " << resultQuaternion.coeffs().transpose();
  }
}

TEST(RotationTransforms, rotationMatrixToRotationVector) {
  auto runTest = [](const AngleAxis_t& angleAxis) {
    auto computedAngleAxis = rotationMatrixToRotationVector<scalar_t>(angleAxis.toRotationMatrix());
    const scalar_t vectol = 1e-6;
    return (computedAngleAxis - angleAxis.angle() * angleAxis.axis()).norm() < vectol;
  };
  auto message = [](const AngleAxis_t& angleAxis) {
    auto computedAngleAxis = rotationMatrixToRotationVector<scalar_t>(angleAxis.toRotationMatrix());
    std::stringstream ss;
    ss << "Failed test for angle: " << angleAxis.angle() << ", axle: " << angleAxis.axis().transpose()
       << "\n computed: " << computedAngleAxis.transpose() << ", should be " << (angleAxis.angle() * angleAxis.axis()).transpose();
    return ss.str();
  };

  // Test edge cases
  const scalar_t smallAngle = 1e-12;
  const scalar_t smallAngle2 = 1e-6;
  const scalar_array_t edgeCases = {0.0, smallAngle, smallAngle2, M_PI_2, M_PI - smallAngle2, M_PI - smallAngle, M_PI};
  const std::vector<vector3_t> testDirections = {vector3_t::UnitX(), vector3_t::UnitY(), vector3_t::UnitZ(),
                                                 vector3_t::Random().normalized()};

  for (auto angle : edgeCases) {
    for (const auto& axis : testDirections) {
      EXPECT_TRUE(runTest({angle, axis})) << message({angle, axis});
      EXPECT_TRUE(runTest({angle, -axis})) << message({angle, -axis});
    }
  }

  // Test range of angles
  const scalar_t dAngle = 1e-4;
  for (scalar_t angle = 0.0; angle < M_PI; angle += dAngle) {
    for (const auto& axis : testDirections) {
      EXPECT_TRUE(runTest({angle, axis})) << message({angle, axis});
      EXPECT_TRUE(runTest({angle, -axis})) << message({angle, -axis});
    }
  }
}

TEST(RotationTransforms, rotationErrorSign) {
  // This just tests the sign correctness of the error. Edge cases are tested in rotationMatrixToRotationVector

  // Pick random rotations
  Quaternion_t lhsToWorld = Quaternion_t(0.1, 0.2, 0.3, 0.4).normalized();
  Quaternion_t rhsToWorld = Quaternion_t(0.5, 0.6, 0.7, 0.8).normalized();

  // Compute rotation error in world
  const auto errorInWorld = rotationErrorInWorld(lhsToWorld.toRotationMatrix(), rhsToWorld.toRotationMatrix());
  const AngleAxis_t errorAngleAxis(errorInWorld.norm(), errorInWorld.normalized());

  // Test for correct composition of the error
  // Error = lhs [-] rhs => lhs = error [+] rhs
  // Comparing as rotation removes ambiguity between the two quaternions that represent the same rotation.
  ASSERT_TRUE(lhsToWorld.toRotationMatrix().isApprox((errorAngleAxis * rhsToWorld).toRotationMatrix()));
}

TEST(RotationTransforms, rotationErrorGlobalvsLocal) {
  // This just tests the frame conventions of the error. Edge cases are tested in rotationMatrixToRotationVector

  // Pick random rotations
  Quaternion_t lhsToWorld = Quaternion_t(0.1, 0.2, 0.3, 0.4).normalized();
  Quaternion_t rhsToWorld = Quaternion_t(0.5, 0.6, 0.7, 0.8).normalized();

  // Compute independently in world and local frame
  const auto errorInWorld = rotationErrorInWorld(lhsToWorld.toRotationMatrix(), rhsToWorld.toRotationMatrix());
  const auto errorInLocal = rotationErrorInLocal(lhsToWorld.toRotationMatrix(), rhsToWorld.toRotationMatrix());

  // Test for equality in world frame. Both lhs and rhs frames are valid for the local representation.
  ASSERT_TRUE(errorInWorld.isApprox(lhsToWorld * errorInLocal));
  ASSERT_TRUE(errorInWorld.isApprox(rhsToWorld * errorInLocal));
}

TEST(RotationTransforms, rotationErrorGradientDescent) {
  // Tests that the box minus rotation error has suitable gradients
  auto adFunction = [](const ad_vector_t& x, const ad_vector_t& p, ad_vector_t& y) {
    vector3_ad_t eulerAngles = x.head<3>();
    vector3_ad_t eulerAnglesReference = p.head<3>();
    y = rotationErrorInWorld(getRotationMatrixFromZyxEulerAngles(eulerAngles), getRotationMatrixFromZyxEulerAngles(eulerAnglesReference));
  };

  CppAdInterface adInterface(adFunction, 3, 3, "rotationErrorJacobianTest");
  adInterface.createModels();

  // Check Jacobian at origin. The Jacobian is not identity because the ZYX euler angles are stored in z - y - x order...
  const matrix3_t originJacobian = (matrix3_t() << 0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0).finished();

  matrix3_t jacOrigin = adInterface.getJacobian(vector3_t::Zero(), vector3_t::Zero());
  ASSERT_TRUE(jacOrigin.isApprox(originJacobian));

  // Check Jacobian at full rotation
  matrix3_t jac2Pi = adInterface.getJacobian(vector3_t::Zero(), vector3_t{0.0, 0.0, 2.0 * M_PI});
  ASSERT_TRUE(jac2Pi.isApprox(originJacobian));

  auto gradientDescent = [&](const vector3_t& eulerAnglesStart, vector3_t eulerAnglesReference) {
    vector3_t eulerAngles = eulerAnglesStart;
    for (int i = 0; i < 1000; i++) {
      matrix3_t jac = adInterface.getJacobian(eulerAngles, eulerAnglesReference);
      vector3_t error = adInterface.getFunctionValue(eulerAngles, eulerAnglesReference);
      // Take step with fixed step size
      eulerAngles -= 0.1 * jac.fullPivHouseholderQr().solve(error);
    }
    return eulerAngles;
  };

  // Able to do gradient descent with exactly Pi offset
  vector3_t eulerAngles = vector3_t::Zero();
  vector3_t eulerAnglesReference = vector3_t{0.0, 0.0, M_PI};
  ASSERT_TRUE(gradientDescent(eulerAngles, eulerAnglesReference).isApprox(eulerAnglesReference, 1e-6));

  // Able to do gradient descent between random angles
  eulerAngles = vector3_t::Random();
  eulerAnglesReference = vector3_t::Random();
  ASSERT_TRUE(gradientDescent(eulerAngles, eulerAnglesReference).isApprox(eulerAnglesReference, 1e-6));
}


#include <frovedis.hpp>
#include <frovedis/ml/clustering/agglomerative.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;

template <class T>
void do_agglomerative_clustering(const std::string& input_file,
                              int ncluster, T threshold,
                              const std::string& link,
                              const std::string& model_file,
                              const std::string& label_file) {
  time_spent data_load(INFO), train(INFO);
  data_load.lap_start();
  auto mat = make_rowmajor_matrix_load<T>(input_file);
  data_load.lap_stop();
  data_load.show_lap("data loading time: ");
  train.lap_start();
  auto model = agglomerative_training<T>(mat, link);
  train.lap_stop();
  train.show_lap("training time: ");
  model.save(model_file);
  auto ret = agglomerative_assign_cluster<T>(model, ncluster, threshold, ncluster);
  make_dvector_scatter(ret).saveline(label_file);
}

int main(int argc, char** argv) {
  use_frovedis use(argc,argv);
  time_spent t(DEBUG);

  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
    ("help,h", "print help")
    ("input,i", value<std::string>(), "input dense matrix")
    ("model,m", value<std::string>(), "output model")
    ("label,o", value<std::string>(), "output label")
    ("nclus,n", value<int>(), "number of clusters (default: 2)")
    ("linkage,l", value<std::string>() , "linkage type - supported: average, single, complete (default: average)")
    ("float,f", "for float type input")
    ("double,d","for double type input(default input type is double)")
    ("threshold,t","threshold distance above which points will not be clustered (default: 0)")
    ("verbose", "set loglevel to DEBUG")
    ("verbose2", "set loglevel to TRACE")
    ("clustering_verbose", "set loglevel to INFO (default loglevel is INFO)");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  std::string input, output_m, output_l;
  std::string linkage = "average";
  int nclus = 2;
  float threshold = 0;

  if(argmap.count("help")){
    std::cerr << opt << std::endl;
    exit(1);
  }

  if(argmap.count("input")){
    input = argmap["input"].as<std::string>();
  } else {
    std::cerr << "input is not specified" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }

  if(argmap.count("model")){
    output_m = argmap["model"].as<std::string>();
  } else {
    std::cerr << "output model is not specified" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }

  if(argmap.count("label")){
    output_l = argmap["label"].as<std::string>();
  } else {
    std::cerr << "output label is not specified" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }

  if(argmap.count("nclus")){
    nclus = argmap["nclus"].as<int>();
  }

  if(argmap.count("threshold")){
    threshold = argmap["threshold"].as<float>();
  }

  if(argmap.count("linkage")){
    linkage = argmap["linkage"].as<std::string>();
  }

  if(argmap.count("verbose")) set_loglevel(DEBUG);
  if(argmap.count("verbose2")) set_loglevel(TRACE);
  if(argmap.count("clustering_verbose")) set_loglevel(INFO);

  if(argmap.count("float"))
    do_agglomerative_clustering<float>(input,nclus,threshold,linkage,output_m,output_l);
  else
    do_agglomerative_clustering<double>(input,nclus,threshold,linkage,output_m,output_l);

  return 0;
}


// Copyright (C) 2011 by the BEM++ Authors
//
// 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 "fiber/linear_scalar_shapeset_barycentric.hpp"
#include "fiber/scalar_traits.hpp"
#include "../type_template.hpp"
#include "../check_arrays_are_close.hpp"

#include <algorithm>
#include "common/armadillo_fwd.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/version.hpp>
#include <complex>

// Tests

BOOST_AUTO_TEST_SUITE(LinearScalarShapesetBarycentric_triangle)

BOOST_AUTO_TEST_CASE_TEMPLATE(size_is_3, ValueType, basis_function_types)
{
    typedef Fiber::LinearScalarShapesetBarycentric<ValueType> Shapeset;
    Shapeset basis(Shapeset::TYPE1);
    BOOST_CHECK_EQUAL(basis.size(), 3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(order_is_1, ValueType, basis_function_types)
{
    typedef Fiber::LinearScalarShapesetBarycentric<ValueType> Shapeset;
    Shapeset basis(Shapeset::TYPE1);
    BOOST_CHECK_EQUAL(basis.order(), 1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_works_for_all_dofs_type1,
                              ValueType, basis_function_types)
{
    const int vertexCount = 3;
    const int elementDim = 2;
    const int pointCount = vertexCount;
    typedef Fiber::LinearScalarShapesetBarycentric<ValueType> Shapeset;
    Shapeset basis(Shapeset::TYPE1);
    typedef typename Shapeset::CoordinateType CoordinateType;
    arma::Mat<CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES, points, Fiber::ALL_DOFS, data);

    Fiber::_3dArray<ValueType> expected(1, // component count
                                        vertexCount,
                                        pointCount);
    std::fill(expected.begin(), expected.end(), 0.);
    expected(0, 0, 0) = 1.;
    expected(0, 0, 1) = 1./3;
    expected(0, 0, 2) = 1./2;

    expected(0, 1, 0) = 0;
    expected(0, 1, 1) = 1./3;
    expected(0, 1, 2) = 0;

    expected(0, 2, 0) = 0;
    expected(0, 2, 1) = 1./3;
    expected(0, 2, 2) = 1./2;

    const CoordinateType tol =
        10 * std::numeric_limits<CoordinateType>::epsilon();
    BOOST_CHECK(check_arrays_are_close<ValueType>(data.values, expected, tol));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_works_for_all_dofs_type2,
                              ValueType, basis_function_types)
{
    const int vertexCount = 3;
    const int elementDim = 2;
    const int pointCount = vertexCount;
    typedef Fiber::LinearScalarShapesetBarycentric<ValueType> Shapeset;
    Shapeset basis(Shapeset::TYPE2);
    typedef typename Shapeset::CoordinateType CoordinateType;
    arma::Mat<CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES, points, Fiber::ALL_DOFS, data);

    Fiber::_3dArray<ValueType> expected(1, // component count
                                        vertexCount,
                                        pointCount);
    std::fill(expected.begin(), expected.end(), 0.);
    expected(0, 0, 0) = 1.;
    expected(0, 0, 1) = 1./2;
    expected(0, 0, 2) = 1./3;

    expected(0, 1, 0) = 0;
    expected(0, 1, 1) = 1./2;
    expected(0, 1, 2) = 1./3;

    expected(0, 2, 0) = 0;
    expected(0, 2, 1) = 0;
    expected(0, 2, 2) = 1./3;

    const CoordinateType tol =
        10 * std::numeric_limits<CoordinateType>::epsilon();
    BOOST_CHECK(check_arrays_are_close<ValueType>(data.values, expected, tol));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_derivatives_works_for_all_dofs_type1,
                              ValueType, basis_function_types)
{
    const int vertexCount = 3;
    const int elementDim = 2;
    const int pointCount = vertexCount;
    typedef Fiber::LinearScalarShapesetBarycentric<ValueType> Shapeset;
    typedef typename Shapeset::CoordinateType CoordinateType;
    Shapeset basis(Shapeset::TYPE1);
    arma::Mat<CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::DERIVATIVES, points, Fiber::ALL_DOFS, data);

    Fiber::_4dArray<ValueType> expected(1, // component count
                                        2, //
                                        vertexCount,
                                        pointCount);
    std::fill(expected.begin(), expected.end(), 0.);

    expected(0, 0, 0, 0) = -2./3.;
    expected(0, 1, 0, 0) = -1./2.;
    expected(0, 0, 0, 1) = -2./3.;
    expected(0, 1, 0, 1) = -1./2.;
    expected(0, 0, 0, 2) = -2./3.;
    expected(0, 1, 0, 2) = -1./2.;

    expected(0, 0, 1, 0) = 1./3.;
    expected(0, 1, 1, 0) = 0.;
    expected(0, 0, 1, 1) = 1./3.;
    expected(0, 1, 1, 1) = 0.;
    expected(0, 0, 1, 2) = 1./3.;
    expected(0, 1, 1, 2) = 0.;

    expected(0, 0, 2, 0) = 1./3.;
    expected(0, 1, 2, 0) = 1./2;
    expected(0, 0, 2, 1) = 1./3.;
    expected(0, 1, 2, 1) = 1./2;
    expected(0, 0, 2, 2) = 1./3.;
    expected(0, 1, 2, 2) = 1./2;

    const CoordinateType tol =
        10 * std::numeric_limits<CoordinateType>::epsilon();
    BOOST_CHECK(check_arrays_are_close<ValueType>(data.derivatives, expected,
						  tol));
}

BOOST_AUTO_TEST_SUITE_END()