#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Three.h>
#include <QApplication>
#include <QObject>
#include <QAction>
#include <QMainWindow>
#include <QInputDialog>
#include <QColorDialog>
#include <QPalette>
#include <QColor>
#include <QStyleFactory>
#include <QMessageBox>

#include <CGAL/boost/graph/helpers.h>
#include <CGAL/Polygon_mesh_processing/compute_normal.h>
#include <CGAL/Heat_method_3/Surface_mesh_geodesic_distances_3.h>

#include "Scene_points_with_normal_item.h"

#include "Messages_interface.h"
#include "Scene_surface_mesh_item.h"
#include "Color_ramp.h"
#include <boost/unordered_map.hpp>
#include "ui_Display_property.h"
#include "id_printing.h"
#include "Scene.h"
#include "triangulate_primitive.h"
#include <CGAL/Buffer_for_vao.h>
#include <CGAL/Three/Triangle_container.h>
#include <CGAL/Dynamic_property_map.h>

#define ARBITRARY_DBL_MIN 1.0E-30
#define ARBITRARY_DBL_MAX 1.0E+30

//Item for heat values
typedef CGAL::Three::Triangle_container Tri;
typedef CGAL::Three::Viewer_interface VI;

class Scene_heat_item
    : public CGAL::Three::Scene_item_rendering_helper
{
  Q_OBJECT

public:
  Scene_heat_item(Scene_surface_mesh_item* item)
    :sm(item->face_graph()), parent(item)
  {
    setTriangleContainer(0, new Triangle_container(VI::PROGRAM_HEAT_INTENSITY,
                                                   true));
    setRenderingMode(Gouraud);
  }
  Scene_item* clone() const Q_DECL_OVERRIDE {return nullptr;}
  QString toolTip() const Q_DECL_OVERRIDE{return QString(); }
  void select(double orig_x,
             double orig_y,
             double orig_z,
             double dir_x,
             double dir_y,
             double dir_z) Q_DECL_OVERRIDE
  {
    parent->select( orig_x, orig_y, orig_z,
                    dir_x, dir_y, dir_z);
  }

  void initializeBuffers(CGAL::Three::Viewer_interface *viewer) const Q_DECL_OVERRIDE
  {
    getTriangleContainer(0)->initializeBuffers(viewer);
    getTriangleContainer(0)->setIdxSize(nb_idx);
    verts.resize(0);
    normals .resize(0);
    colors.resize(0);
    idx.clear();
    idx.shrink_to_fit();
    colors.shrink_to_fit();
    verts.shrink_to_fit();
    normals.shrink_to_fit();
  }

  void draw(CGAL::Three::Viewer_interface *viewer) const Q_DECL_OVERRIDE
  {
    if(!visible())
      return;
    if(!isInit(viewer))
      initGL(viewer);
    if ( getBuffersFilled() &&
         ! getBuffersInit(viewer))
    {
      initializeBuffers(viewer);
      setBuffersInit(viewer, true);
    }
    if(!getBuffersFilled())
    {
      computeElements();
      initializeBuffers(viewer);
    }

    getTriangleContainer(0)->setAlpha(1.0f);
    getTriangleContainer(0)->draw(viewer, false);
  }
  void compute_bbox() const Q_DECL_OVERRIDE
  {
    SMesh::Property_map<vertex_descriptor, Point_3> pprop = sm->points();
    CGAL::Bbox_3 bbox ;

    for(vertex_descriptor vd :vertices(*sm))
    {
      bbox = bbox + pprop[vd].bbox();
    }
    _bbox = Bbox(bbox.xmin(),bbox.ymin(),bbox.zmin(),
                 bbox.xmax(),bbox.ymax(),bbox.zmax());
    is_bbox_computed = true;
  }
  Scene_item::Bbox bbox() const Q_DECL_OVERRIDE {
    if(!is_bbox_computed)
      compute_bbox();
    is_bbox_computed = true;
    return _bbox;
  }

  ~Scene_heat_item(){}
  virtual bool supportsRenderingMode(RenderingMode m) const Q_DECL_OVERRIDE { return m==Gouraud; }
  virtual void invalidateOpenGLBuffers() Q_DECL_OVERRIDE
  {

    setBuffersFilled(false);
    compute_bbox();
    getTriangleContainer(0)->reset_vbos(NOT_INSTANCED);
    is_bbox_computed = false;
  }
  void triangulate_convex_facet(face_descriptor fd,
                                boost::property_map< SMesh, boost::vertex_index_t >::type *im) const
  {
    const CGAL::qglviewer::Vec v_offset = static_cast<CGAL::Three::Viewer_interface*>(CGAL::QGLViewer::QGLViewerPool().first())->offset();
    EPICK::Vector_3 offset = EPICK::Vector_3(v_offset.x, v_offset.y, v_offset.z);

    EPICK::Point_3 p0,p1,p2;
    SMesh::Halfedge_around_face_circulator he(halfedge(fd, *sm), *sm);
    SMesh::Halfedge_around_face_circulator he_end = he;

    while(next(*he, *sm) != prev(*he_end, *sm))
    {
      ++he;
      vertex_descriptor v0(target(*he_end, *sm)),
          v1(target(*he, *sm)),
          v2(target(next(*he, *sm), *sm));
      p0 = sm->point(v0) + offset;
      p1 = sm->point(v1) + offset;
      p2 = sm->point(v2) + offset;
      idx.push_back((*im)[v0]);
      idx.push_back((*im)[v1]);
      idx.push_back((*im)[v2]);
    }
  }
  void triangulate_facet(face_descriptor fd,
                         SMesh::Property_map<face_descriptor, EPICK::Vector_3> *fnormals,
                         boost::property_map< SMesh, boost::vertex_index_t >::type *im) const
  {
    //Computes the normal of the facet
    EPICK::Vector_3 normal = get(*fnormals, fd);

    //check if normal contains NaN values
    if (normal.x() != normal.x() || normal.y() != normal.y() || normal.z() != normal.z())
    {
      qDebug()<<"Warning : normal is not valid. Facet not displayed";
      return;
    }

    typedef FacetTriangulator<SMesh, EPICK, boost::graph_traits<SMesh>::vertex_descriptor> FT;
    const CGAL::qglviewer::Vec off = static_cast<CGAL::Three::Viewer_interface*>(CGAL::QGLViewer::QGLViewerPool().first())->offset();
    EPICK::Vector_3 offset(off.x,off.y,off.z);
    FT triangulation(fd,normal,sm, offset);
    //iterates on the internal faces
    for(FT::CDT::Finite_faces_iterator
        ffit = triangulation.cdt->finite_faces_begin(),
        end = triangulation.cdt->finite_faces_end();
        ffit != end; ++ffit)
    {
      if(ffit->info().is_external)
        continue;
      //add the vertices to the positions
      //adds the vertices, normals and colors to the appropriate vectors
      //adds the indices to the appropriate vector
      idx.push_back((*im)[triangulation.v2v[ffit->vertex(0)]]);
      idx.push_back((*im)[triangulation.v2v[ffit->vertex(1)]]);
      idx.push_back((*im)[triangulation.v2v[ffit->vertex(2)]]);
    }
  }

  void computeElements() const Q_DECL_OVERRIDE
  {
    typedef EPICK::Point_3 Point;
    QApplication::setOverrideCursor(Qt::WaitCursor);
    const CGAL::qglviewer::Vec o = static_cast<CGAL::Three::Viewer_interface*>(CGAL::QGLViewer::QGLViewerPool().first())->offset();
    EPICK::Vector_3 offset(o.x, o.y, o.z);
    SMesh::Property_map<vertex_descriptor, SMesh::Point> positions =
        sm->points();
    SMesh::Property_map<vertex_descriptor, EPICK::Vector_3 > vnormals =
        sm->property_map<vertex_descriptor, EPICK::Vector_3 >("v:normal").first;
    SMesh::Property_map<face_descriptor, EPICK::Vector_3 > fnormals =
        sm->property_map<face_descriptor, EPICK::Vector_3 >("f:normal").first;
    typedef boost::graph_traits<SMesh>::face_descriptor face_descriptor;
    typedef boost::graph_traits<SMesh>::halfedge_descriptor halfedge_descriptor;
    typedef boost::graph_traits<SMesh>::vertex_descriptor vertex_descriptor;
    SMesh::Property_map<vertex_descriptor, CGAL::Color> vcolors =
        sm->property_map<vertex_descriptor, CGAL::Color >("v:color").first;
    SMesh::Property_map<vertex_descriptor, float> vdist=
        sm->property_map<vertex_descriptor, float >("v:dist").first;
    typedef CGAL::Buffer_for_vao<float, unsigned int> CPF;
    verts.clear();
    normals.clear();
    idx.clear();
    colors.clear();
    boost::property_map< SMesh, boost::vertex_index_t >::type
        im = get(boost::vertex_index, *sm);

    idx.reserve(num_faces(*sm) * 3);
    for(face_descriptor fd : faces(*sm))
    {
      if(is_triangle(halfedge(fd,*sm),*sm))
      {
        for(halfedge_descriptor hd : halfedges_around_face(halfedge(fd, *sm),*sm))
        {
          idx.push_back(source(hd, *sm));
        }
      }
      else
      {
        std::vector<Point> facet_points;
        for(halfedge_descriptor hd : halfedges_around_face(halfedge(fd, *sm),*sm))
        {
          facet_points.push_back(positions[target(hd, *sm)]);
        }
        bool is_convex = CPF::is_facet_convex(facet_points, fnormals[fd]);

        if(is_convex && is_quad(halfedge(fd,*sm),*sm) )
        {
          halfedge_descriptor hd = halfedge(fd,*sm);
          //1st half
          idx.push_back(source(hd, *sm));
          idx.push_back(source(next(hd, *sm), *sm));
          idx.push_back(source(next(next(hd, *sm), *sm), *sm));

          //2nd half
          idx.push_back(source(hd, *sm));
          idx.push_back(source(next(next(hd, *sm), *sm), *sm));
          idx.push_back(source(prev(hd, *sm), *sm));
        }
        else if(is_convex)
        {
          triangulate_convex_facet(fd, &im);
        }
        else
        {
          triangulate_facet(fd, &fnormals, &im);
        }
      }
    }
    for(vertex_descriptor vd : vertices(*sm))
    {
      CGAL::Color c = vcolors[vd];
      colors.push_back((float)c.red()/255);
      colors.push_back((float)c.green()/255);
      colors.push_back((float)c.blue()/255);

      Point p = positions[vd] + offset;
      CPF::add_point_in_buffer(p, verts);
      EPICK::Vector_3 n = vnormals[vd];
      CPF::add_normal_in_buffer(n, normals);
      heat_values.push_back(vdist[vd]);
    }
    nb_idx = idx.size();
    getTriangleContainer(0)->allocate(Tri::Vertex_indices, idx.data(),
                                      static_cast<int>(idx.size()*sizeof(unsigned int)));
    getTriangleContainer(0)->allocate(Tri::Smooth_vertices, verts.data(),
                                      static_cast<int>(num_vertices(*sm)*3*sizeof(float)));

    getTriangleContainer(0)->allocate(Tri::Smooth_normals, normals.data(),
                                      static_cast<int>(num_vertices(*sm)*3*sizeof(float)));
    getTriangleContainer(0)->allocate(Tri::VColors, colors.data(),
                                      static_cast<int>(colors.size()*sizeof(float)));
    getTriangleContainer(0)->allocate(Tri::Distances, heat_values.data(),
                                      static_cast<int>(heat_values.size()*sizeof(float)));
    compute_bbox();
    setBuffersFilled(true);
     QApplication::restoreOverrideCursor();
  }

  bool isEmpty() const Q_DECL_OVERRIDE {return false;}
  SMesh *face_graph() { return sm;}
  Scene_surface_mesh_item* getParent() { return parent; }

private:
  SMesh* sm;
  Scene_surface_mesh_item* parent;
  mutable std::vector<float> normals;
  mutable std::vector<unsigned int> idx;
  mutable std::vector<float> verts;
  mutable std::vector<float> colors;
  mutable std::vector<float> heat_values;
  mutable std::size_t nb_idx;
}; // end class Scene_heat_item

class DockWidget :
    public QDockWidget,
    public Ui::DisplayPropertyWidget
{
public:
  DockWidget(QString name, QWidget *parent)
    :QDockWidget(name,parent)
  {
    setupUi(this);
  }
};

typedef boost::graph_traits<SMesh>::halfedge_descriptor halfedge_descriptor;
typedef boost::graph_traits<SMesh>::face_descriptor face_descriptor;
CGAL::Three::Viewer_interface* (&getActiveViewer)() = CGAL::Three::Three::activeViewer;
class DisplayPropertyPlugin :
    public QObject,
    public CGAL::Three::Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
  typedef SMesh::Property_map<boost::graph_traits<SMesh>::vertex_descriptor, double> Vertex_distance_map;
  typedef CGAL::Heat_method_3::Surface_mesh_geodesic_distances_3<SMesh> Heat_method;
  typedef CGAL::Heat_method_3::Surface_mesh_geodesic_distances_3<SMesh, CGAL::Heat_method_3::Intrinsic_Delaunay> Heat_method_idt;
  typedef CGAL::dynamic_vertex_property_t<bool>                        Vertex_source_tag;
  typedef boost::property_map<SMesh, Vertex_source_tag>::type Vertex_source_map;

public:

  bool applicable(QAction*) const Q_DECL_OVERRIDE
  {
    CGAL::Three::Scene_item* item = scene->item(scene->mainSelectionIndex());
    return qobject_cast<Scene_surface_mesh_item*>(item);
  }

  QList<QAction*> actions() const Q_DECL_OVERRIDE
  {
    return _actions;
  }

  QColor textColor(const QColor& color)
  {
    QColor text_color (255, 255, 255);
    if (color.red() * 0.299 + color.green() * 0.587 + color.blue() * 0.114 > 128)
      text_color = QColor (0, 0, 0);
    return text_color;
  }

  void init(QMainWindow* mw, CGAL::Three::Scene_interface* sc, Messages_interface*) Q_DECL_OVERRIDE
  {
    this->scene = sc;
    this->mw = mw;
    this->current_item = NULL;

    QAction *actionDisplayAngles= new QAction(QString("Display Properties"), mw);
    QAction *actionHeatMethod= new QAction(QString("Heat Method"), mw);
    actionHeatMethod->setProperty("submenuName", "Color");

    rm = 1.0;
    rM = 0.0;
    gm = 0.0;
    gM = 1.0;
    bm = 0.0;
    bM = 0.0;
    actionDisplayAngles->setProperty("submenuName", "Color");

    if(actionDisplayAngles) {
      connect(actionDisplayAngles, SIGNAL(triggered()),
              this, SLOT(openDialog()));
      if(actionHeatMethod)
      {
        connect(actionHeatMethod, &QAction::triggered,
                this, [this](){
          this->dock_widget->propertyBox->setCurrentIndex(2);
          this->dock_widget->show();
        });
      }
      _actions << actionDisplayAngles;
      _actions << actionHeatMethod;

    }
    dock_widget = new DockWidget("Property Displaying", mw);
    dock_widget->setVisible(false);
    addDockWidget(dock_widget);
    QPalette palette(Qt::red);
    dock_widget->minColorButton->setPalette(palette);
    dock_widget->minColorButton->setStyle(QStyleFactory::create("Fusion"));
    dock_widget->minColorButton->update();

    palette = QPalette(Qt::green);
    dock_widget->maxColorButton->setPalette(palette);
    dock_widget->maxColorButton->setStyle(QStyleFactory::create("Fusion"));
    dock_widget->maxColorButton->update();
    connect(dock_widget->colorizeButton, SIGNAL(clicked(bool)),
            this, SLOT(colorize()));

    connect(dock_widget->propertyBox, SIGNAL(currentIndexChanged(int)),
            this, SLOT(on_propertyBox_currentIndexChanged(int)));
    connect(dock_widget->zoomToMinButton, &QPushButton::pressed,
            this, &DisplayPropertyPlugin::on_zoomToMinButton_pressed);
    connect(dock_widget->zoomToMaxButton, &QPushButton::pressed,
            this, &DisplayPropertyPlugin::on_zoomToMaxButton_pressed);
    connect(dock_widget->minColorButton, &QPushButton::pressed,
            this, [this]()
    {
      QColor minColor = QColorDialog::getColor();
      if (!minColor.isValid())
      {
        return;
      }

      rm = minColor.redF();
      gm = minColor.greenF();
      bm = minColor.blueF();
      QPalette palette(minColor);
      dock_widget->minColorButton->setPalette(palette);
      dock_widget->minColorButton->update();
      replaceRamp();
    });
    connect(dock_widget->maxColorButton, &QPushButton::pressed,
            this, [this]()
    {
      QColor maxColor = QColorDialog::getColor();
      if(!maxColor.isValid())
        return;
      QPalette palette(maxColor);
      rM = maxColor.redF();
      gM = maxColor.greenF();
      bM = maxColor.blueF();

      dock_widget->maxColorButton->setPalette(palette);
      dock_widget->maxColorButton->update();
      replaceRamp();
    });

    connect(dock_widget->sourcePointsButton, SIGNAL(toggled(bool)),
            this, SLOT(on_sourcePointsButton_toggled(bool)));
    connect(dock_widget->deleteButton, &QPushButton::clicked,
            this, &DisplayPropertyPlugin::delete_group);

    connect(dock_widget->resetButton, &QPushButton::pressed,
            this, &DisplayPropertyPlugin::resetRampExtremas);

    dock_widget->zoomToMaxButton->setEnabled(false);
    dock_widget->zoomToMinButton->setEnabled(false);
    Scene* scene_obj =static_cast<Scene*>(scene);
    connect(scene_obj, &Scene::itemIndexSelected,
            this, &DisplayPropertyPlugin::enableButtons);
    on_propertyBox_currentIndexChanged(0);

  }
private Q_SLOTS:
  void openDialog()
  {
    if(dock_widget->isVisible()) { dock_widget->hide(); }
    else{
      replaceRamp();
      dock_widget->show();
      dock_widget->raise(); }
  }

  void resetRampExtremas()
  {
    Scene_surface_mesh_item* item =
        qobject_cast<Scene_surface_mesh_item*>(scene->item(scene->mainSelectionIndex()));
    if(!item)
      return;
    QApplication::setOverrideCursor(Qt::WaitCursor);
    item->face_graph()->collect_garbage();
    bool ok;
    switch(dock_widget->propertyBox->currentIndex())
    {
    case 0:
      ok = resetAngles(item);
      break;
    default:
      ok = resetScaledJacobian(item);
      break;
    }
    QApplication::restoreOverrideCursor();
    if(!ok)
      QMessageBox::warning(mw, "Error", "You must first run colorize once to initialize the values.");
  }

  void colorize()
  {
    Scene_heat_item* h_item = nullptr;
    Scene_surface_mesh_item* item =
        qobject_cast<Scene_surface_mesh_item*>(scene->item(scene->mainSelectionIndex()));
    if(!item)
    {
      h_item = qobject_cast<Scene_heat_item*>(scene->item(scene->mainSelectionIndex()));
      if(!h_item)
        return;
      item = h_item->getParent();
    }
    QApplication::setOverrideCursor(Qt::WaitCursor);

    replaceRamp();
    item->face_graph()->collect_garbage();

    switch(dock_widget->propertyBox->currentIndex()){
    case 0:
      displayAngles(item);
      break;
      case 1:
        displayScaledJacobian(item);
        break;
    case 2:
      if(!displayHeatIntensity(item))
        return;
      item->setRenderingMode(Gouraud);
      break;
    default:  // Heat Method (Intrinsic Delaunay)
      if(!displayHeatIntensity(item, true))
        return;
      item->setRenderingMode(Gouraud);
      break;
    }

    connect(item, &Scene_surface_mesh_item::itemChanged,
            this, [item](){
      bool does_exist;
      SMesh::Property_map<face_descriptor, double> pmap;
      boost::tie(pmap, does_exist) =
          item->face_graph()->property_map<face_descriptor,double>("f:jacobian");
      if(does_exist)
        item->face_graph()->remove_property_map(pmap);
      boost::tie(pmap, does_exist) =
          item->face_graph()->property_map<face_descriptor,double>("f:angle");
      if(does_exist)
        item->face_graph()->remove_property_map(pmap);
    });
    QApplication::restoreOverrideCursor();
    item->invalidateOpenGLBuffers();
    item->redraw();
    if(dock_widget->propertyBox->currentIndex() != 2){
      dock_widget->zoomToMinButton->setEnabled(true);
      dock_widget->zoomToMaxButton->setEnabled(true);}
  }

  void enableButtons()
  {
    Scene_surface_mesh_item* item =
        qobject_cast<Scene_surface_mesh_item*>(scene->item(scene->mainSelectionIndex()));
    if(! item )
    {
      dock_widget->zoomToMinButton->setEnabled(false);
      dock_widget->zoomToMaxButton->setEnabled(false);
    }

    switch(dock_widget->propertyBox->currentIndex())
    {
    case 0:
      dock_widget->zoomToMinButton->setEnabled(angles_max.count(item)>0 );
      dock_widget->zoomToMaxButton->setEnabled(angles_max.count(item)>0 );
      break;
    case 1:
      dock_widget->zoomToMinButton->setEnabled(jacobian_max.count(item)>0);
      dock_widget->zoomToMaxButton->setEnabled(jacobian_max.count(item)>0);
      break;
    default:
      break;
    }
  }

  void resetProperty()
  {
    Scene_surface_mesh_item* item =
        qobject_cast<Scene_surface_mesh_item*>(sender());
    if(!item)
      return;
    SMesh& smesh = *item->face_graph();
    SMesh::Property_map<face_descriptor, double> jacobians;
    bool found;
    boost::tie(jacobians, found) = smesh.property_map<face_descriptor,double>("f:jacobian");
    if(found)
    {
      smesh.remove_property_map(jacobians);
    }
    SMesh::Property_map<face_descriptor, double> angles;
    boost::tie(angles, found) = smesh.property_map<face_descriptor,double>("f:angle");
    if(found)
    {
      smesh.remove_property_map(angles);
    }
  }

  void displayScaledJacobian(Scene_surface_mesh_item* item)
  {

    SMesh& smesh = *item->face_graph();
    //compute and store the jacobian per face
    bool non_init;
    SMesh::Property_map<face_descriptor, double> fjacobian;
    boost::tie(fjacobian, non_init) = smesh.add_property_map<face_descriptor, double>("f:jacobian", 0);
    if(non_init)
    {
      double res_min = ARBITRARY_DBL_MAX,
          res_max = -ARBITRARY_DBL_MAX;
      SMesh::Face_index min_index, max_index;
      for(boost::graph_traits<SMesh>::face_iterator fit = faces(smesh).begin();
          fit != faces(smesh).end();
          ++fit)
      {
        fjacobian[*fit] = scaled_jacobian(*fit, smesh);
        if(fjacobian[*fit] > res_max)
        {
          res_max = fjacobian[*fit];
          max_index = *fit;
        }
        if(fjacobian[*fit] < res_min)
        {
          res_min = fjacobian[*fit];
          min_index = *fit;
        }
      }
      jacobian_min.erase(item);
      jacobian_min.insert(std::make_pair(item, std::make_pair(res_min, min_index)));
      jacobian_max.erase(item);
      jacobian_max.insert(std::make_pair(item, std::make_pair(res_max, max_index)));
      connect(item, &Scene_surface_mesh_item::itemChanged,
              this, &DisplayPropertyPlugin::resetProperty);
    }
    //scale a color ramp between min and max
    double max = maxBox;
    double min = minBox;
    //fill f:color pmap
    SMesh::Property_map<face_descriptor, CGAL::Color> fcolors =
        smesh.add_property_map<face_descriptor, CGAL::Color >("f:color", CGAL::Color()).first;
    for(boost::graph_traits<SMesh>::face_iterator fit = faces(smesh).begin();
        fit != faces(smesh).end();
        ++fit)
    {
      if(min == max)
        --min;
      double f = (fjacobian[*fit]-min)/(max-min);
      if(f<min)
        f = min;
      if(f>max)
        f = max;
      CGAL::Color color(
            255*color_ramp.r(f),
            255*color_ramp.g(f),
            255*color_ramp.b(f));
      fcolors[*fit] = color;
    }
  }

  bool resetScaledJacobian(Scene_surface_mesh_item* item)
  {
    SMesh& smesh = *item->face_graph();
    if(!smesh.property_map<face_descriptor, double>("f:jacobian").second)
    {
      return false;
    }
    dock_widget->minBox->setValue(jacobian_min[item].first-0.01);
    dock_widget->maxBox->setValue(jacobian_max[item].first);
    return true;
  }

  void displayAngles(Scene_surface_mesh_item* item)
  {
    SMesh& smesh = *item->face_graph();
    typedef boost::property_map<SMesh, boost::vertex_point_t>::type PMap;
    PMap pmap = get(boost::vertex_point, smesh);
    //compute and store smallest angle per face
    bool non_init;
    SMesh::Property_map<face_descriptor, double> fangle;
    boost::tie(fangle, non_init) = smesh.add_property_map<face_descriptor, double>("f:angle", 0);
    if(non_init)
    {
      double res_min = ARBITRARY_DBL_MAX,
          res_max = -ARBITRARY_DBL_MAX;
      SMesh::Face_index index_min, index_max;
      for(boost::graph_traits<SMesh>::face_iterator fit = faces(smesh).begin();
          fit != faces(smesh).end();
          ++fit)
      {
        bool is_face_triangle = is_triangle(halfedge(*fit, smesh), smesh);
        bool normal_is_ok = true;
        EPICK::Vector_3 normal(0,0,0);

        EPICK::Orientation orientation = CGAL::POSITIVE;
        if(!is_face_triangle)
        {
          face_descriptor f = *fit;
          CGAL::Halfedge_around_face_circulator<SMesh>
              he(halfedge(f, smesh), smesh),
              he_end(he);
          do{
            normal_is_ok = true;

            //Initializes the facet orientation

            EPICK::Point_3 S,T;
            T = get(pmap, source(*he, smesh));
            S = get(pmap, target(*he, smesh));
            EPICK::Vector_3 V1((T-S).x(), (T-S).y(), (T-S).z());
            S = get(pmap,source(next(*he,smesh), smesh));
            T = get(pmap, target(next(*he,smesh), smesh));
            EPICK::Vector_3 V2((T-S).x(), (T-S).y(), (T-S).z());

            if(normal == EPICK::Vector_3(0,0,0))
              normal_is_ok = false;
            {
              normal = CGAL::cross_product(V1, V2);
            }
            if(normal_is_ok)
            {
              orientation = EPICK::Orientation_3()(V1, V2, normal);
              if( orientation == CGAL::COPLANAR )
                normal_is_ok = false;
            }
          }while( ++he != he_end && !normal_is_ok);
        }

        std::vector<float> local_angles;
        local_angles.reserve(degree(*fit, smesh));
        for(halfedge_descriptor hd :
                      halfedges_around_face(halfedge(*fit, smesh),smesh))
        {
          halfedge_descriptor hdn = next(hd, smesh);
          EPICK::Vector_3 v1(get(pmap, source(hd, smesh)), get(pmap, target(hd, smesh))),
              v2(get(pmap, target(hdn, smesh)), get(pmap, source(hdn, smesh)));
          float norm1(CGAL::approximate_sqrt(v1.squared_length())), norm2(CGAL::approximate_sqrt(v2.squared_length()));
          float dot_prod = v1*v2;
          float angle = std::acos(dot_prod/(norm1*norm2));
          if(is_face_triangle || !normal_is_ok)
            local_angles.push_back(angle * 180/CGAL_PI);
          else
          {
            bool is_convex = true;
            EPICK::Orientation res = EPICK::Orientation_3()(v1, v2, normal) ;
            if(res!= orientation && res != CGAL::ZERO)
              is_convex = false;
            local_angles.push_back(is_convex ? angle * 180/CGAL_PI : 360 - angle * 180/CGAL_PI );
          }
        }
        std::sort(local_angles.begin(), local_angles.end());
        fangle[*fit]=local_angles.front();

        if(fangle[*fit] > res_max)
        {
          res_max = fangle[*fit];
          index_max = *fit;
        }
        if(fangle[*fit] < res_min)
        {
          res_min = fangle[*fit];
          index_min = *fit;
        }
      }
      angles_min.erase(item);
      angles_min.insert(std::make_pair(item, std::make_pair(res_min, index_min)));
      angles_max.erase(item);
      angles_max.insert(std::make_pair(item, std::make_pair(res_max, index_max)));

      connect(item, &Scene_surface_mesh_item::itemChanged,
              this, &DisplayPropertyPlugin::resetProperty);
    }
    //scale a color ramp between min and max

    float max = maxBox;
    float min = minBox;

    //fill f:color pmap
    SMesh::Property_map<face_descriptor, CGAL::Color> fcolors =
        smesh.add_property_map<face_descriptor, CGAL::Color >("f:color", CGAL::Color()).first;
    for(boost::graph_traits<SMesh>::face_iterator fit = faces(smesh).begin();
        fit != faces(smesh).end();
        ++fit)
    {
      if(min == max)
        --min;
      float f = (fangle[*fit]-min)/(max-min);
      if(f<0)
        f = 0;
      if(f>1)
        f = 1;
      CGAL::Color color(
            255*color_ramp.r(f),
            255*color_ramp.g(f),
            255*color_ramp.b(f));
      fcolors[*fit] = color;
    }
  }

  bool resetAngles(Scene_surface_mesh_item* item)
  {
    SMesh& smesh = *item->face_graph();
    if(!smesh.property_map<face_descriptor, double>("f:angle").second)
    {
      return false;
    }
    dock_widget->minBox->setValue(angles_min[item].first);
    dock_widget->maxBox->setValue(angles_max[item].first);
    return true;
  }

  // AF: This function gets called when we click on the button "Colorize"
  bool displayHeatIntensity(Scene_surface_mesh_item* item, bool iDT = false)
  {
    SMesh& mesh = *item->face_graph();
    bool found = is_source.find(item) != is_source.end();
    if(!found
       || ! source_points
       || source_points->point_set()->is_empty())
    {
      QApplication::restoreOverrideCursor();
      QMessageBox::warning(mw, "Warning","Source vertices are needed for this property.");
      return false;
    }
    if(!is_triangle_mesh(mesh))
    {
      QApplication::restoreOverrideCursor();
      QMessageBox::warning(mw,"Error","The mesh must be triangulated.");
      return false;
    }
    Heat_method * hm = NULL;
    Heat_method_idt * hm_idt = NULL;
    SMesh::Property_map<vertex_descriptor, double> heat_intensity =
      mesh.add_property_map<vertex_descriptor, double>("v:heat_intensity", 0).first;
    if(! iDT){
      if(mesh_heat_method_map.find(item) != mesh_heat_method_map.end()){
        hm = mesh_heat_method_map[item];
      }else {
        hm = new Heat_method(mesh);
        mesh_heat_method_map[item] = hm;
      }
      connect(item, &Scene_surface_mesh_item::aboutToBeDestroyed,
              [this,item](){
                auto it =  mesh_heat_method_map.find(item);
                delete it->second;
                mesh_heat_method_map.erase(it);
              }
              );
    } else {
      if(mesh_heat_method_idt_map.find(item) != mesh_heat_method_idt_map.end()){
        hm_idt = mesh_heat_method_idt_map[item];
      }else {
        hm_idt = new Heat_method_idt(mesh);
        mesh_heat_method_idt_map[item] = hm_idt;
      }
      connect(item, &Scene_surface_mesh_item::aboutToBeDestroyed,
              [this,item](){
                auto it = mesh_heat_method_idt_map.find(item);
                if(it == mesh_heat_method_idt_map.end())
                  return;
                Heat_method_idt *hm_idt = it->second;
                delete hm_idt;
                mesh_heat_method_idt_map.erase(it);
              }
              );
    }

    for(vertex_descriptor vd : vertices(mesh)){
      if(get(is_source[item], vd)){
        if(iDT){
          hm_idt->add_source(vd);
        } else
          hm->add_source(vd);
      }
      else
      {
        if(iDT){
          hm_idt->remove_source(vd);
        } else
          hm->remove_source(vd);
      }
    }

    if(iDT){
      hm_idt->estimate_geodesic_distances(heat_intensity);
    }else{
      hm->estimate_geodesic_distances(heat_intensity);
    }

    double max = 0;
    double min = (std::numeric_limits<double>::max)();

    for(vertex_descriptor vd : vertices(mesh)){
      double hi = heat_intensity[vd];
      if(hi < min)
        min = hi;
      if(hi > max)
        max = hi;
    }
    color_ramp = Color_ramp(rm, rM, gm, gM, bm, bM);
    dock_widget->minBox->setValue(min);
    dock_widget->maxBox->setValue(max);

    //}
    SMesh::Property_map<vertex_descriptor, CGAL::Color> vcolors =
        mesh.add_property_map<vertex_descriptor, CGAL::Color >("v:color", CGAL::Color()).first;
    SMesh::Property_map<vertex_descriptor, float> vdist=
        mesh.add_property_map<vertex_descriptor, float >("v:dist", 0.0).first;
    for(boost::graph_traits<SMesh>::vertex_iterator vit = vertices(mesh).begin();
        vit != vertices(mesh).end();
        ++vit)
    {
      double h =(heat_intensity[*vit]-min)/(max-min);
      CGAL::Color color(
            255*color_ramp.r(h),
            255*color_ramp.g(h),
            255*color_ramp.b(h));
      vcolors[*vit] = color;
      vdist[*vit]=h;
    }
    Scene_group_item* group;
    if(mesh_heat_item_map.find(item) != mesh_heat_item_map.end())
    {
      group = mesh_heat_item_map[item]->parentGroup();
      group->unlockChild(mesh_heat_item_map[item]);
      scene->erase(scene->item_id(mesh_heat_item_map[item]));
    }
    else
    {
      group = new Scene_group_item("Heat Visualization");
      group->setProperty("heat_group", true);
      scene->addItem(group);
      scene->changeGroup(item, group);
      scene->changeGroup(source_points, group);
      group->lockChild(item);
      group->lockChild(source_points);
      dock_widget->deleteButton->setEnabled(true);
      connect(group, &Scene_group_item::aboutToBeDestroyed,
              this, [this](){
        this->dock_widget->deleteButton->setEnabled(false);
      });
    }
    mesh_heat_item_map[item] = new Scene_heat_item(item);
    mesh_heat_item_map[item]->setName(tr("%1 heat").arg(item->name()));
    scene->addItem(mesh_heat_item_map[item]);
    scene->changeGroup(mesh_heat_item_map[item], group);
    group->lockChild(mesh_heat_item_map[item]);
    item->setVisible(false);
    displayLegend();
    if(dock_widget->sourcePointsButton->isChecked())
      dock_widget->sourcePointsButton->toggle();
    return true;
  }

  void replaceRamp()
  {
    color_ramp = Color_ramp(rm, rM, gm, gM, bm, bM);
    displayLegend();
    minBox = dock_widget->minBox->value();
    maxBox = dock_widget->maxBox->value();
  }

  void on_propertyBox_currentIndexChanged(int)
  {
    switch(dock_widget->propertyBox->currentIndex())
    {
    case 0:
    {
      dock_widget->groupBox->  setEnabled(true);
      dock_widget->groupBox_3->setEnabled(true);

      dock_widget->sourcePointsButton->setEnabled(false);

      dock_widget->minBox->setMinimum(0);
      dock_widget->minBox->setMaximum(360);
      dock_widget->minBox->setValue(0);

      dock_widget->maxBox->setMinimum(0);
      dock_widget->maxBox->setMaximum(360);
      Scene_surface_mesh_item* item =
          qobject_cast<Scene_surface_mesh_item*>(scene->item(scene->mainSelectionIndex()));
      if(! item )
        dock_widget->maxBox->setValue(180);
      else if(is_triangle_mesh(*item->face_graph()))
        dock_widget->maxBox->setValue(60);
      else if(is_quad_mesh(*item->face_graph()))
        dock_widget->maxBox->setValue(90);
      break;
    }
    case 1:
      dock_widget->groupBox->  setEnabled(true);
      dock_widget->groupBox_3->setEnabled(true);
      dock_widget->sourcePointsButton->setEnabled(false);

      dock_widget->minBox->setMinimum(-1000);
      dock_widget->minBox->setMaximum(1000);
      dock_widget->minBox->setValue(0);

      dock_widget->maxBox->setMinimum(-1000);
      dock_widget->maxBox->setMaximum(1000);
      dock_widget->maxBox->setValue(2);
      break;
    default:
      dock_widget->maxBox->setMinimum(0);
      dock_widget->maxBox->setMaximum(99999999);
      dock_widget->groupBox->  setEnabled(false);
      dock_widget->groupBox_3->setEnabled(false);
      dock_widget->sourcePointsButton->setEnabled(true);

    }
    replaceRamp();
    enableButtons();
  }

  void closure()Q_DECL_OVERRIDE
  {
    dock_widget->hide();
  }

  void on_zoomToMinButton_pressed()
  {
    Scene_surface_mesh_item* item =
        qobject_cast<Scene_surface_mesh_item*>(scene->item(scene->mainSelectionIndex()));
    if(!item)
      return;
    face_descriptor dummy_fd;
    Point_3 dummy_p;
    switch(dock_widget->propertyBox->currentIndex())
    {
    case 0:
    {
      ::zoomToId(*item->face_graph(),
                 QString("f%1").arg(angles_min[item].second),
                 getActiveViewer(),
                 dummy_fd,
                 dummy_p);
    }
      break;
    case 1:
    {
      ::zoomToId(*item->face_graph(),
                 QString("f%1").arg(jacobian_min[item].second),
                 getActiveViewer(),
                 dummy_fd,
                 dummy_p);
    }
      break;
    default:
      break;
    }
  }

  void on_zoomToMaxButton_pressed()
  {
    Scene_surface_mesh_item* item =
        qobject_cast<Scene_surface_mesh_item*>(scene->item(scene->mainSelectionIndex()));
    if(!item)
      return;
    face_descriptor dummy_fd;
    Point_3 dummy_p;
    switch(dock_widget->propertyBox->currentIndex())
    {
    case 0:
    {
      ::zoomToId(*item->face_graph(),
                 QString("f%1").arg(angles_max[item].second),
                 getActiveViewer(),
                 dummy_fd,
                 dummy_p);
    }
      break;
    case 1:
    {
      ::zoomToId(*item->face_graph(),
                 QString("f%1").arg(jacobian_max[item].second),
                 getActiveViewer(),
                 dummy_fd,
                 dummy_p);
    }
      break;
    default:
      break;
    }
  }

  void delete_group()
  {
    Scene_item* item = scene->item(scene->selectionIndices().first());
    Scene_group_item* group = qobject_cast<Scene_group_item*>(item);
    if(!group || !group->property("heat_group").toBool())
      return;
    for(auto child_id : group->getChildren())
    {
      if(Scene_surface_mesh_item* child = qobject_cast<Scene_surface_mesh_item*>(scene->item(child_id))){
        group->unlockChild(child);
         group->removeChild(child);
         scene->addChild(child);
         child->setVisible(true);
         child->resetColors();
        break;
      }
    }
    scene->erase(scene->item_id(group));

  }

  void on_sourcePointsButton_toggled(bool b)
  {
    if(b)
    {
      Scene_heat_item* h_item = nullptr;
      Scene_surface_mesh_item* item =
          qobject_cast<Scene_surface_mesh_item*>(scene->item(scene->mainSelectionIndex()));
      if(!item)
      {
        h_item = qobject_cast<Scene_heat_item*>(scene->item(scene->mainSelectionIndex()));
        if(h_item)
          item = h_item->getParent();
      }
      if(!item)
      {
        QMessageBox::warning(mw, "Warning", "You must select a Surface_mesh_item to make this work. Aborting.");
        dock_widget->sourcePointsButton->setChecked(false);
        return;
      }
      current_item = item;
      connect(current_item, &Scene_surface_mesh_item::aboutToBeDestroyed,
              this, [this]()
      {
        dock_widget->sourcePointsButton->setChecked(false);
      });
      if(mesh_sources_map.find(item) == mesh_sources_map.end())
      {
        source_points = new Scene_points_with_normal_item();
        source_points->setName(QString("Source vertices for %1").arg(current_item->name()));
        source_points->setColor(QColor(Qt::red));
        source_points->setPointSize(5);
        scene->addItem(source_points);
        connect(source_points, &Scene_points_with_normal_item::aboutToBeDestroyed,
                [this](){
          boost::unordered_map<Scene_surface_mesh_item*, Scene_points_with_normal_item*>::iterator it;
          for(it = mesh_sources_map.begin();
              it != mesh_sources_map.end();
              ++it)
          {
            if(it->second == source_points)
            {
              mesh_sources_map.erase(it);
              break;
            }
          }
        });
      mesh_sources_map[current_item] = source_points;
      }
      else
      {
        source_points=mesh_sources_map[current_item];
      }
      connect(item, SIGNAL(selected_vertex(void*)), this, SLOT(on_vertex_selected(void*)));
      bool non_init = is_source.find(item) == is_source.end();
      if(non_init)
      {
        Vertex_source_map map = get(Vertex_source_tag(), *item->face_graph());
        is_source.insert(std::make_pair(item, map));
        connect(item, &Scene_surface_mesh_item::itemChanged,
                this, &DisplayPropertyPlugin::resetProperty);
        connect(item, &Scene_surface_mesh_item::aboutToBeDestroyed,
                [this, item](){
          if(is_source.find(item) != is_source.end())
          {
            is_source.erase(item);
          }
        });
      }
    }
    else
    {
      if(!current_item)
        return;
      disconnect(current_item, SIGNAL(selected_vertex(void*)), this, SLOT(on_vertex_selected(void*)));
      current_item = NULL;
    }
  }

  void on_vertex_selected(void* void_ptr)
  {
    typedef boost::graph_traits<SMesh>::vertices_size_type size_type;
    size_type h = static_cast<size_type>(reinterpret_cast<std::size_t>(void_ptr));
    vertex_descriptor vd = static_cast<vertex_descriptor>(h) ;
    bool found = is_source.find(current_item) != is_source.end();
    if(found)
    {
      if(!get(is_source[current_item], vd))
      {
        put(is_source[current_item], vd, true);
        source_points->point_set()->insert(current_item->face_graph()->point(vd));
      }
      else
      {
        put(is_source[current_item], vd, false);
        Point_set::iterator it;
        for(it = source_points->point_set()->begin(); it != source_points->point_set()->end(); ++it)
          if(source_points->point_set()->point(*it) == current_item->face_graph()->point(vd))
          {
            source_points->point_set()->remove(it);
            source_points->point_set()->collect_garbage();
            break;
          }
      }
    }

   source_points->invalidateOpenGLBuffers();
   source_points->itemChanged();
  }
private:
  void displayLegend()
  {
    // Create an legend_ and display it
    const int height = 256;
    const int width = 90;
    const int cell_width = width/3;
    const int top_margin = 5;
    const int left_margin = 5;
    const int drawing_height = height - top_margin * 2;
    const int text_height = 20;

    legend_ = QPixmap(width, height + text_height);
    legend_.fill(QColor(200, 200, 200));

    QPainter painter(&legend_);
    painter.setPen(Qt::black);
    painter.setBrush(QColor(200, 200, 200));

    // Build legend_ data
    double min_value(dock_widget->minBox->value()),
        max_value(dock_widget->maxBox->value());
    std::vector<double> graduations(100);
    for(int i=0; i<100; ++i)
      graduations[i] = i/100.0;

    // draw
    int i=0;
    for (std::vector<double>::iterator it = graduations.begin(), end = graduations.end();
         it != end; ++it, i+=2)
    {
      QColor color(255*color_ramp.r(*it),
                   255*color_ramp.g(*it),
                   255*color_ramp.b(*it));
      painter.fillRect(left_margin,
                       drawing_height - top_margin - i,
                       cell_width,
                       2,
                       color);
    }

    // draw right vertical line
    painter.setPen(Qt::blue);

    painter.drawLine(QPoint(left_margin + cell_width+10, drawing_height - top_margin),
                     QPoint(left_margin + cell_width+10,
                            drawing_height - top_margin - static_cast<int>(graduations.size())*2));

    // draw min value and max value
    painter.setPen(Qt::blue);
    QRect min_text_rect(left_margin + cell_width+10,drawing_height - top_margin,
                        50, text_height);
    painter.drawText(min_text_rect, Qt::AlignCenter, tr("%1").arg(min_value, 0, 'f', 1));

    QRect max_text_rect(left_margin + cell_width+10, drawing_height - top_margin - 200,
                        50, text_height);
    painter.drawText(max_text_rect, Qt::AlignCenter, tr("%1").arg(max_value, 0, 'f', 1));

    dock_widget->legendLabel->setPixmap(legend_);
  }
  double scaled_jacobian(const face_descriptor& f , const SMesh &mesh);
  QList<QAction*> _actions;
  Color_ramp color_ramp;
  DockWidget* dock_widget;
  double rm;
  double rM;
  double gm;
  double gM;
  double bm;
  double bM;
  boost::unordered_map<Scene_surface_mesh_item*, std::pair<double, SMesh::Face_index> > jacobian_min;
  boost::unordered_map<Scene_surface_mesh_item*, std::pair<double, SMesh::Face_index> > jacobian_max;

  boost::unordered_map<Scene_surface_mesh_item*, std::pair<double, SMesh::Face_index> > angles_min;
  boost::unordered_map<Scene_surface_mesh_item*, std::pair<double, SMesh::Face_index> > angles_max;
  boost::unordered_map<Scene_surface_mesh_item*, Vertex_source_map> is_source;

  double minBox;
  double maxBox;
  QPixmap legend_;

  Scene_surface_mesh_item* current_item;
  Scene_points_with_normal_item* source_points;
  boost::unordered_map<Scene_surface_mesh_item*, Scene_points_with_normal_item*> mesh_sources_map;
  boost::unordered_map<Scene_surface_mesh_item*, Scene_heat_item*> mesh_heat_item_map;

  boost::unordered_map<Scene_surface_mesh_item*, Heat_method*> mesh_heat_method_map;
  boost::unordered_map<Scene_surface_mesh_item*, Heat_method_idt*> mesh_heat_method_idt_map;
};

  /// Code based on the verdict module of vtk

  /*=========================================================================
  Copyright (c) 2006 Sandia Corporation.
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.
=========================================================================*/

  double DisplayPropertyPlugin::scaled_jacobian( const face_descriptor& f , const SMesh& mesh)
  {
    boost::property_map<SMesh, boost::vertex_point_t>::type
        pmap = get(boost::vertex_point, mesh);
    std::vector<double> corner_areas(degree(f, mesh));
    std::vector<EPICK::Vector_3> edges;
    for(halfedge_descriptor hd : CGAL::halfedges_around_face(halfedge(f, mesh), mesh))
    {
      edges.push_back(EPICK::Vector_3(get(pmap, source(hd, mesh)), get(pmap, target(hd, mesh))));
    }
    std::vector<EPICK::Vector_3> corner_normals;
    for(std::size_t i = 0; i < edges.size(); ++i)
    {
      corner_normals.push_back(CGAL::cross_product(edges[i], edges[(i+1)%(edges.size())]));
    }

    EPICK::Vector_3 unit_center_normal = CGAL::Polygon_mesh_processing::compute_face_normal(f, mesh);
    unit_center_normal *= 1.0/CGAL::approximate_sqrt(unit_center_normal.squared_length());

    for(std::size_t i = 0; i < corner_areas.size(); ++i)
    {
      corner_areas[i] =  unit_center_normal*corner_normals[i];
    }
    std::vector<double> length;
    for(std::size_t i=0; i<edges.size(); ++i)
    {
      length.push_back(CGAL::approximate_sqrt(edges[i].squared_length()));
      if( length[i] < ARBITRARY_DBL_MIN)
        return 0.0;
    }
    double min_scaled_jac = ARBITRARY_DBL_MAX;
    for(std::size_t i=0; i<edges.size(); ++i)
    {
      double scaled_jac = corner_areas[i] / (length[i] * length[(i+edges.size()-1)%(edges.size())]);
      min_scaled_jac = (std::min)( scaled_jac, min_scaled_jac );
    }

    if( min_scaled_jac > 0 )
      return (double) (std::min)( min_scaled_jac, ARBITRARY_DBL_MAX );
    return (double) (std::max)( min_scaled_jac, -ARBITRARY_DBL_MAX );

  }

#include "Display_property_plugin.moc"


#define BOOST_TEST_MODULE hash_set test
#include <boost/test/unit_test.hpp>
#include <boost/interprocess/managed_shared_memory.hpp>
#include <boost/interprocess/allocators/allocator.hpp>
#include <boost/functional/hash.hpp>

#include <string>
#include <cstring>

#include <iostream>

#include "hash_set.hpp"

using namespace boost::interprocess;

template<typename Hash, typename Pred, typename Alloc>
void test_adding_into_hashset(HashSet<int, Hash, Pred, Alloc>& h, std::vector<int> const & es) {
    size_t n = es.size();

    for (size_t i = 0; i <= n; ++i) {
        BOOST_CHECK_EQUAL(h.size(), i);
        for (size_t j = 0; j < i; ++j) {
            BOOST_CHECK_EQUAL(h.contains(es[j]), true);
        }
        for (size_t j = i; j < n; ++j) {
            BOOST_CHECK_EQUAL(h.contains(es[j]), false);
        }

        if (i < n) {
            BOOST_CHECK_EQUAL(h.add(es[i]), true);
        }
    }

    for (size_t i = 0; i <= n; ++i) {
        for (size_t j = 0; j < n; ++j) {
            BOOST_CHECK_EQUAL(h.contains(es[j]), true);
        }

        if (i < n) {
            BOOST_CHECK_EQUAL(h.add(es[i]), false);
        }
    }

    BOOST_CHECK_THROW(overflow_hash_set(h), HashSetFullExcpetion);
}

template<typename Hash, typename Pred, typename Alloc>
void overflow_hash_set(HashSet<int, Hash, Pred, Alloc>& h) {

    int e = 0;
    size_t sz = h.capacity();
    for (size_t i = 0; i < sz; ++i) {
        h.add(e++);
    }
}

size_t zero_hash(int x) {
    return 0;
}

BOOST_AUTO_TEST_CASE( adding_test ) {
    std::vector<int> es;
    es.push_back(0);
    es.push_back(4);
    es.push_back(42);
    es.push_back(-1);

    HashSet<int> h(8, 932874231);
    BOOST_CHECK(h.capacity() >= 8);

    test_adding_into_hashset(h, es);

    HashSet<int, size_t (*) (int)> h2(8, 932874231, zero_hash);
    BOOST_CHECK(h2.capacity() >= 8);

    test_adding_into_hashset(h2, es);
}

BOOST_AUTO_TEST_CASE( sharing_test ) {
    std::vector<int> es;
    es.push_back(0);
    es.push_back(4);
    es.push_back(42);
    es.push_back(-1);

    const char * mem_name("MySharedMemory");

    try {
        shared_memory_object::remove(mem_name);
        typedef allocator<int, managed_shared_memory::segment_manager> ShmemAllocator;
        typedef HashSet<int, boost::hash<int>, std::equal_to<int>, ShmemAllocator>
            SharedHashSet;

        {
            managed_shared_memory segment(create_only, mem_name, 65536);
            const ShmemAllocator alloc_instance(segment.get_segment_manager());

            SharedHashSet * h = segment.construct<SharedHashSet>("MyHashSet")
                (8, 932874231, boost::hash<int>(), std::equal_to<int>(), alloc_instance);

            test_adding_into_hashset(*h, es);
        }

        {
            managed_shared_memory segment2(open_only, mem_name);

            SharedHashSet const * h2 = segment2.find<SharedHashSet>("MyHashSet").first;

            for (size_t i = 0; i < es.size(); ++i) {
                BOOST_CHECK_EQUAL(h2->contains(es[i]), true);
            }

            BOOST_CHECK(!h2->contains(10));
            BOOST_CHECK(!h2->contains(-10));

            segment2.destroy<SharedHashSet>("MyHashSet");
        }

    } catch (...) {
        shared_memory_object::remove(mem_name);
        throw;
    }
    shared_memory_object::remove(mem_name);
}


/*
==============================================================================
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).

Copyright 2007
Pooyan Dadvand, Riccardo Rossi
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain

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

Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNER.

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

THE  SOFTWARE IS  PROVIDED  "AS  IS", WITHOUT  WARRANTY  OF ANY  KIND,
EXPRESS OR  IMPLIED, INCLUDING  BUT NOT LIMITED  TO THE  WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT  SHALL THE AUTHORS OR COPYRIGHT HOLDERS  BE LIABLE FOR ANY
CLAIM, DAMAGES OR  OTHER LIABILITY, WHETHER IN AN  ACTION OF CONTRACT,
TORT  OR OTHERWISE, ARISING  FROM, OUT  OF OR  IN CONNECTION  WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

==============================================================================
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: anonymous $
//   Date:                $Date: 2007-06-19 17:13:15 $
//   Revision:            $Revision: 1.3 $
//
//

// System includes

// External includes
#include <boost/python.hpp>
#include "python/add_parallel_linear_solvers_to_python.h"

//nothing will be compiled if an openmp compiler is not found
#ifdef _OPENMP

	// Project includes
	#include "includes/define.h"
	#include "python/add_equation_systems_to_python.h"
	#include "linear_solvers/cg_solver.h"
	#include "linear_solvers/cg_solver.h"
	#include "linear_solvers/bicgstab_solver.h"
	#include "linear_solvers/tfqmr_solver.h"
	#include "includes/dof.h"
	#include "spaces/parallel_ublas_space.h"
	#include "spaces/ublas_space.h"

	#include "linear_solvers/reorderer.h"
	#include "linear_solvers/direct_solver.h"
	#include "linear_solvers/skyline_lu_factorization_solver.h"

	#include "linear_solvers/preconditioner.h"
	#include "linear_solvers/diagonal_preconditioner.h"
	//#include "linear_solvers/superlu_solver.h"

#endif

namespace Kratos
{

namespace Python
{

  void  AddParallelLinearSolversToPython()
  {
//nothing will be compiled if an openmp compiler is not found
#ifdef _OPENMP

	typedef ParallelUblasSpace<double, CompressedMatrix, Vector> ParallelSpaceType;
	typedef UblasSpace<double, Matrix, Vector> ParallelLocalSpaceType;
	typedef LinearSolver<ParallelSpaceType,  ParallelLocalSpaceType> ParallelLinearSolverType;
	typedef IterativeSolver<ParallelSpaceType,  ParallelLocalSpaceType> ParallelIterativeSolverType;
	typedef CGSolver<ParallelSpaceType,  ParallelLocalSpaceType> ParallelCGSolverType;
	//typedef BICGSTABSolver<SpaceType,  LocalSpaceType> BICGSTABSolverType;
	//typedef TFQMRSolver<SpaceType,  LocalSpaceType> TFQMRSolverType;

		bool (ParallelLinearSolverType::*pointer_to_solve)(ParallelLinearSolverType::SparseMatrixType& rA, ParallelLinearSolverType::VectorType& rX, ParallelLinearSolverType::VectorType& rB) = &ParallelLinearSolverType::Solve;

	  using namespace boost::python;

	  //****************************************************************************************************
	  //preconditioners
	  //****************************************************************************************************
		typedef Preconditioner<ParallelSpaceType,  ParallelLocalSpaceType> ParallelPreconditionerType;

		class_<ParallelPreconditionerType, ParallelPreconditionerType::Pointer>("ParallelPreconditioner")
			  .def(self_ns::str(self))
			  ;

		typedef DiagonalPreconditioner<ParallelSpaceType,  ParallelLocalSpaceType> ParallelDiagonalPreconditionerType;
		class_<ParallelDiagonalPreconditionerType, ParallelDiagonalPreconditionerType::Pointer, bases<ParallelPreconditionerType> >("ParallelDiagonalPreconditioner")
			  .def(self_ns::str(self))
			  ;

	  //****************************************************************************************************
	  //linear solvers
	  //****************************************************************************************************
	  class_<ParallelLinearSolverType, ParallelLinearSolverType::Pointer>("ParallelLinearSolver")
			  .def("Initialize",&ParallelLinearSolverType::Initialize)
			  .def("Solve",pointer_to_solve)
			  //.def("",&LinearSolverType::)
			  .def(self_ns::str(self))
			  ;

	  class_<ParallelIterativeSolverType, ParallelIterativeSolverType::Pointer, bases<ParallelLinearSolverType> >("ParallelIterativeSolver")
			  //.def("",&LinearSolverType::)
			  .def(self_ns::str(self))
			  ;

	  class_<ParallelCGSolverType, ParallelCGSolverType::Pointer, bases<ParallelIterativeSolverType> >("ParallelCGSolver")
			  .def(init<double>())
			  .def(init<double, unsigned int>())
			  .def(init<double, unsigned int,  ParallelPreconditionerType::Pointer>())
			  //.def("",&LinearSolverType::)
			  .def(self_ns::str(self))
			  ;

	//  class_<BICGSTABSolverType, BICGSTABSolverType::Pointer, bases<IterativeSolverType> >("ParallelBICGSTABSolver")
	//		  .def(init<double>())
	//		  .def(init<double, unsigned int>())
	//		  .def(self_ns::str(self))
	//		  .def(init<double, unsigned int,  PreconditionerType::Pointer>())
	//		  ;
	//
	//  class_<TFQMRSolverType, TFQMRSolverType::Pointer, bases<IterativeSolverType> >("ParallelTFQMRSolver")
	//		  .def(init<double>())
	//		  .def(init<double, unsigned int>())
	//		  .def(self_ns::str(self))
	//		  .def(init<double, unsigned int,  PreconditionerType::Pointer>())
	//		  ;

		typedef Reorderer<ParallelSpaceType,  ParallelLocalSpaceType > ParallelReordererType;
		typedef DirectSolver<ParallelSpaceType,  ParallelLocalSpaceType, ParallelReordererType > ParallelDirectSolverType;
		typedef SkylineLUFactorizationSolver<ParallelSpaceType,  ParallelLocalSpaceType, ParallelReordererType > ParallelSkylineLUFactorizationSolverType;

		class_<ParallelReordererType, ParallelReordererType::Pointer >("ParallelReorderer")
			  .def( init< >() )
			  .def(self_ns::str(self))
			  .def( "Initialize",&ParallelReordererType::Initialize)
			  .def( "Reorder",&ParallelReordererType::Reorder)
			  .def( "InverseReorder",&ParallelReordererType::InverseReorder)
			  ;

		class_<ParallelDirectSolverType, ParallelDirectSolverType::Pointer, bases<ParallelLinearSolverType> >("ParallelDirectSolver")
			  .def( init< >() )
			  .def(self_ns::str(self))
			  ;

		class_<ParallelSkylineLUFactorizationSolverType, ParallelSkylineLUFactorizationSolverType::Pointer, bases<ParallelDirectSolverType> >("ParallelSkylineLUFactorizationSolver")
			  .def(init< >())
			  .def(self_ns::str(self))
			  ;

	//	typedef SuperLUSolver<SparseSpaceType, LocalSpaceType> SuperLUSolverType;
	//	class_<SuperLUSolverType, bases<DirsectSolverType>, boost::noncopyable >
	//		( "SuperLUSolver",
	//			init<>() )

//it will compile nothing if openmp is not found
#endif
  }

}  // namespace Python.

} // Namespace Kratos


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Zero Zero (function template)

  Generates the constant @c 0.

  @headerref{<boost/simd/constant/zero.hpp>}

  @par Description

  1.  @code
    template<typename T> T Zero();
    @endcode

  2.  @code
    template<typename T> T Zero( boost::simd::as_<T> const& target );
    @endcode

  Generates a value of type @c T that evaluates to 0.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to <tt>T(0)</tt>.

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/zero.hpp>
#include <boost/simd/constant/simd/zero.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SDK_META_REAL_OF_HPP_INCLUDED
#define BOOST_SIMD_SDK_META_REAL_OF_HPP_INCLUDED

#include <boost/dispatch/meta/value_of.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost { namespace simd { namespace meta
{
  template<class T>
  struct real_of;
}

namespace details
{
  template<class T, class U = typename boost::dispatch::meta::value_of<T>::type>
  struct real_of
    : meta::real_of<U>
  {
  };

  template<class T>
  struct real_of<T, T>
  {
    typedef T type;
  };
}

namespace meta
{
  template<class T>
  struct  real_of
        : details::real_of<T> {};

  template<class T>
  struct real_of<T&>
    : boost::mpl::if_< boost::is_same< typename meta::real_of<T>::type
                                     , T
                                     >
                     , T&
                     , typename details::real_of<T&>::type
                     >
  {
  };

  template<class T>
  struct real_of<T const>
    : boost::mpl::if_< boost::is_same< typename meta::real_of<T>::type
                                     , T
                                     >
                     , T const
                     , typename details::real_of<T const>::type
                     >
  {
  };
} } }

#endif


/*
# Copyright (c) 2014-2020 Reed A. Cartwright <reed@cartwright.ht>
#
# This file is part of the Ultimate Source Code Project.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
*/

#ifndef MUTK_PEELING_HPP
#define MUTK_PEELING_HPP

#include <mutk/memory.hpp>

#include <vector>
#include <utility>

#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/fill.hpp>
#include <boost/range/algorithm/fill_n.hpp>

#include <boost/range/adaptor/sliced.hpp>

namespace mutk {

struct workspace_t;

/* Peeling Operators
PeelUp:       low[o] = mat %*% low[a]
PeelUpStar:   low[o] = (mat %*% low[a])*low[b]
PeelDown:     upp[o] = mat  %*% upp[a]
PeelStar:     upp[o] = upp[a] * low[b]
PeelMate:     low[o] = reshaped[low[a]] %*% upp[b]
*/

class PeelOp {
public:
    PeelOp(int o, int a, int b) :
        output_{o}, a_{a}, b_{b}
    {}

    virtual void operator()(workspace_t *work, const TransitionMatrix &mat) const = 0;

    // Node indexes
    int output_;
    int a_;
    int b_;
};

class PeelUp : public PeelOp {
public:
    PeelUp(int o, int a) :
    PeelOp{o, a, -1} {}
    virtual void operator()(workspace_t *work, const TransitionMatrix &mat) const final;
};

class PeelUpStar : public PeelOp {
public:
    using PeelOp::PeelOp;
    virtual void operator()(workspace_t *work, const TransitionMatrix &mat) const final;
};

class PeelDown : public PeelOp {
public:
    using PeelOp::PeelOp;
    virtual void operator()(workspace_t *work, const TransitionMatrix &mat) const final;
};

class PeelStar : public PeelOp {
public:
    using PeelOp::PeelOp;
    virtual void operator()(workspace_t *work, const TransitionMatrix &mat) const final;
};

class PeelMate : public PeelOp {
public:
    using PeelOp::PeelOp;
    virtual void operator()(workspace_t *work, const TransitionMatrix &mat) const final;
};

struct workspace_t {
    GenotypeArrayVector upper; // Holds P(~Descendent_Data & G=g)
    GenotypeArrayVector lower; // Holds P( Descendent_Data | G=g)
    ParentArrayVector super; // Holds P(~Descendent_Data & G=g) for parent nodes

    bool dirty_lower = false;
    double ln_scale = 0.0;
    size_t matrix_index = 0;

    // Temporary data used by some peeling ops
    TemporaryMatrix temp_buffer;

    // Information about the pedigree
    std::size_t num_nodes = 0;
    using node_range_t = std::pair<std::size_t, std::size_t>;
    node_range_t founder_nodes,
                 germline_nodes,
                 somatic_nodes,
                 library_nodes;

    std::vector<int> ploidies;

    template<typename Rng>
    static auto make_slice(Rng& rng, node_range_t n) -> boost::sliced_range<Rng> {
        return boost::adaptors::slice(rng, n.first, n.second);
    }

    template<typename Rng>
    auto make_founder_slice(Rng& rng) -> boost::sliced_range<Rng> {
        return make_slice(rng, founder_nodes);
    }

    template<typename Rng>
    auto make_germline_slice(Rng& rng) -> boost::sliced_range<Rng> {
        return make_slice(rng, germline_nodes);
    }

    template<typename Rng>
    auto make_somatic_slice(Rng& rng) -> boost::sliced_range<Rng> {
        return make_slice(rng, somatic_nodes);
    }

    template<typename Rng>
    auto make_library_slice(Rng& rng) -> boost::sliced_range<Rng> {
        return make_slice(rng, library_nodes);
    }

    // Resize the workspace to fit a pedigree with sz nodes
    void Resize(std::size_t sz) {
        num_nodes = sz;
        upper.resize(num_nodes);
        super.resize(num_nodes);
        lower.resize(num_nodes);
        for(auto && a: lower) {
            a.setOnes(10);
        }
        temp_buffer.resize(100, 1);
        dirty_lower = false;
    }

    // Cleanup after a backwards peeling algorithm
    void Cleanup() {
        for(auto && a : lower) {
            a.setOnes();
        }
        dirty_lower = false;
    }

    // Cleanup after a backwards peeling algorithm
    // Do not update libraries since they might be set in another operation
    void CleanupFast() {
        for(std::size_t n = 0; n < somatic_nodes.second; ++n) {
            lower[n].setOnes();
        }
        dirty_lower = false;
    }

    // Set the prior probability of the founders given the reference
    void SetGermline(const GenotypeArray &prior) {
        assert(founder_nodes.first <= founder_nodes.second);
        assert(founder_nodes.second <= germline_nodes.second);

        // Set the Upper of the Founder Nodes
        for(auto i = founder_nodes.first; i < founder_nodes.second; ++i) {
            upper[i] = prior;
        }
        // Set the lowers of any germline node
        for(auto i = founder_nodes.first; i < germline_nodes.second; ++i) {
            lower[i].setOnes(prior.size());
        }
    }

    void SetGermline(const GenotypeArray &diploid_prior, const GenotypeArray &haploid_prior) {
        assert(founder_nodes.first <= founder_nodes.second);
        assert(founder_nodes.second <= germline_nodes.second);

        // Set the Upper of the Founder Nodes
        for(auto i = founder_nodes.first; i < founder_nodes.second; ++i) {
            assert(ploidies[i] == 2 || ploidies[i] == 1);
            if(ploidies[i] == 2) {
                upper[i] = diploid_prior;
            } else {
                upper[i] = haploid_prior;
            }
        }
        // Set the lowers of any germline node
        for(auto i = founder_nodes.first; i < germline_nodes.second; ++i) {
            assert(ploidies[i] == 2 || ploidies[i] == 1);
             if(ploidies[i] == 2) {
                lower[i].setOnes(diploid_prior.size());
            } else {
                lower[i].setOnes(haploid_prior.size());
            }
        }
    }

    template<typename G, typename D, typename ...A>
    void CalculateGenotypeLikelihoods(const G& gt, const D& d, A&&... args) {
        ln_scale = 0.0;
        size_t u = 0;
        for(auto pos = library_nodes.first; pos < library_nodes.second; ++pos) {
            ln_scale += gt(d[u++], std::forward<A>(args)..., ploidies[pos], &lower[pos]);
        }
    }

    void ExpGenotypeLikelihoods() {
        for(auto pos = library_nodes.first; pos < library_nodes.second; ++pos) {
            lower[pos] = lower[pos].exp();
        }
    }

    void LogGenotypeLikelihoods() {
        for(auto pos = library_nodes.first; pos < library_nodes.second; ++pos) {
            lower[pos] = lower[pos].log();
        }
    }

    // Set the genotype likelihoods into the lower values of the library_nodes.
    // Scales the genotype likelihoods as needed.
    // Input: log-likelihood values
    template<typename D>
    void SetGenotypeLikelihoods(const D& d) {
        ln_scale = 0.0;
        size_t u = 0;
        for(auto pos = library_nodes.first; pos < library_nodes.second; ++pos,++u) {
            lower[pos].resize(d[u].size());
            boost::copy(d[u], lower[pos].data());
            double temp = lower[pos].maxCoeff();
            lower[pos] = (lower[pos]-temp).exp();
            ln_scale += temp;
        }
    }

    // Copy genotype likelihoods into the lower values of the library_nodes
    template<typename D>
    void CopyGenotypeLikelihoods(const D& d) {
        ln_scale = 0.0;
        size_t u = 0;
        for(auto pos = library_nodes.first; pos < library_nodes.second; ++pos,++u) {
            lower[pos].resize(d[u].size());
            boost::copy(d[u], lower[pos].data());
        }
    }

    // Set all genotype likelihoods to 1
    void ClearGenotypeLikelihoods(size_t num_of_obs_alleles) {
        ln_scale = 0.0;
        size_t hap_sz = num_of_obs_alleles;
        size_t gt_sz = hap_sz*(hap_sz+1)/2;
        for(auto pos = library_nodes.first; pos < library_nodes.second; ++pos) {
            assert(ploidies[pos] == 1 || ploidies[pos] == 2);
            lower[pos].setOnes( (ploidies[pos] == 2) ? gt_sz : hap_sz );
        }
    }
};

} // namespace MUTK

#endif // MUTK_PEELING_HPP


#include "../shortcut_acceptor.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>

#include <algorithm>

BOOST_AUTO_TEST_SUITE(shortcut_acceptor_tests)

BOOST_AUTO_TEST_CASE(example)
{
    //                      6
    //    1                /
    //   / \              /
    //  /   \            /
    // 0 (4) 2  4       /
    //       | / \ (2) /
    //       |/   \   /
    //       3      5
    poly_line line {0,
        std::vector<coordinate> {
            coordinate {0, 0},
            coordinate {1, 1},
            coordinate {2, 0},
            coordinate {2, -1},
            coordinate {3, 0},
            coordinate {4, -1},
            coordinate {5, 2}
        }
    };

    shortcut_acceptor acceptor(line);

    // from first vertex
    std::vector<shortcut> tangents_from_first {
        shortcut {0, 3, 0, shortcut::type::MINIMAL_TANGENT},
        shortcut {0, 4, 2, shortcut::type::MAXIMAL_TANGENT},
        shortcut {0, 5, 3, shortcut::type::MINIMAL_TANGENT},
        shortcut {0, 6, 1, shortcut::type::MAXIMAL_TANGENT},
    };
    std::vector<point_distributor::point_assignment> first_assignments {
        point_distributor::point_assignment {point {point::NO_LINE_ID, 4u, coordinate {0.5, 0}}, 0},
        point_distributor::point_assignment {point {point::NO_LINE_ID, 3u, coordinate {3.75, -0.5}}, 3},
    };
    auto first_accepted = acceptor(0, tangents_from_first, first_assignments);
    BOOST_CHECK_EQUAL(first_accepted.size(), 3);
    BOOST_CHECK_EQUAL(first_accepted[0].last, 1);
    BOOST_CHECK_EQUAL(first_accepted[1].last, 2);
    BOOST_CHECK_EQUAL(first_accepted[2].last, 4);

    // from fourth vertex
    std::vector<shortcut> tangents_from_fourth {
        shortcut {3, 5, 4, shortcut::type::MINIMAL_TANGENT},
        shortcut {3, 6, 5, shortcut::type::MAXIMAL_TANGENT},
    };
    std::vector<point_distributor::point_assignment> fourth_assignments {
        point_distributor::point_assignment {point {point::NO_LINE_ID, 3u, coordinate {3.75, -0.5}}, 1},
    };
    auto fourth_accepted = acceptor(3, tangents_from_fourth, fourth_assignments);
    BOOST_CHECK_EQUAL(fourth_accepted.size(), 2);
    BOOST_CHECK_EQUAL(fourth_accepted[0].last, 4);
    BOOST_CHECK_EQUAL(fourth_accepted[1].last, 5);
}

BOOST_AUTO_TEST_CASE(example_line_test)
{
    //  0
    //  |
    //  |     x
    //  |    3-----4
    //  |    |x
    //  |    |
    //  1----2
    //
    std::vector<coordinate> coords = {
        coordinate {0, 2},      // 0
        coordinate {0, 0},      // 1
        coordinate {1, 0},      // 2
        coordinate {1, 1},      // 3
        coordinate {2, 1},      // 4
    };

    std::vector<point> points = {
        {point::NO_LINE_ID, 0, coordinate {2.75, 2.25}}, // a
        {point::NO_LINE_ID, 1, coordinate {0.05, 1.9}},  // b
        {point::NO_LINE_ID, 2, coordinate {1.1, 0.9}},   // c
        {point::NO_LINE_ID, 3, coordinate {1.1, 1.1}},   // d
    };

    shortcut_acceptor acceptor({0, coords});
    {
        auto accepted = acceptor(0,
                {shortcut {0, 4, 1, shortcut::type::MAXIMAL_TANGENT}},
                {point_distributor::point_assignment {points[3], 0}}
        );
        BOOST_CHECK_EQUAL(accepted.size(), 3);
        BOOST_CHECK_EQUAL(accepted[0].last, 1);
        BOOST_CHECK_EQUAL(accepted[1].last, 2);
        BOOST_CHECK_EQUAL(accepted[2].last, 3);
    }
    {
        auto accepted = acceptor(1,
                { shortcut {1, 3, 1, shortcut::type::MAXIMAL_TANGENT}, shortcut {1, 4, 3, shortcut::type::MINIMAL_TANGENT} },
                { point_distributor::point_assignment {points[2], 1}}
        );
        BOOST_CHECK_EQUAL(accepted.size(), 2);
        BOOST_CHECK_EQUAL(accepted[0].last, 2);
        BOOST_CHECK_EQUAL(accepted[1].last, 3);
    }
    {
        auto accepted = acceptor(2,
                { shortcut {2, 4, 3, shortcut::type::MINIMAL_TANGENT} },
                { point_distributor::point_assignment {points[2], 0} }
        );
        BOOST_CHECK_EQUAL(accepted.size(), 1);
        BOOST_CHECK_EQUAL(accepted[0].last, 3);
    }
    {
        auto accepted = acceptor(3, { }, { });
        BOOST_CHECK_EQUAL(accepted.size(), 1);
        BOOST_CHECK_EQUAL(accepted[0].last, 4);
    }
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_multibody_data_hxx__
#define __pinocchio_multibody_data_hxx__

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/utils/string-generator.hpp"
#include "pinocchio/multibody/liegroup/liegroup-algo.hpp"

#include <boost/bind.hpp>
#include <boost/utility.hpp>

/// @cond DEV

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline DataTpl<Scalar,Options,JointCollectionTpl>::
  DataTpl(const Model & model)
  : joints(0)
  , a((std::size_t)model.njoints,Motion::Zero())
  , oa((std::size_t)model.njoints,Motion::Zero())
  , a_gf((std::size_t)model.njoints,Motion::Zero())
  , oa_gf((std::size_t)model.njoints,Motion::Zero())
  , v((std::size_t)model.njoints,Motion::Zero())
  , ov((std::size_t)model.njoints,Motion::Zero())
  , f((std::size_t)model.njoints,Force::Zero())
  , of((std::size_t)model.njoints,Force::Zero())
  , h((std::size_t)model.njoints,Force::Zero())
  , oh((std::size_t)model.njoints,Force::Zero())
  , oMi((std::size_t)model.njoints,SE3::Identity())
  , liMi((std::size_t)model.njoints,SE3::Identity())
  , tau(VectorXs::Zero(model.nv))
  , nle(VectorXs::Zero(model.nv))
  , g(VectorXs::Zero(model.nv))
  , oMf((std::size_t)model.nframes,SE3::Identity())
  , Ycrb((std::size_t)model.njoints,Inertia::Zero())
  , dYcrb((std::size_t)model.njoints,Inertia::Zero())
  , M(MatrixXs::Zero(model.nv,model.nv))
  , Minv(MatrixXs::Zero(model.nv,model.nv))
  , C(MatrixXs::Zero(model.nv,model.nv))
  , dHdq(Matrix6x::Zero(6,model.nv))
  , dFdq(Matrix6x::Zero(6,model.nv))
  , dFdv(Matrix6x::Zero(6,model.nv))
  , dFda(Matrix6x::Zero(6,model.nv))
  , SDinv(Matrix6x::Zero(6,model.nv))
  , UDinv(Matrix6x::Zero(6,model.nv))
  , IS(MatrixXs::Zero(6,model.nv))
  , vxI((std::size_t)model.njoints,Inertia::Matrix6::Zero())
  , Ivx((std::size_t)model.njoints,Inertia::Matrix6::Zero())
  , oYcrb((std::size_t)model.njoints,Inertia::Zero())
  , doYcrb((std::size_t)model.njoints,Inertia::Matrix6::Zero())
  , ddq(VectorXs::Zero(model.nv))
  , Yaba((std::size_t)model.njoints,Inertia::Matrix6::Zero())
  , u(VectorXs::Zero(model.nv))
  , Ag(Matrix6x::Zero(6,model.nv))
  , dAg(Matrix6x::Zero(6,model.nv))
  , hg(Force::Zero())
  , dhg(Force::Zero())
  , Ig(Inertia::Zero())
  , Fcrb((std::size_t)model.njoints,Matrix6x::Zero(6,model.nv))
  , lastChild((std::size_t)model.njoints,-1)
  , nvSubtree((std::size_t)model.njoints,-1)
  , start_idx_v_fromRow((std::size_t)model.nv,-1)
  , end_idx_v_fromRow((std::size_t)model.nv,-1)
  , U(MatrixXs::Identity(model.nv,model.nv))
  , D(VectorXs::Zero(model.nv))
  , Dinv(VectorXs::Zero(model.nv))
  , tmp(VectorXs::Zero(model.nv))
  , parents_fromRow((std::size_t)model.nv,-1)
  , supports_fromRow((std::size_t)model.nv)
  , nvSubtree_fromRow((std::size_t)model.nv,-1)
  , J(Matrix6x::Zero(6,model.nv))
  , dJ(Matrix6x::Zero(6,model.nv))
  , dVdq(Matrix6x::Zero(6,model.nv))
  , dAdq(Matrix6x::Zero(6,model.nv))
  , dAdv(Matrix6x::Zero(6,model.nv))
  , dtau_dq(MatrixXs::Zero(model.nv,model.nv))
  , dtau_dv(MatrixXs::Zero(model.nv,model.nv))
  , ddq_dq(MatrixXs::Zero(model.nv,model.nv))
  , ddq_dv(MatrixXs::Zero(model.nv,model.nv))
  , iMf((std::size_t)model.njoints,SE3::Identity())
  , com((std::size_t)model.njoints,Vector3::Zero())
  , vcom((std::size_t)model.njoints,Vector3::Zero())
  , acom((std::size_t)model.njoints,Vector3::Zero())
  , mass((std::size_t)model.njoints,(Scalar)(-1))
  , Jcom(Matrix3x::Zero(3,model.nv))
  , kinetic_energy((Scalar)-1)
  , potential_energy((Scalar)-1)
  , JMinvJt()
  , llt_JMinvJt()
  , lambda_c()
  , sDUiJt(MatrixXs::Zero(model.nv,model.nv))
  , torque_residual(VectorXs::Zero(model.nv))
  , dq_after(VectorXs::Zero(model.nv))
  , impulse_c()
  , staticRegressor(Matrix3x::Zero(3,4*(model.njoints-1)))
  , bodyRegressor(BodyRegressorType::Zero())
  , jointTorqueRegressor(MatrixXs::Zero(model.nv,10*(model.njoints-1)))
#if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93)
  , kinematic_hessians(6,std::max(1,model.nv),std::max(1,model.nv)) // the minimum size should be 1 for compatibility reasons
#else
  , kinematic_hessians(6,model.nv,model.nv)
#endif
  {
    typedef typename Model::JointIndex JointIndex;

    /* Create data structure associated to the joints */
    for(JointIndex i=0;i<(JointIndex)(model.njoints);++i)
      joints.push_back(CreateJointData<Scalar,Options,JointCollectionTpl>::run(model.joints[i]));

    /* Init for CRBA */
    M.setZero(); Minv.setZero();
    for(JointIndex i=0;i<(JointIndex)(model.njoints);++i)
    { Fcrb[i].resize(6,model.nv); }

    computeLastChild(model);

    /* Init for Coriolis */

    /* Init for Cholesky */
    computeParents_fromRow(model);
    computeSupports_fromRow(model);

    /* Init universe states relatively to itself */
    a_gf[0] = -model.gravity;

    kinematic_hessians.setZero();
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void DataTpl<Scalar,Options,JointCollectionTpl>::
  computeLastChild(const Model & model)
  {
    typedef typename Model::Index Index;

    std::fill(lastChild.begin(),lastChild.end(),-1);
    for( int i=model.njoints-1;i>=0;--i )
    {
      if(lastChild[(Index)i] == -1) lastChild[(Index)i] = i;
      const Index & parent = model.parents[(Index)i];
      lastChild[parent] = std::max(lastChild[(Index)i],lastChild[parent]);

      nvSubtree[(Index)i]
      = idx_v(model.joints[(Index)lastChild[(Index)i]]) + nv(model.joints[(Index)lastChild[(Index)i]])
      - idx_v(model.joints[(Index)i]);
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void DataTpl<Scalar,Options,JointCollectionTpl>
  ::computeParents_fromRow(const Model & model)
  {
    typedef typename Model::Index Index;

    for(Index joint=1;joint<(Index)(model.njoints);joint++)
    {
      const Index & parent = model.parents[joint];
      const int nvj    = nv   (model.joints[joint]);
      const int idx_vj = idx_v(model.joints[joint]);

      assert(idx_vj >= 0 && idx_vj < model.nv);

      if(parent>0) parents_fromRow[(size_t)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
      else         parents_fromRow[(size_t)idx_vj] = -1;
      nvSubtree_fromRow[(size_t)idx_vj] = nvSubtree[joint];

      start_idx_v_fromRow[(size_t)idx_vj] = idx_vj;
      end_idx_v_fromRow[(size_t)idx_vj] = idx_vj+nvj-1;
      for(int row=1;row<nvj;++row)
      {
        parents_fromRow[(size_t)(idx_vj+row)] = idx_vj+row-1;
        nvSubtree_fromRow[(size_t)(idx_vj+row)] = nvSubtree[joint]-row;
        start_idx_v_fromRow[(size_t)(idx_vj+row)] = start_idx_v_fromRow[(size_t)idx_vj];
        end_idx_v_fromRow[(size_t)(idx_vj+row)] = end_idx_v_fromRow[(size_t)idx_vj];
      }
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void DataTpl<Scalar,Options,JointCollectionTpl>
  ::computeSupports_fromRow(const Model & model)
  {
    typedef typename Model::JointIndex JointIndex;

    for(JointIndex joint_id = 1;
        joint_id < (JointIndex)(model.njoints);
        joint_id++)
    {
      const int nvj    = nv   (model.joints[joint_id]);
      const int idx_vj = idx_v(model.joints[joint_id]);

      assert(idx_vj >= 0 && idx_vj < model.nv);

      const int parent_fromRow = parents_fromRow[(size_t)idx_vj];

      if(parent_fromRow >= 0)
        supports_fromRow[(size_t)idx_vj] = supports_fromRow[(size_t)parent_fromRow];

      supports_fromRow[(size_t)idx_vj].push_back(idx_vj);

      for(int row = 1; row < nvj; ++row)
      {
        supports_fromRow[(size_t)(idx_vj+row)] = supports_fromRow[(size_t)(idx_vj+row-1)];
        supports_fromRow[(size_t)(idx_vj+row)].push_back(idx_vj+row);
      }
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  bool operator==(const DataTpl<Scalar,Options,JointCollectionTpl> & data1,
                  const DataTpl<Scalar,Options,JointCollectionTpl> & data2)
  {
    bool value =
       data1.joints == data2.joints
    && data1.a == data2.a
    && data1.oa == data2.oa
    && data1.a_gf == data2.a_gf
    && data1.oa_gf == data2.oa_gf
    && data1.v == data2.v
    && data1.ov == data2.ov
    && data1.f == data2.f
    && data1.of == data2.of
    && data1.h == data2.h
    && data1.oh == data2.oh
    && data1.oMi == data2.oMi
    && data1.liMi == data2.liMi
    && data1.tau == data2.tau
    && data1.nle == data2.nle
    && data1.g == data2.g
    && data1.oMf == data2.oMf
    && data1.Ycrb == data2.Ycrb
    && data1.dYcrb == data2.dYcrb
    && data1.M == data2.M
    && data1.Minv == data2.Minv
    && data1.C == data2.C
    && data1.dHdq == data2.dHdq
    && data1.dFdq == data2.dFdq
    && data1.dFdv == data2.dFdv
    && data1.dFda == data2.dFda
    && data1.SDinv == data2.SDinv
    && data1.UDinv == data2.UDinv
    && data1.IS == data2.IS
    && data1.vxI == data2.vxI
    && data1.Ivx == data2.Ivx
    && data1.oYcrb == data2.oYcrb
    && data1.doYcrb == data2.doYcrb
    && data1.ddq == data2.ddq
    && data1.Yaba == data2.Yaba
    && data1.u == data2.u
    && data1.Ag == data2.Ag
    && data1.dAg == data2.dAg
    && data1.hg == data2.hg
    && data1.dhg == data2.dhg
    && data1.Ig == data2.Ig
    && data1.Fcrb == data2.Fcrb
    && data1.lastChild == data2.lastChild
    && data1.nvSubtree == data2.nvSubtree
    && data1.start_idx_v_fromRow == data2.start_idx_v_fromRow
    && data1.end_idx_v_fromRow == data2.end_idx_v_fromRow
    && data1.U == data2.U
    && data1.D == data2.D
    && data1.Dinv == data2.Dinv
    && data1.parents_fromRow == data2.parents_fromRow
    && data1.supports_fromRow == data2.supports_fromRow
    && data1.nvSubtree_fromRow == data2.nvSubtree_fromRow
    && data1.J == data2.J
    && data1.dJ == data2.dJ
    && data1.dVdq == data2.dVdq
    && data1.dAdq == data2.dAdq
    && data1.dAdv == data2.dAdv
    && data1.dtau_dq == data2.dtau_dq
    && data1.dtau_dv == data2.dtau_dv
    && data1.ddq_dq == data2.ddq_dq
    && data1.ddq_dv == data2.ddq_dv
    && data1.iMf == data2.iMf
    && data1.com == data2.com
    && data1.vcom == data2.vcom
    && data1.acom == data2.acom
    && data1.mass == data2.mass
    && data1.Jcom == data2.Jcom
    && data1.kinetic_energy == data2.kinetic_energy
    && data1.potential_energy == data2.potential_energy
    && data1.JMinvJt == data2.JMinvJt
    && data1.lambda_c == data2.lambda_c
    && data1.torque_residual == data2.torque_residual
    && data1.dq_after == data2.dq_after
    && data1.impulse_c == data2.impulse_c
    && data1.staticRegressor == data2.staticRegressor
    && data1.bodyRegressor == data2.bodyRegressor
    && data1.jointTorqueRegressor == data2.jointTorqueRegressor
    ;

    // operator== for Eigen::Tensor provides an Expression which might be not evaluated as a boolean
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    typedef Eigen::Map<const typename Data::VectorXs> MapVectorXs;
    value &=
       MapVectorXs(data1.kinematic_hessians.data(),data1.kinematic_hessians.size())
    == MapVectorXs(data2.kinematic_hessians.data(),data2.kinematic_hessians.size());

    return value;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  bool operator!=(const DataTpl<Scalar,Options,JointCollectionTpl> & data1,
                  const DataTpl<Scalar,Options,JointCollectionTpl> & data2)
  {
    return !(data1 == data2);
  }

} // namespace pinocchio

/// @endcond

#endif // ifndef __pinocchio_multibody_data_hxx__