//#define CGAL_PMP_REMESHING_VERBOSE
//#define CGAL_PMP_REMESHING_DEBUG
//#define CGAL_PMP_REMESHING_VERY_VERBOSE
//#define CGAL_PMP_REMESHING_VERBOSE_PROGRESS

#include <QtCore/qglobal.h>

#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>

#include "Scene_surface_mesh_item.h"

#include "Scene_polyhedron_selection_item.h"

#include <CGAL/iterator.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/boost/graph/properties_Polyhedron_3.h>
#include <CGAL/Polygon_mesh_processing/triangulate_faces.h>
#include <CGAL/utility.h>

#include <boost/graph/graph_traits.hpp>
#include <boost/unordered_set.hpp>
#include <CGAL/property_map.h>

#include <QElapsedTimer>
#include <QAction>
#include <QMainWindow>
#include <QApplication>
#include <QString>
#include <QDialog>
#include <QtPlugin>
#include <QMessageBox>

#include <vector>
#include <algorithm>
#include <queue>
#include <sstream>
#include <cmath>

#ifdef CGAL_LINKED_WITH_TBB
#include "tbb/parallel_for.h"
#include "tbb/blocked_range.h"
#include "tbb/partitioner.h"
#endif

#include "ui_Isotropic_remeshing_dialog.h"

typedef Scene_surface_mesh_item Scene_facegraph_item;
typedef Scene_facegraph_item::Face_graph FaceGraph;
typedef boost::graph_traits<FaceGraph>::face_descriptor face_descriptor;

// give a halfedge and a target edge length, put in `out` points
// which the edge equally spaced such that splitting the edge
// using the sequence of points make the edges shorter than
// `target_length`
template <class TriangleMesh, class PointPMap, class PointOutputIterator>
PointOutputIterator
sample_edge(
  typename boost::graph_traits<TriangleMesh>::halfedge_descriptor hd,
  TriangleMesh& triangle_mesh,
  double target_length,
  const PointPMap& pmap,
  PointOutputIterator out)
{
  typedef typename boost::property_traits<PointPMap>::value_type Point_3;
  typedef typename CGAL::Kernel_traits<Point_3>::Kernel::Vector_3 Vector_3;
  typename boost::property_traits<PointPMap>::reference src=get(pmap, source(hd,triangle_mesh) );
  typename boost::property_traits<PointPMap>::reference tgt=get(pmap, target(hd,triangle_mesh) );

  double length = std::sqrt( CGAL::squared_distance(src, tgt) );
  if ( length <= target_length ) return out;

  double nb_points = std::floor( length / target_length );
  Vector_3 unit = (tgt-src) / (nb_points+1);

  for(double i=0; i<nb_points; ++i)
    *out++=src+unit*(i+1);

  return out;
}

// given a set of points that are expected to be on an edge, split
// that edge and retriangulate the face incident to the edge
// Points are sorted so that they are sorted from the source to the target
// of the edge (the sequence does not contains edge endpoints)
template <class TriangleMesh, class PointPMap, class PointRange, class EdgeOutputIterator>
EdgeOutputIterator
split_identical_edges(
  typename boost::graph_traits<TriangleMesh>::halfedge_descriptor hd,
  TriangleMesh& tm,
  const PointPMap& pmap,
  const PointRange& points,
  EdgeOutputIterator out)
{
  typedef typename PointRange::value_type Point_3;
  typedef boost::graph_traits<TriangleMesh> GT;
  typedef typename GT::halfedge_descriptor halfedge_descriptor;

  for(const Point_3& p : points)
  {
    // split the edge
    halfedge_descriptor new_hd=CGAL::Euler::split_edge(hd,tm);
    // set the vertex point
    put(pmap, target(new_hd, tm), p);
    *out++=edge(new_hd, tm);
  }
  *out++=edge(hd, tm);
  return out;
}

// HedgeRange is expected to be a range with value type being
// std::pair<halfedge_descriptor, TriangleMesh*>
// Given a set of halfedges representing different edges
// but with identical endpoints, and a target edge length
// we split all edges identically so that subedges are
// or length <= length
template <class HedgeRange, class Edges_to_protect>
void split_long_duplicated_edge(const HedgeRange& hedge_range,
                                double target_length,
                                Edges_to_protect& edges_to_protect)
{
  typedef typename HedgeRange::value_type Pair;
  typedef typename Pair::first_type halfedge_descriptor;
  typedef typename boost::remove_pointer<
    typename Pair::second_type>::type TriangleMesh;
  typedef typename boost::property_map<TriangleMesh,
    CGAL::vertex_point_t>::type PointPMap;
  typedef typename boost::property_traits<PointPMap>::value_type Point_3;

  if (hedge_range.empty()) return;

  const Pair& p = *hedge_range.begin();
  PointPMap pmap = get(boost::vertex_point, *p.second);

  std::vector<Point_3> points;
  halfedge_descriptor hd = p.first;

  // collect points to be add inside the edges
  sample_edge(hd, *p.second, target_length, pmap, std::back_inserter(points) );

  CGAL_assertion_code(Point_3 src = get(pmap, source(hd, *p.second));)
  CGAL_assertion_code(Point_3 tgt = get(pmap, target(hd, *p.second));)

  // split the edges and collect faces to triangulate
  for(const Pair& h_and_p : hedge_range)
  {
    halfedge_descriptor hc=h_and_p.first;
    TriangleMesh* polyc = h_and_p.second;

    PointPMap pmap_2 = get(boost::vertex_point, *polyc);
    //make sure halfedge are consistently oriented
    CGAL_assertion( get(pmap_2, source(hc, *polyc)) == src );
    CGAL_assertion( get(pmap_2, target(hc, *polyc)) == tgt );

    typedef typename Edges_to_protect::value_type::second_type Edge_set;
    Edge_set& edge_set = edges_to_protect[polyc];

    // now split the halfedge and incident faces
    split_identical_edges(hc,*polyc,pmap_2, points,
                          std::inserter( edge_set, edge_set.begin()));
  }
}

using namespace CGAL::Three;
class Polyhedron_demo_isotropic_remeshing_plugin :
  public QObject,
  public Polyhedron_demo_plugin_interface
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0" FILE "isotropic_remeshing_plugin.json")

  typedef boost::graph_traits<FaceGraph>::edge_descriptor edge_descriptor;
  typedef boost::graph_traits<FaceGraph>::halfedge_descriptor halfedge_descriptor;
  typedef boost::graph_traits<FaceGraph>::face_descriptor face_descriptor;

  typedef boost::unordered_set<edge_descriptor>    Edge_set;
  typedef Scene_polyhedron_selection_item::Is_constrained_map<Edge_set> Edge_constrained_pmap;

public:
  void init(QMainWindow* mainWindow, Scene_interface* scene_interface, Messages_interface*)
  {
    this->scene = scene_interface;
    this->mw = mainWindow;

    actionIsotropicRemeshing_ = new QAction("Isotropic Remeshing", mw);
    actionIsotropicRemeshing_->setProperty("subMenuName", "Polygon Mesh Processing");
    if (actionIsotropicRemeshing_) {
      connect(actionIsotropicRemeshing_, SIGNAL(triggered()),
        this, SLOT(isotropic_remeshing()));
    }
  }

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

  bool applicable(QAction*) const
  {
    if (scene->selectionIndices().size() == 1)
    {
    return qobject_cast<Scene_facegraph_item*>(scene->item(scene->mainSelectionIndex()))
    || qobject_cast<Scene_polyhedron_selection_item*>(scene->item(scene->mainSelectionIndex()));
    }

    Q_FOREACH(int index, scene->selectionIndices())
    {
      //if one polyhedron is found in the selection, it's fine
      if (qobject_cast<Scene_facegraph_item*>(scene->item(index)))
        return true;
    }
    return false;
  }

  typedef boost::property_map<FaceGraph, CGAL::face_patch_id_t<int> >::type Patch_id_pmap;

  void detect_and_split_duplicates(std::vector<Scene_facegraph_item*>& selection,
                                   std::map<FaceGraph*,Edge_set>& edges_to_protect,
                                   double target_length)
  {
    typedef EPICK::Point_3 Point_3;
    typedef std::pair<Point_3,Point_3> Segment_3;

    typedef std::map< Segment_3,
                      std::vector< std::pair<halfedge_descriptor, FaceGraph*> > > MapType;
    typedef boost::property_map<FaceGraph,
      CGAL::vertex_point_t>::type PointPMap;
    MapType duplicated_edges;

    for(Scene_facegraph_item* poly_item : selection){
      FaceGraph& pmesh = *poly_item->polyhedron();
      PointPMap pmap = get(boost::vertex_point, pmesh);
      for(edge_descriptor ed : edges(pmesh)){
        halfedge_descriptor hd = halfedge(ed,pmesh);
        Point_3 p = get(pmap, source(hd,pmesh)), q = get(pmap, target(hd,pmesh));
        Segment_3 s = CGAL::make_sorted_pair(p,q);
        if (s.first==q) hd=opposite(hd,pmesh); // make sure the halfedges are consistently oriented

        duplicated_edges[s].push_back( std::make_pair(hd,&pmesh) );
      }
    }

    // consistently split duplicate edges and triangulate incident faces
    typedef std::pair<face_descriptor, FaceGraph*> Face_and_poly;
    std::set< Face_and_poly > faces_to_triangulate;
    for(const MapType::value_type& p : duplicated_edges)
      if (p.second.size()>1){
        //collect faces to retriangulate
        typedef std::pair<halfedge_descriptor, FaceGraph*> Pair_type;
        for(const Pair_type& h_and_p : p.second)
        {
          halfedge_descriptor hc=h_and_p.first;
          FaceGraph* polyc = h_and_p.second;

          if ( !is_border(hc, *polyc) )
            faces_to_triangulate.insert( Face_and_poly(face(hc,*polyc), polyc) );
          if ( !is_border(opposite(hc, *polyc), *polyc) )
            faces_to_triangulate.insert(
              Face_and_poly(face(opposite(hc, *polyc),*polyc), polyc) );
        }
        // split the edges
        split_long_duplicated_edge(p.second, target_length, edges_to_protect);
      }
    // now retriangulate
    namespace PMP=CGAL::Polygon_mesh_processing;
    for(Face_and_poly f_and_p : faces_to_triangulate)
      PMP::triangulate_face(f_and_p.first, *f_and_p.second);
  }

  void do_split_edges(Scene_polyhedron_selection_item* selection_item,
                      SMesh& pmesh,
                      double target_length)
  {
    std::vector<edge_descriptor> p_edges;
    for(edge_descriptor e : edges(pmesh))
    {
      if(get(selection_item->constrained_edges_pmap(), e))
        p_edges.push_back(e);
    }
    for(face_descriptor f : selection_item->selected_facets)
    {
      for(halfedge_descriptor he : halfedges_around_face(halfedge(f, pmesh), pmesh))
      {
        if (selection_item->selected_facets.find(face(opposite(he, pmesh), pmesh))
            == selection_item->selected_facets.end())
          p_edges.push_back(edge(he, pmesh));
      }
    }
    if (!p_edges.empty())
      CGAL::Polygon_mesh_processing::split_long_edges(
            p_edges
            , target_length
            , *selection_item->polyhedron()
            , PMP::parameters::geom_traits(EPICK())
            .edge_is_constrained_map(selection_item->constrained_edges_pmap()));
    else
      std::cout << "No selected or boundary edges to be split" << std::endl;
  }

public Q_SLOTS:
  void isotropic_remeshing()
  {
    if (scene->selectionIndices().size() > 1)
    {
      isotropic_remeshing_of_several_polyhedra();
      return;
    }
    const Scene_interface::Item_id index = scene->mainSelectionIndex();

    Scene_facegraph_item* poly_item =
      qobject_cast<Scene_facegraph_item*>(scene->item(index));

    Scene_polyhedron_selection_item* selection_item =
      qobject_cast<Scene_polyhedron_selection_item*>(scene->item(index));

    if (poly_item || selection_item)
    {
      // Create dialog box
      QDialog dialog(mw);
      Ui::Isotropic_remeshing_dialog ui
        = remeshing_dialog(&dialog, poly_item, selection_item);

      // Get values
      int i = dialog.exec();
      if (i == QDialog::Rejected)
      {
        std::cout << "Remeshing aborted" << std::endl;
        return;
      }
      bool edges_only = ui.splitEdgesOnly_checkbox->isChecked();
      bool preserve_duplicates = ui.preserveDuplicates_checkbox->isChecked();
      double target_length = ui.edgeLength_dspinbox->value();
      unsigned int nb_iter = ui.nbIterations_spinbox->value();
      unsigned int nb_smooth = ui.nbSmoothing_spinbox->value();
      bool protect = ui.protect_checkbox->isChecked();
      bool smooth_features = ui.smooth1D_checkbox->isChecked();

      // wait cursor
      QApplication::setOverrideCursor(Qt::WaitCursor);

      QElapsedTimer time;
      time.start();

      typedef boost::graph_traits<FaceGraph>::edge_descriptor edge_descriptor;
      typedef boost::graph_traits<FaceGraph>::face_descriptor face_descriptor;

      FaceGraph& pmesh = (poly_item != NULL)
        ? *poly_item->polyhedron()
        : *selection_item->polyhedron();

     Patch_id_pmap fpmap = get(CGAL::face_patch_id_t<int>(), pmesh);
     bool fpmap_valid = false;
     {
       for(face_descriptor f : faces(pmesh))
       {
         if (get(fpmap, f) != 1)
         {
           fpmap_valid = true;
           break;/*1 is the default value for both Surface_mesh and Polyhedron*/
         }
       }
     }

      if (selection_item)
      {
        if (edges_only)
        {
          do_split_edges(selection_item, pmesh, target_length);
        }
        else //not edges_only
        {
            if(protect &&
               !CGAL::Polygon_mesh_processing::internal::constraints_are_short_enough(
                 *selection_item->polyhedron(),
                 selection_item->constrained_edges_pmap(),
                 get(CGAL::vertex_point, *selection_item->polyhedron()),
                 CGAL::Static_property_map<face_descriptor, std::size_t>(1),
                 4. / 3. * target_length))
            {
              QApplication::restoreOverrideCursor();
              //If facets are selected, splitting edges will add facets that won't be selected, and it will mess up the rest.
              //If there is only edges, it will work fine because new edges are dealt with in the code, so we can directly
              //split and continue.
              // Possibility todo: check if the barycenter of a new face is inside an old selected face to
              //select it again.
              if(!selection_item->selected_facets.empty())
              {
                QMessageBox::warning(mw, tr("Error"),
                                      tr("Isotropic remeshing : protect_constraints cannot be set to"
                                         " true with constraints larger than 4/3 * target_edge_length."
                                         " Aborting."));
                return;
              }
              else if(QMessageBox::question(mw, tr("Error"),
                                            tr("Isotropic remeshing : protect_constraints cannot be set to"
                                               " true with constraints larger than 4/3 * target_edge_length."
                                               " Do you wish to split the constrained edges ?")) !=
                      QMessageBox::Yes)
              {
                return;
              }
              else
              {
                QApplication::setOverrideCursor(Qt::WaitCursor);
                do_split_edges(selection_item, pmesh, target_length);
              }
            }

            if (selection_item->selected_facets.empty() && !selection_item->isEmpty())
            {
              if (fpmap_valid)
                CGAL::Polygon_mesh_processing::isotropic_remeshing(faces(*selection_item->polyhedron())
                   , target_length
                   , *selection_item->polyhedron()
                   , CGAL::Polygon_mesh_processing::parameters::number_of_iterations(nb_iter)
                   .protect_constraints(protect)
                   .edge_is_constrained_map(selection_item->constrained_edges_pmap())
                   .relax_constraints(smooth_features)
                   .number_of_relaxation_steps(nb_smooth)
                   .vertex_is_constrained_map(selection_item->constrained_vertices_pmap())
                   .face_patch_map(fpmap));
              else
                CGAL::Polygon_mesh_processing::isotropic_remeshing(faces(*selection_item->polyhedron())
                   , target_length
                   , *selection_item->polyhedron()
                   , CGAL::Polygon_mesh_processing::parameters::number_of_iterations(nb_iter)
                   .protect_constraints(protect)
                   .edge_is_constrained_map(selection_item->constrained_edges_pmap())
                   .relax_constraints(smooth_features)
                   .number_of_relaxation_steps(nb_smooth)
                   .vertex_is_constrained_map(selection_item->constrained_vertices_pmap())
                                                                   );
            }
            else //selected_facets not empty
            {
              if (fpmap_valid)
                CGAL::Polygon_mesh_processing::isotropic_remeshing(selection_item->selected_facets
                  , target_length
                  , *selection_item->polyhedron()
                  , CGAL::Polygon_mesh_processing::parameters::number_of_iterations(nb_iter)
                  .protect_constraints(protect)
                  .edge_is_constrained_map(selection_item->constrained_edges_pmap())
                  .relax_constraints(smooth_features)
                  .number_of_relaxation_steps(nb_smooth)
                  .vertex_is_constrained_map(selection_item->constrained_vertices_pmap())
                  .face_patch_map(fpmap));
              else
                CGAL::Polygon_mesh_processing::isotropic_remeshing(selection_item->selected_facets
                  , target_length
                  , *selection_item->polyhedron()
                  , CGAL::Polygon_mesh_processing::parameters::number_of_iterations(nb_iter)
                  .protect_constraints(protect)
                  .edge_is_constrained_map(selection_item->constrained_edges_pmap())
                  .relax_constraints(smooth_features)
                  .number_of_relaxation_steps(nb_smooth)
                  .vertex_is_constrained_map(selection_item->constrained_vertices_pmap()));
            }
        }

        SMesh mesh_ = *selection_item->polyhedron();
        std::vector<bool> are_edges_removed;
        are_edges_removed.resize(mesh_.number_of_edges()+mesh_.number_of_removed_edges());
        std::vector<bool> are_edges_constrained;
        are_edges_constrained.resize(are_edges_removed.size());
        for(std::size_t i=0; i< are_edges_removed.size(); ++i)
        {
          are_edges_removed[i] = mesh_.is_removed(SMesh::Edge_index(static_cast<int>(i)));
          if(!are_edges_removed[i])
            are_edges_constrained[i] = get(selection_item->constrained_edges_pmap(), SMesh::Edge_index(static_cast<int>(i)));
        }

        int i0, i1,
            nE(mesh_.number_of_edges()+mesh_.number_of_removed_edges());

        //get constrained values in order.
        if (nE > 0)
        {
          i0=0;  i1=nE-1;
          while (1)
          {
            // find first removed and last un-removed
            while (!are_edges_removed[i0] && i0 < i1) ++i0;
            while ( are_edges_removed[i1] && i0 < i1) --i1;
            if (i0 >= i1) break;

            // swap
            std::swap(are_edges_constrained[i0], are_edges_constrained[i1]);
            std::swap(are_edges_removed[i0], are_edges_removed[i1]);
          }
          // remember new size
          nE = are_edges_removed[i0] ? i0 : i0+1;
        }
        selection_item->polyhedron_item()->setColor(
              selection_item->polyhedron_item()->color());
        if(fpmap_valid)
        {
          selection_item->polyhedron_item()->setItemIsMulticolor(true);
          selection_item->polyhedron_item()->computeItemColorVectorAutomatically(true);
        }
        else
        {
          selection_item->polyhedron_item()->setItemIsMulticolor(false);
        }

        selection_item->polyhedron_item()->polyhedron()->collect_garbage();
        //fix constrained_edges_map
        for(int i=0; i< nE; ++i)
        {
          Scene_polyhedron_selection_item::Is_constrained_map<Scene_polyhedron_selection_item::Selection_set_edge>
              pmap = selection_item->constrained_edges_pmap();
          put(pmap, SMesh::Edge_index(i), are_edges_constrained[i]);
        }

        selection_item->poly_item_changed();
        selection_item->clear<face_descriptor>();
        selection_item->changed_with_poly_item();
      }
      else if (poly_item)
      {
        boost::property_map<FaceGraph, CGAL::edge_is_feature_t>::type eif
          = get(CGAL::edge_is_feature, pmesh);
        if (edges_only)
        {
          std::vector<edge_descriptor> edges_to_split;
          for(edge_descriptor e : edges(pmesh))
          {
            if( is_border(e, pmesh) || get(eif, e) )
              edges_to_split.push_back(e);
          }

          if (!edges_to_split.empty())
          {
            if (fpmap_valid)
              CGAL::Polygon_mesh_processing::split_long_edges(
                edges_to_split
                , target_length
                , pmesh
                , PMP::parameters::geom_traits(EPICK())
                . edge_is_constrained_map(eif)
                . face_patch_map(fpmap));
            else
              CGAL::Polygon_mesh_processing::split_long_edges(
                edges_to_split
                , target_length
                , pmesh
                , PMP::parameters::geom_traits(EPICK())
                . edge_is_constrained_map(eif));
          }
          else
            std::cout << "No border to be split" << std::endl;
        }
        else
        {
          // tricks to use the function detect_and_split_duplicates
          // that uses several poly items
          std::map<FaceGraph*, Edge_set > edges_to_protect_map;
          std::vector<Scene_facegraph_item*> poly_items(1, poly_item);
          Edge_set& edges_to_protect = edges_to_protect_map[poly_item->polyhedron()];
          if (preserve_duplicates)
          {
            detect_and_split_duplicates(poly_items, edges_to_protect_map, target_length);

          }
          Scene_polyhedron_selection_item::Is_constrained_map<Edge_set> ecm(&edges_to_protect);
          for(edge_descriptor e : edges(pmesh))
          {
            if (eif[e])
              edges_to_protect.insert(e);
          }

          if(protect &&
             !CGAL::Polygon_mesh_processing::internal::constraints_are_short_enough(
               pmesh,
               ecm,
               get(CGAL::vertex_point, pmesh),
               CGAL::Static_property_map<face_descriptor, std::size_t>(1),
               4. / 3. * target_length))
          {
            QApplication::restoreOverrideCursor();
            QMessageBox::warning(mw, tr("Error"),
                                 tr("Isotropic remeshing : protect_constraints cannot be set to"
                                    " true with constraints larger than 4/3 * target_edge_length."
                                    " Aborting."));
            return;
          }
          if (fpmap_valid)
            CGAL::Polygon_mesh_processing::isotropic_remeshing(
                 faces(*poly_item->polyhedron())
               , target_length
               , *poly_item->polyhedron()
               , CGAL::Polygon_mesh_processing::parameters::number_of_iterations(nb_iter)
               .protect_constraints(protect)
               .number_of_relaxation_steps(nb_smooth)
               .edge_is_constrained_map(ecm)
               .relax_constraints(smooth_features)
               .face_patch_map(fpmap));
          else
            CGAL::Polygon_mesh_processing::isotropic_remeshing(
                 faces(*poly_item->polyhedron())
               , target_length
               , *poly_item->polyhedron()
               , CGAL::Polygon_mesh_processing::parameters::number_of_iterations(nb_iter)
               .protect_constraints(protect)
               .number_of_relaxation_steps(nb_smooth)
               .edge_is_constrained_map(ecm)
               .relax_constraints(smooth_features));
        }
        if (fpmap_valid)
        {
          PMP::connected_components(pmesh, fpmap, PMP::parameters::edge_is_constrained_map(eif));
          poly_item->setItemIsMulticolor(true);
          poly_item->show_feature_edges(true);
        }
        else
          poly_item->setItemIsMulticolor(false);

        poly_item->invalidateOpenGLBuffers();

        Q_EMIT poly_item->itemChanged();
      }
      else{
        std::cout << "Can't remesh that type of thing" << std::endl;
      }
      std::cout << "ok (" << time.elapsed() << " ms)" << std::endl;

      // default cursor
      QApplication::restoreOverrideCursor();
    }
  }

  void isotropic_remeshing_of_several_polyhedra()
  {
    // Remeshing parameters
    bool edges_only = false, preserve_duplicates = false;
    double target_length = 0.;
    unsigned int nb_iter = 1;
    bool protect = false;
    bool smooth_features = true;

    std::vector<Scene_facegraph_item*> selection;
    for(int index : scene->selectionIndices())
    {
      Scene_facegraph_item* poly_item =
        qobject_cast<Scene_facegraph_item*>(scene->item(index));

      if (poly_item == NULL)
      {
        std::cout << scene->item(index)->name().data()
          << " is not a FaceGraph, remeshing skipped\n";
        continue;
      }
      else
      {
        selection.push_back(poly_item);

        if (target_length == 0.)//parameters have not been set yet
        {
        QDialog dialog(mw);
        Ui::Isotropic_remeshing_dialog ui = remeshing_dialog(&dialog, poly_item);
        ui.objectName->setText(QString::number(scene->selectionIndices().size())
          .append(QString(" items to be remeshed")));
        int i = dialog.exec();
        if (i == QDialog::Rejected)
        {
          std::cout << "Remeshing aborted" << std::endl;
          return;
        }

        edges_only = ui.splitEdgesOnly_checkbox->isChecked();
        preserve_duplicates = ui.preserveDuplicates_checkbox->isChecked();
        target_length = ui.edgeLength_dspinbox->value();
        nb_iter = ui.nbIterations_spinbox->value();
        protect = ui.protect_checkbox->isChecked();
        smooth_features = ui.smooth1D_checkbox->isChecked();
        }
      }
    }

    if(target_length == 0.)//parameters have not been set
    {                      // i.e. no item is a polyhedron
      std::cout << "Remeshing aborted" << std::endl;
      return;
    }

    // wait cursor
    QApplication::setOverrideCursor(Qt::WaitCursor);
    int total_time = 0;

    //     typedef boost::graph_traits<FaceGraph>::edge_descriptor edge_descriptor;
    std::map<FaceGraph*,Edge_set > edges_to_protect;

    if(preserve_duplicates)
      detect_and_split_duplicates(selection, edges_to_protect, target_length);

#ifdef CGAL_LINKED_WITH_TBB
    QElapsedTimer time;
    time.start();

      tbb::parallel_for(
        tbb::blocked_range<std::size_t>(0, selection.size()),
        Remesh_polyhedron_item_for_parallel_for<Remesh_polyhedron_item>(
                                                                        selection, edges_to_protect, edges_only, target_length, nb_iter, protect, smooth_features));

    total_time = time.elapsed();

#else

    Remesh_polyhedron_item remesher(edges_only,
      target_length, nb_iter, protect, smooth_features);
    for(Scene_facegraph_item* poly_item : selection)
    {
      QElapsedTimer time;
      time.start();

      remesher(poly_item, edges_to_protect[poly_item->polyhedron()]);

      total_time += time.elapsed();
      std::cout << "Remeshing of " << poly_item->name().data()
                << " done in " << time.elapsed() << " ms" << std::endl;
    }
#endif
    std::cout << "Remeshing of all selected items done in "
      << total_time << " ms" << std::endl;

    for(Scene_facegraph_item* poly_item : selection)
    {
      //destroys the patch_id_map for the Surface_mesh_item to avoid assertions.
      poly_item->resetColors();
      poly_item->invalidateOpenGLBuffers();
      Q_EMIT poly_item->itemChanged();
    }

    // default cursor
    QApplication::restoreOverrideCursor();
  }

private:
  Scene_interface *scene;
  QMainWindow* mw;
  struct Remesh_polyhedron_item
  {
    typedef boost::graph_traits<FaceGraph>::edge_descriptor     edge_descriptor;
    typedef boost::graph_traits<FaceGraph>::halfedge_descriptor halfedge_descriptor;
    typedef boost::graph_traits<FaceGraph>::face_descriptor     face_descriptor;

    bool edges_only_;
    double target_length_;
    unsigned int nb_iter_;
    bool protect_;
    bool smooth_features_;

  protected:
    void remesh(Scene_facegraph_item* poly_item,
                Edge_set& edges_to_protect) const
    {
      //fill face_index property map

      if (edges_only_)
      {
        std::vector<halfedge_descriptor> border;
        CGAL::Polygon_mesh_processing::border_halfedges(
          faces(*poly_item->polyhedron())
          , *poly_item->polyhedron()
          , std::back_inserter(border));
        std::vector<edge_descriptor> border_edges;
        for(halfedge_descriptor h : border)
          border_edges.push_back(edge(h, *poly_item->polyhedron()));

        CGAL::Polygon_mesh_processing::split_long_edges(
            border_edges
          , target_length_
          , *poly_item->polyhedron());
      }
      else
      {
        std::cout << "Isotropic remeshing of "
          << poly_item->name().toStdString() << " started..." << std::endl;
        Scene_polyhedron_selection_item::Is_constrained_map<Edge_set> ecm(&edges_to_protect);
        CGAL::Polygon_mesh_processing::isotropic_remeshing(
            faces(*poly_item->polyhedron())
          , target_length_
          , *poly_item->polyhedron()
          , CGAL::Polygon_mesh_processing::parameters::number_of_iterations(nb_iter_)
          .protect_constraints(protect_)
          .edge_is_constrained_map(ecm)
          .face_patch_map(get(CGAL::face_patch_id_t<int>(), *poly_item->polyhedron()))
          .relax_constraints(smooth_features_));
        std::cout << "Isotropic remeshing of "
          << poly_item->name().toStdString() << " done." << std::endl;
      }
    }

  public:
    Remesh_polyhedron_item(
      const bool edges_only,
      const double target_length,
      const unsigned int nb_iter,
      const bool protect,
      const bool smooth_features)
      : edges_only_(edges_only)
      , target_length_(target_length)
      , nb_iter_(nb_iter)
      , protect_(protect)
      , smooth_features_(smooth_features)
    {}

    Remesh_polyhedron_item(const Remesh_polyhedron_item& remesh)
      : edges_only_(remesh.edges_only_)
      , target_length_(remesh.target_length_)
      , nb_iter_(remesh.nb_iter_)
      , protect_(remesh.protect_)
      , smooth_features_(remesh.smooth_features_)
    {}

    void operator()(Scene_facegraph_item* poly_item,
                    Edge_set& edges_to_protect) const
    {
      remesh(poly_item, edges_to_protect);
    }
  };

#ifdef CGAL_LINKED_WITH_TBB
  template<typename RemeshFunctor>
  struct Remesh_polyhedron_item_for_parallel_for
    : RemeshFunctor
  {
    const std::vector<Scene_facegraph_item*>& selection_;
    std::map<FaceGraph*,Edge_set >& edges_to_protect_;

  public:
    // Constructor
    Remesh_polyhedron_item_for_parallel_for(
      const std::vector<Scene_facegraph_item*>& selection,
      std::map<FaceGraph*,Edge_set >& edges_to_protect,
      const bool edges_only,
      const double target_length,
      const unsigned int nb_iter,
      const bool protect,
      const bool smooth_features)
      : RemeshFunctor(edges_only, target_length, nb_iter, protect, smooth_features)
      , selection_(selection), edges_to_protect_(edges_to_protect)
    {}

    // Constructor
    Remesh_polyhedron_item_for_parallel_for(
      const Remesh_polyhedron_item_for_parallel_for &remesh)
      : RemeshFunctor(remesh)
      , selection_(remesh.selection_)
      , edges_to_protect_(remesh.edges_to_protect_)
    {}

    // operator()
    void operator()(const tbb::blocked_range<size_t>& r) const
    {
      for (size_t i = r.begin(); i != r.end(); ++i)
        RemeshFunctor::remesh(selection_[i], edges_to_protect_[selection_[i]->polyhedron()]);
    }
  };
#endif

  Ui::Isotropic_remeshing_dialog
  remeshing_dialog(QDialog* dialog,
                   Scene_facegraph_item* poly_item,
                   Scene_polyhedron_selection_item* selection_item = NULL)
  {
    Ui::Isotropic_remeshing_dialog ui;
    ui.setupUi(dialog);
    connect(ui.buttonBox, SIGNAL(accepted()), dialog, SLOT(accept()));
    connect(ui.buttonBox, SIGNAL(rejected()), dialog, SLOT(reject()));

    //connect checkbox to spinbox
    connect(ui.splitEdgesOnly_checkbox, SIGNAL(toggled(bool)),
            ui.nbIterations_spinbox, SLOT(setDisabled(bool)));
    connect(ui.splitEdgesOnly_checkbox, SIGNAL(toggled(bool)),
            ui.protect_checkbox, SLOT(setDisabled(bool)));
    connect(ui.protect_checkbox, SIGNAL(toggled(bool)),
            ui.smooth1D_checkbox, SLOT(setDisabled(bool)));
    connect(ui.splitEdgesOnly_checkbox, SIGNAL(toggled(bool)),
            ui.smooth1D_checkbox, SLOT(setDisabled(bool)));
    connect(ui.preserveDuplicates_checkbox, SIGNAL(toggled(bool)),
            ui.protect_checkbox, SLOT(setChecked(bool)));
    connect(ui.preserveDuplicates_checkbox, SIGNAL(toggled(bool)),
            ui.protect_checkbox, SLOT(setDisabled(bool)));

    //Set default parameters
    Scene_interface::Bbox bbox = poly_item != NULL ? poly_item->bbox()
      : (selection_item != NULL ? selection_item->bbox()
        : scene->bbox());
    ui.objectName->setText(poly_item != NULL ? poly_item->name()
      : (selection_item != NULL ? selection_item->name()
        : QString("Remeshing parameters")));

    ui.objectNameSize->setText(
      tr("Object bbox size (w,h,d):  <b>%1</b>,  <b>%2</b>,  <b>%3</b>")
      .arg(bbox.xmax()-bbox.xmin(), 0, 'g', 3)
      .arg(bbox.ymax()-bbox.ymin(), 0, 'g', 3)
      .arg(bbox.zmax()-bbox.zmin(), 0, 'g', 3));

    double diago_length = CGAL::sqrt((bbox.xmax()-bbox.xmin())*(bbox.xmax()-bbox.xmin())
                                   + (bbox.ymax()-bbox.ymin())*(bbox.ymax()-bbox.ymin())
                                   + (bbox.zmax()-bbox.zmin())*(bbox.zmax()-bbox.zmin()));
    double log = std::log10(diago_length);
    unsigned int nb_decimals = (log > 0) ? 5 : (std::ceil(-log)+3);

    ui.edgeLength_dspinbox->setDecimals(nb_decimals);
    ui.edgeLength_dspinbox->setSingleStep(1e-3);
    ui.edgeLength_dspinbox->setRange(1e-6 * diago_length, //min
                                     2.   * diago_length);//max
    ui.edgeLength_dspinbox->setValue(0.05 * diago_length);

    std::ostringstream oss;
    oss << "Diagonal length of the Bbox of the selection to remesh is ";
    oss << diago_length << "." << std::endl;
    oss << "Default is 5% of it" << std::endl;
    ui.edgeLength_dspinbox->setToolTip(QString::fromStdString(oss.str()));

    ui.nbIterations_spinbox->setSingleStep(1);
    ui.nbIterations_spinbox->setRange(1/*min*/, 1000/*max*/);
    ui.nbIterations_spinbox->setValue(1);

    ui.protect_checkbox->setChecked(false);
    ui.smooth1D_checkbox->setChecked(true);

    if (NULL != selection_item)
    {
      //do not preserve duplicates in selection mode
      ui.preserveDuplicates_checkbox->setDisabled(true);
      ui.preserveDuplicates_checkbox->setChecked(false);
    }

    return ui;
  }

private:
  QAction* actionIsotropicRemeshing_;

}; // end Polyhedron_demo_isotropic_remeshing_plugin

#include "Isotropic_remeshing_plugin.moc"


// Include graph.hpp here, because it has the Vertex and Edge
// templates, which are needed by generic_dijkstra.hpp.

#include "graph.hpp"

#include "accountant.hpp"
#include "adaptive_units.hpp"
#include "generic_dijkstra.hpp"
#include "generic_label.hpp"
#include "generic_label_creator.hpp"
#include "generic_permanent.hpp"
#include "generic_tentative.hpp"
#include "generic_tracer.hpp"
#include "utils.hpp"

#include <boost/optional.hpp>

#include <iostream>

#define BOOST_TEST_MODULE dijkstra
#include <boost/test/unit_test.hpp>

using namespace std;

using per_type = generic_permanent<graph, COST, CU>;
using ten_type = generic_tentative<graph, COST, CU>;

// Make sure that we can't find a path if there are no slices.  The
// graph is a simple case of two nodes and a single link.
BOOST_AUTO_TEST_CASE(dijkstra_test_1)
{
  // Play safe.
  adaptive_units<COST>::set_reach_1(100);
  routing::set_st(routing::st_t::first);

  graph g(2);
  vertex src = *(boost::vertices(g).first);
  vertex dst = *(boost::vertices(g).first + 1);
  boost::add_edge(src, dst, g);
  set_units(g, 2);

  generic_label_creator<graph, COST, CU> c(g, 3);
  generic_label<graph, COST, CU> l(0, {0, 2}, edge(), src);
  per_type P(num_vertices(g));
  ten_type T(num_vertices(g));
  dijkstra(g, P, T, l, c, dst);
  // The tracer.
  generic_tracer<graph, cupath, per_type, CU> t(g, 3);
  // Get the path.
  auto op = trace(P, dst, l, t);

  // There are no results for dst.
  BOOST_CHECK(!op);
}

// Make sure that we can find a path if there are slices.  The graph
// is a simple case of two nodes and a single link.
BOOST_AUTO_TEST_CASE(dijkstra_test_2)
{
  // Play.  Safe.
  adaptive_units<COST>::set_reach_1(100);
  routing::set_st(routing::st_t::first);

  graph g(2);
  vertex src = *(boost::vertices(g).first);
  vertex dst = *(boost::vertices(g).first + 1);
  edge e = boost::add_edge(src, dst, g).first;
  boost::get(boost::edge_su, g, e) = {{0, 3}};
  boost::get(boost::edge_weight, g, e) = 1;

  generic_label_creator<graph, COST, CU> c(g, 3);
  generic_label<graph, COST, CU> l(0, {0, 3}, edge(), src);
  per_type P(num_vertices(g));
  ten_type T(num_vertices(g));
  dijkstra(g, P, T, l, c, dst);
  // The tracer.
  generic_tracer<graph, cupath, per_type, CU> t(g, 3);
  // Get the path.
  auto op = trace(P, dst, l, t);

  // The path has one edge and three units.
  BOOST_CHECK(op.value() == cupath(CU(0, 3), {e}));
}

// Make sure we can establish a path with a worse cost than the
// shortest path, but with a different su.
BOOST_AUTO_TEST_CASE(dijkstra_test_3)
{
  // Play.  Safe.  Durex.
  adaptive_units<COST>::set_reach_1(100);
  routing::set_st(routing::st_t::first);

  graph g(3);
  vertex src = *(boost::vertices(g).first);
  vertex mid = *(boost::vertices(g).first + 1);
  vertex dst = *(boost::vertices(g).first + 2);
  edge e1 = boost::add_edge(src, mid, g).first;
  edge e2 = boost::add_edge(src, mid, g).first;
  edge e3 = boost::add_edge(mid, dst, g).first;

  // Props of edge e1.
  boost::get(boost::edge_weight, g, e1) = 1;
  boost::get(boost::edge_su, g, e1) = {{0, 2}};

  // Props of edge e2.
  boost::get(boost::edge_weight, g, e2) = 2;
  boost::get(boost::edge_su, g, e2) = {{1, 3}};

  // Props of edge e3.
  boost::get(boost::edge_weight, g, e3) = 1;
  boost::get(boost::edge_su, g, e3) = {{1, 3}};

  generic_label_creator<graph, COST, CU> c(g, 2);
  generic_label<graph, COST, CU> l(0, {0, 3}, edge(), src);
  per_type P(num_vertices(g));
  ten_type T(num_vertices(g));
  dijkstra(g, P, T, l, c, dst);
  // The tracer.
  generic_tracer<graph, cupath, per_type, CU> t(g, 2);
  // Get the path.
  auto op = trace(P, dst, l, t);

  // We found the path - it should be e2, e3.
  BOOST_CHECK(op.value() == cupath({1, 3}, {e2, e3}));
}

// Make sure we don't remember at node mid the results for edge e1
// that led to that node with a worse cost than edge e2 and with the
// same slices.
BOOST_AUTO_TEST_CASE(dijkstra_test_4)
{
  // Play.  Safe.
  adaptive_units<COST>::set_reach_1(100);
  routing::set_st(routing::st_t::first);

  graph g(3);
  vertex src = *(vertices(g).first);
  vertex mid = *(vertices(g).first + 1);
  vertex dst = *(vertices(g).first + 2);
  edge e1 = add_edge(src, mid, g).first;
  edge e2 = add_edge(src, mid, g).first;
  edge e3 = add_edge(mid, dst, g).first;

  // Props of edge e1.
  boost::get(boost::edge_weight, g, e1) = 2;
  boost::get(boost::edge_su, g, e1) = {{0, 1}};

  // Props of edge e2.
  boost::get(boost::edge_weight, g, e2) = 1;
  boost::get(boost::edge_su, g, e2) = {{0, 1}};

  // Props of edge e3.
  boost::get(boost::edge_weight, g, e3) = 2;
  boost::get(boost::edge_su, g, e3) = {{0, 1}};

  generic_label_creator<graph, COST, CU> c(g, 1);
  generic_label<graph, COST, CU> l(0, {0, 3}, edge(), src);
  per_type P(num_vertices(g));
  ten_type T(num_vertices(g));
  dijkstra(g, P, T, l, c, dst);
  // The tracer.
  generic_tracer<graph, cupath, per_type, CU> t(g, 1);
  // Get the path.
  auto op = trace(P, dst, l, t);

  // We found the path of two labels.
  BOOST_CHECK(op.value() == cupath({0, 1}, {e2, e3}));
  // We don't remember the results for edge e1.
  BOOST_CHECK(P[mid].size() == 1);
}

// Make sure we remember at node mid the results for both edge e1 and
// e2.  e2 offers the shortest path with slice 0, and e1 offers the
// two slices: 0 and 1 albeit at a higher cost.
BOOST_AUTO_TEST_CASE(dijkstra_test_5)
{
  // Play.  Safe.
  adaptive_units<COST>::set_reach_1(100);
  routing::set_st(routing::st_t::first);

  graph g(3);
  vertex src = *(vertices(g).first);
  vertex mid = *(vertices(g).first + 1);
  vertex dst = *(vertices(g).first + 2);
  edge e1 = add_edge(src, mid, g).first;
  edge e2 = add_edge(src, mid, g).first;
  edge e3 = add_edge(mid, dst, g).first;

  // Props of edge e1.
  boost::get(boost::edge_weight, g, e1) = 2;
  boost::get(boost::edge_su, g, e1) = {{0, 2}};

  // Props of edge e2.
  boost::get(boost::edge_weight, g, e2) = 1;
  boost::get(boost::edge_su, g, e2) = {{0, 1}};

  // Props of edge e3.
  boost::get(boost::edge_weight, g, e3) = 2;
  boost::get(boost::edge_su, g, e3) = {{0, 1}};

  generic_label_creator<graph, COST, CU> c(g, 1);
  generic_label<graph, COST, CU> l(0, {0, 3}, edge(), src);
  per_type P(num_vertices(g));
  ten_type T(num_vertices(g));
  dijkstra(g, P, T, l, c, dst);
  // The tracer.
  generic_tracer<graph, cupath, per_type, CU> t(g, 1);
  // Get the path.
  auto op = trace(P, dst, l, t);

  // We found the path.
  BOOST_CHECK(op.value() == cupath({0, 1}, {e2, e3}));

  // We remember at node mid the results for edges e1 and e2.
  BOOST_CHECK(P[mid].size() == 2);
  // We reach node mid by edge e2 at cost 1.
  BOOST_CHECK(get_cost(*P[mid].begin()) == 1);
  BOOST_CHECK(get_edge(*P[mid].begin()) == e2);
  // The second way of reaching node mid is by edge e1 at cost 2.
  BOOST_CHECK(get_cost(*(++P[mid].begin())) == 2);
  BOOST_CHECK(get_edge(*(++P[mid].begin())) == e1);
}

// Make sure that we don't remember the results for edge e2, because
// it offers a worse path then edge e1.  This tests for the existance
// of better old results.
BOOST_AUTO_TEST_CASE(dijkstra_test_6)
{
  graph g(2);
  vertex src = *(vertices(g).first);
  vertex dst = *(vertices(g).first + 1);

  edge e1 = boost::add_edge(src, dst, g).first;
  edge e2 = boost::add_edge(src, dst, g).first;

  // Props of edge e1.
  boost::get(boost::edge_weight, g, e1) = 1;
  boost::get(boost::edge_su, g, e1) = {{0, 1}};

  // Props of edge e2.  During the search, the result for this edge
  // won't be even added, because already the better result for edge
  // e1 will be in place.
  boost::get(boost::edge_weight, g, e2) = 2;
  boost::get(boost::edge_su, g, e2) = {{0, 1}};

  generic_label_creator<graph, COST, CU> c(g, 1);
  generic_label<graph, COST, CU> l(0, {0, 3}, edge(), src);
  per_type P(num_vertices(g));
  ten_type T(num_vertices(g));
  dijkstra(g, P, T, l, c, dst);

  BOOST_CHECK(P[dst].size() == 1);
  BOOST_CHECK(get_cost(*P[dst].begin()) == 1);
  BOOST_CHECK(get_edge(*P[dst].begin()) == e1);
}

// Make sure that we don't remember the results for edge e1, because
// it offers a worse path then edge e2.  This tests the purging of the
// old results.
BOOST_AUTO_TEST_CASE(dijkstra_test_7)
{
  graph g(2);
  vertex src = *(vertices(g).first);
  vertex dst = *(vertices(g).first + 1);

  edge e1 = boost::add_edge(src, dst, g).first;
  edge e2 = boost::add_edge(src, dst, g).first;

  // Props of edge e1.
  boost::get(boost::edge_weight, g, e1) = 2;
  boost::get(boost::edge_su, g, e1) = {{0, 1}};

  // Props of edge e2.  During the Dijkstra::Search search, the result
  // for edge e1 will be removed, because edge e2 offers a better
  // result.
  boost::get(boost::edge_weight, g, e2) = 1;
  boost::get(boost::edge_su, g, e2) = {{0, 1}};

  generic_label_creator<graph, COST, CU> c(g, 1);
  generic_label<graph, COST, CU> l(0, {0, 3}, edge(), src);
  per_type P(num_vertices(g));
  ten_type T(num_vertices(g));
  dijkstra(g, P, T, l, c, dst);

  BOOST_CHECK(P[dst].size() == 1);
  BOOST_CHECK(get_cost(*P[dst].begin()) == 1);
  BOOST_CHECK(get_edge(*P[dst].begin()) == e2);
}

// Test the has_better_or_equal function.
BOOST_AUTO_TEST_CASE(test_has_better_or_equal)
{
  using label = generic_label<graph, COST, CU>;

  graph g(1);
  vertex v = *(vertices(g).first);
  edge ne = *edges(g).second;

  per_type P(num_vertices(g));

  label l(10, {1, 4}, ne, v);

  // The labels is empty, so there are no better labels.
  BOOST_CHECK(!has_better_or_equal(P, l));

  // The labels has only l, so still is a better label.
  P.push(l);
  BOOST_CHECK(has_better_or_equal(P, l));

  // Label l1 is incomparable with l, because of incomparable SUs.
  label l1(30, {10, 11}, ne, v);
  BOOST_CHECK(!has_better_or_equal(P, l1));

  // l <= l2, because l is of lower cost and has the same SU.
  label l2(20, {1, 4}, ne, v);
  BOOST_CHECK(has_better_or_equal(P, l2));

  // l <= l3, because the SU of l3 is a proper subset of the SU of l,
  // and the cost of l is smaller than the cost of l3.
  label l3(10, {1, 3}, ne, v);
  BOOST_CHECK(has_better_or_equal(P, l3));
}

BOOST_AUTO_TEST_CASE(test_purge_worse)
{
  using label = generic_label<graph, COST, CU>;

  graph g(1);
  vertex v = *(vertices(g).first);
  edge ne = *edges(g).second;

  label l1(10, {1, 4}, ne, v);
  label l2(10, {1, 3}, ne, v);
  label l3(20, {1, 4}, ne, v);
  label l4(30, {1, 5}, ne, v);
  ten_type T(num_vertices(g));
  T.push(l2);
  T.push(l3);
  T.push(l4);

  // This should remove both l2 and l3, but not l4.
  purge_worse(T, l1);
  BOOST_CHECK(T[v].size() == 1);
  BOOST_CHECK(*T[v].begin() == l4);
}

BOOST_AUTO_TEST_CASE(test_move_label)
{
  using label = generic_label<graph, COST, CU>;

  graph g(2);
  vertex v1 = *(boost::vertices(g).first);
  vertex v2 = *(boost::vertices(g).first + 1);
  edge ne = *edges(g).second;

  label l1(10, {1, 4}, ne, v1);
  label l2(10, {1, 3}, ne, v2);
  label l3(20, {1, 4}, ne, v1);
  label l4(30, {1, 5}, ne, v2);

  per_type P(num_vertices(g));
  ten_type T(num_vertices(g));

  T.push(l1);
  BOOST_CHECK(move_label(T, P) == l1);
  BOOST_CHECK(T.empty());
  BOOST_CHECK(P[v1].size() == 1);
  BOOST_CHECK(P[v1].front() == l1);

  T.push(l2);
  T.push(l3);
  BOOST_CHECK(move_label(T, P) == l2);
  BOOST_CHECK(*T[v1].begin() == l3);
  BOOST_CHECK(P[v1].front() == l1);
  BOOST_CHECK(P[v2].front() == l2);

  T.push(l4);
  BOOST_CHECK(move_label(T, P) == l3);
  BOOST_CHECK(*T[v2].begin() == l4);
  BOOST_CHECK(P[v1].size() == 2);
  BOOST_CHECK(P[v1].front() == l1);
  BOOST_CHECK(*(++(P[v1].begin())) == l3);
  BOOST_CHECK(P[v2].size() == 1);
  BOOST_CHECK(P[v2].front() == l2);

  BOOST_CHECK(move_label(T, P) == l4);
  BOOST_CHECK(T.empty());
  BOOST_CHECK(P[v1].size() == 2);
  BOOST_CHECK(P[v1].front() == l1);
  BOOST_CHECK(*(++(P[v1].begin())) == l3);
  BOOST_CHECK(P[v2].size() == 2);
  BOOST_CHECK(P[v2].front() == l2);
  BOOST_CHECK(*(++(P[v2].begin())) == l4);
}


#include <tuple>
#include <Eigen/Core>
#include <iostream>

#include <npe.h>

npe_function(sparse_matrix_passthru)
npe_arg(a, sparse_double, sparse_float)
npe_arg(b, npe_matches(a))
npe_begin_code()

// This addition should have no effect which is what we test.
npe_Matrix_a ret1 = a + b;

return npe::move(a);

npe_end_code()


#include <boost/geometry/policies/relate/intersection_ratios.hpp>


/*
==== Author:

Relja Arandjelovic (relja@robots.ox.ac.uk)
Visual Geometry Group,
Department of Engineering Science
University of Oxford

==== Copyright:

The library belongs to Relja Arandjelovic and the University of Oxford.
No usage or redistribution is allowed without explicit permission.
*/

#include "ellipse.h"

#include <stdint.h>
#include <vector>

#include <Eigen/Dense>

ellipse::ellipse( double aX,  double aY, double aA, double aB, double aC ){
    x=aX; y=aY;
    a=aA; b=aB; c=aC;
}

void
ellipse::getCentres( std::vector<ellipse> const &ellipses,
                     double *&x, double *&y  ){

    x= new double[ellipses.size()];
    y= new double[ellipses.size()];
    uint32_t iEllipse= 0;

    for ( std::vector<ellipse>::const_iterator it=ellipses.begin(); it!=ellipses.end(); ++it, ++iEllipse ){
        x[iEllipse]= it->x;
        y[iEllipse]= it->y;
    }

}

void
ellipse::getCentres( std::vector<ellipse> const &ellipses1,
                     std::vector<ellipse> const &ellipses2,
                     std::vector< std::pair<uint32_t, uint32_t> > const &inds,
                     double *&x1, double *&y1, double *&x2, double *&y2,
                     double *&areaDiffSq ){

    x1= new double[inds.size()];
    y1= new double[inds.size()];
    x2= new double[inds.size()];
    y2= new double[inds.size()];
    areaDiffSq= new double[inds.size()];
    uint32_t iPair= 0;

    for ( std::vector< std::pair<uint32_t, uint32_t> >::const_iterator it=inds.begin();
          it!=inds.end();
          ++it, ++iPair ){
        x1[iPair]= ellipses1[ it->first ].x;
        y1[iPair]= ellipses1[ it->first ].y;
        x2[iPair]= ellipses2[ it->second ].x;
        y2[iPair]= ellipses2[ it->second ].y;
        areaDiffSq[iPair]= ellipses2[ it->second ].getPropAreaSq() / ellipses1[ it->first ].getPropAreaSq();
    }

}

void
ellipse::transformAffine( double Haff[] ){

    // transform centre
    double x_new= (Haff[0]*x+Haff[1]*y+Haff[2])/Haff[8];
    double y_new= (Haff[3]*x+Haff[4]*y+Haff[5])/Haff[8];

    // transform a,b,c
    typedef Eigen::Matrix<double, 3, 3> matrix3d;
    matrix3d C0;
    C0 <<   a,   b,  0.0,
            b,   c,  0.0,
          0.0, 0.0, -1.0;

    homography Haff_(Haff);
    double Hinv_[9];
    Haff_.getInverse( Hinv_ );

    matrix3d Hinv;
    Hinv<< Hinv_[0], Hinv_[1], Hinv_[2],
           Hinv_[3], Hinv_[4], Hinv_[5],
           Hinv_[6], Hinv_[7], Hinv_[8];

    Hinv/= Hinv.coeff(2,2);

    // translate Hinv (same as translating C but more efficient)
    Hinv.coeffRef(0,2)-= x;
    Hinv.coeffRef(1,2)-= y;
    matrix3d C0t;
    C0t.noalias()= Hinv.transpose() * C0 * Hinv;

    // now translate C0t to Ct by xt,yt, but the following is not affected
    a= C0t.coeff(0,0);
    b= C0t.coeff(0,1);
    c= C0t.coeff(1,1);
    x= x_new;
    y= y_new;

#if 0
    // this is how it used to be done in OpenCV, not updating though so it might be obsolete..

    C0.at<double>(0,0)=   a; C0.at<double>(0,1)=   b; C0.at<double>(0,2)=  0.0;
    C0.at<double>(1,0)=   b; C0.at<double>(1,1)=   c; C0.at<double>(1,2)=  0.0;
    C0.at<double>(2,0)= 0.0; C0.at<double>(2,1)= 0.0; C0.at<double>(2,2)= -1.0;

    cv::Mat Hinv(3,3, cv::DataType<double>::type);
    for (int i=0; i<3; ++i)
        for (int j=0; j<3; ++j)
            Hinv.at<double>(i,j)= Hinv_[i*3+j];

    Hinv/= Hinv.at<double>(2,2);

    Hinv.at<double>(0,2)-= x;
    Hinv.at<double>(1,2)-= y;
    cv::Mat C0t= Hinv.t()*C0*Hinv;

    a= C0t.at<double>(0,0);
    b= C0t.at<double>(0,1);
    c= C0t.at<double>(1,1);
    x= x_new;
    y= y_new;
#endif

}


/*
 * Type and constant definitions
 * Copyright (C) 2019  Robin Scheibler
 *
 * 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.
 *
 * You should have received a copy of the MIT License along with this program. If
 * not, see <https://opensource.org/licenses/MIT>.
 */

/* This file contains type and constants definitions */
#ifndef __COMMON_HPP__
#define __COMMON_HPP__

#include <iostream>
#include <Eigen/Dense>

extern float libroom_eps;  // epsilon is the precision for floating point computations. It is defined in libroom.cpp

template<size_t D>
using Vectorf = Eigen::Matrix<float, D, 1>;

using MatrixXf = Eigen::MatrixXf;
typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> MatrixXb;
typedef Eigen::Matrix<bool, Eigen::Dynamic, 1> VectorXb;

/* The 'entry' type is simply defined as an array of 2 floats.
 * It represents an entry that is logged by the microphone
 * during the ray_tracing execution.
 * The first one of those float will be the travel time of a ray reaching
 * the microphone. The second one will be the energy of this ray.*/

struct Hit
{
  float distance = 0.f;
  Eigen::ArrayXf transmitted;  // vector of transmitted energy over frequency bands

  Hit(int _nfreq)
  {
    transmitted.resize(_nfreq);
    transmitted.setOnes();
  };
  Hit(const float _d, const Eigen::ArrayXf &_t)
    : distance(_d), transmitted(_t) {}
};

typedef std::vector<std::list<Hit>> HitLog;

size_t get_new_size(size_t val, size_t cur_size)
{
  size_t new_size = cur_size;
  while (val >= new_size)
    new_size *= 2;
  return new_size;
}

class Histogram2D
{
  size_t rows, cols;
  Eigen::ArrayXXf array;
  Eigen::ArrayXXi counts;

  public:
    Histogram2D() {}  // empty constructor
    Histogram2D(int _r, int _c) : rows(_r), cols(_c)
    {
      init(rows, cols);
    }

    void init(int rows, int cols)
    {
      array.resize(rows, cols);
      array.setZero();
      counts.resize(rows, cols);
      counts.setZero();
    }

    void reset()
    {
      array.setZero();
      counts.setZero();
    }

    void resize_rows(int new_rows)
    {
      auto old_rows = array.rows();
      // this will resize the array while preserving the content
      array.conservativeResize(new_rows, Eigen::NoChange);
      counts.conservativeResize(new_rows, Eigen::NoChange);
      // We need to initialize the new elements
      if (new_rows > old_rows)
      {
        array.bottomRows(new_rows - old_rows).setZero();
        counts.bottomRows(new_rows - old_rows).setZero();
      }
    }

    void resize_cols(int new_cols)
    {
      auto old_cols = array.cols();
      // this will resize the array while preserving the content
      array.conservativeResize(Eigen::NoChange, new_cols);
      counts.conservativeResize(Eigen::NoChange, new_cols);
      // We need to initialize the new elements
      if (new_cols > old_cols)
      {
        array.rightCols(new_cols - old_cols).setZero();
        counts.rightCols(new_cols - old_cols).setZero();
      }
    }

    void log(Eigen::Index row, Eigen::Index col, float val)
    {
      if (row >= array.rows())
        resize_rows(get_new_size(row, array.rows()));

      if (col >= array.cols())
        resize_cols(get_new_size(col, array.cols()));

      array.coeffRef(row, col) += val;
      counts.coeffRef(row, col)++;
    }

    void log_col(Eigen::Index col, const Eigen::ArrayXf &val)
    {
      if (col >= array.cols())
        resize_cols(get_new_size(col, array.cols()));

      array.col(col) += val;
      counts.col(col) += 1;
    }

    void log_row(Eigen::Index row, const Eigen::ArrayXf &val)
    {
      if (row >= array.rows())
        resize_rows(get_new_size(row, array.rows()));

      array.row(row) += val;
      counts.row(row) += 1;
    }

    float bin(Eigen::Index row, Eigen::Index col) const
    {
      if (counts.coeff(row, col) != 0)
        return array.coeff(row, col) / counts.coeff(row, col);
      else
        return 0.f;
    }

    Eigen::ArrayXXf get_hist() const
    {
      return array;
    }
};

#endif // __COMMON_HPP__


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_SEGMENT_TO_SEGMENT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_SEGMENT_TO_SEGMENT_HPP

#include <algorithm>
#include <iterator>

#include <boost/core/addressof.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/detail/distance/is_comparable.hpp>
#include <boost/geometry/algorithms/detail/distance/strategy_utils.hpp>
#include <boost/geometry/algorithms/dispatch/distance.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/tags.hpp>

#include <boost/geometry/util/condition.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance
{

// compute segment-segment distance
template<typename Segment1, typename Segment2, typename Strategies>
class segment_to_segment
{
    typedef distance::strategy_t<Segment1, Segment2, Strategies> strategy_type;

public:
    typedef distance::return_t<Segment1, Segment2, Strategies> return_type;

    static inline return_type apply(Segment1 const& segment1, Segment2 const& segment2,
                                    Strategies const& strategies)
    {
        if (geometry::intersects(segment1, segment2, strategies))
        {
            return 0;
        }

        typename point_type<Segment1>::type p[2];
        detail::assign_point_from_index<0>(segment1, p[0]);
        detail::assign_point_from_index<1>(segment1, p[1]);

        typename point_type<Segment2>::type q[2];
        detail::assign_point_from_index<0>(segment2, q[0]);
        detail::assign_point_from_index<1>(segment2, q[1]);

        strategy_type const strategy = strategies.distance(segment1, segment2);

        auto const cstrategy = strategy::distance::services::get_comparable
                                <
                                    strategy_type
                                >::apply(strategy);

        distance::creturn_t<Segment1, Segment2, Strategies> d[4];
        d[0] = cstrategy.apply(q[0], p[0], p[1]);
        d[1] = cstrategy.apply(q[1], p[0], p[1]);
        d[2] = cstrategy.apply(p[0], q[0], q[1]);
        d[3] = cstrategy.apply(p[1], q[0], q[1]);

        std::size_t imin = std::distance(boost::addressof(d[0]),
                                         std::min_element(d, d + 4));

        if (BOOST_GEOMETRY_CONDITION(is_comparable<strategy_type>::value))
        {
            return d[imin];
        }

        switch (imin)
        {
        case 0:
            return strategy.apply(q[0], p[0], p[1]);
        case 1:
            return strategy.apply(q[1], p[0], p[1]);
        case 2:
            return strategy.apply(p[0], q[0], q[1]);
        default:
            return strategy.apply(p[1], q[0], q[1]);
        }
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// segment-segment
template <typename Segment1, typename Segment2, typename Strategy>
struct distance
    <
        Segment1, Segment2, Strategy, segment_tag, segment_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::segment_to_segment<Segment1, Segment2, Strategy>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_SEGMENT_TO_SEGMENT_HPP


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This example shows how to run a CNN based face detector using dlib.  The
    example loads a pretrained model and uses it to find faces in images.  The
    CNN model is much more accurate than the HOG based model shown in the
    face_detection_ex.cpp example, but takes much more computational power to
    run, and is meant to be executed on a GPU to attain reasonable speed.  For
    example, on a NVIDIA Titan X GPU, this example program processes images at
    about the same speed as face_detection_ex.cpp.

    Also, users who are just learning about dlib's deep learning API should read
    the dnn_introduction_ex.cpp and dnn_introduction2_ex.cpp examples to learn
    how the API works.  For an introduction to the object detection method you
    should read dnn_mmod_ex.cpp

    TRAINING THE MODEL
        Finally, users interested in how the face detector was trained should
        read the dnn_mmod_ex.cpp example program.  It should be noted that the
        face detector used in this example uses a bigger training dataset and
        larger CNN architecture than what is shown in dnn_mmod_ex.cpp, but
        otherwise training is the same.  If you compare the net_type statements
        in this file and dnn_mmod_ex.cpp you will see that they are very similar
        except that the number of parameters has been increased.

        Additionally, the following training parameters were different during
        training: The following lines in dnn_mmod_ex.cpp were changed from
            mmod_options options(face_boxes_train, 40,40);
            trainer.set_iterations_without_progress_threshold(300);
        to the following when training the model used in this example:
            mmod_options options(face_boxes_train, 80,80);
            trainer.set_iterations_without_progress_threshold(8000);

        Also, the random_cropper was left at its default settings,  So we didn't
        call these functions:
            cropper.set_chip_dims(200, 200);
            cropper.set_min_object_size(40,40);

        The training data used to create the model is also available at
        http://dlib.net/files/data/dlib_face_detection_dataset-2016-09-30.tar.gz
*/

#include <iostream>
#include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>

using namespace std;
using namespace dlib;

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

template <long num_filters, typename SUBNET> using con5d = con<num_filters,5,5,2,2,SUBNET>;
template <long num_filters, typename SUBNET> using con5  = con<num_filters,5,5,1,1,SUBNET>;

template <typename SUBNET> using downsampler  = relu<affine<con5d<32, relu<affine<con5d<32, relu<affine<con5d<16,SUBNET>>>>>>>>>;
template <typename SUBNET> using rcon5  = relu<affine<con5<45,SUBNET>>>;

using net_type = loss_mmod<con<1,9,9,1,1,rcon5<rcon5<rcon5<downsampler<input_rgb_image_pyramid<pyramid_down<6>>>>>>>>;

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

int main(int argc, char** argv) try
{
    if (argc == 1)
    {
        cout << "Call this program like this:" << endl;
        cout << "./dnn_mmod_face_detection_ex mmod_human_face_detector.dat faces/*.jpg" << endl;
        cout << "\nYou can get the mmod_human_face_detector.dat file from:\n";
        cout << "http://dlib.net/files/mmod_human_face_detector.dat.bz2" << endl;
        return 0;
    }

    net_type net;
    deserialize(argv[1]) >> net;

    image_window win;
    for (int i = 2; i < argc; ++i)
    {
        matrix<rgb_pixel> img;
        load_image(img, argv[i]);

        // Upsampling the image will allow us to detect smaller faces but will cause the
        // program to use more RAM and run longer.
        while(img.size() < 1800*1800)
            pyramid_up(img);

        // Note that you can process a bunch of images in a std::vector at once and it runs
        // much faster, since this will form mini-batches of images and therefore get
        // better parallelism out of your GPU hardware.  However, all the images must be
        // the same size.  To avoid this requirement on images being the same size we
        // process them individually in this example.
        auto dets = net(img);
        win.clear_overlay();
        win.set_image(img);
        for (auto&& d : dets)
            win.add_overlay(d);

        cout << "Hit enter to process the next image." << endl;
        cin.get();
    }
}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


#include <boost/graph/mcgregor_common_subgraphs.hpp>


//pcl
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/console/parse.h>
#include <pcl/common/time.h>
#include <pcl/common/centroid.h>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>

#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>

#include <pcl/search/pcl_search.h>
#include <pcl/common/transforms.h>

#include <pcl/tracking/tracking.h>
#include <pcl/tracking/particle_filter.h>
#include <pcl/tracking/kld_adaptive_particle_filter_omp.h>
#include <pcl/tracking/particle_filter_omp.h>
#include <pcl/tracking/coherence.h>
#include <pcl/tracking/distance_coherence.h>
#include <pcl/tracking/hsv_color_coherence.h>
#include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h>
#include <pcl/tracking/nearest_pair_point_cloud_coherence.h>

#include <pcl/common/common_headers.h>
#include <pcl_conversions/pcl_conversions.h>

#include <boost/format.hpp>

#include <mutex>
#include <thread>
//#include <chrono>

//ros
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

using namespace pcl::tracking;
// using namespace std::chrono_literals;
using namespace std::literals::chrono_literals;

//typedef pcl::PointXYZRGBA RefPointType;
typedef pcl::PointXYZ RefPointType;
typedef ParticleXYZRPY ParticleT;
//typedef pcl::PointCloud<pcl::PointXYZRGBA> Cloud;
typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
typedef Cloud::Ptr CloudPtr;
typedef Cloud::ConstPtr CloudConstPtr;
typedef ParticleFilterTracker<RefPointType, ParticleT> ParticleFilter;

CloudPtr cloud_pass_;
CloudPtr cloud_pass_downsampled_;
CloudPtr target_cloud;

std::mutex mtx_;
ParticleFilter tracker_;
bool new_cloud_;
double downsampling_grid_size_;
int counter;

//Filter along a specified dimension
void filterPassThrough (const CloudConstPtr &cloud, Cloud &result)
{
  //pcl::PassThrough<pcl::PointXYZRGBA> pass;
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (-1.0, 10.0);
  pass.setKeepOrganized (false);
  pass.setInputCloud (cloud);
  pass.filter (result);
}

void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size)
{
  //pcl::ApproximateVoxelGrid<pcl::PointXYZRGBA> grid;
  pcl::ApproximateVoxelGrid<pcl::PointXYZ> grid;
  grid.setLeafSize (static_cast<float> (leaf_size), static_cast<float> (leaf_size), static_cast<float> (leaf_size));
  grid.setInputCloud (cloud);
  grid.filter (result);
}

//Draw the current particles
bool
drawParticles (pcl::visualization::PCLVisualizer& viz)
{
  ParticleFilter::PointCloudStatePtr particles = tracker_.getParticles();
  if (particles && new_cloud_)
    {
      //Set pointCloud with particle's points
      pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ>());
      for (std::size_t i = 0; i < particles->points.size(); i++)
	{
	  pcl::PointXYZ point;

	  point.x = particles->points[i].x;
	  point.y = particles->points[i].y;
	  point.z = particles->points[i].z;
	  particle_cloud->points.push_back (point);
	}

      //Draw red particles
      {
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red_color (particle_cloud, 250, 99, 71);

	if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud"))
	  viz.addPointCloud (particle_cloud, red_color, "particle cloud");
      }
      return true;
    }
  else
    {
      return false;
    }
}

//Draw model reference point cloud
void
drawResult (pcl::visualization::PCLVisualizer& viz)
{
  ParticleXYZRPY result = tracker_.getResult();
  Eigen::Affine3f transformation = tracker_.toEigenMatrix(result);

  //move close to camera a little for better visualization
  transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
  CloudPtr result_cloud (new Cloud ());
  pcl::transformPointCloud<RefPointType> (*(tracker_.getReferenceCloud()), *result_cloud, transformation);

  //Draw blue model reference point cloud
  {
    pcl::visualization::PointCloudColorHandlerCustom<RefPointType> blue_color (result_cloud, 0, 0, 255);

    if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud"))
      viz.addPointCloud (result_cloud, blue_color, "resultcloud");
  }
}

//visualization's callback function
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
  std::lock_guard<std::mutex> lock (mtx_);

  if (!cloud_pass_)
    {
      std::this_thread::sleep_for(1s);
      return;
   }

  //Draw downsampled point cloud from sensor
  if (new_cloud_ && cloud_pass_downsampled_)
    {
      CloudPtr cloud_pass;
      cloud_pass = cloud_pass_downsampled_;

      if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
	{
	  viz.addPointCloud (cloud_pass, "cloudpass");
	  viz.resetCameraViewpoint ("cloudpass");
	}
      bool ret = drawParticles (viz);
      if (ret)
        drawResult (viz);
    }
  new_cloud_ = false;
}

//OpenNI Grabber's cloud Callback function
void cloudCB(const sensor_msgs::PointCloud2ConstPtr& msg)
{
  ROS_INFO_THROTTLE(1, "cloudCB");

  //pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::fromROSMsg(*msg, *cloud);

  std::lock_guard<std::mutex> lock (mtx_);
  cloud_pass_.reset (new Cloud);
  cloud_pass_downsampled_.reset (new Cloud);
  filterPassThrough (cloud, *cloud_pass_);
  gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);

  if(counter < 10){
	counter++;
  }else{
  	//Track the object
	tracker_.setInputCloud(cloud_pass_downsampled_);
	tracker_.compute();
	new_cloud_ = true;
  }

/*
  sensor_msgs::PointCloud2 downsampledCloudMsg;
  pcl::toROSMsg(*downsampledCloud, downsampledCloudMsg);

  downsampledcloud_pub.publish(downsampledCloudMsg);
*/
}

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

  // ros::Subscriber cloud_sub = node.subscribe("/zed/zed_node/point_cloud/cloud_registered", 30, cloudCB);
  ros::Subscriber cloud_sub = node.subscribe("/bluerov2h_follower/depth/points2", 10, cloudCB);

  //read pcd file
  target_cloud.reset(new Cloud());
  if(pcl::io::loadPCDFile (argv[1], *target_cloud) == -1){
    std::cout << "pcd file not found" << std::endl;
    exit(-1);
  }

  counter = 0;

  //Set parameters
  new_cloud_  = false;
  downsampling_grid_size_ =  0.002;

  std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
  default_step_covariance[3] *= 40.0;
  default_step_covariance[4] *= 40.0;
  default_step_covariance[5] *= 40.0;

  std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
  std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);

  //KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> tracker
    //(new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (8));
  KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> tracker;

  ParticleT bin_size;
  bin_size.x = 0.1f;
  bin_size.y = 0.1f;
  bin_size.z = 0.1f;
  bin_size.roll = 0.1f;
  bin_size.pitch = 0.1f;
  bin_size.yaw = 0.1f;

  //Set all parameters for  KLDAdaptiveParticleFilterOMPTracker
  tracker.setMaximumParticleNum(1000);
  tracker.setDelta(0.99);
  tracker.setEpsilon(0.2);
  tracker.setBinSize(bin_size);

  //Set all parameters for  ParticleFilter
  tracker_ = tracker;
  tracker_.setTrans (Eigen::Affine3f::Identity());
  tracker_.setStepNoiseCovariance (default_step_covariance);
  tracker_.setInitialNoiseCovariance (initial_noise_covariance);
  tracker_.setInitialNoiseMean (default_initial_mean);
  tracker_.setIterationNum(1);
  tracker_.setParticleNum(600);
  tracker_.setResampleLikelihoodThr(0.00);
  tracker_.setUseNormal(false);

  //Setup coherence object for tracking
  ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence
    (new ApproxNearestPairPointCloudCoherence<RefPointType>);

  DistanceCoherence<RefPointType>::Ptr distance_coherence
    (new DistanceCoherence<RefPointType>);
  coherence->addPointCoherence (distance_coherence);

  pcl::search::Octree<RefPointType>::Ptr search (new pcl::search::Octree<RefPointType> (0.01));
  coherence->setSearchMethod (search);
  coherence->setMaximumDistance (0.01);

  tracker_.setCloudCoherence(coherence);

  //prepare the model of tracker's target
  Eigen::Vector4f c;
  Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
  CloudPtr transed_ref (new Cloud);
  CloudPtr transed_ref_downsampled (new Cloud);

  pcl::compute3DCentroid<RefPointType> (*target_cloud, c);
  trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
  ROS_INFO_STREAM("compute3DCentroid: c[0]=" << c[0] << " c[1]=" << c[1] << " c[2]=" << c[2]);
  pcl::transformPointCloud<RefPointType> (*target_cloud, *transed_ref, trans.inverse());
  gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);

  //set reference model and trans
  tracker_.setReferenceCloud(transed_ref_downsampled);
  tracker_.setTrans(trans);

  //Setup OpenNIGrabber and viewer
  pcl::visualization::CloudViewer* viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer");
  viewer_->runOnVisualizationThread (viz_cb, "viz_cb");

  //Start viewer and object tracking
  while (!viewer_->wasStopped())
  {
    std::this_thread::sleep_for(1s);
    ros::spinOnce();
  }

  //ros::spin();
}


#include <sm/eigen/gtest.hpp>
#include <aslam/backend/Optimizer.hpp>
#include <aslam/backend/OptimizationProblem.hpp>
#include <aslam/backend/ErrorTerm.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <sm/random.hpp>
#include <aslam/backend/test/ErrorTermTester.hpp>
#include <aslam/backend/test/SampleDvAndError.hpp>

TEST(OptimizerTestSuite, testOptimizerMatrices)
{
  try {
    using namespace aslam::backend;
    boost::shared_ptr<OptimizationProblem> problem_ptr(new OptimizationProblem);
    OptimizationProblem& problem = *problem_ptr;
    const int P = 3;
    const int E = 2;
    // Add some design variables.
    std::vector< boost::shared_ptr<Point2d> > p2d;
    for (int p = 0; p < P; ++p) {
      boost::shared_ptr<Point2d> point(new Point2d(Eigen::Vector2d::Random()));
      p2d.push_back(point);
      problem.addDesignVariable(point);
      point->setBlockIndex(p);
      point->setActive(true);
    }
    // Add some error terms.
    std::vector< boost::shared_ptr<LinearErr> > e1;
    for (int p = 0; p < P; ++p) {
      for (int e = 0; e < E; ++e) {
        boost::shared_ptr<LinearErr> err(new LinearErr(p2d[p].get()));
        e1.push_back(err);
        problem.addErrorTerm(err);
        SCOPED_TRACE("");
        testErrorTerm(err);
      }
    }
    // std::vector< boost::shared_ptr<LinearErr2> > e2;
    // Point2d * ps = p2d[P-1].get();
    // ps->setMarginalized(true);
    // for(int p = 0; p < P-1; ++p)
    //   {
    //  boost::shared_ptr<LinearErr2> err(new LinearErr2(p2d[p].get(), ps) )
    //  e2.push_back( err );
    //  problem.addErrorTerm(err);
    //  SCOPED_TRACE("");
    //  testErrorTerm(err);
    //  eh.testAll();
    //   }
    // Now let's optimize.
    OptimizerOptions options;
    options.verbose = false;
    options.linearSolver = "dense";
    options.levenbergMarquardtLambdaInit = 10;
    options.doSchurComplement = false;
    options.doLevenbergMarquardt = false;
    options.maxIterations = 1;
    Optimizer optimizer(options);
    optimizer.setProblem(problem_ptr);
    optimizer.optimize();
    // for(int p = 0; p < P; ++p)
    //   {
    //  std::cout << "dv[" << p << "] index: " << problem.designVariable(p).blockIndex() << std::endl;
    //   }
    // for(unsigned i = 0; i < problem.numErrorTerms(); ++i)
    //   {
    //  std::cout << "e[" << i << "] has " << problem.errorTerm(i).numDesignVariables() << " design variables\n";
    //  for(unsigned j = 0; j < problem.errorTerm(i).numDesignVariables(); ++j)
    //    {
    //      std::cout << "\tdv[" << j << "] has block index " << problem.errorTerm(i).designVariable(j)->blockIndex() << std::endl;
    //      std::cout << "\tdv[" << j << "]: " << ( (void*)problem.errorTerm(i).designVariable(j)) << std::endl;
    //    }
    //   }
    for (int p = 0; p < P; ++p)
      problem.designVariable(p)->revertUpdate();
    // Now...build the Jacobian matrix by hand.
    Eigen::MatrixXd J(problem.numErrorTerms() * 2, P * 2);
    J.setZero();
    Eigen::VectorXd e(problem.numErrorTerms() * 2);
    e.setZero();
    Eigen::VectorXd rhs(P * 2);
    rhs.setZero();
    Eigen::MatrixXd invR(problem.numErrorTerms() * 2, problem.numErrorTerms() * 2);
    invR.setZero();
    boost::shared_ptr<SparseBlockMatrix> cloneH(optimizer.H().clone());
    cloneH->clear();

    Eigen::VectorXd ei;
    for (unsigned i = 0; i < problem.numErrorTerms(); ++i) {
      int ridx = 2 * i;
      JacobianContainerSparse<> jc(problem.errorTerm(i)->dimension());
      problem.errorTerm(i)->evaluateError();
      problem.errorTerm(i)->getWeightedJacobians(jc, false);
      J.block<2, P * 2>(ridx, 0) = jc.asDenseMatrix(optimizer.H().colBlockIndices());
      problem.errorTerm(i)->getWeightedError(ei, false);
      e.segment<2>(ridx) = ei;
      problem.errorTerm(i)->buildHessian(*cloneH, rhs, false);
    }
    Eigen::MatrixXd estH = J.transpose() * J;
    Eigen::MatrixXd H = optimizer.H().toDense().selfadjointView<Eigen::Upper>();
    sm::eigen::assertNear(estH, H, 1e-14, SM_SOURCE_FILE_POS, "Did we build the Hessian correctly? A is from J^T * J and B is from the optimizer.");
    Eigen::VectorXd estRhs = -J.transpose() * e;
    sm::eigen::assertNear(estRhs, optimizer.rhs(), 1e-14, SM_SOURCE_FILE_POS, "Did we build the right hand side correctly?");
    for (unsigned i = 0; i < problem.numErrorTerms(); ++i) {
      cloneH->clear();
      J.setZero();
      int ridx = 2 * i;
      problem.errorTerm(i)->evaluateError();
      JacobianContainerSparse<> Jc(problem.errorTerm(i)->dimension());
      problem.errorTerm(i)->evaluateJacobians(Jc);
      J = Jc.asDenseMatrix(optimizer.H().colBlockIndices());
      e.segment<2>(ridx) = dynamic_cast<ErrorTermFs<2>*>(problem.errorTerm(i))->error();
      Eigen::MatrixXd invR = problem.errorTerm(i)->vsInvR();
      //std::cout << "invR:\n" << invR << std::endl;
      //std::cout << "J:\n" << J << std::endl;
      problem.errorTerm(i)->buildHessian(*cloneH, rhs, false);
      Eigen::MatrixXd eH = J.transpose() * invR * J;
      sm::eigen::assertNear(*cloneH, eH, 1e-14, SM_SOURCE_FILE_POS, "Did we build the Hessian correctly? A is from J^T * J and B is from the optimizer.");
    }
  } catch (const std::exception& e) {
    FAIL() << e.what();
  }
}

TEST(OptimizerTestSuite, testOptimizerNormalized)
{
  try {
    using namespace aslam::backend;
    boost::shared_ptr<OptimizationProblem> problem_ptr(new OptimizationProblem);
    OptimizationProblem& problem = *problem_ptr;
    std::vector<Eigen::Vector2d> points;
    const int P = 3;
    const int E = 2;
    // Add some design variables.
    double scale = 0.001;
    std::vector< boost::shared_ptr<Point2d> > p2d;
    for (int p = 0; p < P; ++p) {
      Eigen::Vector2d pts = (Eigen::Vector2d::Random());
      points.push_back(pts);
      boost::shared_ptr<Point2d> point(new Point2d(pts));
      // set a random scaling:
      point->setScaling(scale);
      scale *= scale;
      p2d.push_back(point);
      problem.addDesignVariable(point);
      point->setBlockIndex(p);
      point->setActive(true);
    }
    // Add some error terms.
    std::vector< boost::shared_ptr<LinearErr> > e1;
    for (int p = 0; p < P; ++p) {
      for (int e = 0; e < E; ++e) {
        boost::shared_ptr<LinearErr> err(new LinearErr(p2d[p].get()));
        e1.push_back(err);
        problem.addErrorTerm(err);
      }
    }
    // Now let's optimize.
    OptimizerOptions options;
    options.verbose = false;
    options.linearSolver = "dense";
    options.levenbergMarquardtLambdaInit = 10;
    options.levenbergMarquardtEstimateLambdaScale = 1;
    options.doSchurComplement = false;
    options.doLevenbergMarquardt = false;
    options.maxIterations = 1;
    Optimizer optimizer(options);
    optimizer.setProblem(problem_ptr);
    optimizer.optimize();
//    std::cout << "A:" << std::endl << optimizer.H().toDense() << std::endl;
//    std::cout << "b:" << std::endl << optimizer.rhs() << std::endl;
//        boost::shared_ptr<OptimizationProblem> problem_ptr2( new OptimizationProblem );
//        OptimizationProblem & problem2 = *problem_ptr2;
//
//        // reset the points:
//
//
//        for(int p = 0; p < P; p++) {
//
//          p2d[p]->_v = points[p];
//          p2d[p]->_p_v = points[p];
//
//
//          // set a random scaling:
//          double scale = (double)rand() / RAND_MAX;
//          p2d[p]->setScaling(1);
//          std::cout << "scale [" << p << "] " << p2d[p]->scaling() << std::endl;
//          problem2.addDesignVariable(p2d[p]);
//
//
//
//
//        }
//        for(int p = 0; p < e1.size(); p++) {
//          problem2.addErrorTerm( e1[p] );
//        }
//        p2d.clear();
//        // Add some design variables.
//        for(int p = 0; p < P; ++p)
//          {
//          boost::shared_ptr<Point2d> point( new Point2d( points[p] ) );
//          p2d.push_back( point );
//          problem2.addDesignVariable(point);
//          point->setBlockIndex(p);
//          point->setActive(true);
//          }
//
//
//        e1.clear();
//        for(int p = 0; p < P; ++p)
//          {
//      for(int e = 0; e < E; ++e)
//        {
//          boost::shared_ptr<LinearErr> err( new LinearErr(p2d[p].get() ) );
//          e1.push_back( err );
//          problem2.addErrorTerm( err );
//          SCOPED_TRACE("");
//          testErrorTerm(err);
//        }
//          }
//
//        // Now let's optimize.
//        OptimizerOptions options2;
//        options2.verbose = true;
//        options2.linearSolver = "dense";
//        options2.levenbergMarquardtLambdaInit = 10;
//        options2.doSchurComplement = false;
//        options2.doLevenbergMarquardt = false;
//        options2.maxIterations = 10;
//
//        Optimizer optimizer2(options2);
//        optimizer2.setProblem( problem_ptr2 );
//        optimizer2.optimize();
    //   std::cout << "H2: " << std::endl << optimizer2.H().toDense() << std::endl;
    //   std::cout << "H1: " << std::endl << optimizer.H().toDense() << std::endl;
    //   std::cout << "rhs2" << std::endl << optimizer2.rhs() << std::endl;
    //   std::cout << "rhs1" << std::endl << optimizer.rhs() << std::endl;
    // verify
    // sm::eigen::assertNear(optimizer.J(),optimizer2.J(), 1e-3, SM_SOURCE_FILE_POS);
  } catch (const std::exception& e) {
    FAIL() << e.what();
  }
}