#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include "ui_Mean_curvature_flow_skeleton_plugin.h"
#include <CGAL/Mesh_3/properties.h>
#ifdef USE_SURFACE_MESH
#include "Kernel_type.h"
#include "Scene_surface_mesh_item.h"
#include <CGAL/boost/graph/graph_traits_Surface_mesh.h>
#include <CGAL/Mesh_3/properties_Surface_mesh.h>
#else
#include "Scene_polyhedron_item.h"
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/Mesh_3/properties_Polyhedron_3.h>
#endif

#include "Scene_points_with_normal_item.h"
#include "Scene_polylines_item.h"
#include "Scene.h"
#include <QApplication>
#include <QMainWindow>
#include <QInputDialog>
#include <QTime>
#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;

#ifdef USE_SURFACE_MESH
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

#else
typedef Scene_polyhedron_item Scene_face_graph_item;
#endif

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:

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

    this->mw = mainWindow;
    this->scene = scene_interface;
    mcs = NULL;
    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(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("Mean Curvature Flow 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) {
    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_face_graph_item* item) {
    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();

    Face_graph *pMesh = item->polyhedron();

    if (mcs == NULL)
    {
      if (!is_mesh_valid(pMesh))
      {
        return false;
      }

      mcs = new Mean_curvature_skeleton(*pMesh);
      meso_skeleton = new Face_graph(*pMesh);
      input_triangle_mesh = pMesh;
      //set algorithm parameters
      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_is_medially_centered(is_medially_centered);
      mcs->set_area_variation_factor(delta_area);

      Scene_face_graph_item* contracted_item = new Scene_face_graph_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
      InputMeshItemIndex = scene->mainSelectionIndex();

      contractedItemIndex = scene->addItem(contracted_item);

      item->setVisible(false);

      fixedPointsItemIndex = -1;
      nonFixedPointsItemIndex = -1;
      poleLinesItemIndex = -1;
    }
    else
    {
      if (input_triangle_mesh != pMesh)
      {
        if (!is_mesh_valid(pMesh))
        {
          return false;
        }

        delete mcs;

        mcs = new Mean_curvature_skeleton(*pMesh);
        meso_skeleton = new Face_graph(*pMesh);
        input_triangle_mesh = pMesh;
        //set algorithm parameters
        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_is_medially_centered(is_medially_centered);
        mcs->set_area_variation_factor(delta_area);

        Scene_face_graph_item* contracted_item = new Scene_face_graph_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

        InputMeshItemIndex = scene->mainSelectionIndex();

        contractedItemIndex = scene->addItem(contracted_item);

        item->setVisible(false);

        fixedPointsItemIndex = -1;
        nonFixedPointsItemIndex = -1;
        poleLinesItemIndex = -1;
      }
      else
      {
        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);
      }
    }
    return true;
  }

  void update_meso_skeleton()
  {
    clear(*meso_skeleton);
    copy_face_graph(mcs->meso_skeleton(), *meso_skeleton);
    scene->item(contractedItemIndex)->invalidateOpenGLBuffers();
    scene->itemChanged(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_actionSegment();
  void on_actionItemAboutToBeDestroyed(CGAL::Three::Scene_item*);

private:
  Mean_curvature_skeleton* mcs;
  Face_graph* meso_skeleton; // a copy of the meso_skeleton that is displayed
  Face_graph* input_triangle_mesh;
  QDockWidget* dockWidget;
  Ui::Mean_curvature_flow_skeleton_plugin* ui;

  int fixedPointsItemIndex;
  int nonFixedPointsItemIndex;
  int poleLinesItemIndex;
  int contractedItemIndex;
  int InputMeshItemIndex;

  Skeleton skeleton_curve;
}; // end Polyhedron_demo_mean_curvature_flow_skeleton_plugin

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionMCFSkeleton_triggered()
{
  dockWidget->show();
  dockWidget->raise();

  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(!pMesh) return;

    double diag = scene->len_diagonal();
    init_ui(diag);

    fixedPointsItemIndex = -1;
    nonFixedPointsItemIndex = -1;
    poleLinesItemIndex = -1;
    contractedItemIndex = -1;
    InputMeshItemIndex = -1;
  }
}

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()
{
  if (num_vertices(skeleton_curve)==0 ) on_actionSkeletonize();
  if (num_vertices(skeleton_curve)==0 ) return;

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

  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

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

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

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

  Face_graph *smesh = item->polyhedron();

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

  BOOST_FOREACH(boost::graph_traits<Skeleton>::vertex_descriptor v, vertices(skeleton_curve) )
  {
    const Point& skel_pt = skeleton_curve[v].point;
    BOOST_FOREACH(vertex_descriptor mesh_v, 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(*input_triangle_mesh) );
  Facet_with_id_pmap<Face_graph,double> sdf_property_map(*input_triangle_mesh, sdf_values);

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

  // post-process the sdf values
  CGAL::sdf_values_postprocessing(*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(*input_triangle_mesh) );
  Facet_with_id_pmap<Face_graph,std::size_t> segment_property_map(*input_triangle_mesh, segment_ids);

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

  Face_graph* segmented_polyhedron = new Face_graph(*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);
  BOOST_FOREACH(boost::graph_traits<Face_graph>::face_descriptor fd, faces(*segmented_polyhedron))
  {
    put(fpim, fd, static_cast<int>(segment_ids[i++] ));
  }
  scene->item(InputMeshItemIndex)->setVisible(false);
  item_segmentation->setItemIsMulticolor(true);
  item_segmentation->invalidateOpenGLBuffers();
  scene->addItem(item_segmentation);
  item_segmentation->setName(QString("segmentation"));

  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;

    QTime 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()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

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

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

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

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

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

  update_meso_skeleton();
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionCollapse()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

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

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

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

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

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

  update_meso_skeleton();
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionSplit()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

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

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

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

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

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

  update_meso_skeleton();
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionDegeneracy()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

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

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

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

  update_parameters(mcs);
  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;
  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 (fixedPointsItemIndex == -1)
  {
    fixedPointsItemIndex = scene->addItem(fixedPointsItem);
  }
  else
  {
    Scene_item* temp = scene->replaceItem(fixedPointsItemIndex, fixedPointsItem, false);
    delete temp;
  }
  // update scene
  update_meso_skeleton();
  scene->item(fixedPointsItemIndex)->invalidateOpenGLBuffers();
  scene->itemChanged(fixedPointsItemIndex);
  scene->setSelectedItem(index);
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionRun()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

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

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

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

  std::cout << "Run one iteration...\n";

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

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

  CGAL::Three::Scene_interface::Item_id contracted_item_index = scene->mainSelectionIndex();
  Scene_face_graph_item* contracted_item =
    qobject_cast<Scene_face_graph_item*>(scene->item(contracted_item_index));

  // 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;
  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 (fixedPointsItemIndex == -1)
  {
    fixedPointsItemIndex = scene->addItem(fixedPointsItem);
  }
  else
  {
    Scene_item* temp = scene->replaceItem(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);
  }
  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->polyhedron();
  std::vector<Point> pole_points;
  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 (poleLinesItemIndex == -1)
  {
    poleLinesItemIndex = scene->addItem(poleLinesItem);
  }
  else
  {
    scene->replaceItem(poleLinesItemIndex, poleLinesItem, false);
  }
#endif

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

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionSkeletonize()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

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

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

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

  update_parameters(mcs);

  mcs->convert_to_skeleton(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, skeleton_curve);
  CGAL::split_graph_into_polylines( skeleton_curve,
                                    polyline_visitor,
                                    CGAL::internal::IsTerminalDefault() );

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

  // set the fixed points and contracted mesh as invisible
  if (fixedPointsItemIndex >= 0)
  {
    scene->item(fixedPointsItemIndex)->setVisible(false);
    scene->itemChanged(fixedPointsItemIndex);
  }
  scene->item(contractedItemIndex)->setVisible(false);
  scene->itemChanged(contractedItemIndex);
  // display the original mesh in transparent mode
  item->setVisible(false);
  if (InputMeshItemIndex >= 0)
  {
    scene->item(InputMeshItemIndex)->setVisible(true);
    scene->item(InputMeshItemIndex)->setPointsMode();
    scene->itemChanged(InputMeshItemIndex);
  }

  // update scene
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionConverge()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

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

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

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

  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;
  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 (fixedPointsItemIndex == -1)
  {
    fixedPointsItemIndex = scene->addItem(fixedPointsItem);
  }
  else
  {
    Scene_item* temp = scene->replaceItem(fixedPointsItemIndex, fixedPointsItem, false);
    delete temp;
  }

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

  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionItemAboutToBeDestroyed(CGAL::Three::Scene_item* /* item */)
{
  if (mcs != NULL)
  {
    delete mcs;
    mcs = NULL;
  }
}

#include "Mean_curvature_flow_skeleton_plugin.moc"


#include <iostream>
#include <thread>
#include <mutex>
#include <ctime>
#include <array>
#include <exception>

#include <armadillo>

#include <ros/ros.h>
#include <ros/package.h>

#include <gmp_lib/GMP/GMP.h>
#include <gmp_lib/GMP/GMP_MPC.h>
#include <gmp_lib/io/file_io.h>
#include <yaml-cpp/yaml.h>
#include <thread_lib/thread_lib.h>

using namespace as64_;

double y0_;
double yg_;
double Tf;

arma::rowvec pos_lim;
arma::rowvec vel_lim;
arma::rowvec accel_lim;

unsigned n_dof;

unsigned N_horizon;
double pred_time_step;
unsigned N_kernels;
double kernels_std_scaling;

double trunc_kern_thres;

double opt_pos;
double opt_vel;

std::array<double,3> slack_gains;
std::array<double,3> slack_limits;

arma::vec final_state_err_tol;

double time_limit;
unsigned max_iter;
double abs_tol;
double rel_tol;

std::string main_path;

void loadParams(const std::string &filename);
void printElapsTime(const arma::rowvec &elaps_t_data, const std::string &title);
void PRINT_INFO_MSG(const std::string &msg, std::ostream &out=std::cerr);
void PRINT_CONFIRM_MSG(const std::string &msg, std::ostream &out=std::cerr);
void PRINT_WARNING_MSG(const std::string &msg, std::ostream &out=std::cerr);
void PRINT_ERROR_MSG(const std::string &msg, std::ostream &out=std::cerr);

void test_rt_sparsity();

// ======================================
// ============== MAIN ==================
// ======================================

int main(int argc, char **argv)
{
  main_path = ros::package::getPath("gmp_mpc_controller") + "/config/";

  loadParams(main_path + "/test_rt_sparsity_params.yaml");

  std::thread thr = std::thread(test_rt_sparsity);

  int err_code = thr_::setThreadPriority(thr, SCHED_FIFO, 99);
  if (err_code) PRINT_WARNING_MSG("[MainController::makeThreadRT]: Failed to set thread priority! Reason:\n" + thr_::setThreadPriorErrMsg(err_code) + "\n", std::cerr);
  else PRINT_INFO_MSG("[MainController::makeThreadRT]: Set thread priority successfully!\n", std::cerr);

  thr.join();

  return 0;
}

// ===================================
// ============= Utils ===============
// ===================================

void test_rt_sparsity()
{
  // ============= Load training data ============
    arma::rowvec Timed;
    arma::rowvec yd_data;
    try
    {
      gmp_::FileIO fid(main_path + "/test_rt_sparsity_data.bin", gmp_::FileIO::in);
      fid.read("Time", Timed);
      fid.read("y_data", yd_data);
    }
    catch(const std::exception& e)
    {
      PRINT_ERROR_MSG(e.what());
      exit(-1);
    }

    arma::mat Yd_data = arma::repmat(yd_data, n_dof, 1);

    // ============  Create and train a DMP  ==============
    gmp_::GMP::Ptr gmp( new gmp_::GMP(n_dof, 30, 1.5) );
    arma::vec train_err;
    gmp->train("LS", Timed/Timed.back(), Yd_data, &train_err);
    std::cerr << "train_err = \n" << train_err << "\n";

    // ==========  Initialize GMP_MPC ============

    gmp_::GMP_MPC::Ptr gmp_mpc;
    gmp_mpc.reset( new gmp_::GMP_MPC(gmp.get(), N_horizon, pred_time_step, N_kernels, kernels_std_scaling, slack_gains, trunc_kern_thres) );

    gmp_mpc->setObjCostGains(opt_pos, opt_vel);

    gmp_mpc->settings.time_limit = time_limit;
    gmp_mpc->settings.max_iter = max_iter;
    gmp_mpc->settings.abs_tol = abs_tol;
    gmp_mpc->settings.rel_tol = rel_tol;

    arma::mat y_lim = arma::repmat(pos_lim, n_dof, 1);
    arma::mat ydot_lim = arma::repmat(vel_lim, n_dof, 1);
    arma::mat yddot_lim = arma::repmat(accel_lim, n_dof, 1);

    gmp_mpc->setPosLimits(y_lim.col(0), y_lim.col(1));
    gmp_mpc->setVelLimits(ydot_lim.col(0), ydot_lim.col(1));
    gmp_mpc->setAccelLimits(yddot_lim.col(0), yddot_lim.col(1));

    gmp_mpc->setPosSlackLimit(slack_limits[0]);
    gmp_mpc->setVelSlackLimit(slack_limits[1]);
    gmp_mpc->setAccelSlackLimit(slack_limits[2]);

    // ========== Run simulation ===========

    double t = 0;
    double dt = 0.002;

    double s = 0;
    double s_dot = 1/Tf;
    double s_ddot = 0;

    arma::vec O_ndof = arma::vec().zeros(n_dof);

    arma::vec y0 = y0_*arma::vec().ones(n_dof);
    arma::vec yg = yg_*arma::vec().ones(n_dof);

    arma::vec y = y0;
    arma::vec y_dot = O_ndof;
    arma::vec y_ddot = O_ndof;

    gmp_mpc->setInitialState(y, y_dot, y_ddot, s, s_dot, s_ddot);
    gmp_mpc->setFinalState(yg, O_ndof, O_ndof, 1, s_dot, 0, final_state_err_tol);

    auto can_sys_fun = [](double s, double s_dot){ return std::array<double,2>({s_dot, 0.0}); };
    gmp_mpc->setCanonicalSystemFunction(can_sys_fun);

    // gmp->setScaleMethod( gmp_::TrajScale::Ptr( new gmp_::TrajScale_Prop(n_dof) ) );
    gmp->setY0(y0);
    gmp->setGoal(yg);

    arma::rowvec elaps_t_data;
    arma::wall_clock timer;

    bool success = true;

    // =========  Simulation loop  =========
    while (true)
    {
      // --------  Stopping criteria  --------
      if (s > 1.0) break;

      timer.tic();
      gmp_::GMP_MPC::Solution sol = gmp_mpc->solve(s, s_dot);

      if (sol.exit_flag > 0) PRINT_WARNING_MSG(sol.exit_msg + "\n");
      else if (sol.exit_flag < 0)
      {
        success = false;
        PRINT_ERROR_MSG(sol.exit_msg + "\n");
        break;
      }

      y = sol.y;
      y_dot = sol.y_dot;
      y_ddot = sol.y_ddot;
      arma::vec pos_slack_var = sol.pos_slack;
      arma::vec vel_slack_var = sol.vel_slack;
      arma::vec accel_slack_var = sol.accel_slack;
      elaps_t_data = arma::join_horiz( elaps_t_data, arma::vec({timer.toc()*1000}) );

      // --------  Numerical integration  --------
      t = t + dt;

      std::array<double,2> phase_dot = can_sys_fun(s, s_dot);
      s = s + phase_dot[0]*dt;
      s_dot = s_dot + phase_dot[1]*dt;
    }

    if (success)
    {
      PRINT_INFO_MSG("Finished!");
      // emit gui->showMsgSignal(ExecResultMsg(ExecResultMsg::INFO, "Finished!"));
    }

    printElapsTime(gmp_mpc->elaps_t_data, "========= Total elapsed time (ms) =========\n");
    printElapsTime(gmp_mpc->solve_elaps_t_data, "========= Solve elapsed time (ms) =========\n");
    printElapsTime(gmp_mpc->calcH_elaps_t_data, "========= Calc H elapsed time (ms) =========\n");
    std::cerr << "---------------------------\n";

    {
      Eigen::SpMat H = gmp_mpc->H_;
      Eigen::SpMat A = gmp_mpc->A_;

      int nnz = H.nonZeros();
      int n_elem = H.size();
      printf("H sparsity: %.1f %% ( %d / %d )\n", 100*nnz/(double)n_elem, nnz, n_elem);

      nnz = A.nonZeros();
      n_elem = A.size();
      printf("A sparsity: %.1f %% ( %d / %d )\n", 100*nnz/(double)n_elem, nnz, n_elem);
    }

    if (!elaps_t_data.empty())
    {
      double mean_elaps_t = arma::mean(elaps_t_data);
      double std_elaps_t = arma::stddev(elaps_t_data);
      double max_elaps_t = arma::max(elaps_t_data);
      double min_elaps_t = arma::min(elaps_t_data);

      // std::cerr << "======= Elapsed time (ms) ======\n";
      // std::cerr << "std_range: [" << std::max(0.,mean_elaps_t-std_elaps_t) << " -  " << mean_elaps_t + std_elaps_t <<"]\n";
      // std::cerr << "mean     : " << mean_elaps_t << " +/- " << std_elaps_t <<"\n";
      // std::cerr << "min      : " << min_elaps_t << "\n";
      // std::cerr << "max      : " << max_elaps_t << "\n";
      // std::cerr << "==============================\n";

      std::cerr << "======= Final state errors (m) ======\n";
      std::cerr << "pos_err = " << arma::norm(yg - y) << "\n";
      std::cerr << "vel_err = " << arma::norm(y_dot) << "\n";
      std::cerr << "accel_err = " << arma::norm(y_ddot) << "\n";
      std::cerr << "==============================\n";
    }
}

void loadParams(const std::string &filename)
{
  // ============== Load params =============
  try
  {
    YAML::Node node = YAML::LoadFile(filename);

    if ( !YAML::getParam(node, "y0", y0_) )
      throw std::runtime_error("Failed to load param 'y0'...");

    if ( !YAML::getParam(node, "yg", yg_) )
      throw std::runtime_error("Failed to load param 'yg'...");

    if ( !YAML::getParam(node, "Tf", Tf) )
      throw std::runtime_error("Failed to load param 'Tf'...");

    if ( !YAML::getParam(node, "pos_lim", pos_lim) )
      throw std::runtime_error("Failed to load param 'y_lim'...");

    if ( !YAML::getParam(node, "vel_lim", vel_lim) )
      throw std::runtime_error("Failed to load param 'vel_lim'...");

    if ( !YAML::getParam(node, "accel_lim", accel_lim) )
      throw std::runtime_error("Failed to load param 'accel_lim'...");

    if ( !YAML::getParam(node, "n_dof", n_dof) )
      throw std::runtime_error("Failed to load param 'n_dof'...");

    if ( !YAML::getParam(node, "N_horizon", N_horizon) )
      throw std::runtime_error("Failed to load param 'N_horizon'...");

    if ( !YAML::getParam(node, "pred_time_step", pred_time_step) )
      throw std::runtime_error("Failed to load param 'pred_time_step'...");

    if ( !YAML::getParam(node, "N_kernels", N_kernels) )
      throw std::runtime_error("Failed to load param 'N_kernels'...");

    if ( !YAML::getParam(node, "kernels_std_scaling", kernels_std_scaling) )
      throw std::runtime_error("Failed to load param 'kernels_std_scaling'...");

    if ( !YAML::getParam(node, "trunc_kern_thres", trunc_kern_thres) )
      throw std::runtime_error("Failed to load param 'trunc_kern_thres'...");

    if ( !YAML::getParam(node, "opt_pos", opt_pos) )
      throw std::runtime_error("Failed to load param 'opt_pos'...");

    if ( !YAML::getParam(node, "opt_vel", opt_vel) )
      throw std::runtime_error("Failed to load param 'opt_vel'...");

    if ( !YAML::getParam(node, "slack_gains", slack_gains) )
      throw std::runtime_error("Failed to load param 'slack_gains'...");

    if ( !YAML::getParam(node, "slack_limits", slack_limits) )
      throw std::runtime_error("Failed to load param 'slack_limits'...");

    if ( !YAML::getParam(node, "final_state_err_tol", final_state_err_tol) )
      final_state_err_tol = arma::vec().zeros(3,1);

    if ( !YAML::getParam(node, "time_limit", time_limit) )
      throw std::runtime_error("Failed to load param 'time_limit'...");

    if ( !YAML::getParam(node, "max_iter", max_iter) )
      throw std::runtime_error("Failed to load param 'max_iter'...");

    if ( !YAML::getParam(node, "abs_tol", abs_tol) )
      throw std::runtime_error("Failed to load param 'abs_tol'...");

    if ( !YAML::getParam(node, "rel_tol", rel_tol) )
      throw std::runtime_error("Failed to load param 'rel_tol'...");

  }
  catch (std::exception &e)
  {
    PRINT_ERROR_MSG(e.what());
    exit(-1);
  }
}

void printElapsTime(const arma::rowvec &elaps_t_data, const std::string &title)
{
  double mean_elaps_t = arma::mean(elaps_t_data);
  double std_elaps_t = arma::stddev(elaps_t_data);
  double max_elaps_t = arma::max(elaps_t_data);
  double min_elaps_t = arma::min(elaps_t_data);

  if (!title.empty()) std::cerr << title;
  std::cerr << "std_range: [" << std::max(0.,mean_elaps_t-std_elaps_t) << " -  " << mean_elaps_t + std_elaps_t <<"]\n";
  std::cerr << "mean     : " << mean_elaps_t << " +/- " << std_elaps_t <<"\n";
  std::cerr << "min      : " << min_elaps_t << "\n";
  std::cerr << "max      : " << max_elaps_t << "\n";
  std::cerr << "==============================\n";
}

void PRINT_INFO_MSG(const std::string &msg, std::ostream &out)
{
  out << "\033[1m" << "\033[34m" << "[INFO]: " << msg << "\033[0m";
}

void PRINT_CONFIRM_MSG(const std::string &msg, std::ostream &out)
{
  out << "\033[1m" << "\033[32m" << "[INFO]: " << msg << "\033[0m";
}

void PRINT_WARNING_MSG(const std::string &msg, std::ostream &out)
{
  out << "\033[1m" << "\033[33m" << "[WARNING]: " << msg << "\033[0m";
}

void PRINT_ERROR_MSG(const std::string &msg, std::ostream &out)
{
  out << "\033[1m" << "\033[31m" << "[ERROR]: " << msg << "\033[0m";
}


// Boost.Geometry

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014-2017.
// Modifications copyright (c) 2014-2017 Oracle and/or its affiliates.

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

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_SIDE_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_SIDE_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/formulas/spherical.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/geographic/parameters.hpp>
#include <boost/geometry/strategies/side.hpp>
//#include <boost/geometry/strategies/concepts/side_concept.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam FormulaPolicy Geodesic solution formula policy.
\tparam Spheroid Reference model of coordinate system.
\tparam CalculationType \tparam_calculation
 */
template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
{
public:
    geographic()
    {}

    explicit geographic(Spheroid const& model)
        : m_model(model)
    {}

    template <typename P1, typename P2, typename P>
    inline int apply(P1 const& p1, P2 const& p2, P const& p) const
    {
        typedef typename promote_floating_point
            <
                typename select_calculation_type_alt
                    <
                        CalculationType,
                        P1, P2, P
                    >::type
            >::type calc_t;

        typedef typename FormulaPolicy::template inverse
                    <calc_t, false, true, false, false, false> inverse_formula;

        calc_t a1p = azimuth<calc_t, inverse_formula>(p1, p, m_model);
        calc_t a12 = azimuth<calc_t, inverse_formula>(p1, p2, m_model);

        return formula::azimuth_side_value(a1p, a12);
    }

private:
    template <typename ResultType,
              typename InverseFormulaType,
              typename Point1,
              typename Point2,
              typename ModelT>
    static inline ResultType azimuth(Point1 const& point1, Point2 const& point2,
                                     ModelT const& model)
    {
        return InverseFormulaType::apply(get_as_radian<0>(point1),
                                         get_as_radian<1>(point1),
                                         get_as_radian<0>(point2),
                                         get_as_radian<1>(point2),
                                         model).azimuth;
    }

    Spheroid m_model;
};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_SIDE_HPP


#ifndef MP2_HPP
#define MP2_HPP

#include "Integrals.hpp"
#include "Molecule.hpp"
#include "RHF.hpp"
#include <armadillo>
#include <libint2.h>
#include <vector>

namespace willow {
namespace qcmol {

class MP2 : public RHF {

public:
  MP2(const std::vector<libint2::Atom> &atoms, const libint2::BasisSet &bs,
      const Integrals &ints, const bool l_print = true,
      const bool l_grad = false, const int qm_chg = 0,
      const std::vector<QAtom> &atoms_Q = std::vector<QAtom>());

  double mp2_energy() { return emp2; };
  double get_energy() { return (mp2_energy() + rhf_energy()); };
  arma::mat densityMatrixMP2() { return m_pmp2; };

protected:
  int nocc;
  int nvir;
  int ncore;
  int nmo;
  int nbf;

  int iocc1;
  int iocc2;
  int ivir1;
  int ivir2;

  int naocc;
  int navir;

  // Eigen Values and Vectors
  arma::vec eval;
  arma::mat Cmat;
  arma::mat mo_tiajb;
  arma::mat m_pmp2; // densityMatrixMP2

  // arma::vec   mo_ints (const double* ao_tei);
  arma::mat mo_half_ints(const double *ao_tei);
  arma::mat mo_iajb_ints(const arma::mat &mo_hf_int);
  arma::mat cphf(const arma::mat &l_ai, const arma::vec &ao_tei,
                 const bool l_print = true);

  double emp2;
  // double erhf;
};

} // namespace qcmol
} // namespace willow

#endif


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/bipartite.hpp>
#include <boost/graph/max_cardinality_matching.hpp>

#include "2dmapper.hpp"

using namespace boost;
using namespace v8;

typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::undirectedS> Graph;

typedef std::vector<int> PositionVector;
typedef std::vector<PositionVector> PositionGrid;
typedef std::vector<std::map<int, int>> Tracking;
typedef std::vector<graph_traits<adjacency_list<>>::vertex_descriptor> Match;

static void returnNewSwap(std::vector<Match> &matchings, int numRows, int numColumns, PositionGrid &finalPositions) {
    // 1. Assign column_after_step_1 for each element
    // Matching contains the num_columns matchings
    Graph g;
    Tracking track(numColumns);

    auto offset = numColumns;

    // Build bipartite graph. Nodes are the current columns numbered
    // (0, 1, ...) and the destination columns numbered with an offset of
    // numColumns (0 + offset, 1+offset, ...)

    for (auto j = 0; j < numColumns; ++j) {
        add_vertex(g);
    }
    for (auto k = offset; k < offset + numColumns; ++k) {
        add_vertex(g);
    }

    // Add an edge to the graph from (i, j+offset) for every element
    // currently in column i which should go to column j for the new
    // mapping
    for (auto row = 0; row < numRows; ++row) {
        for (auto column = 0; column < numColumns; ++column) {
            auto destination_column = finalPositions[row][column];
            auto hasEdge = edge(column, destination_column + offset, g).second;
            if (!hasEdge) {
                add_edge(column, destination_column + offset, g);
                // Keep manual track of multiple edges between nodes
                track[column][destination_column + offset] = 1;
            } else {
                track[column][destination_column + offset] += 1;
            }
        }
    }

    // Find perfect matching, remove those edges from the graph
    // and do it again:
    for (auto i = 0; i < numRows; ++i) {
        Match mate(numColumns * 2);
        edmonds_maximum_cardinality_matching(g, &mate[0]);
        matchings[i] = mate;
        // Remove all edges of the current perfect matching
        for (auto node = 0; node < numColumns; ++node) {
            auto looper = mate[node];
            auto idx = looper;
            if (track[node][idx] == 1) {
                remove_edge(node, idx, g);
            } else {
                track[node][idx] -= 1;
            }
        }
    }
}

static void jsArrayToPositions(Local<Array> &array, PositionGrid &grid) {
    auto ctx = Nan::GetCurrentContext();
    for (uint32_t j = 0; j < array->Length(); ++j) {
        auto alooper = Local<Array>::Cast(array->Get(ctx, j).ToLocalChecked());
        PositionVector vec(alooper->Length());
        for (uint32_t k = 0; k < alooper->Length(); ++k) {
            vec[k] = alooper->Get(ctx, k).ToLocalChecked()->Int32Value(ctx).FromJust();
        }
        grid[j] = vec;
    }
}

static void matchingsToJS(Isolate *isolate, int size, std::vector<Match> &mathings, Local<Array> &array) {
    auto ctx = isolate->GetCurrentContext();
    for (size_t j = 0; j < mathings.size(); ++j) {
        auto mLooper = mathings[j];
        auto dict = Object::New(isolate);
        for (auto k = 0; k < size; ++k) {
            dict->Set(ctx, k, v8::Integer::New(isolate, mLooper[k]));
        }

        array->Set(ctx, j, dict);
    }
}

static void returnNewSwapWrapper(const Nan::FunctionCallbackInfo<v8::Value>& info) {
    if (info.Length() < 2) {
        Nan::ThrowTypeError("Wrong number of arguments");
        return;
    }

    auto isolate = info.GetIsolate();
    auto ctx = isolate->GetCurrentContext();
    auto row = info[0]->Int32Value(ctx).FromJust();
    auto column = info[1]->Int32Value(ctx).FromJust();
    auto array = Local<Array>::Cast(info[2]);
    PositionGrid grid(array->Length());

    jsArrayToPositions(array, grid);

    std::vector<Match> matchings(row);

    returnNewSwap(matchings, row, column, grid);

    auto result = Array::New(isolate, matchings.size());
    matchingsToJS(isolate, column * 2, matchings, result);
    info.GetReturnValue().Set(result);
}

void twodMapperInit(v8::Local<v8::Object> exports) {
    auto ctx = Nan::GetCurrentContext();
    exports->Set(ctx, Nan::New("returnNewSwap").ToLocalChecked(),
                 Nan::New<v8::FunctionTemplate>(returnNewSwapWrapper)->GetFunction(ctx).ToLocalChecked());
}


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

#include <dpose_core/dpose_core.hpp>
#include <dpose_core/dpose_costmap.hpp>
#include <dpose_goal_tolerance/DposeGoalToleranceConfig.h>

#include <gpp_interface/pre_planning_interface.hpp>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/layered_costmap.h>
#include <dynamic_reconfigure/server.h>

#include <IpIpoptApplication.hpp>
#include <IpTNLP.hpp>

#include <Eigen/Dense>

#include <memory>
#include <string>
#include <vector>

namespace dpose_goal_tolerance {

using index = Ipopt::Index;
using number = Ipopt::Number;
using pose = Eigen::Matrix<number, 3, 1>;
using dpose_core::pose_gradient;

/**
 * @brief Additional cost penalizing the distance to the _goal.
 *
 * The class implements the following cost
 * f0 = w_lin * ((x - x_g)^2 + (y - y_g)^2)
 * f1 = w_rot * (theta - theta_g)^2
 * f = f0 + f1
 *
 * The index g (in x_g for example) denote the goal pose, passed though the init
 * function. The variables w_lin and w_rot represent weights for linear and
 * angular errors. These weights are the weights passed to the constructor
 * scaled by the squared distance between goal and start poses.
 *
 * Call first pose_regularization::get_cost and retrieve the corresponsing
 * Jacobian via pose_regularization::get_jacobian().
 */
struct pose_regularization {
  /// @param _weight_lin unscaled weight for the linear error
  /// @param _weight_rot unscaled weight for the angular error
  /// @throw std::invalid_argument if both weights are zero
  pose_regularization(number _weight_lin, number _weight_rot);

  /// @brief init method. calls this if start or goal change
  /// @param _goal the pose to which we want to be close
  /// @param _start the pose which is used to normalize the weights
  void
  init(const pose &_start, const pose &_goal) noexcept;

  /// @brief computes the cost and the Jacobian with respect to the _pose
  /// @param _pose current pose
  number
  get_cost(const pose &_pose) noexcept;

  /// @brief returns the current Jacobian (from get_cost call)
  inline const pose_gradient::jacobian &
  get_jacobian() const noexcept {
    return J_;
  }

private:
  // cache
  pose_gradient::jacobian J_;
  pose goal_;
  pose norm_;
  pose diff_;

  // parameters
  number weight_lin_;  ///< unscaled weight for the linear error
  number weight_rot_;  ///< unscaled weight for the angular error
};

/**
 * @brief Problem definition for pose cost optimization under constraints.
 *
 * The class defined the following ipopt-problem:
 * f = min(cost(x, y, theta))
 * g0: x^2 + y^2 < tol_lin^2
 * g1: theta^2 < tol_rot^2
 *
 * It searches for the displacements x to given initial pose p, such that the
 * cost of the displaced pose (p + x) is minimized. The displacement x is
 * constrained translationally (g0) and rotationally (g1).
 */
struct problem : public Ipopt::TNLP {
private:
  using cell_vector = dpose_core::cell_vector;
  using parameter = pose_gradient::parameter;

  // input vars
  number lin_tol_sq_ = 1;  ///< squared linear tolerance in cells
  number rot_tol_sq_ = 1;  ///< squared angular tolerance in rads
  pose pose_;              ///< initial pose in cells
  pose offset_;

  // cache
  number cost_;                ///< cost of the current solution
  pose_gradient::jacobian J_;  ///< jacobian of the current solution

  // computation
  pose_gradient pg_;                               ///< the pose-gradient object
  costmap_2d::LayeredCostmap *costmap_ = nullptr;  ///< pointer to a costmap
  cell_vector lethal_cells_;                       ///< vector of lethal cells

  std::vector<pose_regularization> regs_;

  void
  on_new_x(index _n, const number *_x);

public:
  problem() = default;
  problem(costmap_2d::Costmap2DROS &_map, const parameter &_param);
  problem(costmap_2d::LayeredCostmap &_cmp, const parameter &_param);

  /// @brief init function which stets the poses and parameters
  /// @param _start current pose of the robot
  /// @param _goal desired goal pose (subject to optimization)
  /// @param _param parameters
  void
  init(const pose &_start, const pose &_goal,
       const DposeGoalToleranceConfig &_param);

  /// @brief returns the final pose after the optimization
  inline const pose &
  get_pose() const noexcept {
    return pose_;
  }

  inline void
  set_offset(const pose &_offset) noexcept {
    offset_ = _offset;
  }

  // ipopt functions below

  bool
  get_nlp_info(index &_n, index &_m, index &_nonzero_jac_g,
               index &_nonzero_h_lag, IndexStyleEnum &_index_style) override;

  bool
  get_bounds_info(index _n, number *_x_lower, number *_x_upper, index _m,
                  number *_g_lower, number *_g_upper) override;
  bool
  get_starting_point(index _n, bool _init_x, number *_x, bool _init_z,
                     number *_z_L, number *_z_U, index _m, bool _init_lambda,
                     number *lambda) override;

  bool
  eval_f(index _n, const number *_x, bool _new_x, number &_f_value) override;

  bool
  eval_grad_f(index _n, const number *_x, bool _new_x,
              number *_grad_f) override;

  bool
  eval_g(index _n, const number *_x, bool _new_x, index _m,
         number *_g) override;

  bool
  eval_jac_g(index _n, const number *_x, bool _new_x, index _m,
             index _number_elem_jac, index *_i_row, index *_j_col,
             number *_jac_g) override;

  void
  finalize_solution(Ipopt::SolverReturn _status, index _n, const number *_x,
                    const number *_z_L, const number *_z_U, index _m,
                    const number *_g, const number *_lambda, number _obj_value,
                    const Ipopt::IpoptData *_ip_data,
                    Ipopt::IpoptCalculatedQuantities *_ip_cq) override;
};

/**
 * @brief Class implements a goal-tolerance-functionality for global-planners.
 *
 * Add this to the pre_planning group of your GppPlugin in order to use it.
 * The class will **only** change the _goal (_start will remain unchanged).
 * You can define a list of tolerance types on the parameter server.
 *
 * See the README.md for more details.
 */
struct DposeGoalTolerance : public gpp_interface::PrePlanningInterface {
  /// @brief will move the _goal pose out of collision
  /// @param _start current pose of the robot (won't be changed)
  /// @param _goal goal pose (subject to optimization)
  /// @return true if the goal pose could be moved successfully
  bool
  preProcess(Pose &_start, Pose &_goal) override;

  /// @brief called by gpp-plugin
  void
  initialize(const std::string &_name, Map *_map) override;

private:
  bool
  preProcessImpl(pose &_goal);

  // ipopt
  Ipopt::SmartPtr<Ipopt::TNLP> problem_;
  Ipopt::SmartPtr<Ipopt::IpoptApplication> solver_;

  // ros-comm
  Map *map_ = nullptr;
  ros::Publisher pose_pub_;

  // config
  dpose_core::polygon footprint_;
  size_t attempts_;

  // dynamic reconfigure
  using cfg_server = dynamic_reconfigure::Server<DposeGoalToleranceConfig>;
  using cfg_server_ptr = std::unique_ptr<cfg_server>;

  DposeGoalToleranceConfig param_;
  // shutdown the server first
  cfg_server_ptr cfg_server_;
};

}  // namespace dpose_goal_tolerance

#endif  // DPOSE_GOAL_TOLERANCE__DPOSE_GOAL_TOLERANCE__HPP


#ifndef STAN_MATH_PRIM_SCAL_PROB_NEG_BINOMIAL_2_LPMF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_NEG_BINOMIAL_2_LPMF_HPP

#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_less.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/binomial_coefficient_log.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_beta.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/prob/poisson_lpmf.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/random/negative_binomial_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
  namespace math {

    // NegBinomial(n|mu, phi)  [mu >= 0; phi > 0;  n >= 0]
    template <bool propto,
              typename T_n,
              typename T_location, typename T_precision>
    typename return_type<T_location, T_precision>::type
    neg_binomial_2_lpmf(const T_n& n,
                       const T_location& mu,
                       const T_precision& phi) {
      typedef typename stan::partials_return_type<T_n, T_location,
                                                  T_precision>::type
        T_partials_return;

      static const char* function("neg_binomial_2_lpmf");

      if (!(stan::length(n) && stan::length(mu) && stan::length(phi)))
        return 0.0;

      T_partials_return logp(0.0);
      check_nonnegative(function, "Failures variable", n);
      check_positive_finite(function, "Location parameter", mu);
      check_positive_finite(function, "Precision parameter", phi);
      check_consistent_sizes(function,
                             "Failures variable", n,
                             "Location parameter", mu,
                             "Precision parameter", phi);

      if (!include_summand<propto, T_location, T_precision>::value)
        return 0.0;

      using std::log;
      using std::log;

      scalar_seq_view<T_n> n_vec(n);
      scalar_seq_view<T_location> mu_vec(mu);
      scalar_seq_view<T_precision> phi_vec(phi);
      size_t size = max_size(n, mu, phi);

      operands_and_partials<T_location, T_precision>
        ops_partials(mu, phi);

      size_t len_ep = max_size(mu, phi);
      size_t len_np = max_size(n, phi);

      VectorBuilder<true, T_partials_return, T_location> mu__(length(mu));
      for (size_t i = 0, size = length(mu); i < size; ++i)
        mu__[i] = value_of(mu_vec[i]);

      VectorBuilder<true, T_partials_return, T_precision> phi__(length(phi));
      for (size_t i = 0, size = length(phi); i < size; ++i)
        phi__[i] = value_of(phi_vec[i]);

      VectorBuilder<true, T_partials_return, T_precision> log_phi(length(phi));
      for (size_t i = 0, size = length(phi); i < size; ++i)
        log_phi[i] = log(phi__[i]);

      VectorBuilder<true, T_partials_return, T_location, T_precision>
        log_mu_plus_phi(len_ep);
      for (size_t i = 0; i < len_ep; ++i)
        log_mu_plus_phi[i] = log(mu__[i] + phi__[i]);

      VectorBuilder<true, T_partials_return, T_n, T_precision>
        n_plus_phi(len_np);
      for (size_t i = 0; i < len_np; ++i)
        n_plus_phi[i] = n_vec[i] + phi__[i];

      for (size_t i = 0; i < size; i++) {
        if (include_summand<propto>::value)
          logp -= lgamma(n_vec[i] + 1.0);
        if (include_summand<propto, T_precision>::value)
          logp += multiply_log(phi__[i], phi__[i]) - lgamma(phi__[i]);
        if (include_summand<propto, T_location, T_precision>::value)
          logp -= (n_plus_phi[i])*log_mu_plus_phi[i];
        if (include_summand<propto, T_location>::value)
          logp += multiply_log(n_vec[i], mu__[i]);
        if (include_summand<propto, T_precision>::value)
          logp += lgamma(n_plus_phi[i]);

        // if phi is large we probably overflow, defer to Poisson:
        if (phi__[i] > 1e5) {
          logp = poisson_lpmf(n_vec[i], mu__[i]);
        }

        if (!is_constant_struct<T_location>::value)
          ops_partials.edge1_.partials_[i]
            += n_vec[i]/mu__[i]
            - (n_vec[i] + phi__[i])
            / (mu__[i] + phi__[i]);
        if (!is_constant_struct<T_precision>::value)
          ops_partials.edge2_.partials_[i]
            += 1.0 - n_plus_phi[i]/(mu__[i] + phi__[i])
            + log_phi[i] - log_mu_plus_phi[i] - digamma(phi__[i])
            + digamma(n_plus_phi[i]);
      }
      return ops_partials.build(logp);
    }

    template <typename T_n,
              typename T_location, typename T_precision>
    inline
    typename return_type<T_location, T_precision>::type
    neg_binomial_2_lpmf(const T_n& n,
                       const T_location& mu,
                       const T_precision& phi) {
      return neg_binomial_2_lpmf<false>(n, mu, phi);
    }

  }
}
#endif


#include "geometry/boundary.h"
#include "helpers/geometry_objects.h"
#include "helpers/math_helpers.h"
#include "normals/normals.h"

#include <array>
#include "math.h"
#include <iostream>
#include <stdexcept>
#include <Eigen/Dense>

namespace boundary {

namespace geometry {

Boundary::Boundary(boundary::inputs::GeometryInputBase* input){
  input_ = input;
  // Set global variables
  Q_ = input->QOrder();
  mins_.push_back(input->XMin());
  mins_.push_back(input->YMin());
  maxes_.push_back(input->XMax());
  maxes_.push_back(input->YMax());
  initial_cell_size_ = maxes_[0] - mins_[0];
  max_depth_ = input->MaxDepth();
  max_solver_depth_ = input->MaxSolverDepth();

  SetupMesh_(initial_cell_size_, mins_[1], maxes_[1], mins_[0], maxes_[0]);
  RecursiveCalculateMoments_(1, initial_cell_size_);
  PropagateUp_();
};

void Boundary::RecursiveCalculateMoments_(double key, double cell_size){
  // Check if child exists. If it does, check for their children...
  bool has_child = false;
  for (int it = 0; it < 4; it++){
    // Iterating through all four possible child cells
    // TODO: Fix for arbitrary dim
    double new_key = 100*key + (it&1) + (it > 1)*10;
    if (boundary_cells_.count(new_key)){
      has_child = true;
      RecursiveCalculateMoments_(new_key, cell_size/2);
    }
  }
  // If no children, calculate moments
  if (!has_child && boundary_cells_.count(key)) {
    CalculateMoments_(key, cell_size);
  }
}

void Boundary::PropagateUp_(){
  for (int depth = max_depth_; depth >= 0; depth--){
    int n = 2*depth;
    double child_cell_volume = std::pow(initial_cell_size_/std::pow(2, depth + 1), 2);
    // Iterate through all cells at this level
    for (int it=0; it < (1 << n); it++){
      double key = std::pow(10, n);
      for (int jt=0; jt < n; jt++){
        key += ((it>>jt)&1)*std::pow(10, jt);
      }
      // If a boundary cell, sum up all children
      if (boundary_cells_.count(key)){
        for (int it = 0; it < 4; it++){ // Loop through children
          double child_key = 100*key + (it&1) + (it > 1)*10;
          for (int q_mag = Q_; q_mag >= 0; q_mag--){ // Loop through q order
            for (int i = 0; i < q_mag + 1; i++){  // Volume moments
              // Check if child is on boundary
              if (boundary_cells_.count(child_key)){
                // Propagate volume moments
                if (i < q_mag){boundary_cells_[key].volume_moments[i][q_mag - 1 - i] += boundary_cells_[child_key].volume_moments[i][q_mag - 1 - i];}
                // Propagate boundary moments
                boundary_cells_[key].boundary_moments[i][q_mag - i] += boundary_cells_[child_key].boundary_moments[i][q_mag - i];
              }
              // If child cell is interior add whole volume
              else {
                if (cell_map_[child_key] == 1 && (i < q_mag)){
                  boundary_cells_[key].volume_moments[i][q_mag - 1 - i] += child_cell_volume;
                }
              }
            }
          }
        }
        // If child below solver max level, delete
        if (depth >= max_solver_depth_){
          for (int it = 0; it < 4; it++){ // Loop through children
            double child_key = 100*key + (it&1) + (it > 1)*10;
            // Delete child
            boundary_cells_.erase(child_key);
          }
        }
      }
    }
  }
};

void Boundary::SetupMesh_(double cell_size, double y_min, double y_max, double x_min,
                         double x_max){
  // Caculate depth
  int depth = int(std::log2((initial_cell_size_/cell_size)));
  // Iterate through all cells
  double y_curr = y_min;
  double y_next = y_min + cell_size;
  while (y_next <= y_max){
    double x_curr = x_min;
    double x_next = x_curr + cell_size;
    while (x_next <= x_max){
      // corners of cell
      std::array<std::array<double, 2>, 4> corners;
      corners[0] = {x_curr, y_curr}; // lower left
      corners[1] = {x_curr, y_next}; // upper left
      corners[2] = {x_next, y_next}; // upper right
      corners[3] = {x_next, y_curr}; // lower right

      // cell center
      std::vector<double> center {x_curr + cell_size/2, y_curr + cell_size/2};

      // calculate Morton key
      double key = helpers::MortonKey(center, depth, maxes_, mins_);

      std::vector<int> inside{input_->Inside(corners[0]),
                              input_->Inside(corners[1]),
                              input_->Inside(corners[2]),
                              input_->Inside(corners[3])};

      // Refine
      // TODO: come up with a real criterion, this one just refines all cells to max
      if (depth < max_depth_){
        SetupMesh_(cell_size/2, y_curr, center[1], x_curr, center[0]); // lower left
        SetupMesh_(cell_size/2, center[1], y_next, x_curr, center[0]); // upper left
        SetupMesh_(cell_size/2, center[1], y_next, center[0], x_next); // upper right
        SetupMesh_(cell_size/2, y_curr, center[1], center[0], x_next); // lower right
      }

      // Determine if cell is boundary cell
      bool is_boundary = false;
      if (depth == max_depth_){
        // Check if four corners are inside or outside boundary
        is_boundary = IsBoundaryCell(corners[0], corners[1],
                                     corners[2], corners[3], input_);
      }

      else {
        // Check if any of the child cells are boundary cells
        for (int it = 0; it < 4; it++){ // Loop through children
          double child_key = 100*key + (it&1) + (it > 1)*10;
          if (cell_map_[child_key] == 2){
            is_boundary = true;
          }
        }
      }

      if (is_boundary){
        // add to cell map
        cell_map_.insert(std::pair<double, int>(key, 2));
        // make struct with tag and id
        geo_info cell;
        cell.irregular = true;
        helpers::Point cell_center = helpers::Point(center);
        // resize vectors
        cell.normal_derivatives.resize(Q_+1);
        cell.volume_moments.resize(Q_+1);
        cell.boundary_moments.resize(Q_+1);
        for (int i=0; i < Q_+1; ++i){
          cell.normal_derivatives[i].resize(Q_+1);
          cell.volume_moments[i].resize(Q_+1);
          cell.boundary_moments[i].resize(Q_+1);
          for (int j = 0; j < Q_ + 1; ++j){
            cell.normal_derivatives[i][j].resize(2);
          }
        }
        // Compute derivatives of the normal and store in cell struct
        for (int q_mag=Q_; q_mag >= 0 ; q_mag--){
          for (int q1 = 0; q1 < q_mag + 1; q1++){
            std::vector<int> q = {q1, q_mag - q1};
            cell.normal_derivatives[q1][q_mag - q1][0] = normals::Normal::NormalDerivative(q,
                                                            1, cell_center, input_);
            cell.normal_derivatives[q1][q_mag - q1][1] = normals::Normal::NormalDerivative(q,
                                                            2, cell_center, input_);
          }
        }

        // Compute 1d volume fractions and store in cell
        for (int corner=0; corner < 4; corner++){
          // iterate four edges to determine which ones intersect boundary
          if ((inside[corner] == inside[(corner+1)%4]) || inside[corner] == 2){ // no change from corner to corner, TODO: come up with more robust way to do this
            cell.vol_frac_1d[corner] = cell_size*inside[(corner+1)%4];
          }
          else if (inside[(corner+1)%4] == 2){ // next corner is on the boundary
            cell.vol_frac_1d[corner] = cell_size*inside[corner];
          }
          else { // change from corner to corner, indicating boundary cuts through
            // check if edge is horizontal or vertical
            if ((corners[corner][0] - corners[(corner+1)%4][0]) == 0){ // vertical
              // find the intersection of x=corners[i, 0]
              std::vector<double> y_values = input_->BoundaryFunction(corners[corner][0]);
              double y = 0;
              if (y_values.size() == 1){
                y = input_->BoundaryFunction(corners[corner][0])[0];
              }
              else if (y_values.size() == 2){
                // choose the value that is in the cell
                // Robustness TODO: fix this in the case that both values are in cell
                y = WhichValue(y_values, corners[corner][1], corners[(corner+1)%4][1]);
              }
              double edge_inside = abs(y - corners[corner][1]);
              if (inside[corner]){
                cell.vol_frac_1d[corner] = edge_inside;
              }
              else{
                cell.vol_frac_1d[corner] = cell_size - edge_inside;
              }
            }
            else { // horizontal
              // find intersection of y=corners[i, 1]
              std::vector<double> x_values = input_->BoundaryInverse(corners[corner][1]);
              double x = 0;
              if (x_values.size() == 1){
                x = input_->BoundaryFunction(corners[corner][0])[0];
              }
              else if (x_values.size() == 2){
                // choose the value that is in the cell
                // Robustness TODO: fix this in the case that both values are in cell
                x = WhichValue(x_values, corners[corner][0], corners[(corner+1)%4][0]);
              }
              double edge_inside = abs(x - corners[corner][0]);
              if (inside[corner]){
                cell.vol_frac_1d[corner] = edge_inside;
              }
              else {
                cell.vol_frac_1d[corner] = cell_size - edge_inside;
              }
            }
          }
        }
        cell.cell_center = center;
        cell.cell_size = cell_size;
        // add cell to map
        boundary_cells_.insert(std::pair<double, geo_info>(key, cell));
      }

      // Else add to cell map
      else {cell_map_.insert(std::pair<double, int>(key, *std::min_element(inside.begin(), inside.end())));}

      // Add id to cell center map
      id_to_center_.insert(std::pair<double, std::vector<double>>(key, center));

      // Go to next cell
      x_next += cell_size;
      x_curr += cell_size;
    }
    // Go to next row
    y_next += cell_size;
    y_curr += cell_size;
  }
}

double Boundary::WhichValue(std::vector<double> values, double first_bound, double second_bound){
  if (first_bound < second_bound){
    for (std::vector<double>::iterator it = values.begin(); it != values.end(); it++){
      if (*it >= first_bound && *it <= second_bound){
        return *it;
      }
    }
  }

  else if (first_bound > second_bound){
    for (std::vector<double>::iterator it = values.begin(); it != values.end(); it++){
      if (*it <= first_bound && *it >= second_bound){
        return *it;
      }
    }
  }

  else {
    throw std::invalid_argument("No change between bounds.");
  }
  return 1;
};

bool Boundary::IsBoundaryCell(std::array<double, 2> lower_left,
                              std::array<double, 2> upper_left,
                              std::array<double, 2> upper_right,
                              std::array<double, 2> lower_right,
                              boundary::inputs::GeometryInputBase* input){
  // put into array with inside values
  std::vector<int> inside{input->Inside(lower_left),
                          input->Inside(upper_left),
                          input->Inside(upper_right),
                          input->Inside(lower_right)};
  // try changing all 2s to 0s and see if they are all the same
  int it_prev = inside[0];
      if (it_prev == 2){
      it_prev = 0;
    }
  bool same;
  for (int it = 1; it < 4; it++){
    int current = inside[it];
    if (current == 2){
      current = 0;
    }
    if (it_prev == current){
      if (it == 3){
        // they're all the same, not a boundary cell
        same = true;
      }
      continue;
    }
    else{
      same = false;
      break;
    }
  }

  // if they're not try changing all the 2s to 1s and see if they're all the same
  if (!same){
    it_prev = inside[0];
    if (it_prev == 2){
      it_prev = 1;
    }
    for (int it = 1; it < 4; it++){
      int current = inside[it];
      if (current == 2){
        current = 1;
      }
      if (it_prev == current){
        if (it == 3){
          same = true;
        }
        continue;
      }
      else{
        same = false;
        break;
      }
    }
  }

  return !same;
};

std::map<double, geo_info> Boundary::BoundaryCells(){
  return boundary_cells_;
};

double Boundary::DIntegral_(double beginning, double end, std::array<int, 2> q,
                            int index, double fixed_value, std::vector<double> cell_center){
  double next_plus = q[(index + 1) % 2] + 1;
  return std::pow(fixed_value - cell_center[index], q[index]) // fixed integral
        *(1/(next_plus))*(std::pow(end - cell_center[(index+1)%2], next_plus)
                        - std::pow(beginning - cell_center[(index+1)%2], next_plus));
};

double Boundary::CalcD_(double bd_length,
              double fixed_value,
              std::vector<double> cell_center,
              std::array<int, 2> q,
              int d,
              std::array<int, 2> which_d,
              double cell_size){
  int d_op = (d + 1) % 2;
  double d_pm;
  if (bd_length == cell_size){
    d_pm = DIntegral_(cell_center[d_op] - cell_size/2,
                            cell_center[d_op] + cell_size/2,
                            q, d, fixed_value, cell_center);
  }
  else if (bd_length > 0){
    // TODO: fix this to be more general
    std::array<double, 2> corner = {cell_center[0] + cell_size/2*which_d[0],
                                    cell_center[1] + cell_size/2*which_d[1]};
    std::vector<double> int_values;
    if (d == 0){
      int_values = input_->BoundaryFunction(fixed_value);
    }
    else if (d == 1){
      int_values = input_->BoundaryInverse(fixed_value);
    }
    double intersection;
    if (int_values.size() == 1){
      intersection = int_values[0];
    }
    else if (int_values.size() == 2){
      // choose the value that is in the cell
      // Robustness TODO: fix this in the case that both values are in cell
      intersection = WhichValue(int_values, cell_center[d_op] - cell_size/2,
                                cell_center[d_op] + cell_size/2);
    }
    if (input_->Inside(corner)){
      d_pm = DIntegral_(intersection, corner[d_op], q, d, fixed_value, cell_center);
    }
    else {
      d_pm = DIntegral_(corner[d_op] - cell_size, intersection, q, d, fixed_value, cell_center);
    }
  }
  else {
    d_pm = 0;
  }

  return d_pm;
};

void Boundary::CalculateMoments_(double key, double cell_size){
  std::vector<double> cell_center = id_to_center_[key];
  for (int q_mag = Q_; q_mag >= 0; q_mag--){
    Eigen::VectorXf rho = Eigen::VectorXf::Zero(2*(q_mag + 1));
    Eigen::MatrixXf lhs = Eigen::MatrixXf::Zero(2*(q_mag + 1), 2*q_mag + 1);

    int it = -1;
    for (int q1 = 0; q1 < (q_mag + 1); q1++){
      std::array<int, 2> q = {q1, q_mag - q1};
      for (int d = 0; d < 2; d++){
        it += 1;
        std::array<int, 2> e = {0, 0};
        e[d] = 1;

        std::array<int, 2> plus_vec = {1, 1};
        std::array<int, 2> first_minus_vec = {-1, 1};
        std::array<int, 2> second_minus_vec = {1, -1};

        double ptv_bd_length;
        double d_plus;
        double ntve_bd_length;
        double d_minus;
        // set d_plus and d_minus
        if (d == 0){
          // calculate d_plus
          ptv_bd_length = boundary_cells_[key].vol_frac_1d[2];
          double fixed_ptve_x = cell_center[0] + cell_size/2;
          d_plus = CalcD_(ptv_bd_length, fixed_ptve_x, cell_center, q, d, plus_vec, cell_size);

          // calculate d_minus
          ntve_bd_length = boundary_cells_[key].vol_frac_1d[0];
          double fixed_ntve_x = cell_center[0] - cell_size/2;
          d_minus = CalcD_(ntve_bd_length, fixed_ntve_x, cell_center, q,
                           d, first_minus_vec, cell_size);
        }
        else if (d == 1){
          // calculate d_plus
          ptv_bd_length = boundary_cells_[key].vol_frac_1d[1];
          double fixed_ptve_y = cell_center[1] + cell_size/2;
          d_plus = CalcD_(ptv_bd_length, fixed_ptve_y, cell_center, q,
                                 d, plus_vec, cell_size);

          // calculate d_minus
          ntve_bd_length = boundary_cells_[key].vol_frac_1d[3];
          double fixed_ntve_y = cell_center[1] - cell_size/2;
          d_minus = CalcD_(ntve_bd_length, fixed_ntve_y, cell_center, q,
                           d, second_minus_vec, cell_size);
        }
        int s_sum = 0;
        int S = Q_ - q_mag;
        for (int s1 = 0; s1 < S+1; s1++){
          for (int s2 = 0; s2 < S+1; s2++){
            if (s1 + s2 >= S || s1 + s2 < 1){
              continue;
            }
            s_sum += (boundary_cells_[key].normal_derivatives[s1][s2][d]
                      *boundary_cells_[key].boundary_moments[q[0] + s1][q[1] + s2]);
          }
        }
        std::array<int, 2> q_minus_e = {q[0] - e[0], q[1] - e[1]};
        if (q_minus_e[0] >= 0 && q_minus_e[1] >= 0){
          lhs(it, q_minus_e[0]) = q[d];
        }
        // M_B Unknowns (stored based on value of q[0
        lhs(it, q_mag + q[0]) = -boundary_cells_[key].normal_derivatives[0][0][d];
        rho(it) = d_plus - d_minus + s_sum;
      }
    }

    // Solve
    Eigen::VectorXf v_and_b = lhs.colPivHouseholderQr().solve(rho);
    // Unpack V and B
    for (int i = 0; i < q_mag; i++){
      boundary_cells_[key].volume_moments[i][q_mag - 1 - i] = v_and_b(i);
    }
    for (int i = 0; i < q_mag + 1; i++){
      boundary_cells_[key].boundary_moments[i][q_mag - i] = v_and_b(q_mag + i);
    }
  }
};

int Boundary::IJToGlobal(int i_index, int j_index, int depth){
  int num_x = std::pow(2, depth);
  if(i_index < 0 || j_index < 0 || i_index >= num_x || j_index >= num_x){
    return -1;
  }
  return num_x*i_index + j_index;
};

std::array<int, 2> Boundary::NeighborCell(int i_index, int j_index, int edge){
  std::map<int, std::array<int, 2>> neighbor_map = {{0, {i_index, j_index - 1}},
                                                    {1, {i_index + 1, j_index}},
                                                    {2, {i_index, j_index + 1}},
                                                    {3, {i_index - 1, j_index}}};
  return neighbor_map[edge];
}

std::vector<double> Boundary::IJToCenter(int i_index, int j_index, int depth){
  // Remember i->y and j->x
  int num_x = std::pow(2, depth);
  double cell_size = (maxes_[0] - mins_[0])/num_x;
  std::vector<double> cell_center{mins_[0] + j_index*cell_size + cell_size/2,
                                  mins_[1] + i_index*cell_size + cell_size/2};
  return cell_center;
}

int Boundary::Sgn_(double v){
    // branchless sign function.
    // https://helloacm.com/how-to-implement-the-sgn-function-in-c/
    return (v > 0) - (v < 0);
}

std::array<int, 2> Boundary::ProjectedNormal_(int side_index, double nx, double ny){
    // (side_index & 1) iff care about y direction.
    // thus we can create a projected normal using comparison + bit mask.
    std::array<int, 2> normal = {Sgn_((    (side_index & 1)) * nx),
                                 Sgn_((1 ^ (side_index & 1)) * ny)};
    return normal;
}

int Boundary::Parity_(int side_index){
    // return parity of 2 bit number
    return (side_index ^ (side_index >> 1)) & 1;
}

std::array<std::array<int, 2>, 2> Boundary::InterpolationPair(int i, int j, double nx, double ny, int side_index){
  // Remember i->y and j->x
  // want to move along projected direction of negative normal.
  std::array<int, 2> steps = ProjectedNormal_(side_index, -nx, -ny);

  // take step along projected normal.
  int i2 = i + steps[1];
  int j2 = j + steps[0];

  int direction = Parity_(side_index) - (not Parity_(side_index));
  steps[0] = direction * ((side_index & 1) ^ 1);
  steps[1] = direction *  (side_index & 1);

  int i3 = i2 + steps[1];
  int j3 = j2 + steps[0];

  std::array<int, 2> first_pair = {i2, j2};
  std::array<int, 2> second_pair = {i3, j3};
  std::array<std::array<int, 2>, 2> pair = {first_pair, second_pair};
  return pair;
}

std::map<double, int> Boundary::CellMap(){
  return cell_map_;
};

double Boundary::InitialCellSize(){
  return initial_cell_size_;
};

double Boundary::XMin(){
  return mins_[0];
};

double Boundary::XMax(){
  return maxes_[0];
};

double Boundary::YMin(){
  return mins_[1];
};

double Boundary::YMax(){
  return maxes_[1];
};

std::vector<double> Boundary::Mins(){
  return mins_;
};

std::vector<double> Boundary::Maxes(){
  return maxes_;
};

int Boundary::MaxSolverDepth(){
  return max_solver_depth_;
}

} // namespace geometry

} // namespace boundary


//Link to Boost
#define BOOST_TEST_DYN_LINK

//VERY IMPORTANT - include this last
//#include <boost/test/included/unit_test.hpp>
#include <boost/test/unit_test.hpp>

#include "test.h"

// test suite
BOOST_FIXTURE_TEST_SUITE(BetheEquation_suite, SimpleTestFixture, * utf::label("BetheEquation"))
/*
BOOST_DATA_TEST_CASE(eval_test, bdata::random(4, 10) ^ bdata::xrange(20), L, index)
{
    int M = random64(L/2);
    if (M <= 1) M = 2;
    SolvedBetheEquation be(L, M);
    BetheRootCache cache(L, M);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        if (cache.isSingular(i)) continue;
        Vector root = cache.getRoot(i);
        Vector res = be.eval(root);
        for (int j = 0; j < res.size(); j++) {
            BOOST_TEST_INFO("M=" << M << ", root=" << root);
            BOOST_TEST(std::abs(res[j] - var_t(1.0, 0)) == 0, tt::tolerance(1e-5));
        }
    }
}
*/
/*
BOOST_DATA_TEST_CASE(diff_test, bdata::random(4, 10) ^ bdata::xrange(20), L, index)
{
    int M = random64(L/2);
    if (M <= 1) M = 2;
    SolvedBetheEquation be(L, M);
    BetheRootCache cache(L, M);
    elem_t r = 1.0e-4;
    elem_t phi = random(0, 2 * Pi);
    var_t epsilon(r * cos(phi), r * sin(phi));
    Vector varies(M);
    for (int i = 0; i < M; i++) {
        varies[i] = epsilon;
    }
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        Vector root = cache.getRoot(i);
        Vector oldRes = be.eval(root);
        root += varies;
        Vector newRes = be.eval(root);
        Vector expected = be.diff(root) * varies;
        BOOST_TEST_INFO("oldRes=" << oldRes << ", newRes=" << newRes << ", expected dif=" << expected);
        //std::cout << "oldRes=" << oldRes << ", newRes=" << newRes << ", expected dif=" << expected << std::endl;
        BOOST_TEST((newRes - oldRes -expected).norm() < 1e-3);
    }
}*/

/*
BOOST_DATA_TEST_CASE(impliciteDiff_test, bdata::random(4, 10) ^ bdata::xrange(5), L, index)
{
    int M = random64(L/2);
    if (M <= 1) M = 2;
    SolvedBetheEquation be(L, M);
    SimpleBetheHomotopy bh(0.9, &be);

    BetheRootCache cache(L, M);
    elem_t deltaBeta = 1.0e-3;
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        Vector root = cache.getRoot(i);
        Vector uDiff = bh.impliciteDiff(0, root);
        Vector res1 = be.eval(root);
        Vector res2 = be.eval(root + uDiff * deltaBeta);
        Vector diff(M);
        for (int j = 0; j < res1.size(); j++) {
            diff[j] = log(res2[j]/res1[j])/var_t(0, (2.0 * L) * deltaBeta) - 1.0;
        }
        BOOST_TEST_INFO("M=" << M << ", root=" << root <<", diff=" << uDiff * deltaBeta << ", res1=" << res1 << ", res2=" << res2);
        BOOST_TEST(diff.norm() < 1e-1);
    }
}*/

BOOST_AUTO_TEST_SUITE_END()


/*

	This file is part of PEST++.

	PEST++ is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	PEST++ is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with PEST++.  If not, see<http://www.gnu.org/licenses/>.
*/
#include <utility>
#include <cmath>
#include <Eigen/Dense>
#include <algorithm>
#include <vector>
#include "QSqrtMatrix.h"
#include "Transformable.h"
#include "PriorInformation.h"
#include "pest_data_structs.h"
#include "Regularization.h"

using namespace std;
using namespace Eigen;

QSqrtMatrix::QSqrtMatrix(const ObservationInfo *_obs_info_ptr, const PriorInformation *_prior_info_ptr)
: obs_info_ptr(_obs_info_ptr), prior_info_ptr(_prior_info_ptr)
{
}

Eigen::SparseMatrix<double> QSqrtMatrix::get_sparse_matrix(const vector<string> &obs_names, const DynamicRegularization &regul, bool get_sqaure) const
{

	Eigen::SparseMatrix<double> weights(obs_names.size(), obs_names.size());
	unordered_map<string, ObservationRec>::const_iterator found_obsinfo_iter;
	unordered_map<string, ObservationRec>::const_iterator non_found_obsinfo_iter = obs_info_ptr->observations.end();
	PriorInformation::const_iterator found_prior_info;
	PriorInformation::const_iterator not_found_prior_info = prior_info_ptr->end();
	vector<string>::const_iterator b;
	vector<string>::const_iterator e;
	int i = 0;
	double weight = 0;
	double tikhonov_weight = 1.0;
	const string *group = nullptr;
	// PEST convention is the the weights are 1/standard deviation but the regularization weight is the square
	// of this
	bool use_regul = regul.get_use_dynamic_reg();
	if (use_regul) tikhonov_weight = sqrt(regul.get_weight());
	std::vector<Eigen::Triplet<double> > triplet_list;
	for (i = 0, b = obs_names.begin(), e = obs_names.end(); b != e; ++b, ++i)
	{
		found_obsinfo_iter = obs_info_ptr->observations.find(*b);
		found_prior_info = prior_info_ptr->find(*b);
		// This section handles Observations
		if (found_obsinfo_iter != non_found_obsinfo_iter)
		{
			group = &((*found_obsinfo_iter).second.group);
			weight = (*found_obsinfo_iter).second.weight;
			bool is_reg_grp = ObservationGroupRec::is_regularization(*group);
			if (use_regul && is_reg_grp)
			{
				if (regul.get_adj_grp_weights() && is_reg_grp)
				{
					double grp_factor = regul.get_grp_weight_fact(*group);
					weight *= grp_factor;
				}
				weight *= tikhonov_weight;
			}
			if (get_sqaure) weight = weight * weight;
			triplet_list.push_back(Eigen::Triplet<double>(i, i, weight));
		}
		// This section handles Prior Information
		else if (found_prior_info != not_found_prior_info)
		{
			group = &((*found_prior_info).second.get_group());
			weight = (*found_prior_info).second.get_weight();
			bool is_reg_grp = (*found_prior_info).second.is_regularization();
			if (use_regul && is_reg_grp)
			{
				if (regul.get_adj_grp_weights())
				{
					double grp_factor = regul.get_grp_weight_fact(*group);
					weight *= grp_factor;
				}
				weight *= sqrt(regul.get_weight());
			}
			if (get_sqaure) weight = weight * weight;
			triplet_list.push_back(Eigen::Triplet<double>(i, i, weight));
		}
		else {
			assert(true);  //observation not in standard observations or prior information
		}
	}
	weights.resize(obs_names.size(), obs_names.size());
	weights.setZero();
	weights.setFromTriplets(triplet_list.begin(), triplet_list.end());
	return weights;
}

QSqrtMatrix::~QSqrtMatrix(void)
{
}