#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include <CGAL/Three/Scene_group_item.h>
#include "ui_Mean_curvature_flow_skeleton_plugin.h"

#include "Kernel_type.h"
#include "Scene_surface_mesh_item.h"
#include <CGAL/boost/graph/graph_traits_Surface_mesh.h>

#include "Scene_mcf_item.h"
#include "Scene_points_with_normal_item.h"
#include "Scene_polylines_item.h"
#include "Scene.h"
#include <QApplication>
#include <QMainWindow>
#include <QInputDialog>
#include <QElapsedTimer>
#include <QMessageBox>

#include <Eigen/Sparse>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Eigen_solver_traits.h>
#include <CGAL/extract_mean_curvature_flow_skeleton.h>
#include <CGAL/iterator.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>

#include <CGAL/boost/graph/split_graph_into_polylines.h>
#include <CGAL/mesh_segmentation.h>
#include <CGAL/boost/graph/copy_face_graph.h>
#include <CGAL/Facet_with_id_pmap.h>
#include <queue>

namespace PMP = CGAL::Polygon_mesh_processing;

typedef Scene_surface_mesh_item Scene_face_graph_item;
namespace CGAL {

template<>
void set_halfedgeds_items_id (Scene_face_graph_item::Face_graph&)
{}

} // namespace CGAL

typedef Scene_face_graph_item::Face_graph Face_graph;

typedef boost::graph_traits<Face_graph>::vertex_descriptor          vertex_descriptor;
typedef boost::graph_traits<Face_graph>::vertex_iterator            vertex_iterator;
typedef boost::graph_traits<Face_graph>::halfedge_descriptor        halfedge_descriptor;

typedef CGAL::Mean_curvature_flow_skeletonization<Face_graph>      Mean_curvature_skeleton;
typedef Mean_curvature_skeleton::Skeleton Skeleton;

typedef Kernel::Point_3            Point;

struct Polyline_visitor
{
  typedef std::vector<Point> Polyline;
  typedef std::vector<std::size_t> Polyline_of_ids;

  std::list<Polyline>& polylines;
  Skeleton& skeleton;

  Polyline_visitor(std::list<Polyline>& lines, Skeleton& skeleton)
    : polylines(lines),
      skeleton(skeleton)
  {}

  void start_new_polyline()
  {
    Polyline V;
    polylines.push_back(V);
  }

  void add_node(boost::graph_traits<Skeleton>::vertex_descriptor vd)
  {
    Polyline& polyline = polylines.back();
    polyline.push_back(skeleton[vd].point);
  }

  void end_polyline(){}
};

using namespace CGAL::Three;
class Polyhedron_demo_mean_curvature_flow_skeleton_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
  QAction* actionMCFSkeleton;
  QAction* actionConvert_to_medial_skeleton;

public:

  ~Polyhedron_demo_mean_curvature_flow_skeleton_plugin()
  {
    delete ui;
  }

  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface, Messages_interface*) {

    this->mw = mainWindow;
    this->scene = scene_interface;

    dockWidget = NULL;
    ui = NULL;

    actionMCFSkeleton = new QAction(tr(
                                      "Mean Curvature Skeleton (Advanced)"
                                      ), mainWindow);
    actionMCFSkeleton->setProperty("subMenuName", "Triangulated Surface Mesh Skeletonization");
    actionMCFSkeleton->setObjectName("actionMCFSkeleton");

    actionConvert_to_medial_skeleton = new QAction(tr("Extract Medial Skeleton"), mainWindow);
    actionConvert_to_medial_skeleton->setProperty("subMenuName", "Triangulated Surface Mesh Skeletonization");
    actionConvert_to_medial_skeleton->setObjectName("actionConvert_to_medial_skeleton");

    dockWidget = new QDockWidget(tr(
                                   "Mean Curvature Skeleton"
                                   ),mw);
    dockWidget->setVisible(false);
    ui = new Ui::Mean_curvature_flow_skeleton_plugin();
    ui->setupUi(dockWidget);
    dockWidget->setFeatures(QDockWidget::DockWidgetMovable
                          | QDockWidget::DockWidgetFloatable
                          | QDockWidget::DockWidgetClosable);
    dockWidget->setWindowTitle(tr(
                               "Mean Curvature Skeleton"
                                 ));
    addDockWidget(dockWidget);

    connect(ui->pushButton_contract, SIGNAL(clicked()),
            this, SLOT(on_actionContract()));
    connect(ui->pushButton_collapse, SIGNAL(clicked()),
            this, SLOT(on_actionCollapse()));
    connect(ui->pushButton_split, SIGNAL(clicked()),
            this, SLOT(on_actionSplit()));
    connect(ui->pushButton_degeneracy, SIGNAL(clicked()),
            this, SLOT(on_actionDegeneracy()));
    connect(ui->pushButton_run, SIGNAL(clicked()),
            this, SLOT(on_actionRun()));
    connect(ui->pushButton_skeletonize, SIGNAL(clicked()),
            this, SLOT(on_actionSkeletonize()));
    connect(ui->pushButton_converge, SIGNAL(clicked()),
            this, SLOT(on_actionConverge()));
    connect(dynamic_cast<Scene*>(scene), SIGNAL(updated_bbox(bool)),
            this, SLOT(on_actionUpdateBBox(bool)));
    connect(ui->pushButton_segment, SIGNAL(clicked()),
            this, SLOT(on_actionSegment()));

    autoConnectActions();
    QObject* scene_object = dynamic_cast<QObject*>(scene);
    connect(scene_object, SIGNAL(itemAboutToBeDestroyed(CGAL::Three::Scene_item*)),
            this, SLOT(on_actionItemAboutToBeDestroyed(CGAL::Three::Scene_item*)));
  }

  virtual void closure()
  {
    dockWidget->hide();
  }

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

  bool applicable(QAction*) const {
    return qobject_cast<Scene_face_graph_item*>(scene->item(scene->mainSelectionIndex()));
  }

  void init_ui(double diag) {
    on_checkbox_toggled(false);
    connect(ui->is_medially_centered, SIGNAL(toggled(bool)),
            this, SLOT(on_checkbox_toggled(bool)));
    connect(ui->helpButton, &QPushButton::clicked,
            [this]{QMessageBox::about(mw, QString("Help"),
                                    QString("This widget gives access to the low level steps of the mean curvature flow sketonization algorithm. "
                                            "The algorithm is iterative. Each iteration consist in calls to Contract, Collapse, Split, "
                                            "and Degeneracy (repectively mesh contraction, edge collapse, edge split, and degenerate edge"
                                            "removal). The skeleton extraction can be called at any time but for a better result it should be"
                                            "called when the iterations are converging. A segmentation of the surface can be extracted using"
                                            "the distance of the mesh to the skeleton computed.\n"
                                             "All operations can be applied to a polyhedron item or "
                                            "to a surface mesh item. The generated mcf group must be selected in "
                                            "order to continue an on-going set of operations. "));});
    ui->omega_H->setValue(0.1);
    ui->omega_H->setSingleStep(0.1);
    ui->omega_H->setDecimals(3);
    ui->omega_P->setValue(0.2);
    ui->omega_P->setSingleStep(0.1);
    ui->omega_P->setDecimals(3);
    ui->min_edge_length->setDecimals(7);
    ui->min_edge_length->setValue(0.002 * diag);
    ui->min_edge_length->setSingleStep(0.0000001);
    ui->delta_area->setDecimals(7);
    ui->delta_area->setValue(1e-4);
    ui->delta_area->setSingleStep(1e-5);
    ui->is_medially_centered->setChecked(false);

    ui->label_omega_H->setToolTip(QString("omega_H controls the velocity of movement and approximation quality"));
    ui->label_omega_P->setToolTip(QString("omega_P controls the smoothness of the medial approximation"));
    ui->pushButton_contract->setToolTip(QString("contract mesh based on mean curvature flow"));
    ui->pushButton_collapse->setToolTip(QString("collapse short edges"));
    ui->pushButton_split->setToolTip(QString("split obtuse triangles"));
    ui->pushButton_degeneracy->setToolTip(QString("fix degenerate points"));
    ui->pushButton_skeletonize->setToolTip(QString("Turn mesh to a skeleton curve"));
    ui->pushButton_run->setToolTip(QString("run one iteration of contract, collapse, split, detect degeneracy"));
    ui->pushButton_converge->setToolTip(QString("iteratively contract the mesh until convergence"));
  }

  bool check_item_index(int index) {
    if (index < 0)
    {
      QMessageBox msgBox;
      msgBox.setText("Please select an item first");
      msgBox.exec();
      return false;
    }
    return true;
  }

  /// \todo move this function into an include
  bool is_mesh_valid(Face_graph *pMesh) {
    if (! CGAL::is_closed(*pMesh))
    {
      QMessageBox msgBox;
      msgBox.setText("The mesh is not closed.");
      msgBox.exec();
      return false;
    }
    if (! CGAL::is_triangle_mesh(*pMesh))
    {
      QMessageBox msgBox;
      msgBox.setText("The mesh is not a pure triangle mesh.");
      msgBox.exec();
      return false;
    }

    // the algorithm is only applicable on a mesh
    // that has only one connected component

    boost::unordered_map<boost::graph_traits<Face_graph>::face_descriptor,int> cc(num_faces(*pMesh));
    std::size_t num_component = PMP::connected_components(*pMesh, boost::make_assoc_property_map(cc));

    if (num_component != 1)
    {
      QMessageBox msgBox;
      QString str = QString("The mesh is not a single closed mesh.\n It has %1 components.").arg(num_component);
      msgBox.setText(str);
      msgBox.exec();
      return false;
    }
    return true;
  }

  /// \todo remove duplicated code
  // check if the Mean_curvature_skeleton exists
  // or has the same polyheron item
  // check if the mesh is a watertight triangle mesh
  bool check_mesh(Scene_mcf_item* item) {
    Face_graph *pMesh = item->input_triangle_mesh;

    if (item->mcs == NULL)
    {
      if (!is_mesh_valid(pMesh))
      {
        return false;
      }
      createContractedItem(item);

      item->fixedPointsItemIndex = -1;
      item->nonFixedPointsItemIndex = -1;
      item->poleLinesItemIndex = -1;
    }
    else
    {
      item->mcs->set_quality_speed_tradeoff(ui->omega_H->value());
      item->mcs->set_medially_centered_speed_tradeoff(ui->omega_P->value());
      item->mcs->set_min_edge_length(ui->min_edge_length->value());
      item->mcs->set_area_variation_factor(ui->delta_area->value());
      item->mcs->set_is_medially_centered(ui->is_medially_centered->isChecked());
    }
    return true;
  }

  void update_meso_skeleton(Scene_mcf_item* item)
  {
    clear(*item->meso_skeleton);
    copy_face_graph(item->mcs->meso_skeleton(), *item->meso_skeleton);
    scene->item(item->contractedItemIndex)->invalidateOpenGLBuffers();
    scene->itemChanged(item->contractedItemIndex);
  }

  void update_parameters(Mean_curvature_skeleton* mcs)
  {
    double omega_H = ui->omega_H->value();
    double omega_P = ui->omega_P->value();
    double min_edge_length = ui->min_edge_length->value();
    double delta_area = ui->delta_area->value();
    bool is_medially_centered = ui->is_medially_centered->isChecked();

    mcs->set_quality_speed_tradeoff(omega_H);
    mcs->set_medially_centered_speed_tradeoff(omega_P);
    mcs->set_min_edge_length(min_edge_length);
    mcs->set_area_variation_factor(delta_area);
    mcs->set_is_medially_centered(is_medially_centered);
  }

public Q_SLOTS:
  void on_actionMCFSkeleton_triggered();
  void on_actionConvert_to_medial_skeleton_triggered();
  void on_actionContract();
  void on_actionCollapse();
  void on_actionSplit();
  void on_actionDegeneracy();
  void on_actionRun();
  void on_actionSkeletonize();
  void on_actionConverge();
  void on_actionUpdateBBox(bool);
  void on_checkbox_toggled(bool);
  void on_actionSegment();
  void on_actionItemAboutToBeDestroyed(CGAL::Three::Scene_item*);

private:
  Scene_mcf_item *getMCFItem();
  void createContractedItem(Scene_mcf_item* item);
  QDockWidget* dockWidget;
  Ui::Mean_curvature_flow_skeleton_plugin* ui;

}; // end Polyhedron_demo_mean_curvature_flow_skeleton_plugin

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionMCFSkeleton_triggered()
{
  dockWidget->show();
  dockWidget->raise();
  double diag = scene->len_diagonal();
  init_ui(diag);
  getMCFItem();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_checkbox_toggled(bool b)
{
  ui->omega_P->setEnabled(b);
  ui->label_omega_P->setEnabled(b);
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionUpdateBBox(bool)
{
  double diag = scene->len_diagonal();
  ui->min_edge_length->setValue(0.002 * diag);
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionSegment()
{
  Scene_mcf_item* item = getMCFItem();

  if(!item)
  {
    return;
  }

  if (num_vertices(item->skeleton_curve)==0 ) on_actionSkeletonize();
  if (num_vertices(item->skeleton_curve)==0 ) { QApplication::restoreOverrideCursor(); return;}
  QApplication::setOverrideCursor(Qt::WaitCursor);

  QElapsedTimer time;
  time.start();

    // init the polyhedron simplex indices
  CGAL::set_halfedgeds_items_id(*item->input_triangle_mesh);
  boost::property_map<Face_graph, boost::vertex_index_t>::type
    vimap = get(boost::vertex_index, *item->input_triangle_mesh);

  //for each input vertex compute its distance to the skeleton
  std::vector<double> distances(num_vertices(*item->input_triangle_mesh));

  Face_graph *smesh = item->input_triangle_mesh;

  boost::property_map<Face_graph,CGAL::vertex_point_t>::type vpm
    = get(CGAL::vertex_point,*smesh);

  for(boost::graph_traits<Skeleton>::vertex_descriptor v : CGAL::make_range(vertices(item->skeleton_curve)) )
  {
    const Point& skel_pt = item->skeleton_curve[v].point;
    for(vertex_descriptor mesh_v : item->skeleton_curve[v].vertices)
    {
      const Point& mesh_pt = get(vpm,mesh_v);
      distances[get(vimap,mesh_v)] = std::sqrt(CGAL::squared_distance(skel_pt, mesh_pt));
    }
  }

  // create a property-map for sdf values
  std::vector<double> sdf_values( num_faces(*item->input_triangle_mesh) );
  Facet_with_id_pmap<Face_graph,double> sdf_property_map(*item->input_triangle_mesh, sdf_values);

  // compute sdf values with skeleton
  for(boost::graph_traits<Face_graph>::face_descriptor f : faces(*item->input_triangle_mesh))
  {
    double dist = 0;
    for(boost::graph_traits<Face_graph>::halfedge_descriptor hd : halfedges_around_face(halfedge(f, *item->input_triangle_mesh), *item->input_triangle_mesh))
      dist+=distances[get(vimap,target(hd, *item->input_triangle_mesh))];
    sdf_property_map[f] = dist / 3.;
  }

  // post-process the sdf values
  CGAL::sdf_values_postprocessing(*item->input_triangle_mesh, sdf_property_map);

  // create a property-map for segment-ids (it is an adaptor for this case)
  std::vector<std::size_t> segment_ids( num_faces(*item->input_triangle_mesh) );
  Facet_with_id_pmap<Face_graph,std::size_t> segment_property_map(*item->input_triangle_mesh, segment_ids);

  // segment the mesh using default parameters
  std::cout << "Number of segments: "
            << CGAL::segmentation_from_sdf_values(*item->input_triangle_mesh, sdf_property_map, segment_property_map) <<"\n";

  Face_graph* segmented_polyhedron = new Face_graph(*item->input_triangle_mesh);

  Scene_face_graph_item* item_segmentation = new Scene_face_graph_item(segmented_polyhedron);
  int i=0;
  typedef boost::property_map<Face_graph, CGAL::face_patch_id_t<int> >::type Fpim;
  Fpim fpim = get(CGAL::face_patch_id_t<int>(), *segmented_polyhedron);
  int nb_segment=0;
  for(boost::graph_traits<Face_graph>::face_descriptor fd : faces(*segmented_polyhedron))
  {
    int segment = static_cast<int>(segment_ids[i++]);
    if(segment > nb_segment)
      nb_segment = segment + 1;
    put(fpim, fd, segment);

  }
  item_segmentation->setItemIsMulticolor(true);
  item_segmentation->computeItemColorVectorAutomatically(true);
  item_segmentation->setProperty("NbPatchIds", nb_segment); //for join_and_split plugin
  item_segmentation->invalidateOpenGLBuffers();
  scene->addItem(item_segmentation);
  item_segmentation->setName(QString("segmentation"));
  scene->changeGroup(item_segmentation, item);
  Scene_item* parent = scene->item(item->InputMeshItemIndex);
  if(parent)
    parent->setVisible(false);
  scene->setSelectedItem(scene->item_id(item));

  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionConvert_to_medial_skeleton_triggered()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

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

  if(item)
  {
    Face_graph* pMesh = item->polyhedron();

    if ( !is_mesh_valid(pMesh) ) return;

    QElapsedTimer time;
    time.start();
    QApplication::setOverrideCursor(Qt::WaitCursor);

    Skeleton skeleton;
    CGAL::extract_mean_curvature_flow_skeleton(*pMesh, skeleton);

    std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

    //create the polylines representing the skeleton
    Scene_polylines_item* skeleton_item = new Scene_polylines_item();
    skeleton_item->setColor(QColor(175, 0, 255));

    Polyline_visitor polyline_visitor(skeleton_item->polylines, skeleton);
    CGAL::split_graph_into_polylines( skeleton,
                                      polyline_visitor,
                                      CGAL::internal::IsTerminalDefault() );

    skeleton_item->setName(QString("Medial skeleton curve of %1").arg(item->name()));
    scene->setSelectedItem(-1);
    scene->addItem(skeleton_item);
    skeleton_item->invalidateOpenGLBuffers();

    item->setPointsMode();

    QApplication::restoreOverrideCursor();
  }
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionContract()
{
  Scene_mcf_item* item = getMCFItem();

  if (!item || !check_mesh(item))
  {
    return;
  }

  QElapsedTimer time;
  time.start();
  std::cout << "Contract...\n";
  QApplication::setOverrideCursor(Qt::WaitCursor);

  update_parameters(item->mcs);
  item->mcs->contract_geometry();

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  update_meso_skeleton(item);
  QApplication::restoreOverrideCursor();
  scene->setSelectedItem(scene->item_id(item));
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionCollapse()
{
  Scene_mcf_item* item =
    getMCFItem();

  if (!item || !check_mesh(item))
  {
    return;
  }

  QElapsedTimer time;
  time.start();
  std::cout << "Collapse...\n";
  QApplication::setOverrideCursor(Qt::WaitCursor);

  update_parameters(item->mcs);
  std::size_t num_collapses = item->mcs->collapse_edges();
  std::cout << "collapsed " << num_collapses << " edges.\n";

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  update_meso_skeleton(item);
  QApplication::restoreOverrideCursor();
  scene->setSelectedItem(scene->item_id(item));
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionSplit()
{
  Scene_mcf_item* item =
    getMCFItem();

  if (!item || !check_mesh(item))
  {
    return;
  }

  QElapsedTimer time;
  time.start();
  std::cout << "Split...\n";
  QApplication::setOverrideCursor(Qt::WaitCursor);

  update_parameters(item->mcs);
  std::size_t num_split = item->mcs->split_faces();
  std::cout << "split " << num_split << " triangles.\n";

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  update_meso_skeleton(item);
  QApplication::restoreOverrideCursor();
  scene->setSelectedItem(scene->item_id(item));
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionDegeneracy()
{
  Scene_mcf_item* item = getMCFItem();

  if (!item || !check_mesh(item))
  {
    return;
  }

  QElapsedTimer time;
  time.start();
  std::cout << "Detect degeneracy...\n";
  QApplication::setOverrideCursor(Qt::WaitCursor);

  update_parameters(item->mcs);
  item->mcs->detect_degeneracies();

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  Scene_points_with_normal_item* fixedPointsItem = new Scene_points_with_normal_item;
  fixedPointsItem->setName(QString("fixed points of %1").arg(item->name()));
  std::vector<Point> fixedPoints;
  item->mcs->fixed_points(fixedPoints);

  Point_set *ps = fixedPointsItem->point_set();
  for (size_t i = 0; i < fixedPoints.size(); ++i)
  {
    Kernel::Point_3 point (fixedPoints[i].x(), fixedPoints[i].y(), fixedPoints[i].z());
    ps->insert(point);
  }
  ps->select_all ();

  if (item->fixedPointsItemIndex == -1)
  {
    item->fixedPointsItemIndex = scene->addItem(fixedPointsItem);
    scene->changeGroup(fixedPointsItem, item);
    item->lockChild(fixedPointsItem);
  }
  else
  {
    Scene_item* temp = scene->replaceItem(item->fixedPointsItemIndex, fixedPointsItem, false);
    delete temp;
  }
  // update scene
  update_meso_skeleton(item);
  scene->item(item->fixedPointsItemIndex)->invalidateOpenGLBuffers();
  scene->itemChanged(item->fixedPointsItemIndex);
  scene->setSelectedItem(scene->item_id(item));
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionRun()
{
  Scene_mcf_item* item = getMCFItem();

  if (!item || !check_mesh(item))
  {
    return;
  }

  QElapsedTimer time;
  time.start();
  QApplication::setOverrideCursor(Qt::WaitCursor);

  std::cout << "Run one iteration...\n";
  Scene_face_graph_item* contracted_item = NULL;
if(item->contractedItemIndex != -1)
    contracted_item = qobject_cast<Scene_face_graph_item*>(scene->item(item->contractedItemIndex));
scene->setSelectedItem(scene->item_id(item));
//todo : create a new contracted item
if(!contracted_item)
{
  createContractedItem(item);
  contracted_item = qobject_cast<Scene_face_graph_item*>(scene->item(item->contractedItemIndex));
}

  update_parameters(item->mcs);
  item->mcs->contract();

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  // update scene
  Scene_points_with_normal_item* fixedPointsItem = new Scene_points_with_normal_item;
  fixedPointsItem->setName(QString("fixed points of %1").arg(contracted_item->name()));
  std::vector<Point> fixedPoints;
  item->mcs->fixed_points(fixedPoints);

  Point_set *ps = fixedPointsItem->point_set();
  for (size_t i = 0; i < fixedPoints.size(); ++i)
  {
    Kernel::Point_3 point(fixedPoints[i].x(), fixedPoints[i].y(), fixedPoints[i].z());
    ps->insert(point);
  }
  ps->select_all();

  if (item->fixedPointsItemIndex == -1)
  {
    item->fixedPointsItemIndex = scene->addItem(fixedPointsItem);
    scene->changeGroup(fixedPointsItem, item);
    item->lockChild(fixedPointsItem);
  }
  else
  {
    Scene_item* temp = scene->replaceItem(item->fixedPointsItemIndex, fixedPointsItem, false);
    delete temp;
  }

//#define DRAW_NON_FIXED_POINTS
#ifdef DRAW_NON_FIXED_POINTS
  // draw non-fixed points
  Scene_points_with_normal_item* nonFixedPointsItem = new Scene_points_with_normal_item;
  nonFixedPointsItem->setName("non-fixed points");
  nonFixedPointsItem->setColor(QColor(0, 255, 0));
  std::vector<Point> nonFixedPoints;
  mcs->non_fixed_points(nonFixedPoints);
  ps = nonFixedPointsItem->point_set();
  for (size_t i = 0; i < nonFixedPoints.size(); ++i)
  {
    UI_point_3<Kernel> point(nonFixedPoints[i].x(), nonFixedPoints[i].y(), nonFixedPoints[i].z());
    ps->push_back(point);
  }
  if (nonFixedPointsItemIndex == -1)
  {
    nonFixedPointsItemIndex = scene->addItem(nonFixedPointsItem);
    scene->changeGroup(nonFixedPointsItem, item);
    item->lockChild(nonFixedPointsItem);
  }
  else
  {
    scene->replaceItem(nonFixedPointsItemIndex, nonFixedPointsItem, false);
  }
  scene->itemChanged(nonFixedPointsItemIndex);
#endif

//#define DRAW_POLE_LINE
#ifdef DRAW_POLE_LINE
  // draw lines connecting surface points and their correspondent poles
  Scene_polylines_item* poleLinesItem = new Scene_polylines_item();
  Face_graph* pMesh = item->input_triangle_mesh;
  std::vector<Point> pole_points;
  item->mcs->poles(pole_points);
  vertex_iterator vb, ve;
  int id = 0;
  for (boost::tie(vb, ve) = vertices(*pMesh); vb != ve; ++vb)
  {
    std::vector<Point> line;
    line.clear();

    vertex_descriptor v = *vb;
    Point s = v->point();
    Point t = pole_points[id++];

    line.push_back(s);
    line.push_back(t);
    poleLinesItem->polylines.push_back(line);
  }

  if (item->poleLinesItemIndex == -1)
  {
    item->poleLinesItemIndex = scene->addItem(poleLinesItem);
    scene->changeGroup(poleLinesItem, item);
    item->lockChild(poleLinesItem);
  }
  else
  {
    scene->replaceItem(poleLinesItemIndex, poleLinesItem, false);
  }
#endif

  update_meso_skeleton(item);
  scene->setSelectedItem(scene->item_id(item));
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionSkeletonize()
{
  Scene_mcf_item* item = getMCFItem();

  if (!item || !check_mesh(item))
  {
    return;
  }

  QElapsedTimer time;
  time.start();
  QApplication::setOverrideCursor(Qt::WaitCursor);

  update_parameters(item->mcs);

  item->mcs->convert_to_skeleton(item->skeleton_curve);

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  //create the polylines representing the skeleton
  Scene_polylines_item* skeleton = new Scene_polylines_item();
  skeleton->setColor(QColor(175, 0, 255));

  Polyline_visitor polyline_visitor(skeleton->polylines, item->skeleton_curve);
  CGAL::split_graph_into_polylines( item->skeleton_curve,
                                    polyline_visitor,
                                    CGAL::internal::IsTerminalDefault() );

  skeleton->setName(QString("skeleton curve of %1").arg(item->name()));
  skeleton->invalidateOpenGLBuffers();
  if(item->skeletonItemIndex == -1)
  {
    item->skeletonItemIndex = scene->addItem(skeleton);
    scene->changeGroup(skeleton, item);
    item->lockChild(skeleton);
  }
  else
  {
    scene->replaceItem(item->skeletonItemIndex, skeleton, false);
  }

  // set the fixed points and contracted mesh as invisible
  if (item->fixedPointsItemIndex >= 0)
  {
    scene->item(item->fixedPointsItemIndex)->setVisible(false);
    scene->itemChanged(item->fixedPointsItemIndex);
  }
  scene->item(item->contractedItemIndex)->setVisible(false);
  scene->itemChanged(item->contractedItemIndex);
  if (item->InputMeshItemIndex >= 0)
  {
    scene->item(item->InputMeshItemIndex)->setVisible(true);
    scene->item(item->InputMeshItemIndex)->setPointsMode();
    scene->itemChanged(item->InputMeshItemIndex);
  }
  scene->setSelectedItem(scene->item_id(item));
  // update scene
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionConverge()
{
  Scene_mcf_item* item = getMCFItem();

  if (!item || !check_mesh(item))
  {
    return;
  }

  QElapsedTimer time;
  time.start();
  QApplication::setOverrideCursor(Qt::WaitCursor);

  item->mcs->contract_until_convergence();

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  // update scene
  Scene_points_with_normal_item* fixedPointsItem = new Scene_points_with_normal_item;
  fixedPointsItem->setName(QString("fixed points of %1").arg(item->name()));

  std::vector<Point> fixedPoints;
  item->mcs->fixed_points(fixedPoints);

  Point_set *ps = fixedPointsItem->point_set();
  for (size_t i = 0; i < fixedPoints.size(); ++i)
  {
    Kernel::Point_3 point(fixedPoints[i].x(), fixedPoints[i].y(), fixedPoints[i].z());
    ps->insert(point);
  }
  ps->select_all();

  if (item->fixedPointsItemIndex == -1)
  {
    item->fixedPointsItemIndex = scene->addItem(fixedPointsItem);
    scene->changeGroup(fixedPointsItem, item);
    item->lockChild(fixedPointsItem);
  }
  else
  {
    Scene_item* temp = scene->replaceItem(item->fixedPointsItemIndex, fixedPointsItem, false);
    delete temp;
  }

  scene->item(item->fixedPointsItemIndex)->invalidateOpenGLBuffers();
  scene->itemChanged(item->fixedPointsItemIndex);
  update_meso_skeleton(item);
  scene->setSelectedItem(scene->item_id(item));

  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionItemAboutToBeDestroyed(CGAL::Three::Scene_item* corpse )
{
  Scene_mcf_item *mcf= qobject_cast<Scene_mcf_item*>(corpse);

  if(mcf)
  {
    mcf->mcs = NULL;
    mcf->meso_skeleton = NULL;
    mcf->input_triangle_mesh = NULL;
    mcf->fixedPointsItemIndex = -1;
    mcf->nonFixedPointsItemIndex = -1;
    mcf->poleLinesItemIndex = -1;
    mcf->contractedItemIndex = -1;
    mcf->InputMeshItemIndex = -1;
    mcf->meso_skeleton = NULL;
    mcf->input_triangle_mesh = NULL;
  }
}

void
Polyhedron_demo_mean_curvature_flow_skeleton_plugin::createContractedItem(Scene_mcf_item* item)
{
  if(!item)
    return;
  if(item->mcs != NULL)
    delete item->mcs;
  double omega_H = ui->omega_H->value();
  double omega_P = ui->omega_P->value();
  double min_edge_length = ui->min_edge_length->value();
  double delta_area = ui->delta_area->value();
  bool is_medially_centered = ui->is_medially_centered->isChecked();

  item->mcs = new Mean_curvature_skeleton(*item->input_triangle_mesh);
  item->meso_skeleton = new Face_graph(*item->input_triangle_mesh);
  //set algorithm parameters
  item->mcs->set_quality_speed_tradeoff(omega_H);
  item->mcs->set_medially_centered_speed_tradeoff(omega_P);
  item->mcs->set_min_edge_length(min_edge_length);
  item->mcs->set_is_medially_centered(is_medially_centered);
  item->mcs->set_area_variation_factor(delta_area);

  Scene_face_graph_item* contracted_item = new Scene_face_graph_item(item->meso_skeleton);
  contracted_item->setName(QString("contracted mesh of %1").arg(item->name()));
  contracted_item->setItemIsMulticolor(false); //avoids segfault if item was a multicolor surface_mesh

  item->contractedItemIndex = scene->addItem(contracted_item);
  scene->changeGroup(contracted_item, item);
  item->lockChild(contracted_item);
  scene->setSelectedItem(scene->item_id(item));

}

Scene_mcf_item*
Polyhedron_demo_mean_curvature_flow_skeleton_plugin::getMCFItem()
{
  Q_FOREACH(int index, scene->selectionIndices())
  {
    Scene_mcf_item* mcf = qobject_cast<Scene_mcf_item*>(scene->item(index));
    if(mcf)
      return mcf;
  }

  //if the selected item is not an MCF but is a face_graph_item,
  //then create and add a new MCF
  if(scene->mainSelectionIndex() != -1)
  {
    Scene_face_graph_item* item =
        qobject_cast<Scene_face_graph_item*>(scene->item(
                                               scene->mainSelectionIndex()));
    if(item)
    {
      Face_graph* pMesh = item->face_graph();

      if(!pMesh) return NULL;
      Scene_mcf_item* mcf = new Scene_mcf_item(item->face_graph(),
                                               scene->mainSelectionIndex(),
                                               QString("%1 (mcf)").arg(item->name()));
      connect(item, &Scene_face_graph_item::aboutToBeDestroyed,
              [mcf, this]{
        if(scene->item_id(mcf) != -1){
          scene->erase(scene->item_id(mcf));
      }});
      scene->setSelectedItem(scene->addItem(mcf));
      item->setVisible(false);
      scene->itemChanged(item);
      return mcf;
    }
  }
  return NULL;
}

#include "Mean_curvature_flow_skeleton_plugin.moc"


#pragma once

#include <Eigen/Core>
#include <Eigen/StdVector>

class ThinPlateSpline {
public:
  typedef std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>
      PointList;

  ThinPlateSpline() {}
  ThinPlateSpline(const PointList &src, const PointList &dst);
  ~ThinPlateSpline() {}

  /* Solve */
  void solve();

  /* Interpolate */
  Eigen::Vector3d interpolate(const Eigen::Vector3d &p) const;

  /* Source Points */
  const PointList &srcPoints() const { return mSrcPoints; }

  /* Set Source Points */
  void setSrcPoints(const PointList &points) { mSrcPoints = points; }

  /* Destination Points */
  const PointList &dstPoints() const { return mDstPoints; }

  /* Set Destination Points */
  void setDstPoints(const PointList &points) { mDstPoints = points; }

protected:
  /* Radial Basis Function */
  static inline double radialBasis(double r) {
    return r == 0.0 ? r : r * r * log(r);
  }

  /* Data */
  PointList mSrcPoints;
  PointList mDstPoints;
  Eigen::MatrixXd mW;
  Eigen::MatrixXd mL;
};


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      E.H. Hirschel and C. Weiland, Selected Aerothermodynamic Design Problems of Hypersonic
 *          Flight Vehicles (chapter 5), Springer/AIAA, 2009.
 *      D. Dirkx, Continuous Shape Optimization of Entry Vehicles, MSc thesis, Delft University
 *          of Technology, 2011 (Unpublished).
 *
 */

#include <cmath>
#include <iostream>

#include <boost/make_shared.hpp>
#include <memory>

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Mathematics/GeometricShapes/capsule.h"
#include "Tudat/Mathematics/GeometricShapes/conicalFrustum.h"
#include "Tudat/Mathematics/GeometricShapes/sphereSegment.h"
#include "Tudat/Mathematics/GeometricShapes/torus.h"

namespace tudat
{
namespace geometric_shapes
{

//! Default constructor.
Capsule::Capsule( const double noseRadius,
                  const double middleRadius,
                  const double rearLength,
                  const double rearAngle,
                  const double sideRadius )
{
    using std::sin;
    using std::cos;
    using mathematical_constants::PI;

    // Call set functions for number of single and composite surface geometries
    // with predetermined values.
    setNumberOfCompositeSurfaceGeometries( 0 );
    setNumberOfSingleSurfaceGeometries( 4 );

    // Set member shape variables
    noseRadius_ = noseRadius;
    middleRadius_ = middleRadius;
    rearLength_ = rearLength;
    rearAngle_ = rearAngle;
    sideRadius_ = sideRadius;

    // Determine and set extent of spherical nose part.
    double noseSphereAngle_ = asin( ( middleRadius_ - sideRadius_ )
                                    / ( noseRadius_ - sideRadius_ ) );

    // Create nose sphere.
    std::shared_ptr< SphereSegment > noseSphere_ = std::make_shared< SphereSegment >(
                noseRadius_, 0, 2 * PI, 0, noseSphereAngle_ );

    // Declare translation vector.
    Eigen::VectorXd translationVector_ = Eigen::VectorXd( 3 );
    translationVector_( 2 ) = 0.0;
    translationVector_( 1 ) = 0.0;
    translationVector_( 0 ) = - noseRadius_ * cos( noseSphereAngle_ );

    // Set nose translation vector.
    noseSphere_->setOffset( translationVector_ );

    // Set noseSphere_ in singleSurfaceList_.
    setSingleSurfaceGeometry( noseSphere_, 0 );

    //Calculate noseSphere volume
    double noseSphereHeight_ = noseRadius_ * (1.0 - cos( noseSphereAngle_ ));
    double noseSphereVolume_ = (PI / 3.0) * pow(noseSphereHeight_, 2.0) * (
                3.0 * noseRadius - noseSphereHeight_ );

    // Create rear cone, fully revolved.
    std::shared_ptr< ConicalFrustum > cone_ = std::make_shared< ConicalFrustum >(
                rearAngle_, middleRadius_ - sideRadius_ * ( 1.0 - cos( rearAngle_ ) ),
                rearLength_ );

    // Set translation vector of cone.
    translationVector_( 0 ) = -sideRadius_ * ( sin( PI / 2.0 - noseSphereAngle_ )
                                               + sin ( -rearAngle_ ) );
    cone_->setOffset( translationVector_ );

    // Set cone in singleSurfaceList_.
    setSingleSurfaceGeometry( cone_, 1 );

    // Obtain start radius of cone.
    double startRadius_ = cone_->getStartRadius( );

    // Calculate end radius of cone.
    double endRadius_ = startRadius_ + rearLength_ * tan( rearAngle_ );

    // Calculate volume of cone.
    double fullConeLength_ = rearLength_ + endRadius_ / tan( -rearAngle_ );
    double coneVolume_ = (PI / 3.0) *
            ( pow(startRadius_, 2.0) * fullConeLength_ -
              pow(endRadius_, 2.0) * ( fullConeLength_ - rearLength_ ));

    // Calculate rear sphere radius.
    double rearNoseRadius_ = endRadius_ / cos( -rearAngle_ );

    // Create rear sphere ( "end cap" ), fully revolved.
    std::shared_ptr< SphereSegment > rearSphere_ = std::make_shared< SphereSegment >(
                rearNoseRadius_, 0.0, 2.0 * PI, PI / 2.0 - rearAngle_, PI );

    // Set translation vector of rear sphere.
    translationVector_( 0 ) =  ( rearNoseRadius_ * sin( -rearAngle_ ) ) - rearLength_
            - ( sideRadius_ * ( sin( PI / 2.0 - noseSphereAngle_ ) + sin ( -rearAngle_ ) ) );
    rearSphere_->setOffset( translationVector_ );
    setSingleSurfaceGeometry( rearSphere_, 2 );

    // Calcualte volume of rear sphere
    double rearSphereHeight_ = rearNoseRadius_ * ( 1.0 - cos( (PI / 2.0) + rearAngle));
    double rearSphereVolume_ = (PI / 3.0 ) * pow( rearSphereHeight_, 2.0 ) * (
                3.0 * rearNoseRadius_ - rearSphereHeight_);

    // Create torus section of capsule.
    double torusMajorRadius_ = ( noseRadius_ - sideRadius_ ) * sin( noseSphereAngle_ );
    std::shared_ptr< Torus > torus_ = std::make_shared< Torus >(
       torusMajorRadius_, sideRadius_, 0.0, 2.0 * PI, PI / 2.0 - noseSphereAngle_, rearAngle_ );

    //Calculate torus volume
    double minimumDiskRadius_ = middleRadius_ - sideRadius_;
    double integrationLowerLimit_ = -1.0 * sideRadius_ * cos( noseSphereAngle_ );
    double integrationUpperLimit_ = sideRadius_ * sin( -rearAngle_ );
    double torusVolume_ = PI * (
            pow( minimumDiskRadius_, 2.0 ) * (integrationUpperLimit_ - integrationLowerLimit_ )
            + minimumDiskRadius_ * pow( sideRadius_, 2.0 ) * (
                (asin( integrationUpperLimit_ / sideRadius_) + 0.5* sin( 2* asin(integrationUpperLimit_ / sideRadius_)))
                - (asin( integrationLowerLimit_ / sideRadius_) + 0.5* sin( 2* asin(integrationLowerLimit_ / sideRadius_)))
                )
            + pow( sideRadius_, 2.0 ) * ( integrationUpperLimit_ - integrationLowerLimit_ )
            - (pow( integrationUpperLimit_, 3.0 ) - pow( integrationLowerLimit_, 3.0 )) / 3.0
                );

    //Calculate total capsule volume
    capsuleVolume_ = noseSphereVolume_ + coneVolume_ + rearSphereVolume_ + torusVolume_;

    //Calculate total capsule length
    totalLength_ = noseSphereHeight_ - integrationLowerLimit_ + integrationUpperLimit_ +
            rearLength_ + rearSphereHeight_;

    //Calculate frontal area
    frontalArea_ = PI * pow( middleRadius_, 2.0 );

    // Set translation vector of rear sphere.
    translationVector_( 0 ) = -cos( noseSphereAngle_ ) * sideRadius_;
    torus_->setOffset( translationVector_ );
    setSingleSurfaceGeometry( torus_, 3 );

    // Set rotation matrix fo each part to be compatible with flow direction in
    // aerodynamic analysis.
    Eigen::MatrixXd rotationMatrix = Eigen::MatrixXd( 3, 3 );
    double angle_ = PI / 2.0;
    rotationMatrix( 0, 0 ) = cos( angle_ );
    rotationMatrix( 0, 1 ) = 0.0;
    rotationMatrix( 0, 2 ) = sin( angle_ );
    rotationMatrix( 1, 0 ) = 0.0;
    rotationMatrix( 1, 1 ) = 1.0;
    rotationMatrix( 1, 2 ) = 0.0;
    rotationMatrix( 2, 0 ) = -sin( angle_ );
    rotationMatrix( 2, 1 ) = 0.0;
    rotationMatrix( 2, 2 ) = cos( angle_ );

    // Set rotation matrix for single surface geometries.
    for ( unsigned i = 0; i < numberOfSingleSurfaceGeometries_ ; i++ )
    {
        singleSurfaceGeometryList_[ i ]->setRotationMatrix( rotationMatrix );
    }
}

//! Overload ostream to print class information.
std::ostream &operator << ( std::ostream &stream, Capsule& capsule )
{
    stream << "This is a capsule." << std::endl;
    stream << "The defining parameters are: " << std::endl
           << "Nose radius: " << capsule.getNoseRadius( ) << std::endl
           << "Mid radius: " << capsule.getMiddleRadius( ) << std::endl
           << "Rear length: " << capsule.getRearLength( ) << std::endl
           << "Rear angle: " << capsule.getRearAngle( ) << std::endl
           << "Side radius: " << capsule.getSideRadius( ) << std::endl;

    return stream;
}

} // namespace geometric_shapes
} // namespace tudat


// =======================================================================
// Copyright 2015 by Ireneusz Szcześniak
// Author: Ireneusz Szcześniak <www.irkos.org>
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
// =======================================================================

// =======================================================================
// This is the implementation of the Yen algorithm:
//
// Jin Y. Yen, Finding the k shortest loopless paths in a network,
// Management Science, vol. 17, no. 11, July 1971, pages 712-716
//
// Note 1: in the article there are Q^k Q_k Q^k_k used.  As far as I
// can tell they have the same meaning.  Q simply denotes the number
// of the node previous to the last node in a path (which always
// equals to the number of nodes in the path - 1), while (Q) denotes
// the node number Q in a path.  For instance, in the path a-b-c-d,
// the number of the node previous to the last is Q = 3, while (Q) =
// c.  Q^k refers to the k-th shortest path, and simply means the
// number of the node, which is previous to the last in the k-shortest
// path.
// =======================================================================

#ifndef BOOST_GRAPH_YEN_KSP
#define BOOST_GRAPH_YEN_KSP

#include <list>
#include <set>
#include <utility>

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/optional.hpp>

#include "custom_dijkstra_call.hpp"

namespace boost {

  template <typename Graph, typename WeightMap, typename IndexMap>
  std::list<std::pair<typename WeightMap::value_type,
                      std::list<typename Graph::edge_descriptor>>>
  yen_ksp(const Graph& g,
          typename Graph::vertex_descriptor s,
          typename Graph::vertex_descriptor t,
          WeightMap wm, IndexMap im, optional<unsigned> K)
  {
    typedef typename Graph::vertex_descriptor vertex_descriptor;
    typedef typename Graph::edge_descriptor edge_descriptor;
    typedef typename WeightMap::value_type weight_type;
    typedef std::set<edge_descriptor> es_type;
    typedef std::set<vertex_descriptor> vs_type;
    typedef filtered_graph<Graph, is_not_in_subset<es_type>,
                           is_not_in_subset<vs_type>> fg_type;
    typedef std::list<edge_descriptor> path_type;
    typedef std::pair<weight_type, path_type> kr_type;

    // The shortest paths - these we return.
    std::list<kr_type> A;

    // An empty result if the source and destination are the same.
    if (s == t)
      return A;

    // The tentative paths - these are candidate paths.  It's a set,
    // because we want to make sure that a given result can show up in
    // the set of tentative results only once.  The problem is that
    // the algorithm can find the same tentative path many times.
    std::set<kr_type> B;

    // Try to find the (optional) shortest path.
    optional<kr_type> osp = custom_dijkstra_call(g, s, t, wm, im);

    // We quit if there was no shortest path.
    if (!osp)
      return A;

    // The first shortest path found becomes our first solution.
    A.push_back(std::move(osp.get()));

    // In each iteration we produce the next k-th shortest path.
    for (int k = 2; !K || k <= static_cast<int>(K.get()); ++k)
      {
        // The previous shortest result and path.
        const auto &psr = A.back();
        const auto &psp = psr.second;

        // The set of excluded edges.  It's a set, because the
        // algorithm can try to exclude an edge many times.
        es_type exe;
        // The set of excluded vertexes.
        vs_type exv;

        // The edge predicate.
        is_not_in_subset<es_type> ep(exe);
        // The vertex predicate.
        is_not_in_subset<vs_type> vp(exv);

        // The filtered graph.
        fg_type fg(g, ep, vp);

        // The root result: the cost and the root path.
        kr_type rr;
        // The root path.
        const path_type &rp = rr.second;

        // Use the previous shortest path to get tentative paths.  We
        // can go ahead with the loop without checking any condition
        // (the condition in the for-statement is true): the path
        // found must have at least one link, because s != t.
        for(typename path_type::const_iterator i = psp.begin(); true;)
          {
            // An edge of the previous shortest path.
            const edge_descriptor &edge = *i;

            // The spur vertex - we try to deviate at this node.
            const vertex_descriptor &sv = source(edge, g);

            // Iterate over all previous shortest paths.
            // An iteration examines j-th shortest path.
            for(const auto &jr: A)
              {
                // The j-th shortest path.
                const path_type &jp = jr.second;

                // Let's prepare for the comparison.
                typename path_type::const_iterator jpi = jp.begin();
                typename path_type::const_iterator rpi = rp.begin();

                // Iterate as long as the edges are equal.
                while(jpi != jp.end() && rpi != rp.end() && *jpi == *rpi)
                  ++jpi, ++rpi;

                // Make sure we didn't reach the end of jp.  If we
                // did, there is no next edge in jp, which we could
                // exclude.  Also, make sure we reached the end of rp,
                // i.e., the jp begins with the complete rp, and not a
                // head of rp.
                if (jpi != jp.end() && rpi == rp.end())
                  exe.insert(*jpi);
              }

            // Optional spur result.
            optional<kr_type> osr = custom_dijkstra_call(fg, sv, t, wm, im);

            if (osr)
              {
                // The tentative result.
                kr_type tr = std::move(osr.get());
                tr.first += rr.first;
                tr.second.insert(tr.second.begin(), rp.begin(), rp.end());
                B.insert(std::move(tr));
              }

            // We have the condition to break the look here, and not
            // at the beginning, because we don't want to execute the
            // remainer of the loop in vain.
            if (++i == psp.end())
              break;

            // Remove the vertex that in this iteration is the spur,
            // but in the next iteration it's going to be a vertex
            // that should not be considered in the search for a spur
            // path.
            exv.insert(sv);

            // Add the edge to the back of the root result.
            rr.first += get(wm, edge);
            rr.second.push_back(edge);
          }

        // Stop searching when there are no tentative paths.
        if (B.empty())
          break;

        // Take the shortest tentative path and make it the next
        // shortest path.
        A.push_back(std::move(*B.begin()));
        B.erase(B.begin());
      }

    return A;
  }

  template <typename Graph>
  std::list<std::pair<typename property_map<Graph, edge_weight_t>::value_type,
                      std::list<typename Graph::edge_descriptor>>>
  yen_ksp(Graph& g,
          typename Graph::vertex_descriptor s,
          typename Graph::vertex_descriptor t,
          optional<unsigned> K = optional<unsigned>())
  {
    return yen_ksp(g, s, t, get(edge_weight_t(), g),
                   get(vertex_index_t(), g), K);
  }

} // boost

#endif /* BOOST_GRAPH_YEN_KSP */


#include <iostream>
#include <chrono>
#include <string>
#include <cstdlib>
#include <Eigen>
#include "nbodytool.hpp"

void initializeSim(Simulator& sim) {
    for (int i = 0; i < sim.maxObjects; ++i) {
        sim.addObject(rand() % 100, 0.001, Eigen::Vector3d::Random()*10, Eigen::Vector3d::Zero());
    }
}

void benchmark(Simulator& sim, int iters, const std::string& name) {
    std::cout << "Benchmarking " << name                  << std::endl
              << "--------------------------------------" << std::endl
              << "Average over " << iters << " iterations."       << std::endl;

    std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
    double stepAvg = 0;
    for (int i = 0; i < iters; ++i) {
        start = std::chrono::high_resolution_clock::now();
        sim.step();
        end   = std::chrono::high_resolution_clock::now();
        stepAvg += std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    }
    std::cout << "step(): " << stepAvg/iters << " ms" << std::endl << std::endl;
}

void energyConservationTest(Simulator& sim, int iters, const std::string& name) {
    std::cout << "Testing Energy Conservation: " << name                  << std::endl
              << "--------------------------------------" << std::endl
              << "Over " << iters << " iterations."       << std::endl;

    double initialEnergy = sim.totalEnergy();
    std::cout << "Initial Energy: " << initialEnergy << std::endl;

    double avgEnergyChange = 0;
    double prevEnergy = initialEnergy;
    for (int i = 0; i < iters; ++i) {
        sim.step();
        double currEnergy = sim.totalEnergy();
        double energyChange = currEnergy - prevEnergy;
        avgEnergyChange += energyChange;
        prevEnergy = currEnergy;

        std::cout << "Step " << i + 1 << " | Current Energy: " << currEnergy << " | Change: " <<
                     energyChange << " | Avg. Change: " << avgEnergyChange/(i+1) << std::endl;
    }

}

int main() {
    // Simulator2d 1k Euler Gravitational_Direct
    Simulator sim2d_euler_gd_1k(1, 1000, new EulerIntegrator(),
                                new Gravitational_Direct(0.1, Unit::LightYear, Unit::SolarMass, Unit::JulianMillenium));

    Simulator sim2d_verlet_gbh_1k(1, 100, new VerletIntegrator(),
                                   new Gravitational_BarnesHut(1, 0.1, Unit::LightYear, Unit::SolarMass, Unit::JulianMillenium));

    Simulator sim2d_verlet_gbh_10k(1, 10000, new VerletIntegrator(),
                                   new Gravitational_BarnesHut(1, 0.1, Unit::LightYear, Unit::SolarMass, Unit::JulianMillenium));

    initializeSim(sim2d_euler_gd_1k);
    initializeSim(sim2d_verlet_gbh_1k);
    initializeSim(sim2d_verlet_gbh_10k);

    energyConservationTest(sim2d_verlet_gbh_1k, 100000, "Simulator Verlet Gravitational_BarnesHut 1k");

    benchmark(sim2d_euler_gd_1k, 3, "Simulator Euler Gravitational_Direct 1k");
    benchmark(sim2d_verlet_gbh_10k, 5, "Simulator Verlet Gravitational_BarnesHut 10k");

    return 0;
}


//==================================================================================================
/*!
  @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_ARCH_COMMON_SIMD_FUNCTION_COSH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_COSH_HPP_INCLUDED

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/average.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( cosh_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) according x < Threshold e =  exp(x) or exp(x/2) is
      // respectively computed
      // *  in the first case cosh (e+rec(e))/2
      // *  in the second     cosh is (e/2)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      auto test1 = (x > Maxlog<A0>()-Log_2<A0>());
      A0 fac = if_else(test1, Half<A0>(), One<A0>());
      A0 tmp = exp(x*fac);
      A0 tmp1 = Half<A0>()*tmp;
      return if_else(test1, tmp1*tmp, bs::average(tmp, rec(tmp)));
    }
  };
} } }

#endif


#pragma once

#include <Eigen/Dense>
#include <OpenVolumeMesh/Core/PropertyDefines.hh>
#include <cstddef>

#include "Typedefs.hpp"

namespace FeltElements
{
// Forward declarations.
namespace Body
{
struct Material;
struct Forces;
}  // namespace Body

namespace Derivatives
{
[[nodiscard]] Element::StiffnessResidual KR(
	Element::NodePositions const & x,
	Element::BoundaryVtxhIdxs const & boundary_faces_idxs,
	Element::BoundaryNodePositions const & boundary_faces_x,
	Element::ShapeDerivative const & dN_by_dX,
	Body::Material const & material,
	Body::Forces const & forces);

[[nodiscard]] Element::Stiffness Kc(
	Element::ShapeDerivative const & dN_by_dx, Scalar v, Element::Elasticity const & c);
[[nodiscard]] Element::Stiffness Ks(
	Element::ShapeDerivative const & dN_by_dx, Scalar v, Element::Stress const & s);
[[nodiscard]] Element::Stiffness Kp(
	Element::BoundaryNodePositions const & xs, Element::BoundaryVtxhIdxs const & S_to_Vs, Scalar p);

[[nodiscard]] Element::Elasticity c(Scalar J, Scalar lambda, Scalar mu);
[[nodiscard]] Node::Force t(Scalar p, Element::SurfaceGradient const & dX_by_dS);
[[nodiscard]] Element::Forces T(
	Element::ShapeDerivative const & dN_by_dx, Scalar v, Element::Stress const & sigma);
[[nodiscard]] Element::Stress sigma(
	Scalar J, Element::Gradient const & b, Scalar lambda, Scalar mu);

[[nodiscard]] Scalar det_dx_by_dX(Element::Gradient const & F);
[[nodiscard]] Element::Gradient b(Element::Gradient const & F);

[[nodiscard]] Element::Gradient dx_by_dX(
	Element::NodePositions const & x, Element::ShapeDerivative const & dN_by_dX);
[[nodiscard]] Element::Gradient dx_by_dX(
	Element::Gradient const & dx_by_dL, Element::Gradient const & dL_by_dX);

[[nodiscard]] Scalar V(Element::NodePositions const & x);
[[nodiscard]] Scalar v(Element::NodePositions const & x);
[[nodiscard]] Scalar A(BoundaryElement::NodePositions const & s);

[[nodiscard]] Element::Gradient dX_by_dL(Element::NodePositions const & X);
[[nodiscard]] Element::Gradient dL_by_dX(Element::Gradient const & dX_by_dL);
[[nodiscard]] Element::CartesianDerivative dx_by_dN(
	Element::ShapeCartesianTransform const & N_to_x);

[[nodiscard]] Element::SurfaceGradient dX_by_dS(BoundaryElement::NodePositions const & X);

[[nodiscard]] Element::ShapeDerivative dN_by_dX(Element::Gradient const & dL_by_dx);
[[nodiscard]] Element::ShapeDerivative dN_by_dX(Element::ShapeCartesianTransform const & N_to_x);
[[nodiscard]] Element::ShapeDerivative dN_by_dX(Element::NodePositions const & X);

[[nodiscard]] Element::ShapeCartesianTransform N_to_x(Element::NodePositions const & X);

[[nodiscard]] Scalar det_dx_by_dL(Element::NodePositions const & x);

extern Element::IsoCoordDerivative const dL_by_dN;
extern Element::ShapeDerivative const dN_by_dL;
extern Element::SurfaceShapeDerivative const dN_by_dS;
extern Element::ShapeDerivativeDeterminant const det_dN_by_dL;
extern Tensor::Multi<3, 3, 3> const levi_civita;
}  // namespace Derivatives
}  // namespace FeltElements


#pragma once

#include <boost/align/is_aligned.hpp>
#include <boost/simd/algorithm.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/log.hpp>
#include <boost/simd/function/sum.hpp>
#include <boost/simd/pack.hpp>

#include "source.hpp"
#include "summing.hpp"

typedef boost::simd::pack<double> pack_double;

namespace simdmath {

BOOST_SYMBOL_EXPORT
double logsumexp(double const *src, size_t length)
{
    using boost::alignment::is_aligned;
    using boost::simd::aligned_load;
    size_t const vector_size = pack_double::static_size;

    double offset = boost::simd::reduce(
        src, src + length,
        boost::simd::Minf<double>(),
        boost::simd::max,
        boost::simd::Minf<jdouble>());
    pack_double voffset(offset);

    double acc = 0.;
    while (length && !is_aligned(src, pack_double::alignment)) {
        acc += boost::simd::exp(*(src++) - offset);
        --length;
    }
    while (length % vector_size) {
        --length;
        acc += boost::simd::exp(src[length] - offset);
    }
    pack_double vacc = boost::simd::Zero<pack_double>();
    for (size_t i = 0; i < length; i += vector_size) {
        vacc += boost::simd::exp(aligned_load<pack_double>(src, i) - voffset);
    }

    return boost::simd::log(acc + boost::simd::sum(vacc)) + offset;
}

BOOST_SYMBOL_EXPORT
double dot(double const *src1, double const *src2, size_t length)
{
    source_1d<weighted_sum_tag> f
        = source_1d<weighted_sum_tag>(src1, src2, length);
    return balanced_sum(f);
}

}  /* ::simdmath */


#include <vector>
#include <iostream>
#include <fstream>
#include <random>
#include <string>
#include <algorithm>

#include <boost/algorithm/string.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/parsers.hpp>

namespace po = boost::program_options;

typedef float float_t;

struct Point {
	float_t x, y;

	Point() {}
	Point(float_t nx, float_t ny): x(nx), y(ny) {}

	Point operator - (const Point &p) const {
		return Point(x - p.x, y - p.y);
	}

	Point operator + (const Point &p) const {
		return Point(x + p.x, y + p.y);
	}

	float_t operator * (const Point &p) const {
		return x * p.x + y * p.y;
	}

	Point operator * (const float_t k) const {
		return Point(x * k, y * k);
	}
};

struct GraphLayout {
	int n;
	std::vector< std::vector<int> > adj;
	std::vector<Point> layout;
	float_t W, H;

	GraphLayout(
			int n,
			float_t nodeDistW, float_t borderW, float_t edgeW,
			float_t crossW, float_t crossT,
			float_t canvasW, float_t canvasH
	) {
		this->n = n;
		_nw = nodeDistW;
		_bw = borderW;
		_ew = edgeW;
		_cw = crossW;
		_mind = crossT;
		W = canvasW;
		H = canvasH;
		adj.assign(n, std::vector<int>());
	}

	void addEdge(int u, int v) {
		adj[u].push_back(v);
		adj[v].push_back(u);
	}

	template<typename RNG> void initialize_positions(const std::vector<Point> &start, RNG &rng) {
		layout = start;
		if (layout.empty()) {
			layout.resize(n);
			std::uniform_real_distribution<float_t> wdis(0, W);
			std::uniform_real_distribution<float_t> hdis(0, H);
			for (int i = 0; i < n; ++i) {
				layout[i].x = wdis(rng);
				layout[i].y = hdis(rng);
			}
		}
		initEnergy();
	}

	void move(int v, Point d) {
		_tmpne = _ne;
		_tmpbe = _be;
		_tmpee = _ee;
		_tmpce = _ce;
		_tmpPoint = layout[v];
		addEnergy(v, -1);
		layout[v].x = std::clamp<float_t>(
				layout[v].x + d.x, W / 1000, static_cast<const float_t &>(W * 0.999)
		);
		layout[v].y = std::clamp<float_t>(
				layout[v].y + d.y, H / 1000, static_cast<const float_t &>(H * 0.999)
		);
		addEnergy(v, 1);
	}

	// Take care to call this only straight after move
	void undoMove(int v, Point d) {
		layout[v] = _tmpPoint;
		_ne = _tmpne;
		_be = _tmpbe;
		_ee = _tmpee;
		_ce = _tmpce;
	}

	float_t getEnergy() const {
		return _ne + _be + _ee + _ce;
	}

	float_t getNodeDistEnergy() const {
		return _ne;
	}

	float_t getBorderEnergy() const {
		return _be;
	}

	float_t getEdgesEnergy() const {
		return _ee;
	}

	float_t getCrossingsEnergy() const {
		return _ce;
	};

private:
	float_t _mind;
	float_t _ne, _be, _ee, _ce;
	float_t _tmpne, _tmpbe, _tmpee, _tmpce;
	Point _tmpPoint;
	float_t _nw, _bw, _ew, _cw;

	float_t _sqr(float_t x) {
		return x * x;
	}

	float_t _sqdist(int i, int j) {
		return _sqr(layout[i].x - layout[j].x) + _sqr(layout[i].y - layout[j].y);
	}

	float_t _segsqdist(int u, int v, int w) {
		float_t ans = fmin(_sqdist(w, u), _sqdist(w, v));
		Point uw = layout[w] - layout[u];
		Point uv = layout[v] - layout[u];
		float_t cp = uw * uv;
		if (cp < 0) return ans;
		if ((layout[u] - layout[v]) * (layout[w] - layout[v]) < 0) return ans;
		cp /= uv * uv;
		Point h = layout[u] + (uv * cp);
		return _sqr(layout[w].x - h.x) + _sqr(layout[w].y - h.y);
	}

	void initEnergy() {
		_ne = _be = _ee = _ce = 0;
		if (_nw != 0) {
			float_t dE = 0;
			for (int i = 0; i < n; ++i) {
				for (int j = 0; j < i; ++j) {
					dE += 1 / _sqdist(i, j);
				}
			}
			_ne += dE * _nw;
		}
		if (_bw != 0) {
			float_t dE = 0;
			for (int i = 0; i < n; ++i) {
				dE +=
						1 / _sqr(layout[i].x) + 1 / _sqr(W - layout[i].x) +
						1 / _sqr(layout[i].y) + 1 / _sqr(H - layout[i].y);
			}
			_be += _bw * dE;
		}
		if (_ew != 0) {
			float_t dE = 0;
			for (int v = 0; v < n; ++v) {
				for (const auto &u : adj[v]) {
					if (v < u) {
						dE += _sqdist(v, u);
					}
				}
			}
			_ee += _ew * dE;
		}
		if (_cw != 0) {
			float_t dE = 0;
			for (int v = 0; v < n; ++v) {
				for (int u : adj[v]) {
					if (u < v) continue;
					for (int w = 0; w < n; ++w) {
						if (v == w || u == w) continue;
						dE += 1 / fmax(_mind, _segsqdist(u, v, w));
					}
				}
			}
			_ce += _cw * dE;
		}
	}

	void addEnergy(int v, double c) {
		if (_nw != 0) {  // exact equality is fine here
			float_t dE = 0;
			for (int i = 0; i < n; ++i) {
				if (i != v) dE += 1 / _sqdist(v, i);
			}
			_ne += c * _nw * dE;
		}
		if (_bw != 0) {
			float_t dE =
					1 / _sqr(layout[v].x) + 1 / _sqr(W - layout[v].x) +
					1 / _sqr(layout[v].y) + 1 / _sqr(H - layout[v].y);
			_be += c * _cw * dE;
		}
		if (_ew != 0) {
			float_t dE = 0;
			for (const auto &u: adj[v]) {
				dE += _sqdist(v, u);
			}
			_ee += c * _ew * dE;
		}
		if (_cw != 0) {
			float_t dE = 0;
			for (int u = 0; u < n; ++u) {
				for (int w : adj[u]) {
					if (u == v || w == v || w < u) continue;
					dE += 1 / fmax(_mind, _segsqdist(u, w, v));
				}
			}
			for (int u : adj[v]) {
				for (int w = 0; w < n; ++w) {
					if (v == w || u == w) continue;
					dE += 1 / fmax(_mind, _segsqdist(v, u, w));
//					std::cerr << "DEBUG " << _segsqdist(v, u, w) << "\n";
//					std::cerr << "DEBUG " << _segsqdist(u, v, w) << "\n";
//					std::cerr << "====\n";
				}
			}
			_ce += c * _cw * dE;
		}
	}
};

float_t transition_probability(float_t dE, float_t T) {
	// here dE must be positive
	return exp(-dE / T);
}

void print_layout(const GraphLayout &layout) {
	for (int i = 0; i < layout.n; ++i) {
		std::cout << layout.layout[i].x << " " << layout.layout[i].y << ";";
	}
	std::cout
		<< layout.getNodeDistEnergy() << ";" << layout.getBorderEnergy()
		<< ";" << layout.getEdgesEnergy() << ";" << layout.getCrossingsEnergy() << "\n";
}

template<typename RNG>
void simulate_annealing(
		GraphLayout &layout, int iters, float_t startT,
		float_t dT, bool fine_tune, RNG &rng, bool verbose
) {
	std::uniform_int_distribution<int> vdis(0, layout.n - 1);
	std::uniform_real_distribution<float_t> pdis(0, 1);

	auto curS = static_cast<float_t>(hypotl(layout.W, layout.H) / 2);
	auto dS = static_cast<float_t>(powl(0.001, (float_t) 1.0 / iters));
	float_t temp = startT;

	if (fine_tune) {
		curS = static_cast<float_t>(hypotl(layout.W, layout.H) / 300);
		dS = 1.0;
	}

	print_layout(layout);
	for (int i = 0; i < iters; ++i) {
		int v = vdis(rng);
		Point d{};
		d.x = (2 * std::generate_canonical<float_t, 1, RNG>(rng) - 1) * curS;
		d.y = (2 * std::generate_canonical<float_t, 1, RNG>(rng) - 1) * curS;
		float_t was_energy = layout.getEnergy();
		if (verbose) {
			std::cerr << "Iteration #" << i << ": E=" << was_energy << " T=" << temp << std::endl;
			std::cerr << "ne=" << layout.getNodeDistEnergy();
			std::cerr << " be=" << layout.getBorderEnergy();
			std::cerr << " ee=" << layout.getEdgesEnergy();
			std::cerr << " ce=" << layout.getCrossingsEnergy() << std::endl;
		}
		layout.move(v, d);
		float_t new_energy = layout.getEnergy();
		if (
				(fine_tune && new_energy > was_energy) ||
				(new_energy > was_energy &&
					pdis(rng) > transition_probability(new_energy - was_energy, temp)
				)
		) {
			layout.undoMove(v, d);
			if (verbose) {
				std::cerr
					<< "undo occured (dE=" << new_energy - was_energy
					<< ", T=" << temp << ") prob="
					<< 1 - transition_probability(new_energy - was_energy, temp) << ")\n";
			}
		}
		temp *= dT;
		curS *= dS;
		print_layout(layout);
	}
}

int main(int argc, char **argv) {
	freopen("graph.txt", "r", stdin);
	po::options_description desc("Allowed options");
	desc.add_options()
		("help", "show usage")
		("nodes", po::value<float_t>(), "weight of even node distribution in energy")
		("edges", po::value<float_t>(), "weight of length of edges in energy")
		("border", po::value<float_t>(), "weight of proximity to borderline in energy")
		("cross", po::value<float_t>(), "weight of crossings in energy")
		("cross-threshold", po::value<float_t>(), "minimum distance value for node edge interaction")
		("startT", po::value<float_t>(), "statring temperature")
		("cooling", po::value<float_t>(), "schedule of temperature reduction")
		("rounds", po::value<int>(), "number of rounds of annealing")
		("width", po::value<float_t>(), "width of canvas")
		("height", po::value<float_t>(), "height of canvas")
		("starting-layout", po::value<std::string>(), "path to starting layout file (if omitted, random one is used)")
		("seed", po::value<int>(), "random seed")
		("fine-tune", "flag to enable local search (used for fine tuning)")
		("verbose", "verbose mode");
	po::variables_map varmap;
	po::store(po::parse_command_line(argc, argv, desc), varmap);
	po::notify(varmap);

	if (varmap.count("help")) {
		std::cout << desc << std::endl;
		return 0;
	}

	bool failed = false;
	for (const auto &s: {"nodes", "edges", "border", "startT", "cooling", "rounds", "width", "height"}) {
		if (!varmap.count(s)) {
			std::cerr << "Option --" << s << " is required" << std::endl;
			failed = true;
		}
	}
	if (failed) {
		std::cout << desc << std::endl;
		return 1;
	}

	int n, m;
	std::cin >> n >> m;
	GraphLayout layout(
			n,
			varmap["nodes"].as<float_t>(),
			varmap["border"].as<float_t>(),
			varmap["edges"].as<float_t>(),
			(varmap.count("cross") ? varmap["cross"].as<float_t>() : 0),
			(varmap.count("cross-threshold") ? varmap["cross-threshold"].as<float_t>() : 1),
			varmap["width"].as<float_t>(),
			varmap["height"].as<float_t>()
	);
	for (int i = 0; i < m; ++i) {
		int from, to;
		std::cin >> from >> to;
		layout.addEdge(from, to);
	}

	std::vector<Point> start;
	if (varmap.count("starting-layout")) {
		std::ifstream start_file(varmap["starting-layout"].as<std::string>());
		std::string s, last;
		while (std::getline(start_file, s)) {
			last = s;
		}
		std::istringstream ss(last);
		start.resize(static_cast<unsigned long>(n));
		for (int i = 0; i < n; ++i) {
			char sep;
			ss >> start[i].x >> start[i].y >> sep;
		}
	}
	std::mt19937 rng(varmap.count("seed") ? varmap["seed"].as<int>() : 0);
	layout.initialize_positions(start, rng);

	simulate_annealing(
			layout, varmap["rounds"].as<int>(),
			varmap["startT"].as<float_t>(), varmap["cooling"].as<float_t>(),
			static_cast<bool>(varmap.count("fine-tune")), rng,
			static_cast<bool>(varmap.count("verbose"))
	);
}


#ifndef MI_OPEN_GL_UTILITY_HPP
#define MI_OPEN_GL_UTILITY_HPP 1
#include <cassert>
#include <vector>
#include <Eigen/Dense>
#if defined(__APPLE__)
#include <OpenGL/gl.h>
#else
#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__)
#include <windows.h>
#pragma comment(lib, "opengl32.lib")
#endif
#include <GL/gl.h>
#endif

namespace mi4
{
        class OpenGlUtility
        {
        public:
                static void lookAt ( const  Eigen::Vector3d& eye, const Eigen::Vector3d& center, const Eigen::Vector3d& up )
                {
                        const auto forward = ( center - eye ).normalized();
                        const auto side = forward.cross ( up ).normalized();
                        const auto upv  = side.cross ( forward ).normalized();
                        std::vector<double> m ( 16, 0 );

                        for ( int i = 0  ; i < 3 ; ++i ) {
                                m[i * 4 + 0] = side[i];
                                m[i * 4 + 1] = upv[i];
                                m[i * 4 + 2] = -forward[i];
                        }

                        m[3 * 4 + 3] = 1.0;
                        ::glMultMatrixd ( m.data() );
                        ::glTranslated ( -eye.x(), -eye.y(), -eye.z() );
                        return;
                }
                static void perspective ( const double fov, const double aspect, const double znear, const double zfar )
                {
                        assert ( znear - zfar != 0 );
                        std::vector< double > m(16, 0); //std::array

                        const double f = 1.0 / std::tan(0.5 * fov * M_PI / 180.0);
                        m[0] = f * 1.0 / aspect;
                        m[1 * 4 + 1] = f;
                        m[2 * 4 + 2] = ( zfar + znear ) * 1.0 / ( znear - zfar ) ;
                        m[2 * 4 + 3] = -1.0;
                        m[3 * 4 + 2] =  2.0 * zfar * znear / ( znear - zfar ) ;
                        ::glMultMatrixd ( m.data() );
                }
                static void ortho ( const double left, const double  right, const double  bottom, const double  top, const double znear, const double zfar )
                {
                        ::glOrtho ( left, right, bottom, top, znear, zfar );
                        return;
                }
                static void ortho2d ( const double  left, const double  right, const double  bottom, const double  top )
                {
                        ::glOrtho ( left, right, bottom, top, -1, 1 );
                        return;
                }
        };
}
#endif


/*
 * Copyright (c) 2020 Intel Corporation
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/**
 * @file scurve_planner.hpp
 *
 * Maintainer: Yu Yan <yu.yan@intel.com>
 *
 */

#pragma once

#include <RTmotion/global.hpp>
#include <stdlib.h>
#include <math.h>
#include <algorithm>
#include <Eigen/Core>
#include <map>
#include <RTmotion/algorithm/math_utils.hpp>

using RTmotion::MC_ERROR_CODE;

typedef enum {
  POSITION_ID = 0,
  SPEED_ID = 1,
  ACCELERATION_ID = 2,
  JERK_ID = 3
} WaypointIndex;

namespace trajectory_processing
{
struct ScurveCondition
{
  // Condition inputs for planner
  double q0 = 0.0;
  double q1 = 0.0;
  double v0 = 0.0;
  double v1 = 0.0;
  double a0 = 0.0;
  double a1 = 0.0;
  double v_max = 0.0;
  double a_max = 0.0;
  double j_max = 0.0;
};

struct ScurveProfile
{
  // Time domains for offline scurve planning results
  double Tj1 = 0.0;
  double Ta = 0.0;
  double Tj2 = 0.0;
  double Tv = 0.0;
  // Time domains for online scurve planning results
  double Tj2a = 0.0;
  double Tj2c = 0.0;
  double Tj2b = 0.0;
  double Td = 0.0;
  double Th = 0.0;
};

class ScurvePlanner
{
public:
  ScurvePlanner() = default;
  ~ScurvePlanner() = default;

  /**
   * @brief Sign transforms for being able to calculate trajectory with q1 < q0.
   *        Look at 'Trajectory planning for automatic machines and
   * robots(2008)'
   * @param condition  The input scurve condition, i.e. q0, q1, v0, v1, v_max,
   * a_max, j_max
   */
  void signTransforms(ScurveCondition& condition);

  /**
   * @brief Transforms point back to the original sign.
   * @param p Reference to Point, i.e. acc, vel, pos
   */
  void pointSignTransform(Eigen::Vector4d& p);

  virtual MC_ERROR_CODE plan() = 0;
  virtual Eigen::Vector4d getWaypoint(double t) = 0;

  ScurveProfile profile_;
  ScurveCondition condition_;
  double s_ = 0.0;  // Sign transform flag parameter
};

}  // namespace trajectory_processing


// ~/xlin/gsl/testminsky.cc
// 150924 003 Task now -> grid
// Basert på 150921 001 testminskyall.cc
// Unified source for (74) and (77) variants
//
// Todo:  Endre kappa def til arctan ? (se:'destabilizing..')
//

// GSL Manual 26.6
//  http://stackoverflow.com/questions/27913858/how-to-consult-gsl-ode-with-many-parameters-and-harmonic-functions
// http://www.tutorialspoint.com/matlab/matlab_differential.htm
// Løser differentialene vha octave-symbolic se ~/xoct/minsky/differentiate.
// Transformerer octave-løsningene til c++ (e)^ -> exp() og x^y -> pow(x,y)
// se testdiff.cpp i ~/xlin/cpp/scratch
// rk8 bruker ikke jac fjernet f.o.m versjon 014
//
#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv2.h>

#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <iomanip> // setiosflags(ios::fixed)
#include <limits>
#include <map>
#include <string>
#include <sstream>
#include <vector>

#include <boost/array.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <unistd.h>

using namespace std;

// Warn about use of deprecated functions.
#define GNUPLOT_DEPRECATE_WARN
#include "gnuplot-iostream.h"

#ifndef M_PI
#       define M_PI 3.14159265358979323846
#endif

inline void mysleep(unsigned millis){
  ::usleep(millis * 1000);
}

//http://stackoverflow.com/questions/7248627/setting-width-in-c-output-stream:
class formatted_output
{
private:
  int width, precision;
  std::ostream& stream_obj;
public:
  formatted_output(std::ostream& obj, int w, int p): width(w), precision(p),stream_obj(obj) {}
  template<typename T>
  formatted_output& operator<<(const T& output)  {
    stream_obj << std::fixed << std::setw(width) << std::setprecision(precision) << output;
    return *this;
  }
  formatted_output& operator<<(std::ostream& (*func)(std::ostream&))  {
    func(stream_obj);
    return *this;
  }
};

struct param_type {
  int eqset;
  double alfa;
  double beta;
  double delta;
  double nu;
  double r;
  double k1;
  double k2;
  double k3;
  double k4;
  double k5;
  double k6;
  double k7;
  double k8;
} ;

double kappa(double k1, double k2, double k3, double profit){
  double response;
  response = k1 + exp(k2) * exp( k3 * profit);
  return response;
}

double phi(double k4, double empshare){
  double response;
  response = (pow(k4,3)/(1.0-pow(k4,2)))/pow((1.0 - empshare),2) - k4/(1.0-pow(k4,2));
  return response;
}

double psi(double k5, double k6, double k7, double k8, double growthrate){
  double response;
  response = k5 + k6* exp(k7)*exp(k8 * growthrate);
  return response;
}

int func (double t, const double y[], double f[],
           void *params)
     {
       struct param_type *my_params_pointer = (param_type*) params;
        int eqset     = my_params_pointer->eqset;
	double alfa   = my_params_pointer->alfa;
	double beta   = my_params_pointer->beta;
	double delta  = my_params_pointer->delta;
	double nu     = my_params_pointer->nu;
	double r      = my_params_pointer->r;
	double k1     = my_params_pointer->k1;
	double k2     = my_params_pointer->k2;
	double k3     = my_params_pointer->k3;
	double k4     = my_params_pointer->k4;
	double k5     = my_params_pointer->k5;
	double k6     = my_params_pointer->k6;
	double k7     = my_params_pointer->k7;
	double k8     = my_params_pointer->k8;

        if (eqset == 74) {
	  f[0] =  y[0] * ( phi(k4, y[1]) - alfa) ;
	  f[1] =  y[1] * ( kappa(k1, k2, k3, 1 - y[0] - r * y[2]) /nu  - alfa - beta -delta);
	  f[2] =  y[2] * ( r -  kappa(k1, k2, k3, 1 - y[0] - r* y[2])/nu + delta) + kappa(k1, k2, k3,1 - y[0] - r * y[2]) - (1 - y[0]) + y[3];
	  f[3] =  y[3] * ( psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r*y[2]) / nu  - delta) + kappa(k1, k2, k3, 1 - y[0] - r * y[2]) / nu - delta );
	}

	if (eqset == 77) {
	  f[0] =  y[0] * ( phi(k4, y[1]) - alfa) ;
	  f[1] =  y[1] * (( kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] )) /nu ) - alfa - beta -delta);
	  f[2] =  y[2] * ( psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r/(y[2] * y[3] )) / nu - delta) - r) - ( y[2] * y[2] * ( y[3] * kappa(k1, k2, k3,1 - y[0] - r/(y[2] * y[3])) - y[3] * (1 - y[0]) + 1 ));
	  f[3] =  y[3] * ( -psi(k5, k6, k7, k8, ( kappa(k1, k2, k3, 1 - y[0] -r/(y[2] * y[3])) / nu ) - delta) + (kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] )) / nu) - delta );
	}

        return GSL_SUCCESS;
     }

     // fjernet - finnes i minsky.jac.cc = minsky.013.cc
     /*
     int jac (double t, const double y[], double *dfdy,
          double dfdt[], void *params)
     {
      return GSL_SUCCESS;
     }
     */

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

       if (argc < 2) {
         std::cout << "Usage: minsky eqset (74|77) [omega lambda d|upsilon  p|x num_steps])" << endl;
         exit(0);
       }

       int eqset = 74;
       if (argc > 1) eqset   = atoi(argv[1]);
       cout << eqset << endl; // CONTROL
       if (eqset != 74 && eqset != 77) {
         std::cout << "eqset has to be either 74 or 77" << endl;
         exit(0);
       }
       double y[4] = { 0.0, 0.0, 0.0, 0.0 } ;
       if (eqset == 74){
	 //y[4] = { 0.95, 0.9, 0.0, 0.001 } ;   // omega, lambda, d, p
	 y[0] = 0.95;
	 y[1] = 0.9;
	 y[2] = 0.0;
	 y[3] = 0.001;
       }
       if (eqset == 77) {
	 //y[4] = { 0.95, 0.9, 0.12, 100 } ;   // omega, lambda, upsilon, x
	 y[0] = 0.95;
	 y[1] = 0.9;
	 y[2] = 0.12;
	 y[3] = 100.0;

       }
       int num_steps = 100;

       if (argc > 2) y[0] = atof(argv[2]);
       if (argc > 3) y[1] = atof(argv[3]);
       if (argc > 4) y[2] = atof(argv[4]);
       if (argc > 5) y[3] = atof(argv[5]);
       if (argc > 6) num_steps = atoi(argv[6]);

       Gnuplot gp1;
       Gnuplot gp2;
       Gnuplot gp3;
       Gnuplot gp4;

       std::ostringstream oss;
       oss << "set title \"";
       for (int j = 0; j < argc; j++)
	 { oss << argv[j] << ' '; }
       oss << "\\n\"" << std::endl ;
       std::cout << oss.str() << std::endl;

       std::ofstream funkout("minsky.funk.out");
       std::ofstream varout("minsky.var.out");
       formatted_output fout(funkout, 16, 6);
       formatted_output vout(varout, 12, 6);

       std::vector<std::pair<double, double> > xy_pts_A; // omega
       std::vector<std::pair<double, double> > xy_pts_B; // lambda
       std::vector<std::pair<double, double> > xy_pts_C; // d         // upsilon
       std::vector<std::pair<double, double> > xy_pts_D; // p         // x

       std::vector<std::pair<double, double> > xy_pts_E; // p (ponzi)
       std::vector<std::pair<double, double> > xy_pts_F; // d (debt)
       std::vector<std::pair<double, double> > xy_pts_G; // g (growth)
       std::vector<std::pair<double, double> > xy_pts_Y; // Y

       double alfa   = 0.025;
       double beta   = 0.02;
       double delta  = 0.01;
       double nu     = 3.0;
       double r      = 0.03;
       double k1     = -0.0065;
       double k2     = -5 ;
       double k3     =  20;
       double k4     = 0.04;
       double k5     = -0.25;
       double k6     = 0.25;
       double k7     = -0.36;
       double k8     = 12.0;
       double growth = 0.0;
       double Y      = 0.0;

       double zphi;
       double zkappa;
       double zpsi;

       if (eqset == 74) {
         growth = kappa(k1, k2, k3, 1 - y[0] - r * y[2]) / nu - delta;
       }
       else if (eqset == 77) {
	 growth = kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] )) / nu - delta;
       }
       Y = 100.0 * ( 1.0 + growth);

       struct param_type my_params = {eqset, alfa, beta, delta, nu, r, k1, k2, k3, k4, k5, k6, k7, k8 };

       // gsl_odeiv2_system sys = {func, jac, 4, &my_params};
       // http://www.physics.buffalo.edu/phy411-506/tools/gsl/ode/index.html
       // rk8pd bruker ikke jac settes til NULL:
       // gsl_odeiv2_system sys = {func, jac, 4, &my_params};
       gsl_odeiv2_system sys = {func, NULL, 4, &my_params};
       gsl_odeiv2_driver * d =
       gsl_odeiv2_driver_alloc_y_new (&sys, gsl_odeiv2_step_rk8pd,
					1e-6, 1e-6, 0.0);
       int i = 0;

       double t = 0.0, t1 = 100.0;

       xy_pts_A.push_back(std::make_pair(t, y[0]));             //omega
       xy_pts_B.push_back(std::make_pair(t, y[1]));		//lambda
       xy_pts_C.push_back(std::make_pair(t, y[2]));             //d|upsilon
       xy_pts_D.push_back(std::make_pair(t, y[3]));		//p|x

       if (eqset == 77) {
	 xy_pts_E.push_back(std::make_pair(t, 1 / y[3]));         //ponzi
	 xy_pts_F.push_back(std::make_pair(t, 1 / y[3] / y[2]));  //debt
       }
       xy_pts_G.push_back(std::make_pair(t, growth));           //growth
       xy_pts_Y.push_back(std::make_pair(t, Y));                //Y

       zphi   = phi(k4, y[1]);
       if (eqset == 74) {
	 zkappa = kappa(k1, k2, k3, 1 - y[0] - r * y[2]);
	 zpsi   = psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r * y[2]) / nu - delta);
       }
       if (eqset == 77) {
	 zkappa = kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] ));
	 zpsi   = psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r/(y[2] * y[3] )) / nu - delta);
       }
       fout << oss.str() << std::endl;
       fout << "t" << "phi" <<"kappa" << "psi" << std::endl;
       fout << t << zphi << zkappa << zpsi << std::endl;

       vout << oss.str() << std::endl;
       if (eqset == 74)  {
	 vout << "t" << "omega" <<"lambda" << "d" << "p" << "growth" << "Y"  << std::endl;
	 vout << t  << y[0] << y[1] << y[2] << y[3] << growth << Y <<std::endl;
       }
       if (eqset == 77) {
 	 vout << "t" << "omega" <<"lambda" << "upsilon" << "debt" << "ponzi" << "x" << "growth" << "Y"  << std::endl;
	 vout << t  << y[0] << y[1] << y[2] << y[3] <<  1/y[3]/y[2] <<1/y[3] << growth << Y <<std::endl;       }
       if (eqset == 74) {
	 printf ("%6s %10s %10s %10s %10s %10s %10s %10s %10s\n", "t","omega","lambda","d","p","","","growth","Y");
	 printf ("%6.0f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n", t, y[0], y[1], y[2], y[3], 0.0, 0.0, growth, Y);
       }
       if (eqset == 77) {
         printf ("%6s %10s %10s %10s %10s %10s %10s %10s %10s\n", "t","omega","lambda","upsilon","x","ponzi","debt","growth","Y");
         printf ("%6.0f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n", t, y[0], y[1], y[2], y[3], 1/y[3]/y[2], 1/y[3],growth, Y);
       }

       for (i = 1; i <= num_steps; i++)
         {
           double ti = i * t1 / 100.0;
           int status = gsl_odeiv2_driver_apply (d, &t, ti, y);
           if (status != GSL_SUCCESS)
	     {
	       printf ("error, return value=%d\n", status);
	       break;
	     }

           //Funksjonsverdiene:
	   zphi   = phi(k4, y[1]);
	   if (eqset == 74) {
	     zkappa = kappa(k1, k2, k3, 1 - y[0] - r * y[2]);
	     zpsi   = psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r *y[2]) / nu - delta);
	     growth = kappa(k1, k2, k3, 1 - y[0] - r* y[2]) / nu - delta;
	   }
	   if (eqset == 77) {
	     zkappa = kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] ));
	     zpsi   = psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r/(y[2] * y[3] )) / nu - delta);
	     growth = kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] )) / nu - delta;
	   }
	   Y = Y * ( 1 + growth);

	   fout << t << zphi << zkappa << zpsi << std::endl;
	   vout << t << y[0] << y[1] << y[2] << y[3] << growth << Y <<std::endl;
	   if (eqset == 74) {
	     printf ("%6.0f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n", t, y[0], y[1], y[2], y[3], 0.0, 0.0, growth, Y);
	   }
	   if (eqset == 77) {
	     printf ("%6.0f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n", t, y[0], y[1], y[2], y[3], 1/y[3]/y[2], 1/y[3], growth, Y);
	   }
 	   xy_pts_A.push_back(std::make_pair(t, y[0]));
	   xy_pts_B.push_back(std::make_pair(t, y[1]));
	   xy_pts_C.push_back(std::make_pair(t, y[2]));
	   xy_pts_D.push_back(std::make_pair(t, y[3]));
	   if (eqset == 77) {
	     xy_pts_E.push_back(std::make_pair(t, 1 / y[3]));         //ponzi
	     xy_pts_F.push_back(std::make_pair(t, 1 / y[3] / y[2]));  //debt
	   }
	   xy_pts_G.push_back(std::make_pair(t, growth)); //growth
           xy_pts_Y.push_back(std::make_pair(t, Y)); //Y
	 }

       gsl_odeiv2_driver_free (d);

       // GNPLOTS:
       if (eqset == 74){
	 // Omega & Lambda:
	 gp1 << "set terminal x11 1 persist size 640,450 position 50,50\n";
	 gp1 << "set grid\n";
	 gp1 << oss.str() ; // Title string
	 gp1 << "set ylabel 'omega lambda'\n";
	 gp1 << "plot '-' with lines title 'omega', '-' with lines title 'lambda'\n";
	 gp1.send1d(xy_pts_A);
	 gp1.send1d(xy_pts_B);
	 // p & d:
	 gp2 << "set terminal x11 1 persist size 640,450 position 50,536\n";
	 gp2 << "set grid\n";
	 gp2 << oss.str() ; // Title string
	 gp2 << "set ylabel 'p\n";
	 gp2 << "set y2label 'd'\n";
	 gp2 << "set y2tics\n";
	 //gp2 << "plot '-' with lines title 'd\n";
 	 gp2 << "plot '-' with lines title 'p', '-' with lines  title 'd' axes x1y2\n";
	 gp2.send1d(xy_pts_D);
	 gp2.send1d(xy_pts_C);

	 /*	 // p:
	 gp3 << "set terminal x11 1 persist size 640,450 position 690,536\n";
	 gp3 << "set grid\n";
	 gp3 << oss.str() ; // Title string
	 gp3 << "set ylabel 'p\n";
	 gp3 << "plot '-' with lines title 'p\n";
	 gp3.send1d(xy_pts_D);
	 */
       }

       if (eqset == 77) {
	 // Omega & Lambda & Upsilon & X:
	 gp1 << "set terminal x11 1 persist size 640,450 position 50,50\n";
	 gp1 << "set grid\n";
	 gp1 << oss.str() ; // Title string
	 gp1 << "set ylabel 'omega lambda upsilon'\n";
	 gp1 << "set y2label 'x'\n";
	 gp1 << "set y2tics\n";
	 gp1 << "plot '-' with lines title 'omega', '-' with lines title 'lambda', '-' with lines title 'upsilon','-' with lines  title 'x' axes x1y2\n";
	 gp1.send1d(xy_pts_A);
	 gp1.send1d(xy_pts_B);
	 gp1.send1d(xy_pts_C);
	 gp1.send1d(xy_pts_D);
	 // Ponzi & Debt:
	 gp2 << "set terminal x11 2 persist size 640,450 position 50,536\n";
	 gp2 << "set grid\n";
	 gp2 << oss.str() ; // Title string
	 gp2 << "set ylabel 'ponzi'\n";
	 gp2 << "set y2label 'debt'\n";
	 gp2 << "set y2tics\n";
	 gp2 << "plot '-' with lines title 'ponzi', '-' with lines title 'debt' axes x1y2\n";
	 gp2.send1d(xy_pts_E);
	 gp2.send1d(xy_pts_F);
       }
       //  p/ponzi & growth:
       gp3 << "set terminal x11 2 persist size 640,450 position 690,50\n";
       gp3 << "set grid\n";
       gp3 << oss.str() ; // Title string
       gp3 << "set ylabel 'p/ponzi'\n";
       gp3 << "set y2label 'growthi'\n";
       gp3 << "set y2tics\n";
       gp3 << "plot '-' with lines title 'p/ponzi', '-' with lines title 'growth' axes x1y2\n";
       if (eqset == 74) gp3.send1d(xy_pts_D);
       if (eqset == 77) gp3.send1d(xy_pts_E);
       gp3.send1d(xy_pts_G);

       //  Y & d/debt:
       gp4 << "set terminal x11 2 persist size 640,450 position 690,536\n";
       gp4 << "set grid\n";
       gp4 << oss.str() ; // Title string
       gp4 << "set ylabel 'Y'\n";
       gp4 << "set y2label 'd/debt'\n";
       gp4 << "set y2tics\n";
       gp4 << "plot '-' with lines title 'Y', '-' with lines title 'd/debt' axes x1y2\n";
       gp4.send1d(xy_pts_Y);
       if (eqset == 74) gp4.send1d(xy_pts_C);
       if (eqset == 77) gp4.send1d(xy_pts_F);

       return 0;
     }

// EOF