#include <cstdio>
#include <cmath>
#include <cfloat>
#include <cstdarg>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <map>
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>
#include <boost/filesystem.hpp>
#include <omp.h>
#include <H5Cpp.h>

#include "icp/icpPointToPoint.h"
#include "timer/timer.h"

/** \brief Read all files in a directory matching the given extension.
 * \param[in] directory path to directory
 * \param[out] files read file paths
 * \param[in] extension extension to filter for
 */
void read_directory(const boost::filesystem::path directory, std::map<int, boost::filesystem::path>& files, const std::string extension = ".off") {
  files.clear();
  boost::filesystem::directory_iterator end;

  for (boost::filesystem::directory_iterator it(directory); it != end; ++it) {
    if (it->path().extension().string() == extension) {
      if (!boost::filesystem::is_empty(it->path()) && !it->path().empty() && it->path().filename().string() != "") {
        int number = std::stoi(it->path().filename().string());
        files.insert(std::pair<int, boost::filesystem::path>(number, it->path()));
      }
    }
  }
}

/** \brief Just encapsulating vertices and faces. */
class Mesh {
public:
  /** \brief Empty constructor. */
  Mesh() {

  }

  /** \brief Add a vertex.
   * \param[in] vertex vertex to add
   */
  void add_vertex(Eigen::Vector3f& vertex) {
    this->vertices.push_back(vertex);
  }

  /** \brief Get the number of vertices.
   * \return number of vertices
   */
  int num_vertices() const {
    return static_cast<int>(this->vertices.size());
  }

  Eigen::Vector3f vertex(int v) {
    assert(v >= 0 && v < this->vertices.size());
    return this->vertices[v];
  }

  /** \brief Add a face.
   * \param[in] face face to add
   */
  void add_face(Eigen::Vector3i& face) {
    this->faces.push_back(face);
  }

  /** \brief Get the number of faces.
   * \return number of faces
   */
  int num_faces() const {
    return static_cast<int>(this->faces.size());
  }

  Eigen::Vector3i face(int f) {
    assert(f >= 0 && f < this->num_faces());
    return this->faces[f];
  }

  /** \brief Rotate the point cloud around the origin.
   * \param[in] rotation rotation matrix
   */
  void rotate(const Eigen::Matrix3f &rotation) {
    for (int v = 0; v < this->num_vertices(); ++v) {
      this->vertices[v] = rotation*this->vertices[v];
    }
  }

  /** \brief Translate the mesh.
   * \param[in] translation translation vector
   */
  void translate(const Eigen::Vector3f& translation) {
    for (int v = 0; v < this->num_vertices(); ++v) {
      for (int i = 0; i < 3; ++i) {
        this->vertices[v](i) += translation(i);
      }
    }
  }

  /** \brief Scale the mesh.
   * \param[in] scale scale vector
   */
  void scale(const Eigen::Vector3f& scale) {
    for (int v = 0; v < this->num_vertices(); ++v) {
      for (int i = 0; i < 3; ++i) {
        this->vertices[v](i) *= scale(i);
      }
    }
  }

  /** \brief Sample points from the mesh
   * \param[in] mesh mesh to sample from
   * \param[in] n batch index in points
   * \param[in] points pre-initialized tensor holding points
   */
  bool sample(const int N, std::vector<Eigen::Vector3f> &points) {

    // Stores the areas of faces.
    std::vector<float> areas(this->num_faces());
    float sum = 0;

    // Build a probability distribution over faces.
    for (int f = 0; f < this->num_faces(); f++) {
      Eigen::Vector3f a = this->vertices[this->faces[f][0]];
      Eigen::Vector3f b = this->vertices[this->faces[f][1]];
      Eigen::Vector3f c = this->vertices[this->faces[f][2]];

      // Angle between a->b and a->c.
      Eigen::Vector3f ab = b - a;
      Eigen::Vector3f ac = c - a;
      float cos_angle = ab.dot(ac)/(ab.norm()*ac.norm());
      float angle = std::acos(cos_angle);

      // Compute triangle area.
      float area = std::max(0., 0.5*ab.norm()*ac.norm()*std::sin(angle));
      //std::cout << area << " " << std::pow(area, 1./4.) << " " << angle << " " << ab.norm() << " " << ac.norm() << " " << std::sin(angle) << std::endl;

      // Accumulate.
      //area = std::sqrt(area);
      areas[f] = area;
      sum += area;
      //areas.push_back(1);
      //sum += 1;
    }

    //std::cout << sum << std::endl;
    if (sum < 1e-6) {
      std::cout << "[Error] face area sum of " << sum << std::endl;
      return false;
    }

    for (int f = 0; f < this->num_faces(); f++) {
      //std::cout << areas[f] << " ";
      areas[f] /= sum;
      //std::cout << areas[f] << std::endl;
    }

    std::vector<float> cum_areas(areas.size());
    cum_areas[0] = areas[0];

    for (int f = 1; f < this->num_faces(); f++) {
      cum_areas[f] = areas[f] + cum_areas[f - 1];
    }

    for (int f = 0; f < this->num_faces(); f++) {
      int n = std::max(static_cast<int>(areas[f]*N), 1);

      for (int i = 0; i < n; i++) {
        float r1 = 0;
        float r2 = 0;
        do {
          r1 = static_cast<float>(std::rand())/static_cast<float>(RAND_MAX);
          r2 = static_cast<float>(std::rand())/static_cast<float>(RAND_MAX);
        }
        while (r1 + r2 > 1.f);

        int s = std::rand()%3;
        //std::cout << face << " " << areas[face] << std::endl;

        Eigen::Vector3f a = this->vertices[this->faces[f](s)];
        Eigen::Vector3f b = this->vertices[this->faces[f]((s + 1)%3)];
        Eigen::Vector3f c = this->vertices[this->faces[f]((s + 2)%3)];

        Eigen::Vector3f ab = b - a;
        Eigen::Vector3f ac = c - a;

        Eigen::Vector3f point = a + r1*ab + r2*ac;
        points.push_back(point);
      }
    }

    return true;
  }

  /** \brief Sample points from the mesh
   * \param[in] mesh mesh to sample from
   * \param[in] n batch index in points
   * \param[out] points sampled points
   */
  bool sample2(const int N, std::vector<Eigen::Vector3f> &points) {

    std::vector<float> areas(this->num_faces());

    #pragma omp parallel
    {
      #pragma omp for
      for (int f = 0; f < this->num_faces(); f++) {
        Eigen::Vector3f a = this->vertices[this->faces[f][0]];
        Eigen::Vector3f b = this->vertices[this->faces[f][1]];
        Eigen::Vector3f c = this->vertices[this->faces[f][2]];

        // Angle between a->b and a->c.
        Eigen::Vector3f ab = b - a;
        Eigen::Vector3f ac = c - a;
        float cos_angle = ab.dot(ac)/(ab.norm()*ac.norm());
        float angle = std::acos(cos_angle);

        // Compute triangle area.
        float area = std::max(0., 0.5*ab.norm()*ac.norm()*std::sin(angle));

        // Accumulate.
        areas[f] = area;
      }
    }

    float sum = 0;
    for (int f = 0; f < this->num_faces(); f++) {
      sum += areas[f];
    }

    if (sum < 1e-6) {
      std::cout << "[Error] face area sum of " << sum << std::endl;
      return false;
    }

    for (int f = 0; f < this->num_faces(); f++) {
      areas[f] /= sum;
    }

    std::vector<int> cum_sum(this->num_faces() + 1);
    cum_sum[0] = 0;

    for (int f = 1; f < this->num_faces() + 1; f++) {
      cum_sum[f] = std::max(static_cast<int>(std::ceil(areas[f - 1]*N)), 1) + cum_sum[f - 1];
    }

    //std::cout << cum_sum[this->num_faces()] << " " << sum << std::endl;
    points.resize(cum_sum[this->num_faces()]);

    #pragma omp parallel
    {
      for (int f = 0; f < this->num_faces(); f++) {
        int n = cum_sum[f + 1] - cum_sum[f];

        #pragma omp for
        for (int i = 0; i < n; i++) {
          float r1 = 0;
          float r2 = 0;
          do {
            r1 = static_cast<float>(std::rand())/static_cast<float>(RAND_MAX);
            r2 = static_cast<float>(std::rand())/static_cast<float>(RAND_MAX);
          }
          while (r1 + r2 > 1.f);

          int s = std::rand()%3;

          Eigen::Vector3f a = this->vertices[this->faces[f](s)];
          Eigen::Vector3f b = this->vertices[this->faces[f]((s + 1)%3)];
          Eigen::Vector3f c = this->vertices[this->faces[f]((s + 2)%3)];

          Eigen::Vector3f ab = b - a;
          Eigen::Vector3f ac = c - a;

          Eigen::Vector3f point = a + r1*ab + r2*ac;
          //std::cout << points.size() << " " << cum_sum[f] + i << std::endl;
          //assert(cum_sum[f] + i < points.size());
          points[cum_sum[f] + i] = point;
        }
      }
    }

    return true;
  }

  /** \brief Reading an off file and returning the vertices x, y, z coordinates and the
   * face indices.
   * \param[in] filepath path to the OFF file
   * \param[out] mesh read mesh with vertices and faces
   * \return success
   */
  static bool from_off(const std::string filepath, Mesh& mesh) {

    std::ifstream* file = new std::ifstream(filepath.c_str());
    std::string line;
    std::stringstream ss;
    int line_nb = 0;

    std::getline(*file, line);
    ++line_nb;

    if (line != "off" && line != "OFF") {
      std::cout << "[Error] Invalid header: \"" << line << "\", " << filepath << std::endl;
      return false;
    }

    size_t n_edges;
    std::getline(*file, line);
    ++line_nb;

    int n_vertices;
    int n_faces;
    ss << line;
    ss >> n_vertices;
    ss >> n_faces;
    ss >> n_edges;

    for (size_t v = 0; v < n_vertices; ++v) {
      std::getline(*file, line);
      ++line_nb;

      ss.clear();
      ss.str("");

      Eigen::Vector3f vertex;
      ss << line;
      ss >> vertex(0);
      ss >> vertex(1);
      ss >> vertex(2);

      mesh.add_vertex(vertex);
    }

    size_t n;
    for (size_t f = 0; f < n_faces; ++f) {
      std::getline(*file, line);
      ++line_nb;

      ss.clear();
      ss.str("");

      size_t n;
      ss << line;
      ss >> n;

      if(n != 3) {
        std::cout << "[Error] Not a triangle (" << n << " points) at " << (line_nb - 1) << std::endl;
        return false;
      }

      Eigen::Vector3i face;
      ss >> face(0);
      ss >> face(1);
      ss >> face(2);

      mesh.add_face(face);
    }

    if (n_vertices != mesh.num_vertices()) {
      std::cout << "[Error] Number of vertices in header differs from actual number of vertices." << std::endl;
      return false;
    }

    if (n_faces != mesh.num_faces()) {
      std::cout << "[Error] Number of faces in header differs from actual number of faces." << std::endl;
      return false;
    }

    file->close();
    delete file;

    return true;
  }

  /** \brief Write mesh to OFF file.
   * \param[in] filepath path to OFF file to write
   * \return success
   */
  bool to_off(const std::string filepath) {
    std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
    if (!static_cast<bool>(*out)) {
      return false;
    }

    (*out) << "OFF" << std::endl;
    (*out) << this->vertices.size() << " " << this->num_faces() << " 0" << std::endl;

    for (unsigned int v = 0; v < this->vertices.size(); v++) {
      (*out) << this->vertices[v](0) << " " << this->vertices[v](1) << " " << this->vertices[v](2) << std::endl;
    }

    for (unsigned int f = 0; f < this->num_faces(); f++) {
      (*out) << "3 " << this->faces[f](0) << " " << this->faces[f](1) << " " << this->faces[f](2) << std::endl;
    }

    out->close();
    delete out;

    return true;
  }

  /** \brief Write mesh to obj file.
   * \param[in] filepath
   * \param[in] mtl_lib
   * \param[in] materials
   * \return success
   */
  bool to_obj(const std::string filepath) {

    std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
    if (!static_cast<bool>(out)) {
      return false;
    }

    for (unsigned int v = 0; v < this->vertices.size(); v++) {
      (*out) << "v " << this->vertices[v](0) << " " << this->vertices[v](1) << " " << this->vertices[v](2) << std::endl;
    }

    for (unsigned int f = 0; f < this->num_faces(); f++) {
      (*out) << "f " << this->faces[f](0) + 1 << " " << this->faces[f](1) + 1 << " " << this->faces[f](2) + 1 << std::endl;
    }

    out->close();
    delete out;

    return true;
  }

private:

  /** \brief Vertices as (x,y,z)-vectors. */
  std::vector<Eigen::Vector3f> vertices;

  /** \brief Faces as list of vertex indices. */
  std::vector<Eigen::Vector3i> faces;
};

/** \brief Class representing a point cloud in 3D. */
class PointCloud {
public:
  /** \brief Constructor. */
  PointCloud() {

  }

  /** \brief Copy constructor.
   * \param[in] point_cloud point cloud to copy
   */
  PointCloud(const PointCloud &point_cloud) {
    this->points.clear();
    this->colors.clear();

    for (unsigned int i = 0; i < point_cloud.points.size(); i++) {
      this->points.push_back(point_cloud.points[i]);
      this->colors.push_back(point_cloud.colors[i]);
    }
  }

  /** \brief Constructor
   * \param[in] points
   */
  PointCloud(const std::vector<Eigen::Vector3f> &points) {
    this->points.clear();
    this->colors.clear();

    for (unsigned int i = 0; i < points.size(); i++) {
      this->add_point(points[i]);
    }
  }

  /** \brief Constructor
   * \param[in] points
   */
  PointCloud(const int k, const Eigen::Tensor<float, 3, Eigen::RowMajor> &points) {
    this->points.clear();
    this->colors.clear();

    for (unsigned int i = 0; i < points.dimension(1); i++) {
      if (points(k, i, 0) != 0 && points(k, i, 1) != 0 && points(k, i, 0) != 2) {
        this->add_point(Eigen::Vector3f(points(k, i, 0), points(k, i, 1), points(k, i, 2)));
      }
    }
  }

  /** \brief Destructor. */
  ~PointCloud() {

  }

  /** \brief Read point cloud from txt file.
   * \param[in] filepath path to txt file
   * \param[out] point_cloud read point cloud
   */
  static bool from_txt(const std::string &filepath, PointCloud &point_cloud) {
    std::ifstream file(filepath.c_str());
    std::string line;
    std::stringstream ss;

    std::getline(file, line);
    ss << line;

    int n_points;
    ss >> n_points;

    if (n_points <= 0) {
      return false;
    }

    for (int i = 0; i < n_points; i++) {
      std::getline(file, line);

      ss.clear();
      ss.str("");
      ss << line;

      Eigen::Vector3f point;

      ss >> point(0);
      ss >> point(1);
      ss >> point(2);

      point_cloud.add_point(point);
    }

    return true;
  }

  /** \brief Read point cloud from binary file.
   * \param[in] filepath path to binary file
   * \param[out] point_cloud read point cloud
   */
  static bool from_bin(const std::string &filepath, PointCloud &point_cloud) {

    // allocate 4 MB buffer (only ~130*4*4 KB are needed)
    int32_t num = 1000000;
    float *data = (float*)malloc(num*sizeof(float));

    // pointers
    float *px = data+0;
    float *py = data+1;
    float *pz = data+2;
    float *pr = data+3;

    // load point cloud
    FILE *stream;
    stream = fopen (filepath.c_str(), "rb");
    num = fread(data,sizeof(float),num,stream)/4;
    for (int32_t i=0; i<num; i++) {
      point_cloud.add_point(Eigen::Vector3f(-*py,*pz,*px));
      //std::cout << *px << " " << *px << " " << *pz << std::endl;
      px+=4; py+=4; pz+=4; pr+=4;
    }

    fclose(stream);

    //exit(1);
    return true;
  }

  /** \brief Given the angle in radians, construct a rotation matrix around the x-axis.
   * \param[in] radians angle in radians
   * \param[out] rotation rotation matrix
   */
  static void rotation_matrix_x(const float radians, Eigen::Matrix3f &rotation) {
    rotation = Eigen::Matrix3f::Zero();

    rotation(0, 0) = 1;
    rotation(1, 1) = std::cos(radians); rotation(1, 2) = -std::sin(radians);
    rotation(2, 1) = std::sin(radians); rotation(2, 2) = std::cos(radians);
  }

  /** \brief Given the angle in radians, construct a rotation matrix around the y-axis.
   * \param[in] radians angle in radians
   * \param[out] rotation rotation matrix
   */
  static void rotation_matrix_y(const float radians, Eigen::Matrix3f &rotation) {
    rotation = Eigen::Matrix3f::Zero();

    rotation(0, 0) = std::cos(radians); rotation(0, 2) = std::sin(radians);
    rotation(1, 1) = 1;
    rotation(2, 0) = -std::sin(radians); rotation(2, 2) = std::cos(radians);
  }

  /** \brief Given the angle in radians, construct a rotation matrix around the z-axis.
   * \param[in] radians angle in radians
   * \param[out] rotation rotation matrix
   */
  static void rotation_matrix_z(const float radians, Eigen::Matrix3f &rotation) {
    rotation = Eigen::Matrix3f::Zero();

    rotation(0, 0) = std::cos(radians); rotation(0, 1) = -std::sin(radians);
    rotation(1, 0) = std::sin(radians); rotation(1, 1) = std::cos(radians);
    rotation(2, 2) = 1;
  }

  /** \brief Computes the rotation matrix corresponding to the given ray.
   * \param[in] ray ray defining the direction to rotate to
   * \param[out] rotation final rotation
   */
  static void rotation_matrix(const Eigen::Vector3f ray, Eigen::Matrix3f &rotation) {
    Eigen::Matrix3f rotation_x;
    Eigen::Matrix3f rotation_y;
    Eigen::Matrix3f rotation_z;

    Eigen::Vector3f axis_x = Eigen::Vector3f(1, 0, 0);
    Eigen::Vector3f axis_y = Eigen::Vector3f(0, 1, 0);
    Eigen::Vector3f axis_z = Eigen::Vector3f(0, 0, 1);

    Eigen::Vector3f ray_x = ray; ray_x(0) = 0; ray_x /= ray_x.norm();
    Eigen::Vector3f ray_y = ray; ray_y(1) = 0; ray_y /= ray_y.norm();
    Eigen::Vector3f ray_z = ray; ray_z(2) = 0; ray_y /= ray_z.norm();

    float radians_x = std::acos(axis_x.dot(ray_x));
    PointCloud::rotation_matrix_x(radians_x, rotation_x);

    float radians_y = std::acos(axis_y.dot(ray_y));
    PointCloud::rotation_matrix_y(radians_y, rotation_y);

    float radians_z = std::acos(axis_z.dot(ray_z));
    PointCloud::rotation_matrix_z(radians_z, rotation_z);
    std::cout << "[Data] radians " << radians_x << " " << radians_y << " " << radians_z << std::endl;

    rotation = Eigen::Matrix3f::Zero();
    rotation = rotation_z*rotation_y*rotation_x;
  }

  /** \brief Add a point to the point cloud.
   * \param[in] point point to add
   */
  void add_point(const Eigen::Vector3f &point) {
    this->points.push_back(point);
    this->colors.push_back(Eigen::Vector3i::Zero());
  }

  /** \brief Add a colored point to the point cloud.
   * \param[in] point point to add
   * \param[in] color color of the point
   */
  void add_point(const Eigen::Vector3f &point, const Eigen::Vector3i &color) {
    this->points.push_back(point);
    this->colors.push_back(color);
  }

  /** \brief Add points from a point cloud.
   * \param[in] point_cloud point cloud whose points to add
   */
  void add_points(const PointCloud &point_cloud) {
    for (unsigned int i = 0; i < point_cloud.num_points(); i++) {
      this->add_point(point_cloud.points[i], point_cloud.colors[i]);
    }
  }

  /** \brief Merge/add points from another point cloud.
   * \param[in] point_cloud point_cloud to take points from
   */
  void merge(const PointCloud &point_cloud) {
    for (unsigned int i = 0; i < point_cloud.points.size(); i++) {
      this->add_point(point_cloud.points[i], point_cloud.colors[i]);
    }
  }

  /** \brief Get number of points.
   * \return number of points
   */
  unsigned int num_points() const {
    return this->points.size();
  }

  /** \brief Write point cloud to txt file.
   * \param[in] filepath path to file
   * \return success
   */
  bool to_txt(const std::string &filepath) {
    std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
    if (!static_cast<bool>(*out)) {
      return false;
    }

    (*out) << this->points.size() << std::endl;
    for (unsigned int i = 0; i < this->points.size(); i++) {
     (*out) << this->points[i](0) << " " << this->points[i](1) << " " << this->points[i](2) << std::endl;
    }

    out->close();
    delete out;

    return true;
  }

  /** \brief Write point cloud to txt file.
   * \param[in] filepath path to file
   * \return success
   */
  bool to_ply(const std::string &filepath) {
    std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
    if (!static_cast<bool>(*out)) {
      return false;
    }

    if (this->points.size() != this->colors.size()) {
      return false;
    }

    (*out) << "ply" << std::endl;
    (*out) << "format ascii 1.0" << std::endl;
    (*out) << "element vertex " << this->points.size() << std::endl;
    (*out) << "property float32 x" << std::endl;
    (*out) << "property float32 y" << std::endl;
    (*out) << "property float32 z" << std::endl;
    (*out) << "property uchar red" << std::endl;
    (*out) << "property uchar green" << std::endl;
    (*out) << "property uchar blue" << std::endl;
    (*out) << "end_header" << std::endl;

    for (unsigned int i = 0; i < this->points.size(); i++) {
      //(*out) << this->points[i](0) << " " << this->points[i](1) << " " << this->points[i](2) << std::endl;
      (*out) << this->points[i](0) << " " << this->points[i](1) << " " << this->points[i](2) << " "
        << this->colors[i](0) << " " << this->colors[i](1) << " " << this->colors[i](2) << std::endl;
    }

    out->close();
    delete out;

    return true;
  }

  /** \brief Get the extents of the point cloud in all three axes.
   * \param[out] min minimum coordinate values per axis
   * \param[out] max maximum coordinate values per axis
   */
  void extents(Eigen::Vector3f &min, Eigen::Vector3f &max) {
    for (int d = 0; d < 3; d++) {
      min(d) = FLT_MAX;
      max(d) = FLT_MIN;
    }

    for (unsigned int i = 0; i < this->points.size(); i++) {
      for (int d = 0; d < 3; d++) {
        if (this->points[i](d) < min(d)) {
          min(d) = this->points[i](d);
        }
        if (this->points[i](d) > max(d)) {
          max(d) = this->points[i](d);
        }
      }
    }
  }

  /** \brief Scale the point cloud.
   * \param[in] scale scales for each axis
   */
  void scale(const Eigen::Vector3f &scale) {
    for (unsigned int i = 0; i < this->points.size(); i++) {
      for (int d = 0; d < 3; d++) {
        this->points[i](d) *= scale(d);
      }
    }
  }

  /** \brief Scale the point cloud.
   * \param[in] scale overlal scale for all axes
   */
  void scale(float scale) {
    this->scale(Eigen::Vector3f(scale, scale, scale));
  }

  /** \brief Rotate the point cloud around the origin.
   * \param[in] rotation rotation matrix
   */
  void rotate(const Eigen::Matrix3f &rotation) {
    for (unsigned int i = 0; i < this->points.size(); i++) {
      this->points[i] = rotation*this->points[i];
    }
  }

  /** \brief Translate the point cloud.
   * \brief translation translation vector
   */
  void translate(const Eigen::Vector3f &translation) {
    for (unsigned int i = 0; i < this->points.size(); i++) {
      this->points[i] += translation;
    }
  }

  /** \brief Get a point.
   * \param[in] n
   * \return point n
   */
  Eigen::Vector3f get(int n) const {
    assert(n < this->num_points() && n >= 0);
    return this->points[n];
  }

private:
  /** \brief The points of the point cloud. */
  std::vector<Eigen::Vector3f> points;
  /** \brief Colors of the points. */
  std::vector<Eigen::Vector3i> colors;

};

/** \brief Read a Hdf5 file into an Eigen tensor.
 * \param[in] filepath path to file
 * \param[out] dense Eigen tensor
 * \return success
 */
template<int RANK>
bool read_hdf5(const std::string filepath, Eigen::Tensor<float, RANK, Eigen::RowMajor>& dense) {

  try {
    H5::H5File file(filepath, H5F_ACC_RDONLY);
    H5::DataSet dataset = file.openDataSet("tensor");

    /*
     * Get filespace for rank and dimension
     */
    H5::DataSpace filespace = dataset.getSpace();

    /*
     * Get number of dimensions in the file dataspace
     */
    size_t rank = filespace.getSimpleExtentNdims();

    if (rank != RANK) {
      std::cout << "[Error] invalid rank read: " << rank << std::endl;
      exit(1);
    }

    /*
     * Get and print the dimension sizes of the file dataspace
     */
    hsize_t dimsf[rank];
    filespace.getSimpleExtentDims(dimsf);

    std::cout << "[Data] HDF5 size: ";
    for (int i = 0; i < RANK; ++i) {
      std::cout << dimsf[i] << " ";
    }
    std::cout << std::endl;

    /*
     * Define the memory space to read dataset.
     */
    std::cout << "[Data] HDF5 allocating ..." << std::endl;
    H5::DataSpace mspace(rank, dimsf);

    //size_t buffer_size = 1;
    //for (int i = 0; i < RANK; ++i) {
    //  buffer_size *= dimsf[i];
    //}

    std::cout << "[Data] HDF5 casting ..." << std::endl;
    float* buffer = static_cast<float*>(dense.data());
    std::cout << "[Data] HDF5 reading ..." << std::endl;
    dataset.read(buffer, H5::PredType::NATIVE_FLOAT, mspace, filespace);

    //for (int i = 0; i < buffer_size; ++i) {
    //  std::cout << buffer[i] << std::endl;
    //}
  }

  // catch failure caused by the H5File operations
  catch(H5::FileIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSet operations
  catch(H5::DataSetIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSpace operations
  catch(H5::DataSpaceIException error) {
    error.printError();
    return false;
  }
}

/** \brief Perform ICP using a point-to-point distance.
 * \param[in] point_cloud_from
 * \param[in] point_cloud_to
 * \param[out] rotation
 * \param[out] translation
 */
void point_to_point_icp(const PointCloud &point_cloud_from, const PointCloud &point_cloud_to,
    Eigen::Matrix3f &rotation, Eigen::Vector3f &translation, float &residual, int &inliers) {

  double* M = new double[3*point_cloud_to.num_points()];
  for (unsigned int i = 0; i < point_cloud_to.num_points(); i++) {
    for (int d = 0; d < 3; d++) {
      M[i*3 + d] = point_cloud_to.get(i)(d);
    }
  }

  double* T = new double[3*point_cloud_from.num_points()];
  for (unsigned int i = 0; i < point_cloud_from.num_points(); i++) {
    for (int d = 0; d < 3; d++) {
      T[i*3 + d] = point_cloud_from.get(i)(d);
    }
  }

  unsigned int I = std::min(point_cloud_from.num_points(), point_cloud_to.num_points());
  translation(0) = 0; translation(1) = 0; translation(2) = 0;

  for (unsigned int i = 0; i < I; i++) {
    translation += point_cloud_to.get(i) - point_cloud_from.get(i);
  }

  translation /= I;

  Matrix R = Matrix::eye(3);
  Matrix t(3, 1);
  t.val[0][0] = 0;
  t.val[1][0] = 0;
  t.val[2][0] = 0;

  IcpPointToPoint icp(M, point_cloud_to.num_points(), 3);
  icp.setMaxIterations(100);
  icp.setMinDeltaParam(0.0001);

  residual = icp.fit(T, point_cloud_from.num_points(), R, t);
  inliers = icp.getInlierCount();

  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      rotation(i, j) = R.val[i][j];
    }

    translation(i) = t.val[i][0];
  }

  delete[] M;
  delete[] T;
}

int main(int argc, char** argv) {
  if (argc < 5) {
    std::cout << "[Error] Usage: icp txt_file off_directory h5_file out_off out_log" << std::endl;
    exit(1);
  }

  boost::filesystem::path txt_file(argv[1]);
  boost::filesystem::path off_directory(argv[2]);
  boost::filesystem::path h5_file(argv[3]);
  boost::filesystem::path out_off_file(argv[4]);

  boost::filesystem::path out_log_file;
  if (argc > 5) {
    out_log_file = boost::filesystem::path(argv[5]);
  }

  if (!boost::filesystem::is_regular_file(txt_file)) {
    std::cout << "[Error] file " << txt_file.string() << " not found" << std::endl;
    exit(1);
  }

  if (!boost::filesystem::is_directory(off_directory)) {
    std::cout << "[Error] directory " << off_directory.string() << " not found" << std::endl;
    exit(1);
  }

  if (!boost::filesystem::is_regular_file(h5_file)) {
    std::cout << "[Error] file " << h5_file.string() << " not found" << std::endl;
    exit(1);
  }

  std::map<int, boost::filesystem::path> off_files;
  read_directory(off_directory, off_files);
  std::cout << "[ICP] found " << off_files.size() << " files" << std::endl;

  std::vector<int> indices;
  for (std::map<int, boost::filesystem::path>::iterator it = off_files.begin(); it != off_files.end(); it++) {
    indices.push_back(it->first);
  }

  int N_points = 1000000;
  Eigen::Tensor<float, 3, Eigen::RowMajor> points(indices.size(), N_points, 3);
  points.setZero();
  read_hdf5(h5_file.string(), points);

  PointCloud point_cloud;
  PointCloud::from_txt(txt_file.string(), point_cloud);
  std::cout << "[ICP] read " << point_cloud.num_points() << " points" << std::endl;

  std::string log = "n n_points residual inliers\n";
  float min_residual = 1e32;
  float min_index = 0;

  Timer timer;
  float total = 0;

  std::vector<Eigen::Matrix3f> rotations;
  std::vector<Eigen::Vector3f> translations;
  for (unsigned int i = 0; i < indices.size(); i++) {
    rotations.push_back(Eigen::Matrix3f::Identity());
    translations.push_back(Eigen::Vector3f::Zero());
  }

  omp_set_num_threads(16);
  #pragma omp parallel
  {
    #pragma omp for
    for (unsigned int i = 0; i < indices.size(); i++) {
      int n = indices[i];

      PointCloud mesh_point_cloud(i, points);
      //mesh_point_cloud.to_ply(std::to_string(i) + ".ply");
      float residual = 1e32;
      int inliers = 0;

      timer.start();
      point_to_point_icp(point_cloud, mesh_point_cloud, rotations[i], translations[i], residual, inliers);
      timer.stop();

      float elapsed = timer.getElapsedTimeInMilliSec();
      std::cout << "[ICP] residual " << residual << " (inliers " << inliers << ", " << elapsed << "ms)" << std::endl;
      total += elapsed;

      #pragma omp critical
      {
        log += std::to_string(n) + " " + std::to_string(point_cloud.num_points()) + " " + std::to_string(residual) + " " + std::to_string(inliers) + "\n";

        if (residual < min_residual) {
          min_residual = residual;
          min_index = i;
        }
      }
    }
  }

  Mesh mesh;
  std::string off_file = off_files[indices[min_index]].string();
  Mesh::from_off(off_file, mesh);
  mesh.rotate(rotations[min_index].transpose());
  mesh.translate(-translations[min_index]);
  mesh.to_off(out_off_file.string());

  if (!out_log_file.empty()) {
    std::ofstream out(out_log_file.string());
    out << log;
    out.close();
  }
  else {
    std::cout << log;
  }

  std::cout << "[ICP] wrote " << off_file << " to " << out_off_file.string() << std::endl;
  std::cout << "[ICP] needed on average " << total/indices.size() << "ms" << std::endl;

  exit(0);
}


/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com)
 * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file not_adaptivetest.hpp
 * \date March 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include <type_traits>

#include <fl/util/traits.hpp>

#include <fl/util/meta.hpp>
#include <fl/model/sensor/linear_sensor.hpp>
#include <fl/model/sensor/joint_sensor.hpp>
#include <fl/model/process/linear_transition.hpp>

TEST(NotAdaptiveTests, fixed_traits)
{
    using namespace fl;

    typedef LinearGaussianObservationModel<
                Eigen::Matrix<double, 2, 1>,
                Eigen::Matrix<double, 3, 1>
            > ObsrvModel;

    typedef NotAdaptive<ObsrvModel> NAObsrvModel;

    EXPECT_TRUE((
        std::is_same<
            Traits<ObsrvModel>::Obsrv,
            Traits<NAObsrvModel>::Obsrv>::value));

    EXPECT_TRUE((
        std::is_same<
            Traits<ObsrvModel>::Noise,
            Traits<NAObsrvModel>::Noise>::value));

    EXPECT_TRUE((
        std::is_same<
            Traits<ObsrvModel>::State,
            Traits<NAObsrvModel>::State>::value));

    EXPECT_EQ(Traits<NAObsrvModel>::Param::SizeAtCompileTime, 0);
}

TEST(NotAdaptiveTests, dynamic_traits)
{
    using namespace fl;

    typedef LinearGaussianObservationModel<
                Eigen::Matrix<double, -1, 1>,
                Eigen::Matrix<double, -1, 1>
            > ObsrvModel;

    typedef NotAdaptive<ObsrvModel> NAObsrvModel;

    EXPECT_TRUE((
        std::is_same<
            Traits<ObsrvModel>::Obsrv,
            Traits<NAObsrvModel>::Obsrv>::value));

    EXPECT_TRUE((
        std::is_same<
            Traits<ObsrvModel>::Noise,
            Traits<NAObsrvModel>::Noise>::value));

    EXPECT_TRUE((
        std::is_same<
            Traits<ObsrvModel>::State,
            Traits<NAObsrvModel>::State>::value));

    EXPECT_EQ(Traits<NAObsrvModel>::Param::SizeAtCompileTime, 0);
}

TEST(NotAdaptiveTests, fixed_traits_of_adaptive_model)
{
    using namespace fl;

    typedef LinearGaussianObservationModel<
                Eigen::Matrix<double, 2, 1>,
                Eigen::Matrix<double, 3, 1>
            > LocalObsrvModel;

    typedef JointObservationModel<MultipleOf<LocalObsrvModel, 3>> ObsrvModel;
    typedef NotAdaptive<ObsrvModel> NAObsrvModel;

    EXPECT_TRUE((
        std::is_same<
            Traits<ObsrvModel>::Obsrv,
            Traits<NAObsrvModel>::Obsrv>::value));

    EXPECT_TRUE((
        std::is_same<
            Traits<ObsrvModel>::Noise,
            Traits<NAObsrvModel>::Noise>::value));

    EXPECT_TRUE((
        std::is_same<
            Traits<ObsrvModel>::State,
            Traits<NAObsrvModel>::State>::value));

    EXPECT_EQ(Traits<NAObsrvModel>::Param::SizeAtCompileTime, 0);
}

TEST(NotAdaptiveTests, Fixed_LinearObsrvModel)
{
    using namespace fl;

    typedef LinearGaussianObservationModel<
                Eigen::Matrix<double, 2, 1>,
                Eigen::Matrix<double, 3, 1>
            > ObsrvModel;

    typedef NotAdaptive<ObsrvModel> NAObsrvModel;

    typedef typename Traits<ObsrvModel>::Obsrv Obsrv;
    typedef typename Traits<ObsrvModel>::State State;
    typedef typename Traits<ObsrvModel>::Noise Noise;
    typedef typename Traits<ObsrvModel>::Noise Noise;

    ObsrvModel sensor;
    NAObsrvModel na_sensor = NAObsrvModel(ObsrvModel());

    State state = State::Random();
    Noise noise = Noise::Random();

    EXPECT_TRUE(
        fl::are_similar(
            sensor.predict_obsrv(state, noise, 1.0),
            na_sensor.predict_obsrv(state, noise, 1.0)));

    EXPECT_EQ(sensor.predict_obsrv(state, noise, 1.0).rows(),
              na_sensor.predict_obsrv(state, noise, 1.0).rows());
}

TEST(NotAdaptiveTests, Dynamic_LinearObsrvModel)
{
    using namespace fl;

    typedef Eigen::Matrix<double, -1, 1> Obsrv;
    typedef Eigen::Matrix<double, -1, 1> State;

    typedef LinearGaussianObservationModel<Obsrv, State> ObsrvModel;
    typedef NotAdaptive<ObsrvModel> NAObsrvModel;

    typedef typename Traits<ObsrvModel>::Noise Noise;

    ObsrvModel sensor = ObsrvModel(2, 3);
    NAObsrvModel na_sensor = NAObsrvModel(ObsrvModel(2, 3));

    State state = State::Random(3);
    Noise noise = Noise::Random(2);

    EXPECT_TRUE(
        fl::are_similar(
            sensor.predict_obsrv(state, noise, 1.0),
            na_sensor.predict_obsrv(state, noise, 1.0)));

    EXPECT_EQ(sensor.predict_obsrv(state, noise, 1.0).rows(),
              na_sensor.predict_obsrv(state, noise, 1.0).rows());
}

TEST(NotAdaptiveTests, Fixed_JointObsrvModel)
{
    using namespace fl;

    typedef LinearGaussianObservationModel<
                Eigen::Matrix<double, 2, 1>,
                Eigen::Matrix<double, 3, 1>
            > LocalObsrvModel;

    typedef JointObservationModel<MultipleOf<LocalObsrvModel, 3>> ObsrvModel;
    typedef NotAdaptive<ObsrvModel> NAObsrvModel;

    typedef typename Traits<ObsrvModel>::Obsrv Obsrv;
    typedef typename Traits<ObsrvModel>::State State;
    typedef typename Traits<ObsrvModel>::Noise Noise;
    typedef typename Traits<ObsrvModel>::Noise Noise;

    ObsrvModel sensor = ObsrvModel(LocalObsrvModel());
    NAObsrvModel na_sensor = NAObsrvModel(ObsrvModel(LocalObsrvModel()));

    State state = State::Random();
    Noise noise = Noise::Random();

    EXPECT_TRUE(
        fl::are_similar(
            sensor.predict_obsrv(state, noise, 1.0),
            na_sensor.predict_obsrv(state, noise, 1.0)));

    EXPECT_EQ(sensor.predict_obsrv(state, noise, 1.0).rows(),
              na_sensor.predict_obsrv(state, noise, 1.0).rows());
}


/**

Copyright (c) 2016, Hanselmann Fabian, Heller Florian, Heizmann Heinrich, Kübler Marcel, Mehlhaus Jonas, Meißner Pascal, Qattan Mohamad, Reckling Reno, Stroh Daniel
All rights reserved.

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

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

2. 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.

3. 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.

*/

#pragma once

#include <boost/shared_ptr.hpp>
#include "OptimizationAlgorithm.hpp"
#include "NeighbourhoodFunction.hpp"
#include "AcceptanceFunction.hpp"

namespace ISM {

using boost::filesystem::path;

template <class InstanceType>
class RecordHuntAlgorithm : public OptimizationAlgorithm<InstanceType>
{
	public:
		RecordHuntAlgorithm(NeighbourhoodFunctionPtr<InstanceType> neighbourhoodFunction,
			CostFunctionPtr<InstanceType> costFunction,
			AcceptanceFunctionPtr acceptanceFunction)
			: OptimizationAlgorithm<InstanceType>(neighbourhoodFunction, costFunction)
			, mAcceptanceFunction(acceptanceFunction)
		{}

		InstanceType optimize(InstanceType startInstance)
		{

			mAcceptanceFunction->reset();

			InstanceType selectedInstance = startInstance;
			InstanceType bestInstance = startInstance;

			double lowestCost = this->mCostFunction->calculateCost(bestInstance);
			bool keepOptimizing = true;

			while (keepOptimizing)
			{
				keepOptimizing = false;

				this->mNeighbourhoodFunction->setReferenceInstance(selectedInstance);
				double selectedInstanceCost = this->mCostFunction->calculateCost(selectedInstance);

				if (this->mNeighbourhoodFunction->hasNextNeighbour())
				{
					do
					{
						InstanceType nextInstance = this->mNeighbourhoodFunction->getNextNeighbour();
						double nextInstanceCost = this->mCostFunction->calculateCost(nextInstance);

						if (mAcceptanceFunction->isNewCostAcceptable(nextInstanceCost, selectedInstanceCost))
						{
							keepOptimizing = true;
							selectedInstance = nextInstance;
							mAcceptanceFunction->update();

							if (nextInstanceCost < lowestCost)
							{
								bestInstance = nextInstance;
								lowestCost = nextInstanceCost;
							}

							break;
						}

					} while (this->mNeighbourhoodFunction->hasNextNeighbour());
				}
				else
				{
					//The selected instance has no neighbours, we can't continue the optimization
					return bestInstance;
				}
			}

			return bestInstance;
		}

	private:
		AcceptanceFunctionPtr mAcceptanceFunction;

};

template<class InstanceType>
using RecordHuntAlgorithmPtr = boost::shared_ptr<RecordHuntAlgorithm<InstanceType>>;

}


//
// Copyright (c) 2009--2010
// Thomas Klimpel and Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_MTL_COMPRESSED2D_HPP
#define BOOST_NUMERIC_BINDINGS_MTL_COMPRESSED2D_HPP

#include <boost/numeric/bindings/detail/adaptor.hpp>
#include <boost/numeric/bindings/detail/copy_const.hpp>
#include <boost/numeric/bindings/mtl/detail/convert_to.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace detail {

template< typename T, typename Parameters, typename Id, typename Enable >
struct adaptor< mtl::compressed2D< T, Parameters >, Id, Enable > {

    typedef typename copy_const< Id, T >::type value_type;
    typedef typename copy_const<
        Id,
        //typename Id::index_type
        //typename Id::size_type // (Seems to be the actually used index_type)
        std::ptrdiff_t // (Seems to be an actual usable type for bindings purposes)
    >::type index_type;
    typedef typename convert_to<
        tag::data_order,
        typename Parameters::orientation
    >::type data_order;
    typedef mpl::map<
        mpl::pair< tag::value_type, value_type >,
        mpl::pair< tag::index_type, index_type >,
        mpl::pair< tag::entity, tag::matrix >,
        mpl::pair< tag::size_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::size_type<2>, std::ptrdiff_t >,
        mpl::pair< tag::matrix_type, tag::general >,
        mpl::pair< tag::data_structure, tag::compressed_sparse >,
        mpl::pair< tag::data_order, data_order >,
        mpl::pair< tag::index_base, mpl::int_<0> >
    > property_map;

    static std::ptrdiff_t size1( const Id& id ) {
        return id.num_rows();
    }

    static std::ptrdiff_t size2( const Id& id ) {
        return id.num_cols();
    }

    static value_type* begin_value( Id& id ) {
        return id.address_data();
    }

    static value_type* end_value( Id& id ) {
        return id.address_data() + id.nnz();
    }

    static index_type* begin_compressed_index_major( Id& id ) {
        return reinterpret_cast<index_type*>(id.address_major());
    }

    static index_type* end_compressed_index_major( Id& id ) {
        return reinterpret_cast<index_type*>(id.address_major() + id.dim1() + 1);
    }

    static index_type* begin_index_minor( Id& id ) {
        return reinterpret_cast<index_type*>(id.address_minor());
    }

    static index_type* end_index_minor( Id& id ) {
        return reinterpret_cast<index_type*>(id.address_minor() + id.nnz());
    }

};

} // namespace detail
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#pragma once

#include <boost/ut.hpp>
#include <chrono>

#include <libembeddedhal/frequency.hpp>
#include <libembeddedhal/percent.hpp>

namespace {
template<typename Rep, typename Period>
inline std::ostream& operator<<(
  std::ostream& os,
  const std::chrono::duration<Rep, Period>& p_duration)
{
  return (os << p_duration.count() << " * (" << Period::num << "/"
             << Period::den << ")s");
}
}  // namespace

namespace embed {
inline std::ostream& operator<<(std::ostream& os, const frequency& p_frequency)
{
  return (os << p_frequency.cycles_per_second() << "_Hz");
}

inline std::ostream& operator<<(std::ostream& os, const duty_cycle& p_duty)
{
  return (os << "duty_cycle { high: " << p_duty.high << ","
             << " low: " << p_duty.low << " }");
}

inline std::ostream& operator<<(std::ostream& os, const percent& p_percent)
{
  return (os << "percent { " << static_cast<float>(p_percent) << " }");
}
}  // namespace embed


///////////////////////////////////////////////////////////////////////////////
// statistics::detail::fusion::example::serialize.cpp                  		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <fstream>
#include <string>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/int.hpp>

#include <boost/typeof/typeof.hpp>
#include <boost/utility/result_of.hpp>
#include <boost/assert.hpp>
#include <boost/range.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/include/at_c.hpp>
#include <boost/fusion/include/pair.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/make_map.hpp>
#include <boost/fusion/include/make_vector.hpp>

#include <boost/statistics/detail/fusion/joint_view/binder.hpp>
#include <boost/statistics/detail/fusion/joint_view/bind_iterator.hpp>
//#include <boost/statistics/detail/fusion/serialization/save.hpp>
//#include <boost/statistics/detail/fusion/serialization/load.hpp>
//#include <boost/statistics/detail/fusion/serialization/map.hpp>
//#include <boost/statistics/detail/fusion/serialization/vector.hpp>

#include <boost/fusion/include/make_vector.hpp>

#include <libs/statistics/detail/fusion/example/serialize.h>

void example_bind_range2(std::ostream& os){

    os << "example_joint_view_binder -> ";

    using namespace boost;
    namespace stat = boost::statistics::detail;
    namespace jv = stat::fusion::joint_view;

    typedef mpl::int_<0> key0_;
    typedef mpl::int_<1> key1_;
    typedef mpl::int_<2> key2_;

    typedef unsigned d0_;
    typedef unsigned d1_;
    typedef unsigned d2_;
    typedef fusion::pair<key0_,d0_>       			p0_;
    typedef fusion::pair<key1_,d1_>                 p1_;
    typedef fusion::pair<key2_,d2_>                 p2_;
    typedef fusion::map<p0_>                      	map0_;
    typedef fusion::map<p1_>                      	map1_;
	typedef std::vector<map1_>						vec1_;
    typedef boost::fusion::vector<map0_&,vec1_>		seq_;
	typedef jv::bind_range2							range2_;

    const d0_ d0 = 0;
    const d1_ d1 = 0;
    const d2_ d2 = 0;
    map0_ map0(boost::fusion::make_pair<key0_>(d0));
    map1_ map1(boost::fusion::make_pair<key1_>(d1));
    map1_ map2(boost::fusion::make_pair<key1_>(d2));
	vec1_ vec1;
	vec1.push_back( map1 );
	vec1.push_back( map2 );

	range2_ range2;
//	meta_::call(map0,vec1);
	range2(boost::fusion::make_vector(map0,vec1));

	seq_ seq(map0,vec1);
    range2(seq);

 	os << "<-";
}


// Boost.Geometry

// Copyright (c) 2019 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_MAKE_MAKE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_MAKE_MAKE_HPP

#include <boost/geometry/geometries/infinite_line.hpp>
#include <boost/geometry/core/access.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace make
{

template <typename Type, typename Coordinate>
inline
model::infinite_line<Type> make_infinite_line(Coordinate const& x1,
    Coordinate const& y1, Coordinate const& x2, Coordinate const& y2)
{
    model::infinite_line<Type> result;
    result.a = y1 - y2;
    result.b = x2 - x1;
    result.c = -result.a * x1 - result.b * y1;
    return result;
}

template <typename Type, typename Point>
inline
model::infinite_line<Type> make_infinite_line(Point const& a, Point const& b)
{
    return make_infinite_line<Type>(geometry::get<0>(a), geometry::get<1>(a),
        geometry::get<0>(b), geometry::get<1>(b));
}

template <typename Type, typename Segment>
inline
model::infinite_line<Type> make_infinite_line(Segment const& segment)
{
    return make_infinite_line<Type>(geometry::get<0, 0>(segment),
        geometry::get<0, 1>(segment),
        geometry::get<1, 0>(segment),
        geometry::get<1, 1>(segment));
}

template <typename Type, typename Point>
inline
model::infinite_line<Type> make_perpendicular_line(Point const& a, Point const& b, Point const& c)
{
    // https://www.math-only-math.com/equation-of-a-line-perpendicular-to-a-line.html
    model::infinite_line<Type> const line = make_infinite_line<Type>(a, b);
    model::infinite_line<Type> result;
    result.a = line.b;
    result.b = -line.a;
    // Lines with any result.c are perpendicular to a->b
    // Calculate this result such that it goes through point c
    result.c = -result.a * geometry::get<0>(c) - result.b * geometry::get<1>(c);
    return result;
}

}} // namespace detail::make
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_MAKE_MAKE_HPP


#include <boost/graph/planar_canonical_ordering.hpp>


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2012 Bryce Adelstein-Lelbach
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
////////////////////////////////////////////////////////////////////////////////

#if !defined(OCTOPUS_903E5732_EB7C_4CBC_A13E_24DF0582AF0E)
#define OCTOPUS_903E5732_EB7C_4CBC_A13E_24DF0582AF0E

#include <octopus/octree/octree_server.hpp>
#include <octopus/science/refinement_criteria.hpp>
#include <octopus/science/dt_prediction.hpp>
#include <octopus/vector2d.hpp>
#include <octopus/io/writer.hpp>
#include <octopus/face.hpp>

#include <boost/cstdint.hpp>

// NOTE: (to self) Don't forgot to update default_science_table when
// science_table is updated.

#define OCTOPUS_SCIENCE_TABLE_VERSION 0x01

namespace octopus
{

/// A table that does... science. This is a vtable defining the physics through
/// a set constants and serializable functions. Note that the constants in this
/// class are distinctly different from parameters in config_data. Constants
/// in a science table represent values that vary from problem to problem, but
/// are fixed within the scope of each problem.
// NOTE: Aggregate for laziness.
// NOTE: Users should never copy this.
struct science_table
{
    /// Defines the physical boundaries. Returns true if a face at a location in
    /// the octree and a particular level of refinement is a physical boundary.
    hpx::util::function<
        bool(
            array<boost::int64_t, 3> const&
          , face
          , boost::uint64_t
            )
    > physical_boundaries;

    /// The reconstruction scheme.
    hpx::util::function<
        void(
/*
            vector2d<double> const&
          , vector2d<double>&
          , vector2d<double>&
*/
            std::vector<state> const&
          , std::vector<state>&
          , std::vector<state>&
            )
    > reconstruct;

    boost::uint64_t ghost_zone_length; /// The width of ghost zones on all sides,
                                      /// measured in number of grid points.

    hpx::util::function<
        void(octree_server&)
    > initialize;

    hpx::util::function<
        void(
            octree_server&
          , state&
          , array<double, 3> const&
          , face
        )
    > enforce_outflow;

    hpx::util::function<
        void(
            state&
          , array<double, 3> const&
            )
    > enforce_limits;

    hpx::util::function<
        void(state&)
    > reflect_z;

    hpx::util::function<
        double(
            octree_server&
          , state const&
          , array<double, 3> const&
          , axis
            )
    > max_eigenvalue;

    ///< The distance between zones in the root node/most coarse / refinement
    ///  level (aka level 0).
    // h0, (2*GRID_DIM/double(GNX-2*BW)), TODO: validate min/max
    hpx::util::function<double()> initial_dx;

    hpx::util::function<
        double(
            octree_server& ///< Root
            )
    > initial_dt;

    hpx::util::function<
        /// (timestep N + 1 size, timestep N + gap size)
        dt_prediction(
            octree_server& ///< Root
            )
    > predict_dt;

    hpx::util::function<
        void(
            state&
          , array<double, 3> const&
            )
    > conserved_to_primitive;

    hpx::util::function<
        void(
            state&
          , array<double, 3> const&
            )
    > primitive_to_conserved;

    hpx::util::function<
        state(
            octree_server&
          , state const&
          , array<double, 3> const&
            )
    > source;

    hpx::util::function<
        state(
            octree_server&
          , state&
          , array<double, 3> const&
          , array<boost::uint64_t, 3> const&
          , axis
            )
    > flux;

    hpx::util::function<
        hpx::id_type(
            octree_init_data const& init
          , std::vector<hpx::id_type> const& localities
            )
    > distribute;

    // FIXME: Rename to refine.
    refinement_criteria refine_policy;

    writer output;

    science_table()
     : physical_boundaries()
     , reconstruct()
     , ghost_zone_length(0)
     , initialize()
     , enforce_outflow()
     , enforce_limits()
     , reflect_z()
     , max_eigenvalue()
     , initial_dx()
     , initial_dt()
     , predict_dt()
     , conserved_to_primitive()
     , primitive_to_conserved()
     , source()
     , flux()
     , distribute()
     , refine_policy()
     , output()
    {}

    template <typename Archive>
    void serialize(Archive& ar, const unsigned int version)
    {
        ar & physical_boundaries;
        ar & reconstruct;
        ar & ghost_zone_length;

        ar & initialize;

        ar & enforce_outflow;
        ar & enforce_limits;

        ar & reflect_z;
        ar & max_eigenvalue;

        ar & initial_dx;

        ar & initial_dt;
        ar & predict_dt;

        ar & conserved_to_primitive;
        ar & primitive_to_conserved;

        ar & source;
        ar & flux;

        ar & distribute;
        ar & refine_policy;

        ar & output;
    }
};

OCTOPUS_EXPORT science_table default_science_table();

}

BOOST_CLASS_VERSION(octopus::science_table, OCTOPUS_SCIENCE_TABLE_VERSION)
BOOST_CLASS_TRACKING(octopus::science_table, boost::serialization::track_never)

#endif // OCTOPUS_903E5732_EB7C_4CBC_A13E_24DF0582AF0E


#include "kabsch.h"
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <Eigen/Geometry>

using namespace probreg;
namespace py = pybind11;

PYBIND11_MODULE(_kabsch, m) {
    m.def("kabsch", &computeKabsch);

#ifdef VERSION_INFO
    m.attr("__version__") = VERSION_INFO;
#else
    m.attr("__version__") = "dev";
#endif
}


#include <nt2/arithmetic/include/functions/divfix.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/toint.hpp>

#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>

#include <nt2/constant/constant.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/include/functions/splat.hpp>

NT2_TEST_CASE_TPL ( divfix_real__2_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::divfix;
  using nt2::tag::divfix_;
  using boost::simd::native;
  using nt2::meta::cardinal_of;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename nt2::meta::call<divfix_(T,T)>::type sr_t;

  // specific values tests
  NT2_TEST_ULP_EQUAL(divfix(nt2::Four<vT>(),nt2::Three<vT>())[0], nt2::One<sr_t>(), 0);
  NT2_TEST_ULP_EQUAL(divfix(nt2::Four<vT>(),nt2::Zero<vT>())[0], nt2::Inf<sr_t>(), 0);
  NT2_TEST_ULP_EQUAL(divfix(nt2::Inf<vT>(), nt2::Inf<vT>())[0], nt2::Nan<T>(), 0);
  NT2_TEST_ULP_EQUAL(divfix(nt2::Mfour<vT>(),nt2::Zero<vT>())[0], nt2::Minf<sr_t>(), 0);
  NT2_TEST_ULP_EQUAL(divfix(nt2::Minf<vT>(), nt2::Minf<vT>())[0], nt2::Nan<T>(), 0);
  NT2_TEST_ULP_EQUAL(divfix(nt2::Mone<vT>(), nt2::Mone<vT>())[0], nt2::One<T>(), 0);
  NT2_TEST_ULP_EQUAL(divfix(nt2::Nan<vT>(), nt2::Nan<vT>())[0], nt2::Nan<T>(), 0);
  NT2_TEST_ULP_EQUAL(divfix(nt2::One<vT>(), nt2::One<vT>())[0], nt2::One<T>(), 0);
  NT2_TEST_ULP_EQUAL(divfix(nt2::Zero<vT>(), nt2::Zero<vT>())[0], nt2::Nan<T>(), 0);
} // end of test for floating_


//  Copyright (c) 2014 Grant Mercer
//
//  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)

#include <hpx/hpx_init.hpp>
#include <hpx/hpx.hpp>
#include <hpx/include/parallel_numeric.hpp>
#include <hpx/include/parallel_algorithm.hpp>
#include <hpx/include/iostreams.hpp>

#include <boost/iterator/counting_iterator.hpp>

#include <algorithm>
#include <iterator>
#include <string>
#include <vector>

///////////////////////////////////////////////////////////////////////////////
int hpx_main()
{
    // lets say we have two vectors that simulate.. 10007D
    std::vector<double> xvalues(10007);
    std::vector<double> yvalues(10007);
    std::fill(std::begin(xvalues), std::end(xvalues), 1.0);
    std::fill(std::begin(yvalues), std::end(yvalues), 1.0);

    double result =
        hpx::parallel::transform_reduce(
            hpx::parallel::execution::par,
            boost::counting_iterator<size_t>(0),
            boost::counting_iterator<size_t>(10007),
            0.0,
            std::plus<double>(),
            [&xvalues, &yvalues](size_t i)
            {
                return xvalues[i] * yvalues[i];
            }
        );
    // print the result
    hpx::cout << result << hpx::endl;

    return hpx::finalize();
}

int main(int argc, char* argv[])
{
    // By default this should run on all available cores
    std::vector<std::string> const cfg = {
        "hpx.os_threads=all"
    };

    // Initialize and run HPX
    return hpx::init(argc, argv, cfg);
}


// C++ standard headers
#include <exception>
#include <string>
#include <math.h>

// Boost headers
#include <boost/shared_ptr.hpp>

// ROS headers
#include <ros/ros.h>
#include <ros/topic.h>

#include <nav_msgs/Odometry.h>

// OpenCV headers
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

// Hearts headers
#include <hearts_nav_msgs/RobotPose.h>
#include <hearts_nav_msgs/GetRobotPose.h>

#include <hearts_tools/transformations.h>

#define PI 3.14159265
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

static const std::string odomTopic       = "/mobile_base_controller/odom";
//static const std::string gtodomTopic       = "/ground_truth_odom";

//Odometry trajectory
nav_msgs::Odometry::ConstPtr odometry;

//Global parameters
bool debug = false;
int count = 0;

//uint32_t *vecmap, *veczoom;

std::string type2str(int type) {
  std::string r;

  uchar depth = type & CV_MAT_DEPTH_MASK;
  uchar chans = 1 + (type >> CV_CN_SHIFT);

  switch ( depth ) {
    case CV_8U:  r = "8U"; break;
    case CV_8S:  r = "8S"; break;
    case CV_16U: r = "16U"; break;
    case CV_16S: r = "16S"; break;
    case CV_32S: r = "32S"; break;
    case CV_32F: r = "32F"; break;
    case CV_64F: r = "64F"; break;
    default:     r = "User"; break;
  }

  r += "C";
  r += (chans+'0');

  return r;
}

void odomCallback(const nav_msgs::Odometry::ConstPtr& msgodom)
{
  odometry = msgodom;
}

bool srvCallback(hearts_nav_msgs::GetRobotPose::Request &req, hearts_nav_msgs::GetRobotPose::Response &res)
{
	/*std::cout << "Odometry: " << odometry->pose.pose.position.x << ", " <<
	odometry->pose.pose.position.y << ", " <<
	odometry->pose.pose.position.z << ") (" <<

	odometry->pose.pose.orientation.x << ", " <<
	odometry->pose.pose.orientation.y << ", " <<
	odometry->pose.pose.orientation.z << ", " <<
	odometry->pose.pose.orientation.w << ") " << std::endl;*/

	/*std::cout << "origin: (" <<
	info.origin.position.x << ", " <<
	info.origin.position.y << ", " <<
	info.origin.position.z << ") (" <<

	info.origin.orientation.x << ", " <<
	info.origin.orientation.y << ", " <<
	info.origin.orientation.z << ", " <<
	info.origin.orientation.w << ") " << std::endl;*/

	///Get robot pose in occupancy map coordinates
	//Get robot position
	cv::Mat pos = cv::Mat::zeros(3, 1, CV_32FC1);
	pos.at<float>(0) = (float)odometry->pose.pose.position.y;
	pos.at<float>(1) = (float)odometry->pose.pose.position.x;
	pos.at<float>(2) = 1;

	//Get occupancy map transformation parameters
	cv::Mat k = cv::Mat::zeros(3, 3, CV_32FC1);
	k.at<float>(0,0) = 6.46; k.at<float>(1,1) = 4.84;
	k.at<float>(0,2) = 320; k.at<float>(1,2) = 240;
	k.at<float>(2,2) = 1;

	//Get robot position in occupancy coordinates
	cv::Mat kpos = cv::Mat::zeros(3, 1, CV_32FC1);
	kpos = k*pos;

	//Set robot origin
	cv::Mat o = cv::Mat::zeros(3, 1, CV_32FC1);
	o.at<float>(0) = 320; o.at<float>(1) = 240;

	//Get robot orientation from quaternion to Euclidean form
	double siny = +2.0 * (odometry->pose.pose.orientation.w * odometry->pose.pose.orientation.z);
	double cosy = +1.0 - 2.0 * (odometry->pose.pose.orientation.z * odometry->pose.pose.orientation.z);
	double yaw = atan2(siny, cosy);

	//Set Rt transformation matrix from robot to occupancy map coordinates
	cv::Mat rot = cv::Mat::zeros(3, 3, CV_32FC1);
	rot.at<float>(0,0) = (float)cos(-1*yaw); rot.at<float>(0,1) = (float)-1*sin(-1*yaw);
	rot.at<float>(1,0) = (float)sin(-1*yaw); rot.at<float>(1,1) = (float)cos(-1*yaw);

	//rot.at<float>(0,2) = kpos.at<float>(0)-320;;
	//rot.at<float>(1,2) = kpos.at<float>(1)-240;
	rot.at<float>(0,2) = kpos.at<float>(0)-o.at<float>(0);;
	rot.at<float>(1,2) = kpos.at<float>(1)-o.at<float>(1);
	rot.at<float>(2,2) = 1;

	//std::cout << "(" << rot.at<float>(0,0) << ", " << rot.at<float>(0,1), rot.at<float>(0,2) << " // "
	//		  << rot.at<float>(1,0) << ", " << rot.at<float>(1,1), rot.at<float>(1,2) << ")" << std::endl;

	///Send messages
	std::vector<float_t> veco, vecpos, veck, vecrot;
	int o_w, o_h, pos_w, pos_h, k_w, k_h, rot_w, rot_h;

	Transformations::CvMatf_ToVectMsg(o, &veco, o_w, o_h);
	Transformations::CvMatf_ToVectMsg(pos, &vecpos, pos_w, pos_h);
	Transformations::CvMatf_ToVectMsg(k, &veck, k_w, k_h);
	Transformations::CvMatf_ToVectMsg(rot, &vecrot, rot_w, rot_h);

	res.robot_pose.id = count++;

	res.robot_pose.o = veco;
	res.robot_pose.o_w = o_w;
	res.robot_pose.o_h = o_h;

	res.robot_pose.pos = vecpos;
	res.robot_pose.pos_w = pos_w;
	res.robot_pose.pos_h = pos_h;

	res.robot_pose.yaw = yaw;

	res.robot_pose.k = veck;
	res.robot_pose.k_w = k_w;
	res.robot_pose.k_h = k_h;

	res.robot_pose.rt = vecrot;
	res.robot_pose.rt_w = rot_w;
	res.robot_pose.rt_h = rot_h;

	if (debug)
	{
		///Display current robot pose
		std::cout << "\n o" << o  << "\n pos" << pos << "\n yaw" << yaw << "\n K" << k << "\n Rt" << rot << std::endl;
	}

	return true;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// Entry point
int main(int argc, char** argv)
{
  // Init the ROS node
  ros::init(argc, argv, "mapping_pose_srv");

  ROS_INFO("Starting mapping_pose_srv application ...");

  // Precondition: Valid clock
  ros::NodeHandle nh, nh2;
  if (!ros::Time::waitForValid(ros::WallDuration(10.0))) // NOTE: Important when using simulated clock
  {
    ROS_FATAL("Timed-out waiting for valid time.");
    return EXIT_FAILURE;
  }

  for (int i = 0; i < argc; i++)
  {
	  if ( strcmp( argv[i], "-debug") == 0 )
		  debug = true;
  }

  ros::ServiceServer service = nh.advertiseService("/brl_hearts/navigation/mapping_pose_srv", srvCallback);

  ROS_INFO_STREAM("Subscribing to " << odomTopic << " ...");
  ros::Subscriber subodom = nh2.subscribe(odomTopic, 1, odomCallback);

  //enter a loop that processes ROS callbacks. Press CTRL+C to exit the loop
  ros::Rate loop_rate(5);

  ros::spin();

  return EXIT_SUCCESS;
}


// boost\math\distributions\non_central_f.hpp

// Copyright John Maddock 2008.

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_SPECIAL_NON_CENTRAL_F_HPP
#define BOOST_MATH_SPECIAL_NON_CENTRAL_F_HPP

#include <boost/math/distributions/non_central_beta.hpp>
#include <boost/math/distributions/detail/generic_mode.hpp>
#include <boost/math/special_functions/pow.hpp>

namespace boost
{
   namespace math
   {
      template <class RealType = double, class Policy = policies::policy<> >
      class non_central_f_distribution
      {
      public:
         typedef RealType value_type;
         typedef Policy policy_type;

         non_central_f_distribution(RealType v1_, RealType v2_, RealType lambda) : v1(v1_), v2(v2_), ncp(lambda)
         {
            const char* function = "boost::math::non_central_f_distribution<%1%>::non_central_f_distribution(%1%,%1%)";
            RealType r;
            detail::check_df(
               function,
               v1, &r, Policy());
            detail::check_df(
               function,
               v2, &r, Policy());
            detail::check_non_centrality(
               function,
               lambda,
               &r,
               Policy());
         } // non_central_f_distribution constructor.

         RealType degrees_of_freedom1()const
         {
            return v1;
         }
         RealType degrees_of_freedom2()const
         {
            return v2;
         }
         RealType non_centrality() const
         { // Private data getter function.
            return ncp;
         }
      private:
         // Data member, initialized by constructor.
         RealType v1;   // alpha.
         RealType v2;   // beta.
         RealType ncp; // non-centrality parameter
      }; // template <class RealType, class Policy> class non_central_f_distribution

      typedef non_central_f_distribution<double> non_central_f; // Reserved name of type double.

      // Non-member functions to give properties of the distribution.

      template <class RealType, class Policy>
      inline const std::pair<RealType, RealType> range(const non_central_f_distribution<RealType, Policy>& /* dist */)
      { // Range of permissible values for random variable k.
         using boost::math::tools::max_value;
         return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>());
      }

      template <class RealType, class Policy>
      inline const std::pair<RealType, RealType> support(const non_central_f_distribution<RealType, Policy>& /* dist */)
      { // Range of supported values for random variable k.
         // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
         using boost::math::tools::max_value;
         return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>());
      }

      template <class RealType, class Policy>
      inline RealType mean(const non_central_f_distribution<RealType, Policy>& dist)
      {
         const char* function = "mean(non_central_f_distribution<%1%> const&)";
         RealType v1 = dist.degrees_of_freedom1();
         RealType v2 = dist.degrees_of_freedom2();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            v1, &r, Policy())
               ||
            !detail::check_df(
               function,
               v2, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy()))
               return r;
         if(v2 <= 2)
            return policies::raise_domain_error(
               function,
               "Second degrees of freedom parameter was %1%, but must be > 2 !",
               v2, Policy());
         return v2 * (v1 + l) / (v1 * (v2 - 2));
      } // mean

      template <class RealType, class Policy>
      inline RealType mode(const non_central_f_distribution<RealType, Policy>& dist)
      { // mode.
         static const char* function = "mode(non_central_chi_squared_distribution<%1%> const&)";

         RealType n = dist.degrees_of_freedom1();
         RealType m = dist.degrees_of_freedom2();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            n, &r, Policy())
               ||
            !detail::check_df(
               function,
               m, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy()))
               return r;
         RealType guess = m > 2 ? RealType(m * (n + l) / (n * (m - 2))) : RealType(1);
         return detail::generic_find_mode(
            dist,
            guess,
            function);
      }

      template <class RealType, class Policy>
      inline RealType variance(const non_central_f_distribution<RealType, Policy>& dist)
      { // variance.
         const char* function = "variance(non_central_f_distribution<%1%> const&)";
         RealType n = dist.degrees_of_freedom1();
         RealType m = dist.degrees_of_freedom2();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            n, &r, Policy())
               ||
            !detail::check_df(
               function,
               m, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy()))
               return r;
         if(m <= 4)
            return policies::raise_domain_error(
               function,
               "Second degrees of freedom parameter was %1%, but must be > 4 !",
               m, Policy());
         RealType result = 2 * m * m * ((n + l) * (n + l)
            + (m - 2) * (n + 2 * l));
         result /= (m - 4) * (m - 2) * (m - 2) * n * n;
         return result;
      }

      // RealType standard_deviation(const non_central_f_distribution<RealType, Policy>& dist)
      // standard_deviation provided by derived accessors.

      template <class RealType, class Policy>
      inline RealType skewness(const non_central_f_distribution<RealType, Policy>& dist)
      { // skewness = sqrt(l).
         const char* function = "skewness(non_central_f_distribution<%1%> const&)";
         BOOST_MATH_STD_USING
         RealType n = dist.degrees_of_freedom1();
         RealType m = dist.degrees_of_freedom2();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            n, &r, Policy())
               ||
            !detail::check_df(
               function,
               m, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy()))
               return r;
         if(m <= 6)
            return policies::raise_domain_error(
               function,
               "Second degrees of freedom parameter was %1%, but must be > 6 !",
               m, Policy());
         RealType result = 2 * constants::root_two<RealType>();
         result *= sqrt(m - 4);
         result *= (n * (m + n - 2) *(m + 2 * n - 2)
            + 3 * (m + n - 2) * (m + 2 * n - 2) * l
            + 6 * (m + n - 2) * l * l + 2 * l * l * l);
         result /= (m - 6) * pow(n * (m + n - 2) + 2 * (m + n - 2) * l + l * l, RealType(1.5f));
         return result;
      }

      template <class RealType, class Policy>
      inline RealType kurtosis_excess(const non_central_f_distribution<RealType, Policy>& dist)
      {
         const char* function = "kurtosis_excess(non_central_f_distribution<%1%> const&)";
         BOOST_MATH_STD_USING
         RealType n = dist.degrees_of_freedom1();
         RealType m = dist.degrees_of_freedom2();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            n, &r, Policy())
               ||
            !detail::check_df(
               function,
               m, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy()))
               return r;
         if(m <= 8)
            return policies::raise_domain_error(
               function,
               "Second degrees of freedom parameter was %1%, but must be > 8 !",
               m, Policy());
         RealType l2 = l * l;
         RealType l3 = l2 * l;
         RealType l4 = l2 * l2;
         RealType result = (3 * (m - 4) * (n * (m + n - 2)
            * (4 * (m - 2) * (m - 2)
            + (m - 2) * (m + 10) * n
            + (10 + m) * n * n)
            + 4 * (m + n - 2) * (4 * (m - 2) * (m - 2)
            + (m - 2) * (10 + m) * n
            + (10 + m) * n * n) * l + 2 * (10 + m)
            * (m + n - 2) * (2 * m + 3 * n - 4) * l2
            + 4 * (10 + m) * (-2 + m + n) * l3
            + (10 + m) * l4))
            /
            ((-8 + m) * (-6 + m) * boost::math::pow<2>(n * (-2 + m + n)
            + 2 * (-2 + m + n) * l + l2));
            return result;
      } // kurtosis_excess

      template <class RealType, class Policy>
      inline RealType kurtosis(const non_central_f_distribution<RealType, Policy>& dist)
      {
         return kurtosis_excess(dist) + 3;
      }

      template <class RealType, class Policy>
      inline RealType pdf(const non_central_f_distribution<RealType, Policy>& dist, const RealType& x)
      { // Probability Density/Mass Function.
         typedef typename policies::evaluation<RealType, Policy>::type value_type;
         typedef typename policies::normalise<
            Policy,
            policies::promote_float<false>,
            policies::promote_double<false>,
            policies::discrete_quantile<>,
            policies::assert_undefined<> >::type forwarding_policy;

         value_type alpha = dist.degrees_of_freedom1() / 2;
         value_type beta = dist.degrees_of_freedom2() / 2;
         value_type y = x * alpha / beta;
         value_type r = pdf(boost::math::non_central_beta_distribution<value_type, forwarding_policy>(alpha, beta, dist.non_centrality()), y / (1 + y));
         return policies::checked_narrowing_cast<RealType, forwarding_policy>(
            r * (dist.degrees_of_freedom1() / dist.degrees_of_freedom2()) / ((1 + y) * (1 + y)),
            "pdf(non_central_f_distribution<%1%>, %1%)");
      } // pdf

      template <class RealType, class Policy>
      RealType cdf(const non_central_f_distribution<RealType, Policy>& dist, const RealType& x)
      {
         const char* function = "cdf(const non_central_f_distribution<%1%>&, %1%)";
         RealType r;
         if(!detail::check_df(
            function,
            dist.degrees_of_freedom1(), &r, Policy())
               ||
            !detail::check_df(
               function,
               dist.degrees_of_freedom2(), &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               dist.non_centrality(),
               &r,
               Policy()))
               return r;

         if((x < 0) || !(boost::math::isfinite)(x))
         {
            return policies::raise_domain_error<RealType>(
               function, "Random Variable parameter was %1%, but must be > 0 !", x, Policy());
         }

         RealType alpha = dist.degrees_of_freedom1() / 2;
         RealType beta = dist.degrees_of_freedom2() / 2;
         RealType y = x * alpha / beta;
         RealType c = y / (1 + y);
         RealType cp = 1 / (1 + y);
         //
         // To ensure accuracy, we pass both x and 1-x to the
         // non-central beta cdf routine, this ensures accuracy
         // even when we compute x to be ~ 1:
         //
         r = detail::non_central_beta_cdf(c, cp, alpha, beta,
            dist.non_centrality(), false, Policy());
         return r;
      } // cdf

      template <class RealType, class Policy>
      RealType cdf(const complemented2_type<non_central_f_distribution<RealType, Policy>, RealType>& c)
      { // Complemented Cumulative Distribution Function
         const char* function = "cdf(complement(const non_central_f_distribution<%1%>&, %1%))";
         RealType r;
         if(!detail::check_df(
            function,
            c.dist.degrees_of_freedom1(), &r, Policy())
               ||
            !detail::check_df(
               function,
               c.dist.degrees_of_freedom2(), &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               c.dist.non_centrality(),
               &r,
               Policy()))
               return r;

         if((c.param < 0) || !(boost::math::isfinite)(c.param))
         {
            return policies::raise_domain_error<RealType>(
               function, "Random Variable parameter was %1%, but must be > 0 !", c.param, Policy());
         }

         RealType alpha = c.dist.degrees_of_freedom1() / 2;
         RealType beta = c.dist.degrees_of_freedom2() / 2;
         RealType y = c.param * alpha / beta;
         RealType x = y / (1 + y);
         RealType cx = 1 / (1 + y);
         //
         // To ensure accuracy, we pass both x and 1-x to the
         // non-central beta cdf routine, this ensures accuracy
         // even when we compute x to be ~ 1:
         //
         r = detail::non_central_beta_cdf(x, cx, alpha, beta,
            c.dist.non_centrality(), true, Policy());
         return r;
      } // ccdf

      template <class RealType, class Policy>
      inline RealType quantile(const non_central_f_distribution<RealType, Policy>& dist, const RealType& p)
      { // Quantile (or Percent Point) function.
         RealType alpha = dist.degrees_of_freedom1() / 2;
         RealType beta = dist.degrees_of_freedom2() / 2;
         RealType x = quantile(boost::math::non_central_beta_distribution<RealType, Policy>(alpha, beta, dist.non_centrality()), p);
         if(x == 1)
            return policies::raise_overflow_error<RealType>(
               "quantile(const non_central_f_distribution<%1%>&, %1%)",
               "Result of non central F quantile is too large to represent.",
               Policy());
         return (x / (1 - x)) * (dist.degrees_of_freedom2() / dist.degrees_of_freedom1());
      } // quantile

      template <class RealType, class Policy>
      inline RealType quantile(const complemented2_type<non_central_f_distribution<RealType, Policy>, RealType>& c)
      { // Quantile (or Percent Point) function.
         RealType alpha = c.dist.degrees_of_freedom1() / 2;
         RealType beta = c.dist.degrees_of_freedom2() / 2;
         RealType x = quantile(complement(boost::math::non_central_beta_distribution<RealType, Policy>(alpha, beta, c.dist.non_centrality()), c.param));
         if(x == 1)
            return policies::raise_overflow_error<RealType>(
               "quantile(complement(const non_central_f_distribution<%1%>&, %1%))",
               "Result of non central F quantile is too large to represent.",
               Policy());
         return (x / (1 - x)) * (c.dist.degrees_of_freedom2() / c.dist.degrees_of_freedom1());
      } // quantile complement.

   } // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_SPECIAL_NON_CENTRAL_F_HPP


/*******************************************************************************
 *
 * Forward fixpoint iterators of varying complexity and precision.
 *
 * The interleaved fixpoint iterator is described in G. Amato and F. Scozzari's
 * paper: Localizing widening and narrowing. In Proceedings of SAS 2013,
 * pages 25-42. LNCS 7935, 2013.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#ifndef IKOS_FWD_FIXPOINT_ITERATORS_HPP
#define IKOS_FWD_FIXPOINT_ITERATORS_HPP

#include <map>
#include <boost/shared_ptr.hpp>
#include <crab/common/types.hpp>
#include <crab/common/debug.hpp>
#include <crab/common/stats.hpp>
#include <crab/iterators/wto.hpp>
#include <crab/iterators/fixpoint_iterators_api.hpp>
#include <crab/iterators/thresholds.hpp>

namespace ikos {

  namespace interleaved_fwd_fixpoint_iterator_impl {

    template< typename NodeName, typename CFG, typename AbstractValue >
    class wto_iterator;

    template< typename NodeName, typename CFG, typename AbstractValue >
    class wto_processor;

  } // namespace interleaved_fwd_fixpoint_iterator_impl

  template< typename NodeName, typename CFG, typename AbstractValue >
  class interleaved_fwd_fixpoint_iterator:
      public forward_fixpoint_iterator< NodeName, CFG, AbstractValue > {

    friend class interleaved_fwd_fixpoint_iterator_impl::wto_iterator< NodeName, CFG, AbstractValue >;

  private:
    typedef std::map< NodeName, AbstractValue > invariant_table_t;
    typedef boost::shared_ptr< invariant_table_t > invariant_table_ptr;
    typedef wto< NodeName, CFG > wto_t;
    typedef interleaved_fwd_fixpoint_iterator_impl::wto_iterator< NodeName, CFG, AbstractValue > wto_iterator_t;
    typedef interleaved_fwd_fixpoint_iterator_impl::wto_processor< NodeName, CFG, AbstractValue > wto_processor_t;
    typedef crab::iterators::thresholds<z_number> thresholds_t;

  private:
    CFG _cfg;
    wto_t _wto;
    invariant_table_ptr _pre, _post;
    // number of iterations until triggering widening
    unsigned int _widening_delay;
    // number of narrowing iterations. If the narrowing operator is
    // indeed a narrowing operator this parameter is not
    // needed. However, there are abstract domains for which a sound
    // narrowing operation is not available so we must enforce
    // termination.
    unsigned int _descending_iterations;
    // whether jump set is used for widening
    bool _use_widening_jump_set;
    // set of thresholds to jump during widening
    thresholds_t _jump_set;

  private:
    void set(invariant_table_ptr table, NodeName node, const AbstractValue& v) {
      std::pair< typename invariant_table_t::iterator, bool > res =
          table->insert(std::make_pair(node, v));
      if (!res.second) {
        (res.first)->second = v;
      }
    }

    void set_pre(NodeName node, const AbstractValue& v) {
      this->set(this->_pre, node, v);
    }

    void set_post(NodeName node, const AbstractValue& v) {
      this->set(this->_post, node, v);
    }

    AbstractValue get(invariant_table_ptr table, NodeName n) {
      typename invariant_table_t::iterator it = table->find(n);
      if (it != table->end()) {
        return it->second;
      } else {
        return AbstractValue::bottom();
      }
    }

  public:
    interleaved_fwd_fixpoint_iterator(CFG cfg,
                                      unsigned int widening_delay,
                                      unsigned int descending_iterations,
                                      size_t jump_set_size):
        _cfg(cfg),
        _wto(cfg),
        _pre(boost::make_shared<invariant_table_t>()),
        _post(boost::make_shared<invariant_table_t>()),
        _widening_delay(widening_delay),
        _descending_iterations(descending_iterations),
        _use_widening_jump_set (jump_set_size > 0) {

      if (_use_widening_jump_set) {
        crab::CrabStats::resume ("Fixpo");
        // select statically some widening points to jump to.
        _jump_set = _cfg.initialize_thresholds_for_widening(jump_set_size);
        crab::CrabStats::stop ("Fixpo");
      }
    }

    CFG get_cfg() const {
      return this->_cfg;
    }

    const wto_t& get_wto() const {
      return this->_wto;
    }

    AbstractValue get_pre(NodeName node) {
      return this->get(this->_pre, node);
    }

    AbstractValue get_post(NodeName node) {
      return this->get(this->_post, node);
    }

   private:

    AbstractValue extrapolate(NodeName /* node */, unsigned int iteration,
                              AbstractValue before, AbstractValue after) {

      CRAB_LOG("fixpo", crab::outs() << "Increasing iteration=" << iteration << "\n";);

      if (iteration <= _widening_delay) {
        CRAB_LOG("fixpo",
                 crab::outs() << "Widening \n";
                 auto widen_res = before | after;
                 crab::outs() << "Prev   : " << before << "\n"
                           << "Current: " << after << "\n"
                           << "Res    : " << widen_res << "\n");
        //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.join");
        //crab::ScopedCrabStats __st__(AbstractValue::getDomainName() + ".join");
        return before | after;
      } else {
        CRAB_LOG("fixpo",
                 crab::outs() << "Widening \n";
                 crab::outs() << "Prev   : " << before << "\n"
                           << "Current: " << after << "\n");

        if (_use_widening_jump_set) {
          CRAB_LOG("fixpo",
                   auto widen_res = before.widening_thresholds (after, _jump_set);
                   crab::outs() << "Res    : " << widen_res << "\n");
          //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.widening");
          //crab::ScopedCrabStats __st__(AbstractValue::getDomainName() + ".widening");
          return before.widening_thresholds (after, _jump_set);
        } else {
          CRAB_LOG("fixpo",
                   auto widen_res = before || after;
                   crab::outs() << "Res    : " << widen_res << "\n");
          //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.widening");
          //crab::ScopedCrabStats __st__(AbstractValue::getDomainName() + ".widening");
          return before || after;
        }
      }
    }

    AbstractValue refine(NodeName /* node */, unsigned int iteration,
                         AbstractValue before, AbstractValue after) {

      CRAB_LOG("fixpo",
               crab::outs() << "Decreasing iteration=" << iteration << "\n";);

      if (iteration == 1) {
        CRAB_LOG("fixpo",
                 crab::outs() << "Narrowing \n";
                 auto narrow_res = before && after;
                 crab::outs() << "Prev   : " << before << "\n"
                           << "Current: " << after << "\n"
                           << "Res    : " << narrow_res << "\n");
        //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.meet");
        //crab::ScopedCrabStats __st__(AbstractValue::getDomainName() + ".meet");
        return before & after;
      } else {
        CRAB_LOG("fixpo",
                 crab::outs() << "Narrowing \n";
                 auto narrow_res = before && after;
                 crab::outs() << "Prev   : " << before << "\n"
                 << "Current: " << after << "\n"
                           << "Res    : " << narrow_res << "\n");
        //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.narrowing");
        //crab::ScopedCrabStats __st__(AbstractValue::getDomainName() + ".narrowing");
        return before && after;
      }
    }

   public:

    void run(AbstractValue init) {
      crab::ScopedCrabStats __st__("Fixpo");
      this->set_pre(this->_cfg.entry(), init);
      wto_iterator_t iterator(this);
      this->_wto.accept(&iterator);
      wto_processor_t processor(this);
      this->_wto.accept(&processor);
      this->_pre.reset();
      this->_post.reset();
    }

    virtual ~interleaved_fwd_fixpoint_iterator() { }

  }; // class interleaved_fwd_fixpoint_iterator

  namespace interleaved_fwd_fixpoint_iterator_impl {

    template< typename NodeName, typename CFG, typename AbstractValue >
    class wto_iterator: public wto_component_visitor< NodeName, CFG > {

    public:
      typedef interleaved_fwd_fixpoint_iterator< NodeName, CFG, AbstractValue > interleaved_iterator_t;
      typedef wto_vertex< NodeName, CFG > wto_vertex_t;
      typedef wto_cycle< NodeName, CFG > wto_cycle_t;
      typedef wto< NodeName, CFG > wto_t;
      typedef typename wto_t::wto_nesting_t wto_nesting_t;

    private:
      interleaved_iterator_t *_iterator;

    public:
      wto_iterator(interleaved_iterator_t *iterator): _iterator(iterator) { }

      void visit(wto_vertex_t& vertex) {
        AbstractValue pre;
        NodeName node = vertex.node();
        if (node == this->_iterator->get_cfg().entry()) {
          pre = this->_iterator->get_pre(node);
        } else {
          auto prev_nodes = this->_iterator->_cfg.prev_nodes(node);
          pre = AbstractValue::bottom();
          CRAB_LOG ("fixpo", crab::outs() << "Joining predecessors ...\n");
          for (NodeName prev : prev_nodes) {
            //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.join");
            //crab::ScopedCrabStats __st__(AbstractValue::getDomainName() + ".join");
            pre |= this->_iterator->get_post(prev);
          }
          this->_iterator->set_pre(node, pre);
        }
        CRAB_LOG ("fixpo", crab::outs() << "Analyzing node ...\n");
        this->_iterator->analyze(node, pre);
        this->_iterator->set_post(node, pre);
      }

      void visit(wto_cycle_t& cycle) {
        NodeName head = cycle.head();
        wto_nesting_t cycle_nesting = this->_iterator->_wto.nesting(head);
        auto prev_nodes = this->_iterator->_cfg.prev_nodes(head);
        AbstractValue pre = AbstractValue::bottom();
        CRAB_LOG ("fixpo", crab::outs() << "Merging predecessors at widening point ...\n");
        for (NodeName prev : prev_nodes) {
          if (!(this->_iterator->_wto.nesting(prev) > cycle_nesting)) {
            //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.join");
            //crab::ScopedCrabStats __st__(AbstractValue::getDomainName() + ".join");
            pre |= this->_iterator->get_post(prev);
          }
        }
        for(unsigned int iteration = 1; ; ++iteration) {
          // Increasing iteration sequence with widening
          this->_iterator->set_pre(head, pre);
          AbstractValue post(pre);
          CRAB_LOG ("fixpo", crab::outs() << "Analyzing node ...\n");
          this->_iterator->analyze(head, post);
          this->_iterator->set_post(head, post);
          for (typename wto_cycle_t::iterator it = cycle.begin(); it != cycle.end(); ++it) {
            it->accept(this);
          }
          AbstractValue new_pre = AbstractValue::bottom();
          for (NodeName prev : prev_nodes) {
            //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.join");
            //crab::ScopedCrabStats __st__(AbstractValue::getDomainName() + ".join");
            new_pre |= this->_iterator->get_post(prev);
          }
          //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.leq");
          //crab::CrabStats::resume (AbstractValue::getDomainName() + ".leq");
          if (new_pre <= pre) {
            //crab::CrabStats::stop (AbstractValue::getDomainName() + ".leq");
            // Post-fixpoint reached
            CRAB_LOG ("fixpo", crab::outs() << "post-fixpoint reached\n");
            this->_iterator->set_pre(head, new_pre);
            pre = new_pre;
            break;
          } else {
            //crab::CrabStats::stop (AbstractValue::getDomainName() + ".leq");
            pre = this->_iterator->extrapolate(head, iteration, pre, new_pre);
          }
        }
        for(unsigned int iteration = 1; ; ++iteration) {
          // Decreasing iteration sequence with narrowing
          AbstractValue post(pre);
          CRAB_LOG ("fixpo", crab::outs() << "Analyzing node ...\n");
          this->_iterator->analyze(head, post);
          this->_iterator->set_post(head, post);
          for (typename wto_cycle_t::iterator it = cycle.begin(); it != cycle.end(); ++it) {
            it->accept(this);
          }
          AbstractValue new_pre = AbstractValue::bottom();
          for (NodeName prev : prev_nodes) {
            //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.join");
            //crab::ScopedCrabStats __st__(AbstractValue::getDomainName() + ".join");
            new_pre |= this->_iterator->get_post(prev);
          }
          //crab::CrabStats::count (AbstractValue::getDomainName() + ".count.leq");
          //crab::CrabStats::resume (AbstractValue::getDomainName() + ".leq");
          if (pre <= new_pre) {
            //crab::CrabStats::stop (AbstractValue::getDomainName() + ".leq");
            CRAB_LOG ("fixpo", crab::outs() << "No more refinement possible.\n");
            // No more refinement possible (pre == new_pre)
            break;
          } else {
            //crab::CrabStats::stop (AbstractValue::getDomainName() + ".leq");
            if (iteration > this->_iterator->_descending_iterations) break;
            pre = this->_iterator->refine(head, iteration, pre, new_pre);
            this->_iterator->set_pre(head, pre);
          }
        }
      }

    }; // class wto_iterator

    template< typename NodeName, typename CFG, typename AbstractValue >
    class wto_processor: public wto_component_visitor< NodeName, CFG > {

    public:
      typedef interleaved_fwd_fixpoint_iterator< NodeName, CFG, AbstractValue > interleaved_iterator_t;
      typedef wto_vertex< NodeName, CFG > wto_vertex_t;
      typedef wto_cycle< NodeName, CFG > wto_cycle_t;

    private:
      interleaved_iterator_t *_iterator;

    public:
      wto_processor(interleaved_iterator_t *iterator): _iterator(iterator) { }

      void visit(wto_vertex_t& vertex) {
        NodeName node = vertex.node();
        this->_iterator->process_pre(node, this->_iterator->get_pre(node));
        this->_iterator->process_post(node, this->_iterator->get_post(node));
      }

      void visit(wto_cycle_t& cycle) {
        NodeName head = cycle.head();
        this->_iterator->process_pre(head, this->_iterator->get_pre(head));
        this->_iterator->process_post(head, this->_iterator->get_post(head));
        for (typename wto_cycle_t::iterator it = cycle.begin(); it != cycle.end(); ++it) {
          it->accept(this);
        }
      }

    }; // class wto_processor

  } // interleaved_fwd_fixpoint_iterator_impl
} // namespace ikos
#endif // IKOS_FWD_FIXPOINT_ITERATORS


#include "lipton-tarjan.h"
#include <iostream>
#include <fstream>
#include <csignal>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/graph/copy.hpp>
#include "colors.h"
#include "strutil.h"
using namespace std;
using namespace boost;

map<VertDesc, uint> vert2uint;
map<uint, VertDesc> uint2vert;

Graph load_graph(string fname)
{
        ifstream in(fname);
        if( !in ){
                cerr << "file \"in\" not found!\n";
                exit(1);
        }

        string str;
        vector<pair<uint, uint>> edges;
        uint max_v = 0;
        while( getline(in, str) ){
                uint   colon = str.find(",");
                string stra  = str.substr(0, colon); trim(stra);
                string strb  = str.substr(colon+1 ); trim(strb);
                uint   a     = lexical_cast<uint>(stra);
                uint   b     = lexical_cast<uint>(strb);
                max_v = max(max(max_v, a), b);
                edges.push_back(make_pair(a, b));
        }
        Graph g(max_v+1);
        VertIter vi, vi_end;
        uint i = 0;
        for( tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi ){
                vert2uint[*vi] = i;
                uint2vert[i] = *vi;
                ++i;
        }
        for( auto& e : edges ){
                auto src = uint2vert[e.first];
                auto tar = uint2vert[e.second];
                add_edge(src, tar, g);
        }

        vert2uint[Graph::null_vertex()] = -1;
        return g;
}

int main(int argc, char* argv[])
{
        vector<string> fname;
        if( argc < 2 ){
                cout << "Usage: lt [filename]\n";
                return 0;
        }
        for( uint i = 1; i < argc; ++i ) fname.push_back(argv[i]);

        for( auto& f : fname ){
                cout << "loading graph\n";
                auto g = load_graph(f);
                uint n = num_vertices(g);

                auto m = get(vertex_index, g);
                VertIter vi, vend;
                uint i = 0;
                for( tie(vi, vend) = vertices(g); vi != vend; ++vi ){
                        m[*vi] = i;
                        ++i;
                }

                cout << "n: " << n << '\n';
                uint e = num_edges(g);

                cout << "starting lipton tarjan...\n";
                Graph g_copy;
                copy_graph(g, g_copy);
                cout << "g\n";
                print_graph(g);
                cout << "g_copy\n";
                print_graph(g_copy);
                auto p = lipton_tarjan(g, g_copy);
        }
}