#include "gtest/gtest.h"
#include "solvers/qp_solver.hpp"
#include <Eigen/IterativeLinearSolvers>

using namespace qp_solver;

template <typename _Scalar=double>
class _SimpleQP : public QP<2, 3, _Scalar>
{
public:
    Eigen::Matrix<_Scalar, 2, 1> SOLUTION;
    _SimpleQP()
    {
        this->P << 4, 1,
                   1, 2;
        this->q << 1, 1;
        this->A << 1, 1,
                   1, 0,
                   0, 1;
        this->l << 1, 0, 0;
        this->u << 1, 0.7, 0.7;

        this->SOLUTION << 0.3, 0.7;
    }
};

using SimpleQP = _SimpleQP<double>;

TEST(QPSolverTest, testSimpleQP) {
    SimpleQP qp;
    QPSolver<SimpleQP> prob;

    prob.settings().max_iter = 1000;

    prob.setup(qp);
    prob.solve(qp);
    Eigen::Vector2d sol = prob.primal_solution();

    EXPECT_TRUE(sol.isApprox(qp.SOLUTION, 1e-2));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, SOLVED);
}

TEST(QPSolverTest, testSinglePrecisionFloat) {
    using SimpleQPf = _SimpleQP<float>;
    SimpleQPf qp;
    QPSolver<SimpleQPf> prob;

    prob.setup(qp);
    prob.solve(qp);
    Eigen::Vector2f sol = prob.primal_solution();

    EXPECT_TRUE(sol.isApprox(qp.SOLUTION, 1e-2));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, SOLVED);
}

TEST(QPSolverTest, testConstraintViolation) {
    SimpleQP qp;
    QPSolver<SimpleQP> prob;

    prob.settings().eps_rel = 1e-4f;
    prob.settings().eps_abs = 1e-4f;

    prob.setup(qp);
    prob.solve(qp);
    Eigen::Vector2d sol = prob.primal_solution();

    // check feasibility (with some epsilon margin)
    Eigen::Vector3d lower = qp.A*sol - qp.l;
    Eigen::Vector3d upper = qp.A*sol - qp.u;
    EXPECT_GE(lower.minCoeff(), -1e-3);
    EXPECT_LE(upper.maxCoeff(), 1e-3);
}

TEST(QPSolverTest, testAdaptiveRho) {
    SimpleQP qp;
    QPSolver<SimpleQP> prob;

    prob.settings().adaptive_rho = false;
    prob.settings().adaptive_rho_interval = 10;

    prob.setup(qp);
    prob.solve(qp);

    EXPECT_EQ(prob.info().status, SOLVED);
}

TEST(QPSolverTest, testAdaptiveRhoImprovesConvergence) {
    SimpleQP qp;
    QPSolver<SimpleQP> prob;

    prob.settings().warm_start = false;
    prob.settings().max_iter = 1000;
    prob.settings().rho = 0.1;

    // solve whithout adaptive rho
    prob.settings().adaptive_rho = false;
    prob.setup(qp);
    prob.solve(qp);
    int prev_iter = prob.info().iter;

    // solve with adaptive rho
    prob.settings().adaptive_rho = true;
    prob.settings().adaptive_rho_interval = 10;
    prob.solve(qp);

    auto info = prob.info();
    EXPECT_LT(info.iter, prob.settings().max_iter);
    EXPECT_LT(info.iter, prev_iter); // adaptive rho should improve :)
    EXPECT_EQ(info.status, SOLVED);
}

/* BUG: Eigen::ConjugateGradient fails with assert when using fixed-size matrix.
 *      Only works in Release mode. */
#ifdef EIGEN_NO_DEBUG
TEST(QPSolverTest, testConjugateGradientLinearSolver)
{
    SimpleQP qp;
    QPSolver<SimpleQP, Eigen::ConjugateGradient, Eigen::Lower | Eigen::Upper> prob;

    prob.setup(qp);
    prob.solve(qp);
    Eigen::Vector2d sol = prob.primal_solution();

    auto info = prob.info();
    EXPECT_TRUE(sol.isApprox(qp.SOLUTION, 1e-2));
    EXPECT_EQ(info.status, SOLVED);
    EXPECT_LT(info.iter, prob.settings().max_iter); // convergence test
}
#endif

TEST(QPSolverTest, TestConstraint) {
    using qp_t = QP<5, 5, double>;
    using solver_t = QPSolver<qp_t>;
    solver_t prob;

    qp_t qp;
    qp.P.setIdentity();
    qp.q.setConstant(-1);
    qp.A.setIdentity();

    int type_expect[5];
    qp.l(0) = -1e+17;
    qp.u(0) = 1e+17;
    type_expect[0] = solver_t::LOOSE_BOUNDS;
    qp.l(1) = -101;
    qp.u(1) = 1e+17;
    type_expect[1] = solver_t::INEQUALITY_CONSTRAINT;
    qp.l(2) = -1e+17;
    qp.u(2) = 123;
    type_expect[2] = solver_t::INEQUALITY_CONSTRAINT;
    qp.l(3) = -1;
    qp.u(3) = 1;
    type_expect[3] = solver_t::INEQUALITY_CONSTRAINT;
    qp.l(4) = 42;
    qp.u(4) = 42;
    type_expect[4] = solver_t::EQUALITY_CONSTRAINT;

    prob.setup(qp);

    for (int i = 0; i < qp.l.rows(); i++) {
        EXPECT_EQ(prob.constr_type[i], type_expect[i]);
    }
}


#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"


#ifndef QPWBC_H_INCLUDED
#define QPWBC_H_INCLUDED

#include "qrw/InvKin.hpp" // For pseudoinverse
#include "qrw/Params.hpp"
#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include <limits>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "osqp.h"
#include "other/st_to_cc.hpp"

class QPWBC {
 private:

  Params* params_;  // Object that stores parameters

  int cpt_ML = 0;
  int cpt_P = 0;

  // Set to True after the creation of the QP problem during the first call of the solver
  bool initialized = false;

  // Weight matrices of initial QP
  Eigen::Matrix<double, 6, 6> Q1 = Eigen::Matrix<double, 6, 6>::Identity();
  Eigen::Matrix<double, 12, 12> Q2 = Eigen::Matrix<double, 12, 12>::Identity();

  // Friction coefficient
  const double mu = 0.9;

  // Generatrix of the linearized friction cone
  Eigen::Matrix<double, 20, 12> G = Eigen::Matrix<double, 20, 12>::Zero();

  // Transformation matrices
  Eigen::Matrix<double, 6, 6> Y = Eigen::Matrix<double, 6, 6>::Zero();
  Eigen::Matrix<double, 6, 12> X = Eigen::Matrix<double, 6, 12>::Zero();
  Eigen::Matrix<double, 6, 6> Yinv = Eigen::Matrix<double, 6, 6>::Zero();
  Eigen::Matrix<double, 6, 12> A = Eigen::Matrix<double, 6, 12>::Zero();
  Eigen::Matrix<double, 6, 1> gamma = Eigen::Matrix<double, 6, 1>::Zero();
  Eigen::Matrix<double, 12, 12>  H = Eigen::Matrix<double, 12, 12>::Zero();
  Eigen::Matrix<double, 12, 1> g = Eigen::Matrix<double, 12, 1>::Zero();

  // Results
  // Eigen::Matrix<double, 12, 1> lambdas = Eigen::Matrix<double, 12, 1>::Zero();
  Eigen::MatrixXd f_res = Eigen::MatrixXd::Zero(12, 1);
  Eigen::MatrixXd ddq_res = Eigen::MatrixXd::Zero(12, 1);

  // Matrix ML
  const static int size_nz_ML = 20*12; //4 * (4 * 2 + 1);
  csc *ML;  // Compressed Sparse Column matrix

  // Matrix NK
  const static int size_nz_NK = 20;
  double v_NK_up[size_nz_NK] = {};   // matrix NK (upper bound)
  double v_NK_low[size_nz_NK] = {};  // matrix NK (lower bound)
  double v_warmxf[size_nz_NK] = {};  // matrix NK (lower bound)

  // Matrix P
  const static int size_nz_P = 6*13; // 6*13; // 12*13/2;
  csc *P;  // Compressed Sparse Column matrix

  // Matrix Q
  const static int size_nz_Q = 12;
  double Q[size_nz_Q] = {};  // Q is full of zeros

  // OSQP solver variables
  OSQPWorkspace *workspce = new OSQPWorkspace();
  OSQPData *data;
  OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));

 public:

  QPWBC(); // Constructor
  void initialize(Params& params);

  // Functions
  inline void add_to_ML(int i, int j, double v, int *r_ML, int *c_ML, double *v_ML); // function to fill the triplet r/c/v
  inline void add_to_P(int i, int j, double v, int *r_P, int *c_P, double *v_P); // function to fill the triplet r/c/v
  int create_matrices();
  int create_ML();
  int create_weight_matrices();
  void compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA);
  void update_PQ();
  int call_solver();
  int retrieve_result(const Eigen::MatrixXd &f_cmd);
  int run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA, const Eigen::MatrixXd &k_contact);

  // Getters
  Eigen::MatrixXd get_f_res();
  Eigen::MatrixXd get_ddq_res();
  Eigen::MatrixXd get_H();

  // Utils
  void my_print_csc_matrix(csc *M, const char *name);
  void save_csc_matrix(csc *M, std::string filename);
  void save_dns_matrix(double *M, int size, std::string filename);

};

#endif  // QPWBC_H_INCLUDED


#define BOOST_TEST_MODULE test_btime

#include <boost/test/unit_test.hpp>
#include "Btime/DayCountCalculator.h"

BOOST_AUTO_TEST_SUITE(btime_test_suite)

    BOOST_AUTO_TEST_CASE(actual_count) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const std::string from_date = "2001-12-25";
        const std::string to_date = "2002-1-1";
        DayCountCalculator *myCalc;
        Actual_360 actualCalc = Actual_360{};
        myCalc = &actualCalc;
        double calculated_values = myCalc->compute_daycount(from_date, to_date);
        double expected_values = 7.0;

        BOOST_TEST_MESSAGE(" - Calculated Value: " << calculated_values);
        BOOST_TEST_MESSAGE(" - Expected Value: " << expected_values);
        BOOST_TEST_MESSAGE(" - Diff " << calculated_values - expected_values);
        BOOST_TEST(expected_values == calculated_values, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(thirty_count) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const std::string from_date = "2001-12-25";
        const std::string to_date = "2002-1-1";
        DayCountCalculator *myCalc;
        Thirty_360 thirty360Calc = Thirty_360{};
        myCalc = &thirty360Calc;
        double calculated_values = myCalc->compute_daycount(from_date, to_date);
        double expected_values = 6.0;

        BOOST_TEST_MESSAGE(" - Calculated Value: " << calculated_values);
        BOOST_TEST_MESSAGE(" - Expected Value: " << expected_values);
        BOOST_TEST_MESSAGE(" - Diff " << calculated_values - expected_values);
        BOOST_TEST(expected_values == calculated_values, boost::test_tools::tolerance(1e-15));
    }

BOOST_AUTO_TEST_SUITE_END()


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

//
// Compare results of truncated left shift to gmp, see:
// https://svn.boost.org/trac/boost/ticket/12790
//

#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/timer.hpp>
#include "test.hpp"

#if !defined(TEST1) && !defined(TEST2) && !defined(TEST3)
#define TEST1
#define TEST2
#define TEST3
#endif

template <class T>
T generate_random(unsigned bits_wanted)
{
   static boost::random::mt19937               gen;
   typedef boost::random::mt19937::result_type random_type;

   T        max_val;
   unsigned digits;
   if (std::numeric_limits<T>::is_bounded && (bits_wanted == (unsigned)std::numeric_limits<T>::digits))
   {
      max_val = (std::numeric_limits<T>::max)();
      digits  = std::numeric_limits<T>::digits;
   }
   else
   {
      max_val = T(1) << bits_wanted;
      digits  = bits_wanted;
   }

   unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
   while ((random_type(1) << bits_per_r_val) > (gen.max)())
      --bits_per_r_val;

   unsigned terms_needed = digits / bits_per_r_val + 1;

   T val = 0;
   for (unsigned i = 0; i < terms_needed; ++i)
   {
      val *= (gen.max)();
      val += gen();
   }
   val %= max_val;
   return val;
}

template <class T>
void test_value(const T& val)
{
   boost::multiprecision::mpz_int z(val.str()), mask(1);
   mask <<= std::numeric_limits<T>::digits;
   --mask;

   for (unsigned i = 0; i <= std::numeric_limits<T>::digits + 2; ++i)
   {
      BOOST_CHECK_EQUAL((val << i).str(), boost::multiprecision::mpz_int(((z << i) & mask)).str());
   }
}

void test(const boost::mpl::int_<200>&) {}

template <int N>
void test(boost::mpl::int_<N> const&)
{
   test(boost::mpl::int_<N + 4>());

   typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<N, N, boost::multiprecision::unsigned_magnitude>, boost::multiprecision::et_off> mp_type;

   std::cout << "Running tests for precision: " << N << std::endl;

   mp_type mp(-1);
   test_value(mp);

   for (unsigned i = 0; i < 1000; ++i)
      test_value(generate_random<mp_type>(std::numeric_limits<mp_type>::digits));
}

int main()
{
   test(boost::mpl::int_<24>());
   return boost::report_errors();
}


#include <cassert>
#include <limits>
#include <vector>

#include "bateman.hpp"

#if defined(MULTIPRECISION_DIGITS10)
#include <boost/multiprecision/cpp_dec_float.hpp>
using Real_t = boost::multiprecision::number<
    boost::multiprecision::cpp_dec_float<MULTIPRECISION_DIGITS10> >;
#ifndef CHAIN_LENGTH
// cpp_dec_float is more precise than double even with ~16 digits.
#define CHAIN_LENGTH 200
#endif
Real_t abs_cb(Real_t arg){
    return boost::multiprecision::abs(arg);
}
Real_t exp_cb(Real_t arg){
    return boost::multiprecision::exp(arg);
}
Real_t log_cb(Real_t arg){
    return boost::multiprecision::log(arg);
}
Real_t pow_cb(Real_t arg, Real_t ex){
    return boost::multiprecision::pow(arg, ex);
}
#else
#include <cmath>
using Real_t = double;
#ifndef CHAIN_LENGTH
// IEEE754 seem to handle up to 130 decays
#define CHAIN_LENGTH 130
#endif
Real_t abs_cb(Real_t arg){
    return std::abs(arg);
}
Real_t exp_cb(Real_t arg){
    return std::exp(arg);
}
Real_t log_cb(Real_t arg){
    return std::log(arg);
}
Real_t pow_cb(Real_t arg, Real_t ex){
    return std::pow(arg, ex);
}
#endif

int main(int argc, char *argv[]){
    std::vector<Real_t> y0 {{ 1.0, 1.0, 1.0 }};
    std::vector<Real_t> lmbd {{ 2.0, 3.0, 4.0 }};
    Real_t t = 1.0;
    auto y = bateman::bateman_full(y0, lmbd, t, exp_cb);

    std::vector<Real_t> yref
    {{ 0.1353352832366127, 0.22088349810536145, 0.2749602834949804 }};

    assert (abs_cb(y[0] - yref[0]) < 1e-16);
    assert (abs_cb(y[1] - yref[1]) < 1e-16);
    assert (abs_cb(y[2] - yref[2]) < 1e-16);

    auto p = bateman::bateman_parent(lmbd, t, exp_cb);
    std::vector<Real_t> pref
    {{ 0.1353352832366127, 0.1710964297374975, 0.16223035616885698 }};

    assert (abs_cb(p[0] - pref[0]) < 1e-16);
    assert (abs_cb(p[1] - pref[1]) < 1e-16);
    assert (abs_cb(p[2] - pref[2]) < 1e-16);

    const int N = CHAIN_LENGTH;
    const Real_t NN = static_cast<Real_t>(CHAIN_LENGTH);
    Real_t logN = log_cb(N);
    auto lmbd2 = std::vector<Real_t>(N);
    std::vector<Real_t> p2ref(N);
    for (int i=0; i<N; ++i){
        lmbd2[i] = (i+1)*logN;
        //p2ref[i] = pow_cb(N-1, i)/pow(N, i+1);
        p2ref[i] = pow_cb((NN-1)/NN, i)/NN;
    }
    auto p2 = bateman::bateman_parent(lmbd2, static_cast<Real_t>(1), exp_cb);
    Real_t atol = std::numeric_limits<Real_t>::epsilon()*10;
    for (int i=0; i<N; ++i)
        assert (abs_cb(p2[i] - p2ref[i]) < atol);
    return 0;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/monoid/monoid.hpp>

#include <boost/hana/core/datatype.hpp>
#include <boost/hana/detail/assert.hpp>
using namespace boost::hana;

struct integer {
    int value;
    constexpr explicit integer(int i) : value{i} { }
};

struct integer2 {
    int value;
    constexpr explicit integer2(int i) : value{i} { }
};

constexpr auto operator+(integer a, integer b)
{ return integer{a.value + b.value}; }

constexpr auto operator+(integer2 a, integer b)
{ return integer{a.value + b.value}; }

constexpr auto operator+(integer a, integer2 b)
{ return integer{a.value + b.value}; }

constexpr auto operator+(integer2 a, integer2 b)
{ return integer2{a.value + b.value}; }

using Integer = datatype_t<integer>;
using Integer2 = datatype_t<integer2>;

int main() {
    // same type
    BOOST_HANA_CONSTEXPR_ASSERT(zero<Integer>.value == 0);
    BOOST_HANA_CONSTEXPR_ASSERT(plus(integer{3}, integer{5}).value == 3 + 5);

    BOOST_HANA_CONSTEXPR_ASSERT(zero<Integer2>.value == 0);
    BOOST_HANA_CONSTEXPR_ASSERT(plus(integer2{3}, integer2{5}).value == 3 + 5);

    // mixed types
    BOOST_HANA_CONSTEXPR_ASSERT(plus(integer{3}, integer2{5}).value == 3 + 5);
    BOOST_HANA_CONSTEXPR_ASSERT(plus(integer2{3}, integer{5}).value == 3 + 5);
}


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2009
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/concepts/real_concept.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"
#include "table_type.hpp"

#ifndef SC_
#define SC_(x) static_cast<typename table_type<T>::type>(BOOST_JOIN(x, L))
#endif

template <class T>
T ibeta_forwarder(T a, T b, T x)
{
   T derivative;
   boost::math::detail::ibeta_imp(a, b, x, boost::math::policies::policy<>(), false, true, &derivative);
   return derivative;
}

template <class Real, class T>
void do_test_beta(const T& data, const char* type_name, const char* test_name)
{
   typedef Real                   value_type;

   typedef value_type (*pg)(value_type, value_type, value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::ibeta_derivative<value_type, value_type, value_type>;
#else
   pg funcp = boost::math::ibeta_derivative;
#endif

   boost::math::tools::test_result<value_type> result;

#if !(defined(ERROR_REPORTING_MODE) && !defined(BETA_INC_FUNCTION_TO_TEST))
   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test ibeta_derivative against data:
   //
   result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(funcp, 0, 1, 2),
      extract_result<Real>(3));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "beta (incomplete)", test_name);
#endif

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = ibeta_forwarder<value_type>;
#else
   funcp = ibeta_forwarder;
#endif

   if(boost::math::tools::digits<value_type>() > 40)
   {
      //
      // test ibeta_derivative against data:
      //
      result = boost::math::tools::test_hetero<Real>(
         data,
         bind_func<Real>(funcp, 0, 1, 2),
         extract_result<Real>(3));
      handle_test_result(result, data[result.worst()], result.worst(), type_name, "beta (incomplete, internal call test)", test_name);
   }
}

template <class T>
void test_beta(T, const char* name)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // The contents are as follows, each row of data contains
   // five items, input value a, input value b, integration limits x, beta(a, b, x) and ibeta(a, b, x):
   //
#if !defined(TEST_DATA) || (TEST_DATA == 1)
#  include "ibeta_derivative_small_data.ipp"

   do_test_beta<T>(ibeta_derivative_small_data, name, "Incomplete Beta Function Derivative: Small Values");
#endif

#if !defined(TEST_DATA) || (TEST_DATA == 2)
#  include "ibeta_derivative_data.ipp"

   do_test_beta<T>(ibeta_derivative_data, name, "Incomplete Beta Function Derivative: Medium Values");

#endif
#if !defined(TEST_DATA) || (TEST_DATA == 3)
#  include "ibeta_derivative_large_data.ipp"

   do_test_beta<T>(ibeta_derivative_large_data, name, "Incomplete Beta Function Derivative: Large and Diverse Values");
#endif

#if !defined(TEST_DATA) || (TEST_DATA == 4)
#  include "ibeta_derivative_int_data.ipp"

   do_test_beta<T>(ibeta_derivative_int_data, name, "Incomplete Beta Function Derivative: Small Integer Values");
#endif
}

template <class T>
void test_spots(T)
{
   using std::ldexp;
   T tolerance = boost::math::tools::epsilon<T>() * 40000;
      BOOST_CHECK_CLOSE(
         ::boost::math::ibeta_derivative(
            static_cast<T>(2),
            static_cast<T>(4),
            ldexp(static_cast<T>(1), -557)),
         static_cast<T>(4.23957586190238472641508753637420672781472122471791800210e-167L), tolerance * 4);
      BOOST_CHECK_CLOSE(
         ::boost::math::ibeta_derivative(
            static_cast<T>(2),
            static_cast<T>(4.5),
            ldexp(static_cast<T>(1), -557)),
         static_cast<T>(5.24647512910420109893867082626308082567071751558842352760e-167L), tolerance * 4);
}


//----------------------------------------------------------------------------
// Author:		Martin Klemsa
//----------------------------------------------------------------------------
#ifndef _ctoolhu_random_generator_included_
#define _ctoolhu_random_generator_included_

#include "engine.hpp"
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_smallint.hpp>

#ifdef _DEBUG_RAND
#include <ctoolhu/event/events.h>
#include <ctoolhu/event/firer.hpp>
#include <string>
#endif

namespace Ctoolhu::Random {

	//shortcut for the generator template we'll be using
	template <class Distribution>
	using RandomGenerator = boost::variate_generator<Private::RandomEngine &, Distribution>;

	//generator with run-time bounds
	template <
		class Distribution,
		typename Boundary = int
	>
	class Generator : public RandomGenerator<Distribution> {

		using base_t = RandomGenerator<Distribution>;

	  public:

		//for number generators
		Generator(Boundary lower, Boundary upper)
			: base_t(Private::SingleRandomEngine::Instance(), Distribution(lower, upper)) {}

		//for bool generator
		Generator()
			: base_t(Private::SingleRandomEngine::Instance(), Distribution()) {}

#ifdef _DEBUG_RAND
		auto operator()()
		{
			auto res = this->base_t::operator()();
			Event::Fire(Event::Message{"random: " + std::to_string(res)});
			return res;
		}
#endif
	};

	//generator with compile-time bounds
	template <
		int LowerBound,
		int UpperBound,
		class Distribution = std::uniform_int_distribution<>
	>
	class StaticGenerator : public RandomGenerator<Distribution> {

	  public:

		StaticGenerator()
			: RandomGenerator<Distribution>(Private::SingleRandomEngine::Instance(), Distribution(LowerBound, UpperBound)) {}
	};

	//expose typical usages of the dynamic generator
	using IntGenerator =		Generator<std::uniform_int_distribution<>>;
	using SmallIntGenerator =	Generator<boost::uniform_smallint<>>;
	using FloatGenerator =		Generator<std::uniform_real_distribution<float>, float>;
	using BoolGenerator =		Generator<std::bernoulli_distribution>;

} //ns Ctoolhu::Random

#endif


#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 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

int main()
{
    using namespace boost;
    typedef adjacency_list< vecS, vecS, undirectedS,
        property< vertex_distance_t, int >, property< edge_weight_t, int > >
        Graph;
    typedef std::pair< int, int > E;
    const int num_nodes = 5;
    E edges[]
        = { E(0, 2), E(1, 3), E(1, 4), E(2, 1), E(2, 3), E(3, 4), E(4, 0) };
    int weights[] = { 1, 1, 2, 7, 3, 1, 1 };
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
    Graph g(num_nodes);
    property_map< Graph, edge_weight_t >::type weightmap = get(edge_weight, g);
    for (std::size_t j = 0; j < sizeof(edges) / sizeof(E); ++j)
    {
        graph_traits< Graph >::edge_descriptor e;
        bool inserted;
        boost::tie(e, inserted) = add_edge(edges[j].first, edges[j].second, g);
        weightmap[e] = weights[j];
    }
#else
    Graph g(edges, edges + sizeof(edges) / sizeof(E), weights, num_nodes);
#endif
    std::vector< graph_traits< Graph >::vertex_descriptor > p(num_vertices(g));

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
    property_map< Graph, vertex_distance_t >::type distance
        = get(vertex_distance, g);
    property_map< Graph, vertex_index_t >::type indexmap = get(vertex_index, g);
    prim_minimum_spanning_tree(g, *vertices(g).first, &p[0], distance,
        weightmap, indexmap, default_dijkstra_visitor());
#else
    prim_minimum_spanning_tree(g, &p[0]);
#endif

    for (std::size_t i = 0; i != p.size(); ++i)
        if (p[i] != i)
            std::cout << "parent[" << i << "] = " << p[i] << std::endl;
        else
            std::cout << "parent[" << i << "] = no parent" << std::endl;

    return EXIT_SUCCESS;
}


/*    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


#include <iostream>
#include <random>
#include <limits>
#include <vector>

// TODO To be deleted
#include <xtensor/xarray.hpp>
#include <xtensor/xio.hpp>
#include <xtensor/xrandom.hpp>
// TODO To be deleted
#include <Eigen/Dense>
#include "arrays/array.h"

#include <cxxopts.hpp>

using namespace std;

enum Test {
	ONE = 1,
	NONE = -1
};

void xtensor_example() {
	// Example of init
	cout << endl << endl;
	xt::xarray<int> arr {1, 2, 3, 4, 5, 6, 7, 8, 9};
	cout << arr.shape()[0] << endl;
	auto r = argwhere(arr < 5);
	cout << r.data() << endl;
	arr.reshape({3, 3});
	cout << arr.shape().size() << endl;
	cout << arr << endl;

	// Example of operations
	cout << endl << endl;
	cout << arr + arr << endl;
	cout << arr * arr << endl;
	cout << arr / arr << endl;

	// Example for using randome engine
	cout << endl << endl;
	auto rnd1 = xt::random::get_default_random_engine();
	auto rnd2 = xt::random::get_default_random_engine();
	rnd1.seed(123);
	rnd2.seed(123);
	cout << xt::random::rand<double>({3, 3}, -10, 10, rnd1) << endl;
	cout << xt::random::rand<double>({3, 3}, -10, 10, rnd2) << endl;
	cout << xt::random::rand<double>({4, 4, 4, 4}) << endl;

	// random generator
	cout << endl << endl;
	auto rand1 = minstd_rand0(123);
	auto rand2 = minstd_rand0(123);
	cout << endl;
	cout << xt::random::rand<double>({2, 2}, -10, 10, rand1) << endl;
	cout << xt::random::rand<double>({2, 2}, -10, 10, rand2) << endl;

	// Brodcasting
	cout << endl << endl;
	auto rnd = minstd_rand0(123);
	auto A = xt::random::rand<double>({3, 3}, -10, 10, rnd);
	cout << "A = " << endl << A << endl;
	xt::xarray<double> b = xt::random::rand<double>({3}, -10, 10, rnd);
	cout << "b = " << endl << b << endl;
	cout << "A + b  = " << endl << A + b << endl;
	b.reshape({3, 1});
	cout << "b' = " << endl << b << endl;
	cout << "A + b' = " << endl << A + b << endl;
}

void eigen_example() {
	// Eigen test
	cout << endl << endl;
	Eigen::MatrixXd m = Eigen::MatrixXd::Random(3,3);
	m = (m + Eigen::MatrixXd::Constant(3,3,1.2)) * 50;
	cout << "m =" << endl << m << endl << endl;
	auto v = Eigen::VectorXd(3);
	v << 1, 2, 3;
	cout << "v =" << endl << v << endl << endl;
	cout << "m * v =" << endl << m * v << endl << endl;
	cout << "m .* v =" << endl << m.array().colwise() * v.array() << endl;
}

void cpp_test() {
	// infinity
	cout << endl << endl;
	auto infd = numeric_limits<double>::infinity();
	cout << infd << endl;
	auto infi = numeric_limits<int>::infinity();
	cout << infi << endl;
	cout << (infd < infi) << endl;
	cout << (infd == infi) << endl;
	cout << (infi == infi) << endl;
	cout << (infd == infd) << endl;
	cout << -1 * infd << endl;
	cout << -1 * infi << endl;

	// Enum test
	cout << endl << endl;
	cout << ONE << endl;
	cout << NONE << endl;

	// Random generator
	auto rand1 = minstd_rand0(123);
	auto rand2 = minstd_rand0(123);
	auto dist = uniform_int_distribution(0, 10);
	for (int i = 0; i < 10; ++i) cout << dist(rand1) << " " << dist(rand2) << " | ";
}

void test_array() {
	auto shape = vector<unsigned int>({3, 3, 3});
	auto a = Array(shape);
	auto b = Array({3, 3, 3});
}

int main(int argc, char* argv[]) {
//	eigen_example();
//	cpp_test();
//	xtensor_example();
	test_array();
	return 0;
}


// =======================================================================
// 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;
}


#include <Discregrid/All>
#include <Eigen/Dense>
#include <cxxopts/cxxopts.hpp>

#include "resource_path.hpp"

#include <string>
#include <iostream>
#include <array>

using namespace Eigen;

std::istream& operator>>(std::istream& is, std::array<unsigned int, 3>& data)
{
	is >> data[0] >> data[1] >> data[2];
	return is;
}

std::istream& operator>>(std::istream& is, AlignedBox3d& data)
{
	is	>> data.min()[0] >> data.min()[1] >> data.min()[2]
		>> data.max()[0] >> data.max()[1] >> data.max()[2];
	return is;
}

int main(int argc, char* argv[])
{
	cxxopts::Options options(argv[0], "Generates a signed distance field from a closed two-manifold triangle mesh.");
	options.positional_help("[input OBJ file]");

	options.add_options()
	("h,help", "Prints this help text")
	("r,resolution", "Grid resolution", cxxopts::value<std::array<unsigned int, 3>>()->default_value("10 10 10"))
	("d,domain", "Domain extents (bounding box), format: \"minX minY minZ maxX maxY maxZ\"", cxxopts::value<AlignedBox3d>())
	("i,invert", "Invert SDF")
	("o,output", "Ouput file in cdf format", cxxopts::value<std::string>()->default_value(""))
	("input", "OBJ file containing input triangle mesh", cxxopts::value<std::vector<std::string>>())
	;

	try
	{
		options.parse_positional("input");
		options.parse(argc, argv);
	}
	catch (cxxopts::OptionException const& e)
	{
		std::cout << "error parsing options: " << e.what() << std::endl;
		exit(1);
	}
	if (options.count("help"))
	{
		std::cout << options.help() << std::endl;
		std::cout << std::endl << std::endl << "Example: GenerateSDF -r \"50 50 50\" dragon.obj" << std::endl;
		exit(0);
	}
	if (!options.count("input"))
	{
		std::cout << "ERROR: No input mesh given." << std::endl;
		std::cout << options.help() << std::endl;
		std::cout << std::endl << std::endl << "Example: GenerateSDF -r \"50 50 50\" dragon.obj" << std::endl;
		exit(1);
	}
	auto resolution = options["r"].as<std::array<unsigned int, 3>>();
	auto filename = options["input"].as<std::vector<std::string>>().front();

	if (!std::ifstream(filename).good())
	{
		std::cerr << "ERROR: Input file does not exist!" << std::endl;
		exit(1);
	}

	std::cout << "Load mesh...";
	Discregrid::TriangleMesh mesh(filename);
	std::cout << "DONE" << std::endl;

	std::cout << "Set up data structures...";
	Discregrid::MeshDistance md(mesh);
	std::cout << "DONE" << std::endl;

	auto domain = options["d"].as<Eigen::AlignedBox3d>();
	if (domain.isEmpty())
	{
		for (auto const& x : mesh.vertices())
		{
			domain.extend(x);
		}
		domain.max() += 1.0e-3 * domain.diagonal().norm() * Vector3d::Ones();
		domain.min() -= 1.0e-3 * domain.diagonal().norm() * Vector3d::Ones();
	}

	Discregrid::CubicLagrangeDiscreteGrid sdf(domain, resolution);
	auto func = Discregrid::DiscreteGrid::ContinuousFunction{};
	if (options.count("invert"))
	{
		func = [&md](Vector3d const& xi){return -1.0 * md.signedDistanceCached(xi);};
	}
	else
	{
		func = [&md](Vector3d const& xi){return md.signedDistanceCached(xi);};
	}

	std::cout << "Generate discretization..." << std::endl;
	sdf.addFunction(func, true);
	std::cout << "DONE" << std::endl;

	std::cout << "Serialize discretization...";
	auto output_file = options["o"].as<std::string>();
	if (output_file == "")
	{
		output_file = filename;
		if (output_file.find(".") != std::string::npos)
		{
			auto lastindex = output_file.find_last_of(".");
			output_file = output_file.substr(0, lastindex);
		}
		output_file += ".cdf";
	}
	sdf.save(output_file);
	std::cout << "DONE" << std::endl;

	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


//==================================================================================================
/*!
  @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_REM_PIO2_STRAIGHT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_REM_PIO2_STRAIGHT_HPP_INCLUDED

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/detail/constant/pio2_1.hpp>
#include <boost/simd/detail/constant/pio2_2.hpp>
#include <boost/simd/detail/constant/pio2_3.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_one_else_zero.hpp>
#include <boost/simd/function/genmask.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD(rem_pio2_straight_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_ < bd::floating_<A0>, X>
                          )
  {
    using result_t = std::pair<A0, A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 const& x) const BOOST_NOEXCEPT
    {
      auto test = x > Pio_4<A0>();
      A0 xr = x-Pio2_1<A0>();
      xr -= Pio2_2<A0>();
      xr -= Pio2_3<A0>();
      return {if_one_else_zero(test),if_else(test, xr, x)};
    }
  };

} } }

#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 <iostream>
#include <Eigen/Dense>
#include <vector>
#include <memory>
#include "../include/trainer.h"
#include "../include/model.h"
#include "../include/optimizer.h"
#include "../datasets/include/mnist.h"

int main()
{
    using namespace Eigen;
    using namespace MyDL;
    using std::cout;
    using std::endl;
    using std::make_shared;
    using std::shared_ptr;

    int batch_size = 100;
    int input_channels = 1;
    int input_height = 28;
    int input_width = 28;
    // int filter_num = 30;
    int filter_num = 10;
    int filter_size = 5;
    int pad = 0;
    int stride = 1;
    int hidden_size = 100;
    int output_size = 10;
    double weight_init_std = 0.01;

    double learning_rate = 0.001;
    int epochs = 10;

    auto model = make_shared<SimpleConvModel>(input_channels,
                                              input_height,
                                              input_width,
                                              filter_num,
                                              filter_size,
                                              pad,
                                              stride,
                                              hidden_size,
                                              output_size,
                                              weight_init_std);
    auto optimizer = make_shared<Adam>(learning_rate);
    auto dataset = make_shared<MnistEigenDataset>(batch_size);

    Trainer trainer(model, optimizer, dataset, epochs = epochs);

    trainer.train();

    return 0;
}