#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);
}


// 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 "Mesh.hpp"

#include "igl/barycentric_coordinates.h"
#include "Util.hpp"

#include <Eigen/QR>

#include <iostream>
#include <cstdlib>
#include <iostream>

template <typename T> using Mat = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
template <typename T> using VecC3 = Eigen::Matrix<T, 3, 1>;
template <typename T> using VecC2 = Eigen::Matrix<T, 2, 1>;
template <typename T> using VecR3 = Eigen::Matrix<T, 1, 3>;

template <typename T>
Mesh<T>::Mesh()
{
#ifdef ENABLE_CUDA
  std::cout << " ----**** Creating mesh with CUDA solver ****----" << std::endl;
#else
  std::cout << " ----**** Creating mesh with CPU solver ****----" << std::endl;
#endif

  color_counter =  Eigen::MatrixXi::Zero(TEXTURE_RESOLUTION, TEXTURE_RESOLUTION);

  texture.red   = Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>::Constant(TEXTURE_RESOLUTION,TEXTURE_RESOLUTION, static_cast<unsigned char>(115));
  texture.green = Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>::Constant(TEXTURE_RESOLUTION,TEXTURE_RESOLUTION, static_cast<unsigned char>(115));
  texture.blue  = Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>::Constant(TEXTURE_RESOLUTION,TEXTURE_RESOLUTION, static_cast<unsigned char>(115));
}

template <typename T>
Mesh<T>::~Mesh()
{
  std::ofstream residual_file;
  residual_file.open("error.csv");
  residual_file << "pc_idx,level,iteration,residuals\n";

  for (int pci = 0; pci < residuals.size(); ++pci)
  {
    for (int li = 0; li < residuals[pci].size(); ++li)
    {
      for (int it = 0; it < residuals[pci][li].size(); ++it)
        residual_file << pci << "," << li << "," << it << "," << residuals[pci][li][it] << std::endl;
    }
  }

  residual_file.close();
}

template <typename T>
void Mesh<T>::cleanup()
{
#ifdef ENABLE_CUDA
    cudaDeviceSynchronize();
#endif

    for (auto& m : JtJ)
        m.clear();

    for (auto& v : Jtz)
        v.clear();
}

inline int subdivided_side_length(const int level, const int base_resolution)
{
  return (base_resolution-1)*static_cast<int>(std::pow(2,level))+1;
}

template <typename T>
void upsample(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V, const Eigen::MatrixXi& F, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V_out, Eigen::MatrixXi& F_out)
{
    const int old_resolution = static_cast<int>(std::sqrt(static_cast<double>(V.rows())));
    const int resolution = subdivided_side_length(1, old_resolution);
    const int faces = (resolution - 1) * (resolution - 1) * 2;
    const int vertices = resolution * resolution;

    F_out = Eigen::MatrixXi::Zero(faces, 3);
    V_out = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(vertices, 3);

#pragma omp parallel for
    for (int y_step = 0; y_step < old_resolution-1; ++y_step)
    {
      for (int x_step = 0; x_step < old_resolution-1; ++x_step)
      {

        Eigen::Vector3i f_u = F.row(x_step*2 + y_step*(old_resolution-1)*2);
        Eigen::Vector3i f_l = F.row(x_step*2 + y_step*(old_resolution-1)*2 + 1);

        const VecC3<T> old_v0 = V.row(f_u(0));
        const VecC3<T> old_v1 = V.row(f_u(1));
        const VecC3<T> old_v2 = V.row(f_u(2));

        const VecC3<T> old_v3 = V.row(f_l(0));
        const VecC3<T> old_v4 = V.row(f_l(1));
        //const TvecC3 old_v5 = V.row(f_l(2));

        const VecC3<T> v0 = old_v0;
        const VecC3<T> v1 = (old_v0 + old_v1) * T(0.5);
        const VecC3<T> v2 = old_v1;
        const VecC3<T> v3 = (old_v0 + old_v2) * T(0.5);
        const VecC3<T> v4 = (old_v2 + old_v1) * T(0.5);
        const VecC3<T> v5 = (old_v3 + old_v1) * T(0.5);
        const VecC3<T> v6 = old_v2;
        const VecC3<T> v7 = (old_v3 + old_v4) * T(0.5);
        const VecC3<T> v8 = old_v3;

        const int new_v_xi = x_step * 2;
        const int new_v_yi = y_step * 2;

        int
          v0i = new_v_xi       + new_v_yi * resolution,
          v1i = new_v_xi + 1 + new_v_yi * resolution,
          v2i = new_v_xi + 2+ new_v_yi * resolution,
          v3i = new_v_xi + (new_v_yi+1) * resolution,
          v4i = new_v_xi + 1 + (new_v_yi+1) * resolution,
          v5i = new_v_xi + 2 + (new_v_yi+1) * resolution,
          v6i = new_v_xi       + (new_v_yi+2) * resolution,
          v7i = new_v_xi + 1 + (new_v_yi+2) * resolution,
          v8i = new_v_xi + 2 + (new_v_yi+2) * resolution;

        V_out.row(v0i) = v0;
        V_out.row(v1i) = v1;
        V_out.row(v3i) = v3;
        V_out.row(v4i) = v4;

        if (x_step == old_resolution-2)
        {
          V_out.row(v2i) = v2;
          V_out.row(v5i) = v5;
        }

        if (y_step == old_resolution-2)
        {
          V_out.row(v6i) = v6;
          V_out.row(v7i) = v7;
        }

        if (x_step == old_resolution - 2 && y_step == old_resolution-2)
          V_out.row(v8i) = v8;

        const int new_f_xi = x_step * 2;
        const int new_f_yi = y_step * 2;

        F_out.row(2*new_f_xi     + new_f_yi * (resolution-1) * 2) << v0i,v1i,v3i;
        F_out.row(2*new_f_xi + 1 + new_f_yi * (resolution-1) * 2) << v4i,v3i,v1i;
        F_out.row(2*new_f_xi + 2 + new_f_yi * (resolution-1) * 2) << v1i,v2i,v4i;
        F_out.row(2*new_f_xi + 3 + new_f_yi * (resolution-1) * 2) << v5i,v4i,v2i;
        F_out.row(2*new_f_xi +     (new_f_yi+1) * (resolution-1)*2) << v3i,v4i,v6i;
        F_out.row(2*new_f_xi + 1 + (new_f_yi+1) * (resolution-1)*2) << v7i,v6i,v4i;
        F_out.row(2*new_f_xi + 2 + (new_f_yi+1) * (resolution-1)*2) << v4i,v5i,v7i;
        F_out.row(2*new_f_xi + 3 + (new_f_yi+1) * (resolution-1)*2) << v8i,v7i,v5i;
      }
    }
}

template <typename T>
void upsample(const unsigned int levels, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V, const Eigen::MatrixXi& F, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V_out, Eigen::MatrixXi& F_out)
{
    if (levels < 1)
    {
      V_out = V;
      F_out = F;
      return;
    }

    Eigen::MatrixXi F_in = F;
    Mat<T> V_in = V;

    for (unsigned int i = 0; i < levels; ++i)
    {
      upsample(V_in, F_in, V_out, F_out);
      F_in = F_out;
      V_in = V_out;
    }
}

// Compute barycentric coordinates based on the corner points of a triangle
template <typename T>
void barycentric(const VecC2<T>& p, const Eigen::Matrix<T, 2, 1>& a, const Eigen::Matrix<T, 2, 1>& b, const Eigen::Matrix<T, 2, 1>& c, T &u, T &v, T &w)
{
    Eigen::Matrix<T, 2, 1> v0 = b - a,
                                           v1 = c - a,
                                           v2 = p - a;
    T a00 = v0.dot(v0);
    T a01 = v0.dot(v1);
    T a11 = v1.dot(v1);
    T a20 = v2.dot(v0);
    T a21 = v2.dot(v1);
    T den = 1 / (a00 * a11 - a01 * a01);
    v =  (a11 * a20 - a01 * a21) * den;
    w = (a00 * a21 - a01 * a20) * den;
    u = T(1.0) - v - w;
}

template <typename T>
template <typename Derived>
void Mesh<T>::align_to_point_cloud(const Eigen::MatrixBase<Derived>& P)
{
  const VecR3<T> bb_min = P.colwise().minCoeff();
  const VecR3<T> bb_max = P.colwise().maxCoeff();
  const VecR3<T> bb_d = (bb_max - bb_min).cwiseAbs();

  JtJ.resize(MESH_LEVELS);
  Jtz.resize(MESH_LEVELS);
  V.resize(MESH_LEVELS);
  F.resize(MESH_LEVELS);

  const int max_resolution = subdivided_side_length(MESH_LEVELS - 1, MESH_RESOLUTION);
  const Eigen::Matrix<T, 2, 1> a(0.f, 0.f), b(1.f, 0.f), c(0.f, 1.f), d(1.f, 1.f);

  for (int li = 0; li < MESH_LEVELS; ++li)
  {
    const T scaling_factor(MESH_SCALING_FACTOR);
    const int resolution = subdivided_side_length(li, MESH_RESOLUTION);

    // Scaling matrix
    const Eigen::Transform<T, 3, Eigen::Affine> scaling(Eigen::Scaling(VecC3<T>(scaling_factor*bb_d(0)/T(resolution), 0.f,scaling_factor*bb_d(2)/T(resolution))));

    //const TvecR3 pc_mean = P.colwise().mean();
    // P_centr: mean of the point cloud
    VecR3<T> P_centr = bb_min + 0.5f*(bb_max - bb_min);
    P_centr(1) = 0.0;

    const Eigen::Transform<T, 3, Eigen::Affine> t(Eigen::Translation<T, 3>(P_centr.transpose()));

    transform = t.matrix();

    V[li].resize(resolution*resolution, 3);
    F[li].resize((resolution-1)*(resolution-1)*2, 3);
    JtJ[li].resize(resolution);
    Jtz[li].resize(resolution);

  #pragma omp parallel for
    for (int z_step = 0; z_step < resolution; ++z_step)
    {
      for (int x_step = 0; x_step < resolution; ++x_step)
      {
        Eigen::Matrix<T, 1, 4> v; v << VecR3<T>(T(x_step)-T(resolution-1)/2.f,T(0.0), T(z_step)-T(resolution-1)/2.f),T(1.0);
        VecR3<T> pos;

        if (li == 0) // Only transform the first layer. Subsequent layers only denot a difference between the first
          pos << (v * scaling.matrix().transpose() * transform.transpose()).template head<3>();
        else
          pos << (v * scaling.matrix().transpose()).template head<3>();

        V[li].row(x_step + z_step*resolution) << pos;
      }
    }

  #pragma omp parallel for
    for (int y_step = 0; y_step < resolution-1; ++y_step)
    {
      for (int x_step = 0; x_step < resolution-1; ++x_step)
      {
        F[li].row(x_step*2 + y_step*(resolution-1)*2)     << x_step+   y_step   *resolution,x_step+1+y_step*   resolution,x_step+(y_step+1)*resolution;
        F[li].row(x_step*2 + y_step*(resolution-1)*2 + 1) << x_step+1+(y_step+1)*resolution,x_step+ (y_step+1)*resolution,x_step+1+y_step*resolution;
      }
    }

    const int factor = static_cast<int>(std::pow(2, MESH_LEVELS - li - 1));

    // Place constraints for every vertex on the highest resolution grid for all levels
    for (int i = 0; i < (max_resolution-1) * (max_resolution-1); ++i)
    {

      const int x = i % (max_resolution-1);
      const int y = i / (max_resolution-1);

      const int gx = x / factor;
      const int gy = y / factor;

      const int t = (gx + (resolution-1) * gy) * 2;

      // inside current resolution rect
      const T tx(static_cast<T>(x % factor) / static_cast<T>(factor));
      const T ty(static_cast<T>(y % factor) / static_cast<T>(factor));

      T u,v,w;
      const VecC2<T> p(tx, ty);

      if (tx + ty > T(1.))
      {
        // lower right triangle
        barycentric(p, d, c, b, u, v, w);
        JtJ[li].update_triangle(t+1, u, v);
        Jtz[li].update_triangle(t+1, u, v, 0.f);
      }
      else
      {
        // upper left triangle
        barycentric(p, a, b, c, u, v, w);
        JtJ[li].update_triangle(t, u, v);
        Jtz[li].update_triangle(t, u, v, 0.f);
      }

      // Special cases > last cell, last column, last row
      if (x + 1 >= max_resolution - 1 && y + 1 >= max_resolution - 1)
      {
        const VecC2<T> p2(T(1.), T(1.));
        barycentric(p2, d, c, b, u, v, w);
        JtJ[li].update_triangle(t+1, u, v);
        Jtz[li].update_triangle(t+1, u, v, 0.f);
      }
      if (x + 1 >= max_resolution - 1)
      {
        const VecC2<T> p2(T(1.), ty);
        barycentric(p2, d, c, b, u, v, w);
        JtJ[li].update_triangle(t+1, u, v);
        Jtz[li].update_triangle(t+1, u, v, 0.f);
      }
      if (y + 1 >= max_resolution - 1)
      {
        const VecC2<T> p2(tx, T(1.));
        barycentric(p2, d, c, b, u, v, w);
        JtJ[li].update_triangle(t+1, u, v);
        Jtz[li].update_triangle(t+1, u, v, 0.f);
      }
    }
  }
}

template <typename T>
void Mesh<T>::get_mesh(const unsigned int level, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V_out, Eigen::MatrixXi& F_out, ColorData& colordata) const
{
    const int mesh_resolution = JtJ[level].get_mesh_width();

    colordata.UV.resize(mesh_resolution*mesh_resolution, 2);
    const T tdx = T(1.0) / (mesh_resolution-1);
    for(int i = 0; i < mesh_resolution; i++)
    {
      for(int j = 0; j < mesh_resolution; j++)
      {
        const int index = j * mesh_resolution + i;
        colordata.UV(index, 0) = i*tdx;
        colordata.UV(index, 1) = j*tdx;
      }
    }

    colordata.texture.red = texture.red;
    colordata.texture.green = texture.green;
    colordata.texture.blue = texture.blue;

    get_mesh(level, V_out, F_out);
}

template <typename T>
void Mesh<T>::get_mesh(const unsigned int level, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V_out, Eigen::MatrixXi& F_out) const
{
  if (level < 1 || level > MESH_LEVELS - 1)
  {
    V_out = V[0];
    F_out = F[0];
  }
  else
  {
    upsample(level, V[0], F[0], V_out, F_out);

    for (unsigned int li = 1; li <= level; ++li)
    {
      Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> V_upsampled;
      Eigen::MatrixXi F_upsampled;
      upsample(level-li, V[li], F[li], V_upsampled, F_upsampled);

       V_out.col(1) += V_upsampled.col(1);
    }
  }
}

// Project all points onto the 2D plane, we only need to project onto XZ plane as the algorithm only runs on height fields
// Also why we only need to project only when the input point cloud changes
template <typename T>
void Mesh<T>::project_points(const int level, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& bc) const
{
  bc = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(current_target_point_cloud.rows(), 3);

  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Vl;
  Eigen::MatrixXi Fl;
  get_mesh(level, Vl, Fl);

  const int resolution = subdivided_side_length(level, MESH_RESOLUTION);

  // Upper left triangle. Used to compute the index of the triangle that is hit
  const VecC2<T> V_ul(Vl.row(0)(0), Vl.row(0)(2));
  const VecC2<T> V_00(Vl.row(Fl.row(0)(0))(0), Vl.row(Fl.row(0)(0))(2));
  const VecC2<T> V_01(Vl.row(Fl.row(0)(1))(0), Vl.row(Fl.row(0)(1))(2));
  const VecC2<T> V_10(Vl.row(Fl.row(0)(2))(0), Vl.row(Fl.row(0)(2))(2));

  const double dx = (V_01- V_00).norm();
  const double dy = (V_10 - V_00).norm();

#pragma omp parallel for
  for (int pi = 0; pi < current_target_point_cloud.rows(); ++pi)
  {
    const VecC2<T> current_point(current_target_point_cloud.row(pi)(0), current_target_point_cloud.row(pi)(2));
    const VecC2<T> offset(current_point - V_ul);

    const int c = static_cast<int>(offset(0) / dx);
    const int r = static_cast<int>(offset(1) / dy);

    const int inner_size = resolution - 1;

    // Indices are outside of mesh borders => a triangle cannot be hit
    if (c >= inner_size || r >= inner_size || c < 0 || r < 0)
    {
      bc.row(pi) << -1, T(0.0), T(0.0);
      continue;
    }
    const  VecC3<T> ul_3d = Vl.row(c + r*resolution);
    const  VecC3<T> br_3d = Vl.row(c + 1 + (r+1)*resolution);

    const VecC2<T> ul_reference(ul_3d(0), ul_3d(2));
    const VecC2<T> br_reference(br_3d(0), br_3d(2));

    const double ul_squared_dist = (ul_reference - current_point).squaredNorm();
    const double br_squared_dist = (br_reference - current_point).squaredNorm();

    VecC2<T> v_a;
    VecC2<T> v_b;
    VecC2<T> v_c;

    int f_idx = -1;

    // Find corner vertices of hit triangle
    if (ul_squared_dist <= br_squared_dist)
    {
      f_idx = 2 * c + r * 2 * inner_size;

      v_a << Vl.row(c + r*resolution)(0), Vl.row(c + r*resolution)(2);
      v_b << Vl.row(c+1 + r*resolution)(0),Vl.row(c+1 + r*resolution)(2);
      v_c << Vl.row(c + (r+1)*resolution)(0),Vl.row(c + (r+1)*resolution)(2);
    }else
    {
      f_idx = 2 * c + r * 2 * inner_size + 1;

      v_a << Vl.row(c+1 + (r+1)*resolution)(0),Vl.row(c+1 + (r+1)*resolution)(2);
      v_b << Vl.row(c + (r+1)*resolution)(0),Vl.row(c + (r+1)*resolution)(2);
      v_c << Vl.row(c+1 + r*resolution)(0),Vl.row(c+1 + r*resolution)(2);
    }

    T u,v,w;
    barycentric(current_point, v_a, v_b, v_c, u, v, w);

    bc.row(pi) << T(f_idx), u, v;
  }
}

template <typename T>
void Mesh<T>::set_target_point_cloud(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& P)
{
  current_target_point_cloud = P;

  residuals.push_back(std::vector<std::vector<T>>(MESH_LEVELS-1));

  // First fuse to base level
  Mat<T> bc;
  project_points(0, bc);
  update_JtJ(0, bc); // Update lh
  update_Jtz(0, bc, current_target_point_cloud.col(1));

  for (int li = 1; li < MESH_LEVELS; ++li)
  {
    Mat<T> V_upsampled;
    Eigen::MatrixXi F_upsampled;

    get_mesh(li, V_upsampled, F_upsampled);
    project_points(li, bc);

    update_JtJ(li, bc); // Update lh
  }
}

template <typename T>
void Mesh<T>::set_target_point_cloud(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& P, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& C)
{
  set_target_point_cloud(P);

  if (C.rows() != P.rows())
    return;

  const VecC3<T> bb_min = V[0].row(0);
  const VecC3<T> bb_max = V[0].row(V[0].rows()-1);

  const T cdx = (bb_max(0)-bb_min(0)) / static_cast<T>(TEXTURE_RESOLUTION-1);
  const T cdz = (bb_max(2)-bb_min(2)) / static_cast<T>(TEXTURE_RESOLUTION-1);

  for(int i=0; i<current_target_point_cloud.rows(); i++)
  {
    T x = current_target_point_cloud(i, 0) - bb_min(0);
    T z = current_target_point_cloud(i, 2) - bb_min(2);
    x = std::floor(x / cdx);
    z = std::floor(z / cdz);

    const int xi = static_cast<int>(x);
    const int zi = static_cast<int>(z);

    if (xi >= color_counter.rows() || zi >= color_counter.cols() || xi < 0 || zi < 0)
      continue;

    const int count = color_counter(xi, zi);

    texture.red(xi, zi)   = static_cast<unsigned char>((texture.red(xi, zi)  * count + (C(i,0)*255)) / (count + 1));
    texture.green(xi, zi) = static_cast<unsigned char>((texture.green(xi, zi) * count + (C(i,1)*255)) / (count + 1));
    texture.blue(xi, zi)  = static_cast<unsigned char>((texture.blue(xi, zi)  * count + (C(i,2)*255)) / (count + 1));
    ++color_counter(xi, zi);
  }
}

template <typename T>
void Mesh<T>::update_JtJ(const int level, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& bc)
{
  for (int i = 0; i < bc.rows(); ++i)
  {
    const VecR3<T>& row = bc.row(i);

    if (static_cast<int>(row(0)) == -1)
      continue;

    JtJ[level].update_triangle(static_cast<int>(row(0)), row(1), row(2));
  }
}

template <typename T>
void Mesh<T>::update_Jtz(const int level, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& bc, const Eigen::Matrix<T, Eigen::Dynamic, 1>& z)
{
  for (int i = 0; i < bc.rows(); ++i)
  {
    const VecR3<T>& row = bc.row(i);

    if (static_cast<int>(row(0)) == -1)
      continue;

    Jtz[level].update_triangle(static_cast<int>(row(0)), row(1), row(2), z(i));
  }
}

template <typename T>
void Mesh<T>::solve(const int iterations)
{

  // First fuse to ba se level
  Mat<T> bc;

// lh and rh for first layer already updated in set_target_point_cloud
#ifdef ENABLE_CUDA
  sor_gpu(iterations, 0, V[0].col(1));
#else
  sor_parallel(iterations, 0, V[0].col(1));
#endif

  for (int li = 1; li < MESH_LEVELS; ++li)
  {
    Mat<T> residual_height = Mat<T>::Zero(current_target_point_cloud.rows(), current_target_point_cloud.cols());

    Mat<T> V_upsampled;
    Eigen::MatrixXi F_upsampled;
    get_mesh(li, V_upsampled, F_upsampled);
    project_points(li, bc);

    // Compute residual
#pragma omp parallel for
    for (int pi = 0; pi < bc.rows(); ++pi)
    {
      if (static_cast<int>(bc.row(pi)(0)) == -1) // No hit
        continue;

      const VecC3<T> v0 = V_upsampled.row(F_upsampled.row(static_cast<int>(bc.row(pi)(0)))(0));
      const VecC3<T> v1 = V_upsampled.row(F_upsampled.row(static_cast<int>(bc.row(pi)(0)))(1));
      const VecC3<T> v2 = V_upsampled.row(F_upsampled.row(static_cast<int>(bc.row(pi)(0)))(2));

      // Ideally these should be equal, then there is no error
      const VecC3<T> solved_point = bc.row(pi)(1) * v0 + bc.row(pi)(2) * v1 + (1.f - bc.row(pi)(1) - bc.row(pi)(2)) * v2;
      const VecC3<T> measured_point = current_target_point_cloud.row(pi);

      residual_height.row(pi) = measured_point - solved_point;
    }

    residuals.back()[li-1].push_back(residual_height.array().sum());

    // Update equation rh with residual
    update_Jtz(li, bc, residual_height.col(1));

#ifdef ENABLE_CUDA
      sor_gpu(iterations, li, V[li].col(1));
#else
      sor_parallel(iterations, li, V[li].col(1));
#endif
  }
}

// Basic solve without any parallelism
template <typename T>
void Mesh<T>::sor(const int iterations, const int level, Eigen::Ref<Eigen::Matrix<T, Eigen::Dynamic, 1>> h) const
{
  const auto& Jtz_vec = Jtz[level];
  const auto& Jtj_mat = JtJ[level];

  // For each iteration, solve system for each vertex
  for(int it = 0; it < iterations; it++)
  {
    for (int vi = 0; vi < h.rows(); vi++)
      sor_inner(vi, Jtj_mat, Jtz_vec, h.data());
  }
}

/*
 Solve system for a single vertex vi in the mesh
 vi is the vertex index,
 JtJ is the MatrixGrid, matrix on the lhs
 Jtz is the vector on the rhs
 h is an array of the vertex heights
*/
template <typename T>
CUDA_HOST_DEVICE inline void sor_inner(const int vi, const JtJMatrixGrid<T>& JtJ, const JtzVector<T>& Jtz_vec, T* h)
{
  T xn = Jtz_vec.get(vi);
  T acc = 0;

  T vals[6];
  int ids[6];

  T a;
  JtJ.get_matrix_values_for_vertex(vi, vals, ids, a);

  for (int j = 0; j < 6; ++j)
    acc += vals[j] * h[ids[j]];

  xn -= acc;

  // Weighting of previous height vs newly solved height
  // only use new height if w = 1.
  const T w = 1.0;
  h[vi] = (1.f-w) * h[vi] + w*xn/a;
}

// Run solve in parallel by using four-color reordering
template <typename T>
void Mesh<T>::sor_parallel(const int iterations, const int level, Eigen::Ref<Eigen::Matrix<T, Eigen::Dynamic, 1>> h) const
{
  const auto& Jtz_vec = Jtz[level];
  const auto& Jtj_mat = JtJ[level];
  const int resolution = JtJ[level].get_mesh_width();

  for(int it = 0; it < iterations; it++)
  {
#pragma omp parallel for collapse(2)
    for (int x = 0; x < resolution; x+=2)
      for (int y = 0; y < resolution; y+=2)
      {
        const int vi = x + y * resolution;
        sor_inner(vi, Jtj_mat, Jtz_vec, h.data());
      }

#pragma omp parallel for collapse(2)
    for (int x = 1; x < resolution; x+=2)
      for (int y = 0; y < resolution; y+=2)
      {
        const int vi = x + y * resolution;
        sor_inner(vi, Jtj_mat, Jtz_vec, h.data());
      }

#pragma omp parallel for collapse(2)
    for (int x = 0; x < resolution; x+=2)
      for (int y = 1; y < resolution; y+=2)
      {
        const int vi = x + y * resolution;
        sor_inner(vi, Jtj_mat, Jtz_vec, h.data());
      }

#pragma omp parallel for collapse(2)
    for (int x = 1; x < resolution; x+=2)
      for (int y = 1; y < resolution; y+=2)
      {
        const int vi = x + y * resolution;
        sor_inner(vi, Jtj_mat, Jtz_vec, h.data());
      }
  }
}

// Define CUDA specific functions here
#ifdef ENABLE_CUDA

// This function solves a single iteration for a point based on the index
// xoffset and yoffset define the offset of the four-color reordering
__global__ void solve_kernel(const int xoffset, const int yoffset, const int mesh_width, const JtzVector<float> Jtz_vec, const JtJMatrixGrid<float> JtJ_mat, float* h) {
	const int idx = blockIdx.x * blockDim.x + threadIdx.x;

    if (idx >= mesh_width*mesh_width)
    {
	    return;
    }

	const int x = ((idx) % mesh_width);
	const int y = ((idx) / mesh_width);
	const int vi = x + y * mesh_width;

  // Make sure that indices is okay for our offsets
	if ((x % 2 == xoffset) && (y % 2 == yoffset))
		sor_inner(vi, JtJ_mat, Jtz_vec, h);
}

// GPU solve for non-float meshes, does not work
template <typename T>
void Mesh<T>::sor_gpu(const int, const int, Eigen::Ref<Eigen::Matrix<T, Eigen::Dynamic, 1>>)
{
    assert(false && "GPU solver works only with float meshes");
}

/*
 Function for solving the system on the GPU using CUDA
 Similar to the parallel solve this method uses four-color reordering
 and therefore there are four solve_kernel calls,
 GPU solver also only works with float meshes
*/
template <>
void Mesh<float>::sor_gpu(const int iterations, const int level, Eigen::Ref<Eigen::Matrix<float, Eigen::Dynamic, 1>> h)
{
    const JtJMatrixGrid<float> JtJ_mat = JtJ[level];
    const JtzVector<float> Jtz_vec = Jtz[level];
    const int mesh_width = JtJ[level].get_mesh_width();
    const int mesh_width_squared = mesh_width*mesh_width;
    const int mat_width = JtJ[level].get_matrix_width();
    //const int mat_width_squared = mat_width*mat_width;

    float *devH;
    CUDA_CHECK(cudaMalloc(&devH, h.rows() * sizeof(float)));
    CUDA_CHECK(cudaMemcpy(devH, h.data(), h.rows() * sizeof(float), cudaMemcpyHostToDevice));

    dim3 block(64);
    dim3 grid((mesh_width_squared + block.x - 1) / block.x);

    for (int it = 0; it < iterations; it++) {
	    solve_kernel<<<grid, block>>>(0, 0, mesh_width, Jtz_vec, JtJ_mat, devH);
	    cudaDeviceSynchronize();
	    solve_kernel<<<grid, block>>>(1, 0, mesh_width, Jtz_vec, JtJ_mat, devH);
	    cudaDeviceSynchronize();
	    solve_kernel<<<grid, block>>>(0, 1, mesh_width, Jtz_vec, JtJ_mat, devH);
	    cudaDeviceSynchronize();
	    solve_kernel<<<grid, block>>>(1, 1, mesh_width, Jtz_vec, JtJ_mat, devH);
	    cudaDeviceSynchronize();
    }

    CUDA_CHECK(cudaMemcpy(h.data(), devH, h.rows() * sizeof(float), cudaMemcpyDeviceToHost));
    CUDA_CHECK(cudaFree(devH));
}
#endif


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2015.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Stephan Aiche $
// $Authors: Stephan Aiche $
// --------------------------------------------------------------------------

#include <OpenMS/SIMULATION/EGHFitter1D.h>
#include <OpenMS/TRANSFORMATIONS/FEATUREFINDER/InterpolationModel.h>
#include <OpenMS/MATH/STATISTICS/StatisticFunctions.h>
#include <OpenMS/CONCEPT/Constants.h>
#include <OpenMS/CONCEPT/LogStream.h>
#include <OpenMS/CONCEPT/Factory.h>

#include <boost/math/special_functions/fpclassify.hpp>

#define DEBUG_EGHFITTER

namespace OpenMS
{
  int EGHFitter1D::EGHFitterFunctor::operator()(const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
  {
    Size n = m_data->n;
    RawDataArrayType set = m_data->set;

    CoordinateType H  = x(0);
    CoordinateType tR = x(1);
    CoordinateType sigma_square = x(2);
    CoordinateType tau = x(3);

    CoordinateType t_diff, t_diff2, denominator = 0.0;

    CoordinateType fegh = 0.0;

    // iterate over all points of the signal
    for (Size i = 0; i < n; i++)
    {
      double t = set[i].getPos();

      t_diff = t - tR;
      t_diff2 = t_diff * t_diff; // -> (t - t_R)^2

      denominator = 2 * sigma_square + tau * t_diff; // -> 2\sigma_{g}^{2} + \tau \left(t - t_R\right)

      if (denominator > 0.0)
      {
        fegh = H * exp(-t_diff2 / denominator);
      }
      else
      {
        fegh = 0.0;
      }

      fvec(i) = (fegh - set[i].getIntensity());
    }
    return 0;
  }

  // compute Jacobian matrix for the different parameters
  int EGHFitter1D::EGHFitterFunctor::df(const Eigen::VectorXd& x, Eigen::MatrixXd& J)
  {
    Size n =  m_data->n;
    RawDataArrayType set = m_data->set;

    CoordinateType H  = x(0);
    CoordinateType tR = x(1);
    CoordinateType sigma_square = x(2);
    CoordinateType tau = x(3);

    CoordinateType derivative_H, derivative_tR, derivative_sigma_square, derivative_tau = 0.0;
    CoordinateType t_diff, t_diff2, exp1, denominator = 0.0;

    // iterate over all points of the signal
    for (Size i = 0; i < n; i++)
    {
      CoordinateType t = set[i].getPos();

      t_diff = t - tR;
      t_diff2 = t_diff * t_diff; // -> (t - t_R)^2

      denominator = 2 * sigma_square + tau * t_diff; // -> 2\sigma_{g}^{2} + \tau \left(t - t_R\right)

      if (denominator > 0)
      {
        exp1 = exp(-t_diff2 / denominator);

        // \partial H f_{egh}(t) = \exp\left( \frac{-\left(t-t_R \right)}{2\sigma_{g}^{2} + \tau \left(t - t_R\right)} \right)
        derivative_H = exp1;

        // \partial t_R f_{egh}(t) &=& H \exp \left( \frac{-\left(t-t_R \right)}{2\sigma_{g}^{2} + \tau \left(t - t_R\right)} \right) \left( \frac{\left( 4 \sigma_{g}^{2} + \tau \left(t-t_R \right) \right) \left(t-t_R \right)}{\left( 2\sigma_{g}^{2} + \tau \left(t - t_R\right) \right)^2} \right)
        derivative_tR = H * exp1 * (((4 * sigma_square + tau * t_diff) * t_diff) / (denominator * denominator));

        // \partial \sigma_{g}^{2} f_{egh}(t) &=& H \exp \left( \frac{-\left(t-t_R \right)^2}{2\sigma_{g}^{2} + \tau \left(t - t_R\right)} \right) \left( \frac{ 2 \left(t - t_R\right)^2}{\left( 2\sigma_{g}^{2} + \tau \left(t - t_R\right) \right)^2} \right)
        derivative_sigma_square = H * exp1 * ((2 * t_diff2) / (denominator * denominator));

        // \partial \tau f_{egh}(t) &=& H \exp \left( \frac{-\left(t-t_R \right)^2}{2\sigma_{g}^{2} + \tau \left(t - t_R\right)} \right) \left( \frac{ \left(t - t_R\right)^3}{\left( 2\sigma_{g}^{2} + \tau \left(t - t_R\right) \right)^2} \right)
        derivative_tau = H * exp1 * ((t_diff * t_diff2) / (denominator * denominator));
      }
      else
      {
        derivative_H = 0.0;
        derivative_tR = 0.0;
        derivative_sigma_square = 0.0;
        derivative_tau = 0.0;
      }

      // set the jacobian matrix
      J(i, 0) = derivative_H;
      J(i, 1) = derivative_tR;
      J(i, 2) = derivative_sigma_square;
      J(i, 3) = derivative_tau;
    }
    return 0;
  }

  EGHFitter1D::EGHFitter1D() :
    LevMarqFitter1D()
  {
    setName(getProductName());
    defaults_.setValue("statistics:variance", 1.0, "Variance of the model.", ListUtils::create<String>("advanced"));
    defaultsToParam_();
  }

  EGHFitter1D::EGHFitter1D(const EGHFitter1D& source) :
    LevMarqFitter1D(source)
  {
    setParameters(source.getParameters());
    updateMembers_();
  }

  EGHFitter1D::~EGHFitter1D()
  {
  }

  EGHFitter1D& EGHFitter1D::operator=(const EGHFitter1D& source)
  {
    if (&source == this)
      return *this;

    LevMarqFitter1D::operator=(source);
    setParameters(source.getParameters());
    updateMembers_();

    return *this;
  }

  EGHFitter1D::QualityType EGHFitter1D::fit1d(const RawDataArrayType& set, InterpolationModel*& model)
  {
    // Calculate bounding box
    CoordinateType min_bb = set[0].getPos(), max_bb = set[0].getPos();
    for (Size pos = 1; pos < set.size(); ++pos)
    {
      CoordinateType tmp = set[pos].getPos();
      if (min_bb > tmp)
        min_bb = tmp;
      if (max_bb < tmp)
        max_bb = tmp;
    }

    // Enlarge the bounding box by a few multiples of the standard deviation

    const CoordinateType stdev = sqrt(statistics_.variance()) * tolerance_stdev_box_;
    min_bb -= stdev;
    max_bb += stdev;

    // Set advanced parameters for residual_  und jacobian_ method
    EGHFitter1D::Data d;
    d.n = set.size();
    d.set = set;

    // Compute start parameters
    setInitialParameters_(set);

    Eigen::VectorXd x_init(4);
    x_init(0) = height_;
    x_init(1) = retention_;
    x_init(2) = sigma_square_;
    x_init(3) = tau_;

    EGHFitterFunctor functor(4, &d);
    optimize_(x_init, functor);

    // Set optimized parameters
    height_ = x_init[0];
    retention_ = x_init[1];
    sigma_square_ = x_init[2];
    tau_ = x_init[3];

#ifdef DEBUG_EGHFITTER
    LOG_DEBUG << "Fitter returned \n";
    LOG_DEBUG << "height:       " << height_ << "\n";
    LOG_DEBUG << "retention:    " << retention_ << "\n";
    LOG_DEBUG << "sigma_square: " << sigma_square_ << "\n";
    LOG_DEBUG << "tau:          " << tau_ << std::endl;
#endif

    // build model
    model = static_cast<InterpolationModel*>(Factory<BaseModel<1> >::create("EGHModel"));
    model->setInterpolationStep(interpolation_step_);

    Param tmp;
    tmp.setValue("statistics:variance", statistics_.variance());
    tmp.setValue("statistics:mean", statistics_.mean());

    tmp.setValue("bounding_box:compute", "false"); // disable auto computation of bounding box
    tmp.setValue("bounding_box:min", min_bb);
    tmp.setValue("bounding_box:max", max_bb);

    tmp.setValue("egh:height", height_);
    tmp.setValue("egh:retention", retention_);

    tmp.setValue("egh:guess_parameter", "false"); // disable guessing of parameters from A/B
    tmp.setValue("egh:tau", tau_);
    tmp.setValue("egh:sigma_square", sigma_square_);

    model->setParameters(tmp);

    // calculate pearson correlation
    std::vector<float> real_data;
    real_data.reserve(set.size());
    std::vector<float> model_data;
    model_data.reserve(set.size());

    for (Size i = 0; i < set.size(); ++i)
    {
      real_data.push_back(set[i].getIntensity());
      model_data.push_back(model->getIntensity(DPosition<1>(set[i].getPosition())));
    }

    QualityType correlation = Math::pearsonCorrelationCoefficient(real_data.begin(), real_data.end(), model_data.begin(), model_data.end());
    if (boost::math::isnan(correlation))
      correlation = -1.0;

    return correlation;
  }

  void EGHFitter1D::setInitialParameters_(const RawDataArrayType& set)
  {
    // sum over all intensities
    CoordinateType sum = 0.0;
    for (Size i = 0; i < set.size(); ++i)
      sum += set[i].getIntensity();

    // calculate the median
    //Size median = 0;
    //float count = 0.0;
    Size apex_rt = 0;
    CoordinateType apex = 0.0;
    for (Size i = 0; i < set.size(); ++i)
    {
      //count += set[i].getIntensity();
      //if ( count <= sum / 2 ) median = i;

      if (set[i].getIntensity() > apex)
      {
        apex = set[i].getIntensity();
        apex_rt = i;
      }

    }

    // calculate the height of the peak
    height_ = set[apex_rt].getIntensity();

    // calculate retention time
    retention_ = set[apex_rt].getPos();

    // guess A / B for alpha = 0.5 -> left/right half max distance

    Size i = apex_rt;
    while (i > 0)
    {
      if (set[i].getIntensity() / height_ < 0.5)
        break;
      else
        --i;
    }
    CoordinateType A = retention_ - set[i + 1].getPos();

    i = apex_rt;
    while (i < set.size())
    {
      if (set[i].getIntensity() / height_ < 0.5)
        break;
      else
        ++i;
    }
    CoordinateType B = set[i - 1].getPos() - retention_;

    // compute estimates for tau / sigma_square based on A/B
    CoordinateType log_alpha = log(0.5);

    tau_ = (-1 / log_alpha) * (B - A);
    sigma_square_ = (-1 / (2 * log_alpha)) * (B * A);

#ifdef DEBUG_EGHFITTER
    LOG_DEBUG << "Initial parameters\n";
    LOG_DEBUG << "height:       " << height_ << "\n";
    LOG_DEBUG << "retention:    " << retention_ << "\n";
    LOG_DEBUG << "A:            " << A << "\n";
    LOG_DEBUG << "B:            " << B << "\n";
    LOG_DEBUG << "sigma_square: " << sigma_square_ << "\n";
    LOG_DEBUG << "tau:          " << tau_ << std::endl;
#endif
  }

  void EGHFitter1D::updateMembers_()
  {
    LevMarqFitter1D::updateMembers_();
    statistics_.setVariance(param_.getValue("statistics:variance"));
  }

}