/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \file tests/src/spmdm.cpp  Tests sparse-matrix-dense-matrix products.
*   \test  Tests sparse-matrix-dense-matrix products.
**/

//
// include necessary system headers
//
#include <iostream>
#include <cmath>

//
// ublas includes
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1
//#define VIENNACL_WITH_OPENCL 1
//#define VIENNACL_WITH_CUDA 1
//#define VIENNACL_DEBUG_KERNEL 1
//#define VIENNACL_BUILD_INFO 1

//
// ViennaCL includes
//
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/linalg/direct_solve.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/ell_matrix.hpp"
#include "viennacl/hyb_matrix.hpp"
#include "viennacl/linalg/prod.hpp"       //generic matrix-vector product
#include "viennacl/linalg/norm_2.hpp"     //generic l2-norm for vectors
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"

using namespace boost::numeric;

template< typename ScalarType >
int check_matrices(const ublas::matrix< ScalarType >& ref_mat, const ublas::matrix< ScalarType >& mat, ScalarType eps) {

  std::size_t size1, size2;
  size1 = ref_mat.size1(); size2 = ref_mat.size2();
  if ( (size1 != mat.size1()) || (size2 != mat.size2()) )
    return EXIT_FAILURE;

  for (unsigned int i = 0; i < size1; i++)
    for (unsigned int j = 0; j < size2; j++)
    {
      ScalarType rel_error = std::abs(ref_mat(i,j) - mat(i,j)) / std::max(std::abs(ref_mat(i,j)), std::abs(mat(i,j)));
      if ( rel_error > eps ) {
        std::cout << "ERROR: Verification failed at (" << i <<", "<< j << "): "
                  << " Expected: " << ref_mat(i,j) << ", got: " << mat(i,j) << " (relative error: " << rel_error << ")" << std::endl;
        return EXIT_FAILURE;
      }
    }

  std::cout << "Everything went well!" << std::endl;
  return EXIT_SUCCESS;
}

template<typename NumericT, typename ResultLayoutT, typename FactorLayoutT>
int test(NumericT epsilon)
{
  int retVal = EXIT_SUCCESS;

  ublas::compressed_matrix<NumericT>    ublas_lhs;

  if (viennacl::io::read_matrix_market_file(ublas_lhs, "../examples/testdata/mat65k.mtx") == EXIT_FAILURE)
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return EXIT_FAILURE;
  }

  // add some extra weight to diagonal in order to avoid issues with round-off errors
  for (std::size_t i=0; i<ublas_lhs.size1(); ++i)
    ublas_lhs(i,i) *= NumericT(1.5);

  std::size_t cols_rhs = 1;

  viennacl::compressed_matrix<NumericT> compressed_lhs;
  viennacl::ell_matrix<NumericT>        ell_lhs;
  viennacl::coordinate_matrix<NumericT> coo_lhs;
  viennacl::hyb_matrix<NumericT>     hyb_lhs;

  ublas::matrix<NumericT> ublas_result;
  viennacl::matrix<NumericT, ResultLayoutT> result;

  viennacl::copy( ublas_lhs, compressed_lhs);
  viennacl::copy( ublas_lhs, ell_lhs);
  viennacl::copy( ublas_lhs, coo_lhs);
  viennacl::copy( ublas_lhs, hyb_lhs);

  ublas::matrix<NumericT> ublas_rhs1(ublas_lhs.size2(), cols_rhs);
  viennacl::matrix<NumericT, FactorLayoutT> rhs1(ublas_lhs.size2(), cols_rhs);

  ublas::matrix<NumericT> ublas_rhs2;
  viennacl::matrix<NumericT, FactorLayoutT> rhs2;

  ublas::matrix<NumericT> temp(ublas_rhs1.size1(), cols_rhs);

  for (unsigned int i = 0; i < ublas_rhs1.size1(); i++)
    for (unsigned int j = 0; j < ublas_rhs1.size2(); j++)
      ublas_rhs1(i,j) = NumericT(0.5) + NumericT(0.1) * random<NumericT>();
  viennacl::copy( ublas_rhs1, rhs1);

  ublas_rhs2 = ublas::trans( ublas_rhs1);
  viennacl::copy( ublas_rhs2, rhs2);

  /* gold result */
  ublas_result = ublas::prod( ublas_lhs, ublas_rhs1);

  /******************************************************************/
  std::cout << "Testing compressed(CSR) lhs * dense rhs" << std::endl;
  result = viennacl::linalg::prod( compressed_lhs, rhs1);

  temp.clear();
  viennacl::copy( result, temp);
  retVal = check_matrices(ublas_result, temp, epsilon);

  /******************************************************************/
  std::cout << "Testing compressed(ELL) lhs * dense rhs" << std::endl;
  result.clear();
  result = viennacl::linalg::prod( ell_lhs, rhs1);

  temp.clear();
  viennacl::copy( result, temp);
  check_matrices(ublas_result, temp, epsilon);

  /******************************************************************/

  std::cout << "Testing compressed(COO) lhs * dense rhs" << std::endl;
  result.clear();
  result = viennacl::linalg::prod( coo_lhs, rhs1);

  temp.clear();
  viennacl::copy( result, temp);
  check_matrices(ublas_result, temp, epsilon);

  /******************************************************************/

  std::cout << "Testing compressed(HYB) lhs * dense rhs" << std::endl;
  result.clear();
  result = viennacl::linalg::prod( hyb_lhs, rhs1);

  temp.clear();
  viennacl::copy( result, temp);
  check_matrices(ublas_result, temp, epsilon);

  /******************************************************************/

  /* gold result */
  ublas_result = ublas::prod( ublas_lhs, ublas::trans(ublas_rhs2));

  /******************************************************************/
  std::cout << std::endl << "Testing compressed(CSR) lhs * transposed dense rhs:" << std::endl;
  result.clear();
  result = viennacl::linalg::prod( compressed_lhs, viennacl::trans(rhs2));

  temp.clear();
  viennacl::copy( result, temp);
  retVal = check_matrices(ublas_result, temp, epsilon);

  /******************************************************************/
  std::cout << "Testing compressed(ELL) lhs * transposed dense rhs" << std::endl;
  result.clear();
  result = viennacl::linalg::prod( ell_lhs, viennacl::trans(rhs2));

  temp.clear();
  viennacl::copy( result, temp);
  check_matrices(ublas_result, temp, epsilon);

  /******************************************************************/
  std::cout << "Testing compressed(COO) lhs * transposed dense rhs" << std::endl;
  result.clear();
  result = viennacl::linalg::prod( coo_lhs, viennacl::trans(rhs2));

  temp.clear();
  viennacl::copy( result, temp);
  check_matrices(ublas_result, temp, epsilon);

  /******************************************************************/

  std::cout << "Testing compressed(HYB) lhs * transposed dense rhs" << std::endl;
  result.clear();
  result = viennacl::linalg::prod( hyb_lhs, viennacl::trans(rhs2));

  temp.clear();
  viennacl::copy( result, temp);
  check_matrices(ublas_result, temp, epsilon);

  /******************************************************************/
  if (retVal == EXIT_SUCCESS) {
    std::cout << "Tests passed successfully" << std::endl;
  }

  return retVal;
}

//
// -------------------------------------------------------------
//
int main()
{
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "## Test :: Sparse-Dense Matrix Multiplication" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << std::endl;

  int retval = EXIT_SUCCESS;

  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << std::endl;
  {
    typedef float NumericT;
    NumericT epsilon = static_cast<NumericT>(1E-4);
    std::cout << "# Testing setup:" << std::endl;
    std::cout << "  eps:     " << epsilon << std::endl;
    std::cout << "  numeric: float" << std::endl;
    std::cout << "  layout:  row-major, row-major" << std::endl;
    retval = test<NumericT, viennacl::row_major, viennacl::row_major>(epsilon);
    if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
    else
        return retval;

    std::cout << "# Testing setup:" << std::endl;
    std::cout << "  eps:     " << epsilon << std::endl;
    std::cout << "  numeric: float" << std::endl;
    std::cout << "  layout:  row-major, column-major" << std::endl;
    retval = test<NumericT, viennacl::row_major, viennacl::column_major>(epsilon);
    if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
    else
        return retval;

    std::cout << "# Testing setup:" << std::endl;
    std::cout << "  eps:     " << epsilon << std::endl;
    std::cout << "  numeric: float" << std::endl;
    std::cout << "  layout:  column-major, row-major" << std::endl;
    retval = test<NumericT, viennacl::column_major, viennacl::row_major>(epsilon);
    if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
    else
        return retval;

    std::cout << "# Testing setup:" << std::endl;
    std::cout << "  eps:     " << epsilon << std::endl;
    std::cout << "  numeric: float" << std::endl;
    std::cout << "  layout:  column-major, column-major" << std::endl;
    retval = test<NumericT, viennacl::column_major, viennacl::column_major>(epsilon);
    if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
    else
        return retval;

  }
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << std::endl;

#ifdef VIENNACL_WITH_OPENCL
  if ( viennacl::ocl::current_device().double_support() )
#endif
  {
    {
      typedef double NumericT;
      NumericT epsilon = 1.0E-12;
      std::cout << "# Testing setup:" << std::endl;
      std::cout << "  eps:     " << epsilon << std::endl;
      std::cout << "  numeric: double" << std::endl;
      std::cout << "  layout:  row-major, row-major" << std::endl;
      retval = test<NumericT, viennacl::row_major, viennacl::row_major>(epsilon);
      if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
      else
        return retval;

      std::cout << "# Testing setup:" << std::endl;
      std::cout << "  eps:     " << epsilon << std::endl;
      std::cout << "  numeric: double" << std::endl;
      std::cout << "  layout:  row-major, column-major" << std::endl;
      retval = test<NumericT, viennacl::row_major, viennacl::column_major>(epsilon);
      if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
      else
        return retval;

      std::cout << "# Testing setup:" << std::endl;
      std::cout << "  eps:     " << epsilon << std::endl;
      std::cout << "  numeric: double" << std::endl;
      std::cout << "  layout:  column-major, row-major" << std::endl;
      retval = test<NumericT, viennacl::column_major, viennacl::row_major>(epsilon);
      if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
      else
        return retval;

      std::cout << "# Testing setup:" << std::endl;
      std::cout << "  eps:     " << epsilon << std::endl;
      std::cout << "  numeric: double" << std::endl;
      std::cout << "  layout:  column-major, column-major" << std::endl;
      retval = test<NumericT, viennacl::column_major, viennacl::column_major>(epsilon);
      if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
      else
        return retval;
    }
    std::cout << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << std::endl;
  }
#ifdef VIENNACL_WITH_OPENCL
  else
    std::cout << "No double precision support, skipping test..." << std::endl;
#endif

  std::cout << std::endl;
  std::cout << "------- Test completed --------" << std::endl;
  std::cout << std::endl;

  return retval;
}


#include "Day20-ParticleSwarm.h"

#include "Particle.h"
#include "ParticleSystem.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>
__END_LIBRARIES_DISABLE_WARNINGS

namespace
{

const unsigned NUM_SIMULATION_STEPS = 1'000;

}

namespace AdventOfCode
{
namespace Year2017
{
namespace Day20
{

std::vector<Particle> createParticlesFromLines(const std::vector<std::string>& particleLines)
{
    const auto allSeparatorsPredicate = boost::is_any_of("pva=<>, ");
    std::vector<Particle> particles;

    unsigned particleID = 0;

    for (auto particleLine : particleLines)
    {
        // Remove leading and trailing separators
        boost::trim_if(particleLine, allSeparatorsPredicate);

        std::vector<std::string> tokens;
        boost::split(tokens, particleLine, allSeparatorsPredicate, boost::token_compress_on);

        if (tokens.size() != 9)
        {
            throw std::runtime_error("Expected exactly 9 tokens.");
        }

        Particle::Vector3D position{std::stoi(tokens[0]), std::stoi(tokens[1]), std::stoi(tokens[2])};
        Particle::Vector3D velocity{std::stoi(tokens[3]), std::stoi(tokens[4]), std::stoi(tokens[5])};
        Particle::Vector3D acceleration{std::stoi(tokens[6]), std::stoi(tokens[7]), std::stoi(tokens[8])};

        particles.emplace_back(particleID++, std::move(position), std::move(velocity), std::move(acceleration));
    }

    return particles;
}

unsigned particleIDClosestToOriginLongTerm(const std::vector<std::string>& particleLines)
{
    std::vector<Particle> particles = createParticlesFromLines(particleLines);

    ParticleSystem particleSystem{std::move(particles)};

    particleSystem.simulate(NUM_SIMULATION_STEPS);
    return particleSystem.particleIDClosestToOrigin();
}

unsigned numParticlesLeftAfterCollisions(const std::vector<std::string>& particleLines)
{
    std::vector<Particle> particles = createParticlesFromLines(particleLines);

    ParticleSystem particleSystem{std::move(particles)};

    particleSystem.simulateWithCollisions(NUM_SIMULATION_STEPS);
    return particleSystem.numParticles();
}

}
}
}


#include <iostream>
#include <unordered_map>
#include <unordered_set>

// Boost
#include <boost/program_options.hpp>
#include <boost/foreach.hpp>

// Eigen
#include <Eigen/Core>

// FCL Headers
#include <fcl/collision.h>
#include <fcl/collision_node.h>
#include <fcl/traversal/traversal_node_setup.h>
#include <fcl/continuous_collision.h>

// Yaml
#include <yaml-cpp/yaml.h>

// Nanoflann
#include <nanoflann.hpp>
#include "KDTreeVectorOfVectorsAdaptor.h"

// OMPL
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/prm/SPARS.h>
#include <ompl/geometric/planners/prm/SPARStwo.h>

// local includes
#include "common.h"
#include "searchgraph.h"
#include "Timer.hpp"

#include "fclHelper.h"
#include "NanoFlannSearchgraphAdaptor.h"
#include "fclStateValidityChecker.hpp"
#include "fclMotionValidator.hpp"

using namespace std;
using namespace fcl;
using namespace nanoflann;
using namespace ompl;

// see http://motion.me.ucsb.edu/book-lrpk/pdfs/LecturesPlanningKinematics-FB+SLS-v0.91.pdf, p91
// computes i'th halton number for given prime base p
double halton(int i, int p)
{
  double S = 0;
  int i_tmp = i;
  double f = 1.0 / p;
  while(i_tmp > 0) {
    std::div_t result = std::div(i_tmp, p);
    int q = result.quot;
    int r = result.rem;
    S += f * r;
    i_tmp = q;
    f /= p;
  }
  return S;
}

Vec3f toVec3f(const position_t& pos)
{
  return Vec3f(pos.x(), pos.y(), pos.z());
}

struct GenerationType {
  enum Type
  {
    Grid,
    Halton,
    SPARS,
    SPARS2,
  };
};

class HaltonStateSampler : public ompl::base::StateSampler
{
public:
  HaltonStateSampler(
    const ompl::base::StateSpace *space)
    : ompl::base::StateSampler(space)
    , m_i(0)
  {
    const ompl::base::RealVectorBounds &bounds = static_cast<const ompl::base::RealVectorStateSpace *>(space_)->getBounds();
    auto diff = bounds.getDifference();

    m_scale = std::max(std::max(diff[0], diff[1]), diff[3]);
  }

  void sampleUniform(ompl::base::State *state) override
  {
    const ompl::base::RealVectorBounds &bounds = static_cast<const ompl::base::RealVectorStateSpace *>(space_)->getBounds();
    while (true) {
      ++m_i;
      double x = halton(m_i, 2) * m_scale + bounds.low[0];
      if (bounds.low[0] <= x && bounds.high[0] >=x) {
        double y = halton(m_i, 3) * m_scale + bounds.low[1];
        if (bounds.low[1] <= y && bounds.high[1] >=y) {
          double z = halton(m_i, 5) * m_scale + bounds.low[2];
          if (bounds.low[2] <= z && bounds.high[2] >=z) {
            ompl::base::RealVectorStateSpace::StateType *rstate = static_cast<ompl::base::RealVectorStateSpace::StateType *>(state);
            rstate->values[0] = x;
            rstate->values[1] = y;
            rstate->values[2] = z;
            break;
          }
        }
      }
    }
  }

  void sampleUniformNear(ompl::base::State *state, const ompl::base::State *near, const double distance) override
  {
    throw std::runtime_error("sampleUniformNear not implemented!");
  }

  void sampleGaussian(ompl::base::State *state, const ompl::base::State *mean, const double stdDev) override
  {
    throw std::runtime_error("sampleGaussian not implemented!");
  }
private:
  int m_i;
  double m_scale;
};

ompl::base::StateSamplerPtr allocHaltonStateSampler(const ompl::base::StateSpace* space)
{
  return ompl::base::StateSamplerPtr(new HaltonStateSampler(space));
}

std::istream& operator>>(std::istream& in, GenerationType::Type& type)
{
    std::string token;
    in >> token;
    if (token == "Grid")
        type = GenerationType::Grid;
    else if (token == "Halton")
        type = GenerationType::Halton;
    else if (token == "SPARS")
        type = GenerationType::SPARS;
    else if (token == "SPARS2")
        type = GenerationType::SPARS2;
    else
        in.setstate(std::ios_base::failbit);
    return in;
}

bool checkBounds(
  const std::array<fclHelper::range, 3>& bbox,
  const position_t& pos)
{
  return pos.x() >= bbox[0].min
      && pos.x() <= bbox[0].max
      && pos.y() >= bbox[1].min
      && pos.y() <= bbox[1].max
      && pos.z() >= bbox[2].min
      && pos.z() <= bbox[2].max;
}

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

  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");
  std::string environmentFile;
  std::string robotFile;
  std::string addVerticesFile;
  std::string outputFile;
  std::string configFile;
  // size_t numPoints;
  // double searchRadius;
  double xmin,xmax,ymin,ymax,zmin,zmax;
  GenerationType::Type genType = GenerationType::Grid;
  size_t dimension;
  float fixedZ;
  desc.add_options()
      ("help", "produce help message")
      ("environment,e", po::value<std::string>(&environmentFile)->required(), "input file for environment (STL)")
      ("robot,r", po::value<std::string>(&robotFile)->required(), "input file for robot (STL)")
      ("addVertices,a", po::value<std::string>(&addVerticesFile), "input file for additional vertices (YAML)")
      ("output,o", po::value<std::string>(&outputFile)->required(), "output file for graph (YAML)")
      ("config,c", po::value<std::string>(&configFile)->required(), "config file for advanced parameters (YAML)")
      ("type", po::value<GenerationType::Type>(&genType)->default_value(GenerationType::Grid)->multitoken(), "Method, one of [Grid,Halton,SPARS,SPARS2]. Default: Grid")
      ("dimension", po::value<size_t>(&dimension)->default_value(3), "roadmap dimension (2 or 3); For 2 the z-coordinate used is fixedZ")
      ("fixedZ", po::value<float>(&fixedZ)->default_value(0.0), "z-coordinate used if dimension is set to 2")
      // ("numPoints", po::value<size_t>(&numPoints)->default_value(100), "Number of points to sample in bounding box")
      // ("searchRadius", po::value<double>(&searchRadius)->default_value(0.5), "kd tree search radius")
      ("xmin", po::value<double>(&xmin), "bounding box minimum x")
      ("xmax", po::value<double>(&xmax), "bounding box maximum x")
      ("ymin", po::value<double>(&ymin), "bounding box minimum y")
      ("ymax", po::value<double>(&ymax), "bounding box maximum y")
      ("zmin", po::value<double>(&zmin), "bounding box minimum z")
      ("zmax", po::value<double>(&zmax), "bounding box maximum z")
  ;

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

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

  YAML::Node cfg = YAML::LoadFile(configFile);

  CollisionGeometry* environment = fclHelper::createCollisionGeometryFromFile(environmentFile);
  CollisionGeometry* robot = fclHelper::createCollisionGeometryFromFile(robotFile);

  std::array<fclHelper::range, 3> bbox = fclHelper::boundingBox(environment);
  if (vm.count("xmin")) bbox[0].min = xmin;
  if (vm.count("xmax")) bbox[0].max = xmax;
  if (vm.count("ymin")) bbox[1].min = ymin;
  if (vm.count("ymax")) bbox[1].max = ymax;
  if (vm.count("zmin")) bbox[2].min = zmin;
  if (vm.count("zmax")) bbox[2].max = zmax;

  fcl::Transform3f env_tf;//(Vec3f(-0.5, -0.5, -0.5));
  for (size_t i = 0; i < 3; ++i) {
    bbox[i].min += env_tf.getTranslation()[i];
    bbox[i].max += env_tf.getTranslation()[i];
  }

  std::cout << "Bounding box (environment): " << std::endl;
  std::cout << bbox[0].min << "," << bbox[0].max << std::endl;
  std::cout << bbox[1].min << "," << bbox[1].max << std::endl;
  std::cout << bbox[2].min << "," << bbox[2].max << std::endl;

  std::array<fclHelper::range, 3> bbox2 = fclHelper::boundingBox(robot);
  std::cout << "Bounding box (robot): " << std::endl;
  std::cout << bbox2[0].min << "," << bbox2[0].max << std::endl;
  std::cout << bbox2[1].min << "," << bbox2[1].max << std::endl;
  std::cout << bbox2[2].min << "," << bbox2[2].max << std::endl;

  // adjust bounding box
  for (size_t i = 0; i < 3; ++i) {
    bbox[i].min -= bbox2[i].min;
    bbox[i].max -= bbox2[i].max;
  }

  Timer timer;
  searchGraph_t roadmap;

  size_t addEdgesForVertices = 0;
  if (addVerticesFile.size() > 0) {
    YAML::Node config = YAML::LoadFile(addVerticesFile);

    for (const auto& node : config["vertices"]) {
      const auto& pos = node["pos"];
      position_t p(
        pos[0].as<float>(),
        pos[1].as<float>(),
        pos[2].as<float>());
      std::string name = node["name"].as<std::string>();

      fcl::Transform3f robot_tf(toVec3f(p));
      CollisionRequest request;
      CollisionResult result;
      bool collision = false;

      if (environment && robot) {
        collide(environment, env_tf, robot, robot_tf, request, result);
        collision = result.isCollision();
      }

      collision |= !checkBounds(bbox, p);

      if (!collision) {
        auto v = boost::add_vertex(roadmap);
        roadmap[v].pos = p;
        roadmap[v].name = name;
      } else {
        std::cout << "Warning: additional vertex " << name << " is in collision!" << std::endl;
        // std::cout << result.getContact(0).pos << " " << p.x() << std::endl;
      }
    }
  }

  if (genType == GenerationType::Grid) {
    auto node = cfg["grid"];
    const float gridSize = node["gridSize"].as<float>();
    float zmin = bbox[2].min + bbox2[2].min + gridSize/2;
    float zmax = bbox[2].max;
    if (dimension == 2) {
      zmin = fixedZ;
      zmax = fixedZ + gridSize / 2.0;
    }

    for (float x = bbox[0].min + bbox2[0].min + gridSize/2; x < bbox[0].max; x += gridSize) {
      for (float y = bbox[1].min + bbox2[1].min + gridSize/2; y < bbox[1].max; y += gridSize) {
        for (float z = zmin; z < zmax; z += gridSize) {
          fcl::Transform3f robot_tf(Vec3f(x, y, z));
          CollisionRequest request;
          CollisionResult result;
          bool collision = false;

          // CollisionObject<float> obj1(environment, env_tf);
          // CollisionObject<float> obj2(robot, robot_tf);
          if (environment && robot) {
            collide(environment, env_tf, robot, robot_tf, request, result);
            collision = result.isCollision();
          }

          if (!collision) {
            auto v = boost::add_vertex(roadmap);
            roadmap[v].pos = position_t(x, y, z);
            roadmap[v].name = "v" + std::to_string(v);
          }
        }
      }
    }
    // add edges for all vertices
    addEdgesForVertices = num_vertices(roadmap);
  }
  else if (genType == GenerationType::Halton) {
    auto node = cfg["halton"];
    const size_t numPoints = node["numPoints"].as<size_t>();
    const double scale = std::max(std::max(bbox[0].max - bbox[0].min, bbox[1].max - bbox[1].min), bbox[2].max - bbox[2].min);
    size_t numSamples = 0;
    for (size_t i = 0; ;++i) {
      double x = halton(i+1, 2) * scale + bbox[0].min;
      if (bbox[0].min <= x && bbox[0].max >=x) {
        double y = halton(i+1, 3) * scale + bbox[1].min;
        if (bbox[1].min <= y && bbox[1].max >=y) {
          double z = halton(i+1, 5) * scale + bbox[2].min;
          if (bbox[2].min <= z && bbox[2].max >=z) {
            // std::cout << x << "," << y << "," << z << std::endl;
            // position_t pos(x, y, z);

            fcl::Transform3f env_tf; //(Vec3f(0, 0, 0));
            fcl::Transform3f robot_tf(Vec3f(x, y, z));
            CollisionRequest request;
            CollisionResult result;

            // CollisionObject<float> obj1(environment, env_tf);
            // CollisionObject<float> obj2(robot, robot_tf);

            collide(environment, env_tf, robot, robot_tf, request, result);

            if (!result.isCollision()) {
              auto v = boost::add_vertex(roadmap);
              roadmap[v].pos = position_t(x, y, z);
              roadmap[v].name = "v" + std::to_string(v);
            }

            ++numSamples;
            if (numSamples == numPoints) {
              break;
            }
          }
        }
      }
    }

    // add edges for all vertices
    addEdgesForVertices = num_vertices(roadmap);
  }

  else if (genType == GenerationType::SPARS
        || genType == GenerationType::SPARS2) {
    // add edges for additional vertices, only
    addEdgesForVertices = num_vertices(roadmap);

    base::StateSpacePtr space(new base::RealVectorStateSpace(dimension));
    base::RealVectorBounds bounds(dimension);
    for (size_t i = 0; i < dimension; ++i) {
      bounds.setLow(i, bbox[i].min);
      bounds.setHigh(i, bbox[i].max);
    }
    space->as<base::RealVectorStateSpace>()->setBounds(bounds);
    if (genType == GenerationType::SPARS) {
      auto node = cfg["spars"];
      if (node["sampler"].as<std::string>() == "halton") {
        space->setStateSamplerAllocator(allocHaltonStateSampler);
      }
    }

    base::SpaceInformationPtr si(new base::SpaceInformation(space));
    base::StateValidityCheckerPtr stateValidityChecker(new fclStateValidityChecker(si, environment, robot, env_tf));
    si->setStateValidityChecker(stateValidityChecker);
    base::MotionValidatorPtr motionValidator(new fclMotionValidator(si, environment, robot, env_tf));
    si->setMotionValidator(motionValidator);
    // si->setStateValidityCheckingResolution(0.03); // 3%; TODO: remove and add proper motion validator
    si->setup();

    base::ProblemDefinitionPtr pdef(new base::ProblemDefinition(si));
    base::ScopedState<base::RealVectorStateSpace> start(space);
    // fill start state
    base::ScopedState<base::RealVectorStateSpace> goal(space);
    // fill goal state
    pdef->setStartAndGoalStates(start, goal);

    if (genType == GenerationType::SPARS) {
      auto node = cfg["spars"];

      geometric::SPARS p(si);
      p.setProblemDefinition(pdef);
      // std::cout << p.getDenseDeltaFraction()
      // << "Delta: " << p.getSparseDeltaFraction() * si->getMaximumExtent()
      // << " " << p.getStretchFactor()
      // << " " << p.getMaxFailures() << std::endl;
      // p.setSparseDeltaFraction(1.0 / si->getMaximumExtent() );

      p.setDenseDeltaFraction(node["denseDelta"].as<float>() / si->getMaximumExtent() );
      p.setSparseDeltaFraction(node["sparseDelta"].as<float>() / si->getMaximumExtent() );
      p.setStretchFactor(node["stretchFactor"].as<float>());
      p.setMaxFailures(node["maxFailures"].as<int>());

      // p.constructRoadmap(/*base::timedPlannerTerminationCondition(30)*/base::IterationTerminationCondition(node["maxIter"].as<int>()), true);
      p.constructRoadmap(base::timedPlannerTerminationCondition(30), true);

      p.printDebug();
#if 1
      const auto& roadmapOMPL = p.getRoadmap();
      // const auto& roadmapOMPL = p.getDenseGraph();
      auto stateProperty = boost::get(geometric::SPARS::vertex_state_t(), roadmapOMPL);

      std::unordered_map<geometric::SPARS::SparseVertex, vertex_t> vertexMap;
      for (size_t i = 0; i < boost::num_vertices(roadmapOMPL); ++i) {
        base::State* state = stateProperty[i];
        if (state) {
          position_t p (-1, -1, -1);
          const base::RealVectorStateSpace::StateType* typedState = state->as<base::RealVectorStateSpace::StateType>();
          if (dimension == 3) {
            p = position_t((*typedState)[0], (*typedState)[1], (*typedState)[2]);
          } else if (dimension == 2) {
            p = position_t((*typedState)[0], (*typedState)[1], fixedZ);
          }
          auto v = boost::add_vertex(roadmap);
          roadmap[v].pos = p;
          roadmap[v].name = "v" + std::to_string(v);
          vertexMap[i] = v;
        }
      }

      // add OMPL edges
      BOOST_FOREACH (const geometric::SPARS::SparseEdge e, boost::edges(roadmapOMPL)) {
        size_t i = boost::source(e, roadmapOMPL);
        size_t j = boost::target(e, roadmapOMPL);

        add_edge(vertexMap.at(i), vertexMap.at(j), roadmap);
      }
      #else
      const auto& roadmapOMPL = p.getDenseGraph();
      auto stateProperty = boost::get(geometric::SPARS::vertex_state_t(), roadmapOMPL);

      size_t offset = num_vertices(roadmap);
      for (size_t i = 0; i < boost::num_vertices(roadmapOMPL); ++i) {
        base::State* state = stateProperty[i];
        position_t p (-1, -1, -1);
        if (state) {
          const base::RealVectorStateSpace::StateType* typedState = state->as<base::RealVectorStateSpace::StateType>();
          p = position_t((*typedState)[0], (*typedState)[1], (*typedState)[2]);
        }
        auto v = boost::add_vertex(roadmap);
        roadmap[v].pos = p;
        roadmap[v].name = "v" + std::to_string(v);
      }

      // add OMPL edges
      BOOST_FOREACH (const geometric::SPARS::DenseEdge e, boost::edges(roadmapOMPL)) {
        size_t i = boost::source(e, roadmapOMPL);
        size_t j = boost::target(e, roadmapOMPL);

        add_edge(i + offset, j + offset, roadmap);
      }
#endif

    }

    if (genType == GenerationType::SPARS2) {
      auto node = cfg["spars2"];

      geometric::SPARStwo p(si);
      p.setProblemDefinition(pdef);

      p.setDenseDeltaFraction(node["denseDelta"].as<float>() / si->getMaximumExtent() );
      p.setSparseDeltaFraction(node["sparseDelta"].as<float>() / si->getMaximumExtent() );
      p.setStretchFactor(node["stretchFactor"].as<float>());
      p.setMaxFailures(node["maxFailures"].as<int>());

      // p.constructRoadmap(/*base::timedPlannerTerminationCondition(30)*/base::IterationTerminationCondition(node["maxIter"].as<int>()), true);
      p.constructRoadmap(base::timedPlannerTerminationCondition(30), true);

      const auto& roadmapOMPL = p.getRoadmap();
      auto stateProperty = boost::get(geometric::SPARStwo::vertex_state_t(), roadmapOMPL);

      std::unordered_map<geometric::SPARS::SparseVertex, vertex_t> vertexMap;
      for (size_t i = 0; i < boost::num_vertices(roadmapOMPL); ++i) {
        base::State* state = stateProperty[i];
        if (state) {
          position_t p (-1, -1, -1);
          const base::RealVectorStateSpace::StateType* typedState = state->as<base::RealVectorStateSpace::StateType>();
          if (dimension == 3) {
            p = position_t((*typedState)[0], (*typedState)[1], (*typedState)[2]);
          } else if (dimension == 2) {
            p = position_t((*typedState)[0], (*typedState)[1], fixedZ);
          }
          auto v = boost::add_vertex(roadmap);
          roadmap[v].pos = p;
          roadmap[v].name = "v" + std::to_string(v);
          vertexMap[i] = v;
        }
      }

      // add OMPL edges
      BOOST_FOREACH (const geometric::SPARStwo::Edge e, boost::edges(roadmapOMPL)) {
        size_t i = boost::source(e, roadmapOMPL);
        size_t j = boost::target(e, roadmapOMPL);

        add_edge(vertexMap.at(i), vertexMap.at(j), roadmap);
      }
    }

  }

  typedef KDTreeSingleIndexAdaptor< L2_Simple_Adaptor<float, NanoFlannSearchgraphAdaptor>, NanoFlannSearchgraphAdaptor, 3 > kd_tree_t;
  NanoFlannSearchgraphAdaptor adaptor(roadmap);
  kd_tree_t index(3, adaptor);
  index.buildIndex();

  size_t totalNeighbors = 0;
  size_t totalDuplicates = 0;

  // find duplicates during the process
  // since the additional vertices are at the beginning, they should not be marked
  std::vector<bool> vertexDuplicates(num_vertices(roadmap), false);

  for (size_t i = 0; i < num_vertices(roadmap); ++i) {
    if (!vertexDuplicates[i]) {
      std::vector<std::pair<size_t, float> > ret_matches;

      nanoflann::SearchParams params;
      /*const size_t nMatches =*/ index.radiusSearch(&(roadmap[i].pos)[0], 1e-3, ret_matches, params);

      // totalNeighbors += ret_matches.size() - 1;

      for (const auto& match : ret_matches) {
        if (i != match.first) {
          vertexDuplicates[match.first] = true;
          ++totalDuplicates;
          // std::cout << "duplicate: " << getVertexName(match.first, vertexNames) << std::endl;
        }
      }
    }
  }

  std::unordered_map<size_t, std::unordered_set<size_t> > edgeMap;

  auto node = cfg["connection"];
  // bool radiusSearch = node["mode"].as<std::string>() == "radius";
  float radius = node["maxRadius"].as<float>();
  size_t maxNeighbors = node["maxNeighbors"].as<float>();

  for (size_t i = 0; i < addEdgesForVertices; ++i) {
    if (!vertexDuplicates[i]) {
#if 0
      std::vector<std::pair<size_t, float> > ret_matches;

      nanoflann::SearchParams params;
      /*const size_t nMatches =*/ index.radiusSearch(&(roadmap[i].pos)[0], searchRadius, ret_matches, params);

      // totalNeighbors += ret_matches.size() - 1;

      for (const auto& match : ret_matches) {
        size_t j = match.first;
#else
      size_t num_results = maxNeighbors + 1; // +1 because it finds itself, too
      std::vector<size_t>   ret_index(num_results);
      std::vector<float> out_dist_sqr(num_results);
      num_results = index.knnSearch(&(roadmap[i].pos)[0], num_results, &ret_index[0], &out_dist_sqr[0]);
      if (num_results == 0) {
        std::cout << "WARNING: Couldn't find any neighbors for " << roadmap[i].name << std::endl;
      }
      ret_index.resize(num_results);
      out_dist_sqr.resize(num_results);
      for (size_t k = 0; k < num_results; ++k) {
        size_t j = ret_index[k];
#endif
        if (i != j
            && !vertexDuplicates[j]
            && out_dist_sqr[k] < radius * radius) {

          ++totalNeighbors;

          // edges are unidirectional, so avoid adding any existing edge
          if (edgeMap[j].find(i) == edgeMap[j].end()) {
            Transform3f robot_tf_beg(toVec3f(roadmap[i].pos));
            Transform3f robot_tf_end(toVec3f(roadmap[j].pos));

            ContinuousCollisionRequest request;//(10, 0.0001, fcl::CCDM_SCREW,
                        // fcl::GST_LIBCCD, fcl::CCDC_CONSERVATIVE_ADVANCEMENT);
            ContinuousCollisionResult result;
            bool collision = false;

            if (environment && robot) {
              continuousCollide(environment, env_tf, env_tf,
                                robot, robot_tf_beg, robot_tf_end,
                                request, result);
              collision = result.is_collide;
            }

            if (!collision) {
              // edges.push_back(edge(i, j));
              add_edge(i, j, roadmap);
              edgeMap[i].insert(j);
            }
          }
        }
      }
    }

    // cout << "radiusSearch(): radius=" << search_radius << " -> " << nMatches << " matches\n";
    // for (size_t i=0;i<nMatches;i++) {
    //   cout << "idx["<< i << "]=" << ret_matches[i].first << " dist["<< i << "]=" << ret_matches[i].second << endl;
    // }
    // cout << "\n";
  }

  timer.stop();
  std::cout << "Elapsed Time: " << timer.elapsedSeconds() << std::endl;

  std::cout << "Avg. possible neighbors: " << totalNeighbors / (double)num_vertices(roadmap) << std::endl;
  std::cout << "# duplicates: " << totalDuplicates << std::endl;
  std::cout << "# vertices: " << num_vertices(roadmap) - totalDuplicates << " #edges: " << num_edges(roadmap) << std::endl;

  delete environment;
  delete robot;

  // assign edge names
  size_t i = 0;
  BOOST_FOREACH (const edge_t e, boost::edges(roadmap)) {
    roadmap[e].name = "e" + std::to_string(i);
    ++i;
  }

  saveSearchGraph(roadmap, outputFile);

  return 0;
}


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

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/gammaln.hpp>
#include <boost/simd/function/std.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/five.hpp>
#include <boost/simd/constant/mtwo.hpp>

STF_CASE_TPL (" gammaln",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::gammaln;
  using r_t = decltype(gammaln(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(gammaln(bs::Inf<T>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Minf<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Nan<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Zero<T>()), bs::Inf<r_t>(), 0);
#endif
  STF_ULP_EQUAL(gammaln(bs::Mone<T>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Mtwo<T>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::One<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Two<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(gammaln(bs::Mzero<T>()),         bs::Inf<r_t>(), 0);

  STF_ULP_EQUAL(gammaln(bs::Halfeps<T>()),       bs::std_(gammaln)(bs::Halfeps<T>()), 1);
  STF_ULP_EQUAL(gammaln(bs::Eps<T>()),           bs::std_(gammaln)(bs::Eps<T>()), 0.5);
  STF_ULP_EQUAL(gammaln(bs::Half<T>()),          bs::std_(gammaln)(bs::Half<T>()), 0.5);
  STF_ULP_EQUAL(gammaln(T(1.5)),                 bs::std_(gammaln)(T(1.5)), 1.5);
  STF_ULP_EQUAL(gammaln(T(2.5)),                 bs::std_(gammaln)(T(2.5)), 0.5);
  STF_ULP_EQUAL(gammaln(T(13)) ,                 bs::std_(gammaln)(T(13)), 0.5);
  STF_ULP_EQUAL(gammaln(T(13.5)) ,               bs::std_(gammaln)(T(13.5)), 0.5);
  STF_ULP_EQUAL(gammaln(T(-0.1)),                bs::std_(gammaln)(T(-0.1)),         1);
  STF_ULP_EQUAL(gammaln(-bs::Half<T>()),         bs::std_(gammaln)(-bs::Half<T>()),  0.5);
  STF_ULP_EQUAL(gammaln(-bs::Halfeps<T>()),      bs::std_(gammaln)(-bs::Halfeps<T>()), 0.5);
  STF_ULP_EQUAL(gammaln(T(-27.5)),               bs::std_(gammaln)(T(-27.5)),            3);
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <boost/multiprecision/gmp.hpp>

using namespace boost::multiprecision;

int main()
{
   mpf_float a;
   mpz_int b;
   mpq_rational c;
}


/* boost random.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org/libs/random for documentation.
 *
 * $Id: random.hpp 24096 2004-07-27 03:43:34Z dgregor $
 *
 * Revision history
 *  2000-02-18  portability fixes (thanks to Beman Dawes)
 *  2000-02-21  shuffle_output, inversive_congruential_schrage,
 *              generator_iterator, uniform_smallint
 *  2000-02-23  generic modulus arithmetic helper, removed *_schrage classes,
 *              implemented Streamable and EqualityComparable concepts for
 *              generators, added Bernoulli distribution and Box-Muller
 *              transform
 *  2000-03-01  cauchy, lognormal, triangle distributions; fixed
 *              uniform_smallint; renamed gaussian to normal distribution
 *  2000-03-05  implemented iterator syntax for distribution functions
 *  2000-04-21  removed some optimizations for better BCC/MSVC compatibility
 *  2000-05-10  adapted to BCC and MSVC
 *  2000-06-13  incorporated review results
 *  2000-07-06  moved basic templates from namespace detail to random
 *  2000-09-23  warning removals and int64 fixes (Ed Brey)
 *  2000-09-24  added lagged_fibonacci generator (Matthias Troyer)
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_HPP
#define BOOST_RANDOM_HPP

// generators
#include <boost/random/linear_congruential.hpp>
#include <boost/random/additive_combine.hpp>
#include <boost/random/inversive_congruential.hpp>
#include <boost/random/shuffle_output.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/lagged_fibonacci.hpp>
#include <boost/random/ranlux.hpp>
#include <boost/random/linear_feedback_shift.hpp>
#include <boost/random/xor_combine.hpp>

namespace boost {
  typedef random::xor_combine<random::xor_combine<random::linear_feedback_shift<uint32_t, 32, 31, 13, 12, 0>, 0,
    random::linear_feedback_shift<uint32_t, 32, 29, 2, 4, 0>, 0, 0>, 0,
                      random::linear_feedback_shift<uint32_t, 32, 28, 3, 17, 0>, 0, 0> taus88;
} // namespace  boost

// misc
#include <boost/random/random_number_generator.hpp>

// distributions
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/triangle_distribution.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/cauchy_distribution.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/geometric_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/lognormal_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/uniform_on_sphere.hpp>

#endif // BOOST_RANDOM_HPP


////////////////////////////////////////////////////////////////////////////////
#include "region_grow.h"
#include <cellogram/navigation.h>
#include <cellogram/boundary_loop.h>
#include <igl/slice.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Dense>
#include <algorithm>
#include <queue>
#include <iostream>
////////////////////////////////////////////////////////////////////////////////

namespace cellogram {

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

	void region_grow(std::vector<std::vector<int>> &Graph, const Eigen::Matrix<bool, 1, Eigen::Dynamic> &crit_pass, Eigen::VectorXi &region)
	{
		int n = crit_pass.size();
		int cId = 1;
		std::queue<int> Q;
		Eigen::VectorXi visited = Eigen::VectorXi::Zero(n);
		std::vector<int> id(n, 0);

		for (int i = 0; i < n; i++)
		{
 			Q.push(i);
			while (!Q.empty())
			{
				int u = Q.front();
				Q.pop();

				if (crit_pass(u) && visited(u) == 0)
				{
					//add connections to queue if not already visited
					for (std::vector<int>::iterator it = Graph[u].begin(); it != Graph[u].end(); ++it) {
						if (visited(*it) == 0)
						{
							Q.push(*it);
						}
					}
					id[u] = cId;
				}
				// set vertex to visited
				visited(u) = 1;

				//std::cout << i << " - " << crit_pass[i] << " - " << id[u] << " - " << u << std::endl;
			}
			// While loop left which means that the next vertex found is from a new group
			cId++;
		}

		// Loop through region to clean up IDs
		std::vector<int> map = id;
		std::sort(map.begin(), map.end());
		auto newEnd = std::unique(map.begin(), map.end());
		map.erase(newEnd, map.end());

		for (int i = 0; i < map.size(); i++)
		{
			std::replace(id.begin(), id.end(), map[i], i);
		}

		// Save into eigen format
		region = Eigen::VectorXi::Zero(n);
		for (int i = 0; i < id.size(); i++)
		{
			region(i) = id[i];
		}
	}

// -----------------------------------------------------------------------------
} // namespace cellogram


#include <Eigen/Geometry>
#include <sm/eigen/gtest.hpp>
#include <sm/eigen/NumericalDiff.hpp>
#include <aslam/backend/KinematicChain.hpp>
#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/EuclideanExpression.hpp>
#include <aslam/backend/test/ExpressionTests.hpp>

using namespace aslam::backend;

Eigen::Vector3d Zero = Eigen::Vector3d::Zero();
Eigen::Matrix3d Identity = Eigen::Matrix3d::Identity();
Eigen::Vector3d Ones = Eigen::Vector3d::Ones();
Eigen::Matrix3d MinusIdentity = -Eigen::Matrix3d::Identity();
Eigen::Vector3d X = Eigen::Vector3d::UnitX();
Eigen::Vector3d Y = Eigen::Vector3d::UnitY();
Eigen::Vector3d Z = Eigen::Vector3d::UnitZ();

TEST(KinematicChainTestSuites, testTheoreticallyOneFrame) {
  {
    CoordinateFrame A = CoordinateFrame(RotationExpression());
    std::string msg = "Testing all nothing.";

    sm::eigen::assertEqual(A.getPG().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getVG().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getAG().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getR_G_L().toRotationMatrix(), Identity, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getOmegaG().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getAlphaG().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
  }
  {
    CoordinateFrame A(MinusIdentity, Ones, Ones, Ones, Ones, Ones);
    std::string msg = "Testing all one.";

    sm::eigen::assertEqual(A.getPG().toValue(), Ones, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getVG().toValue(), Ones, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getAG().toValue(), Ones, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getR_G_L().toRotationMatrix(), MinusIdentity, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getOmegaG().toValue(), Ones, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getAlphaG().toValue(), Ones, SM_SOURCE_FILE_POS, msg);
  }
}
TEST(KinematicChainTestSuites, testTheoreticallyTwoFrames) {
  {
    CoordinateFrame B(Identity, Ones, Ones, Ones, Ones, Ones);
    CoordinateFrame A(B, Identity, Ones, Ones, Ones, Ones, Ones);
    std::string msg = "Testing all one.";

    sm::eigen::assertEqual(A.getPG().toValue(), Ones * 2, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getVG().toValue(), Ones * 2, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getAG().toValue(), Ones * 2, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getR_G_L().toRotationMatrix(), Identity, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getOmegaG().toValue(), Ones * 2, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getAlphaG().toValue(), Ones * 2, SM_SOURCE_FILE_POS, msg);
  }

  {
    CoordinateFrame B(MinusIdentity, Ones, Ones, Ones, Ones, Ones);
    CoordinateFrame A(B, MinusIdentity, Ones, Ones, Ones, Ones, Ones);
    std::string msg = "Testing all one or -Identity.";

    sm::eigen::assertEqual(A.getPG().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getVG().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getAG().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getR_G_L().toRotationMatrix(), Identity, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getOmegaG().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getAlphaG().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
  }

  {
    CoordinateFrame B(Identity, Ones, X, Ones, Ones, Ones);
    CoordinateFrame A(B, Identity, Y, Zero, Zero, Zero, Zero);
    std::string msg = "Testing simple non trivial example";

    sm::eigen::assertEqual(A.getPG().toValue(), Ones + Y, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getVG().toValue(), Ones + Z, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getAG().toValue(), Ones + Ones.cross(Y) + X.cross(X.cross(Y)), SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getR_G_L().toRotationMatrix(), Identity, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getOmegaG().toValue(), X, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(A.getAlphaG().toValue(), Ones, SM_SOURCE_FILE_POS, msg);
  }

  {
    EuclideanPoint a_m_mrV(Eigen::Vector3d::Random());
    EuclideanPoint t_m_riV(Eigen::Vector3d::Random());
    EuclideanPoint w_m_mrV(Eigen::Vector3d::Random());
    EuclideanPoint dw_m_mrV(Eigen::Vector3d::Random());
    EuclideanExpression a_m_mr(&a_m_mrV);
    EuclideanExpression t_m_ri(&t_m_riV);
    EuclideanExpression w_m_mr(&w_m_mrV);
    EuclideanExpression dw_m_mr(&dw_m_mrV);

    CoordinateFrame M(Identity, X, w_m_mr, Y, dw_m_mr, a_m_mr);
    CoordinateFrame I(M, MinusIdentity, t_m_ri);
    std::string msg = "Testing robot - imu example";

//    aG = pp->getR_G_L() * a + pp->getOmegaG().cross(pp->getR_G_L() * v) + pp->getAlphaG().cross(pp->getR_G_L() * p) + pp->getOmegaG().cross(pp->getOmegaG().cross(pp->getR_G_L() * p));

    EuclideanExpression a_m_mi = a_m_mr + dw_m_mr.cross(t_m_ri) + w_m_mr.cross(w_m_mr.cross(t_m_ri));
    sm::eigen::assertEqual(I.getPP().toValue(), t_m_ri.toValue(), SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(I.getAP().toValue(), Zero, SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(I.getOmegaG().toValue(), w_m_mr.toValue(), SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(M.getAG().toValue(), a_m_mr.toValue(), SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(M.getAlphaG().toValue(), dw_m_mr.toValue(), SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(I.getAlphaG().toValue(), dw_m_mr.toValue(), SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(I.getAG().toValue(), a_m_mi.toValue(), SM_SOURCE_FILE_POS, msg);

    testExpression(I.getAG(), 4);
    testExpression(a_m_mi, 4);

    Eigen::MatrixXd m1 = evaluateJacobian(I.getAG(), 4, true);
    Eigen::MatrixXd m2 = evaluateJacobian(a_m_mi, 4, false);
    sm::eigen::assertEqual(m1, m2, SM_SOURCE_FILE_POS, msg);
  }

  //TODO add further test including random
  // CoordinateFrame (R_L_P, p, omega, v, alpha, a)
//  CoordinateFrame
//    C, B(C), A(B);

}

TEST(KinematicChainTestSuites, testTheoreticallyThreeFrames) {
  {
    CoordinateFrame A(Identity, Zero, Z, Zero, Zero, Zero);
    CoordinateFrame B(A, Identity, X, (-Z).eval(), Zero, Zero, Zero);
    CoordinateFrame C(B, Identity, (-Y).eval(), Zero, Zero, Zero, Zero);
    std::string msg = "Testing global acceleration with two complementary rotations";

    sm::eigen::assertEqual(B.getAG().toValue(),(-X).eval(), SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(C.getAG().toValue(), (-X).eval(), SM_SOURCE_FILE_POS, msg);
  }

  {
    CoordinateFrame A(Identity, Zero, Z, Zero, Zero, Zero);
    CoordinateFrame B(A, Identity, X, Zero, Zero, Zero, Zero);
    CoordinateFrame C(B, Identity, X, Zero, Zero, Zero, Zero);
    std::string msg = "Testing global acceleration with fixed second";

    sm::eigen::assertEqual(B.getAG().toValue(),(-X).eval(), SM_SOURCE_FILE_POS, msg);
    sm::eigen::assertEqual(C.getAG().toValue(), (X * -2).eval(), SM_SOURCE_FILE_POS, msg);
  }
}


#ifndef DLIB_MODIFIED_NEWTON_H
#define DLIB_MODIFIED_NEWTON_H

#include <dlib/optimization.h>

// modified Newton search strategy using generalized inverse of Hessian

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

namespace dlib
{
    template <typename inv_hessian_funct>
    class modified_newton_search_strategy_obj
    {
    public:
        explicit modified_newton_search_strategy_obj(
            const inv_hessian_funct& inv_hess
        ) : inv_hessian(inv_hess) {}

        double get_wolfe_rho (
        ) const { return 0.01; }

        double get_wolfe_sigma (
        ) const { return 0.9; }

        unsigned long get_max_line_search_iterations (
        ) const { return 100; }

        template <typename T>
        const matrix<double,0,1> get_next_direction (
            const T& x,
            const double ,
            const T& funct_derivative
        )
        {
            return -inv_hessian(x)*funct_derivative;
        }

    private:
        inv_hessian_funct inv_hessian;
    };

    template <typename inv_hessian_funct>
    modified_newton_search_strategy_obj<inv_hessian_funct> modified_newton_search_strategy (
        inv_hessian_funct inv_hessian
    ) { return modified_newton_search_strategy_obj<inv_hessian_funct>(inv_hessian); }
} // namespace dlib

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

#endif // DLIB_MODIFIED_NEWTON_H