//#define CGAL_USE_BOOST_BIMAP
#define CGAL_MESH_2_OPTIMIZER_VERBOSE
#define CGAL_MESH_2_OPTIMIZERS_DEBUG

#include <fstream>
#include <vector>
#include <list>
#include <boost/config.hpp>
#include <boost/version.hpp>

// CGAL headers
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Delaunay_mesher_2.h>
#include <CGAL/Delaunay_mesh_face_base_2.h>
#include <CGAL/Delaunay_mesh_vertex_base_2.h>
#include <CGAL/Delaunay_mesh_size_criteria_2.h>
#include <CGAL/Mesh_2/Lipschitz_sizing_field_2.h>
#include <CGAL/Lipschitz_sizing_field_criteria_2.h>
#include <CGAL/Constrained_voronoi_diagram_2.h>
#include <CGAL/Triangulation_conformer_2.h>
#include <CGAL/lloyd_optimize_mesh_2.h>
#include <CGAL/IO/File_poly.h>
#include <CGAL/Random.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Timer.h>
#include <CGAL/IO/write_vtu.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif

// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>
#include <QDragEnterEvent>
#include <QDropEvent>
#include <QMessageBox>

// GraphicsView items and event filters (input classes)
#include "TriangulationCircumcircle.h"
#include "DelaunayMeshInsertSeeds.h"
#include <CGAL/Qt/GraphicsViewPolylineInput.h>
#include <CGAL/Qt/DelaunayMeshTriangulationGraphicsItem.h>
#include <CGAL/Qt/Converter.h>
// the two base classes
#include "ui_Constrained_Delaunay_triangulation_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

// for viewportsBbox(QGraphicsScene*)
#include <CGAL/Qt/utility.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Segment_2 Segment_2;
typedef K::Iso_rectangle_2 Iso_rectangle_2;
typedef CGAL::Delaunay_mesh_vertex_base_2<K>  Vertex_base;
typedef CGAL::Delaunay_mesh_face_base_2<K> Face_base;

typedef Face_base Fb;
typedef CGAL::Triangulation_data_structure_2<Vertex_base, Fb>  TDS;
typedef CGAL::Exact_predicates_tag              Itag;
typedef CGAL::Constrained_Delaunay_triangulation_2<K, TDS, Itag> CDT;
typedef CGAL::Constrained_voronoi_diagram_2<CDT> CVD;
typedef CGAL::Delaunay_mesh_size_criteria_2<CDT> Criteria;

typedef CGAL::Lipschitz_sizing_field_2<CDT> Lipschitz_sizing_field;
typedef CGAL::Lipschitz_sizing_field_criteria_2<CDT, Lipschitz_sizing_field> Lipschitz_criteria;
typedef CGAL::Delaunay_mesher_2<CDT, Lipschitz_criteria> Lipschitz_mesher;

typedef CDT::Vertex_handle Vertex_handle;
typedef CDT::Face_handle Face_handle;
typedef CDT::All_faces_iterator All_faces_iterator;

using namespace CGAL::parameters;

void
discoverInfiniteComponent(const CDT & ct)
{
  //when this function is called, all faces are set "in_domain"
  Face_handle start = ct.infinite_face();
  std::list<Face_handle> queue;
  queue.push_back(start);

  while(! queue.empty())
  {
    Face_handle fh = queue.front();
    queue.pop_front();
    fh->set_in_domain(false);

    for(int i = 0; i < 3; i++)
    {
      Face_handle fi = fh->neighbor(i);
      if(fi->is_in_domain()
         && !ct.is_constrained(CDT::Edge(fh,i)))
        queue.push_back(fi);
    }
  }
}

template<typename SeedList>
void
discoverComponents(const CDT & ct,
                   const SeedList& seeds)
{
  if (ct.dimension() != 2)
    return;

  // tag all faces inside
  for(typename CDT::All_faces_iterator fit = ct.all_faces_begin();
      fit != ct.all_faces_end();
      ++fit)
      fit->set_in_domain(true);

  // mark "outside" infinite component of the object
  discoverInfiniteComponent(ct);

  // mark "outside" components with a seed
  for(typename SeedList::const_iterator sit = seeds.begin();
      sit != seeds.end();
      ++sit)
  {
    typename CDT::Face_handle fh_loc = ct.locate(*sit);

    if(fh_loc == nullptr || !fh_loc->is_in_domain())
      continue;

    std::list<typename CDT::Face_handle> queue;
    queue.push_back(fh_loc);
    while(!queue.empty())
    {
      typename CDT::Face_handle f = queue.front();
      queue.pop_front();
      f->set_in_domain(false);

      for(int i = 0; i < 3; ++i)
      {
        typename CDT::Face_handle ni = f->neighbor(i);
        if(ni->is_in_domain()
          && !ct.is_constrained(typename CDT::Edge(f,i))) //same component
        {
          queue.push_back(ni);
        }
      }
    }
  }
}

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Constrained_Delaunay_triangulation_2
{
  Q_OBJECT

private:
  CDT cdt;
  QGraphicsScene scene;
  std::list<Point_2> m_seeds;

  CGAL::Qt::DelaunayMeshTriangulationGraphicsItem<CDT> * dgi;

  CGAL::Qt::GraphicsViewPolylineInput<K> * pi;
  CGAL::Qt::TriangulationCircumcircle<CDT> *tcc;
  CGAL::Qt::DelaunayMeshInsertSeeds<CDT> *dms;

public:
  MainWindow();

  void clear();

private:
  template <typename Iterator>
  void insert_polyline(Iterator b, Iterator e)
  {
    Point_2 p, q;
    typename CDT::Vertex_handle vh, wh;
    Iterator it = b;
    vh = cdt.insert(*it);
    p = *it;
    ++it;
    for(; it != e; ++it){
      q = *it;
      if(p != q){
        wh = cdt.insert(*it);
        cdt.insert_constraint(vh,wh);
        vh = wh;
        p = q;
      } else {
        std::cout << "duplicate point: " << p << std::endl;
      }
    }
    Q_EMIT( changed());
  }

public Q_SLOTS:
  void open(QString);

  void processInput(CGAL::Object o);

  void on_actionShowVertices_toggled(bool checked);

  void on_actionShowDelaunay_toggled(bool checked);

  void on_actionShowTriangulationInDomain_toggled(bool checked);

  void on_actionShow_constrained_edges_toggled(bool checked);

  void on_actionShow_voronoi_edges_toggled(bool checked);

  void on_actionShow_faces_in_domain_toggled(bool checked);

  void on_actionShow_blind_faces_toggled(bool checked);

  void on_actionShow_seeds_toggled(bool checked);

  void on_actionInsertPolyline_toggled(bool checked);

  void on_actionInsertSeeds_OnOff_toggled(bool checked);

  void on_actionCircumcenter_toggled(bool checked);

  void on_actionClear_triggered();

  void on_actionRecenter_triggered();

  void on_actionLoadConstraints_triggered();

  void loadWKT(QString);

  void loadFile(QString);

  void loadPolyConstraints(QString);

  void loadPolygonConstraints(QString);

  void loadEdgConstraints(QString);

  void on_actionSaveConstraints_triggered();

  void saveConstraints(QString);

  void on_actionMakeGabrielConform_triggered();

  void on_actionMakeDelaunayConform_triggered();

  void on_actionMakeDelaunayMesh_triggered();

  void on_actionMakeLipschitzDelaunayMesh_triggered();

  void on_actionInsertRandomPoints_triggered();

  void on_actionTagBlindFaces_triggered();

  void on_actionLloyd_optimization_triggered();

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow()
{
  setupUi(this);

  this->graphicsView->setAcceptDrops(false);

  // Add a GraphicItem for the CDT triangulation
  dgi = new CGAL::Qt::DelaunayMeshTriangulationGraphicsItem<CDT>(&cdt);
  QColor facesColor(::Qt::blue);
  facesColor.setAlpha(150);
  dgi->setFacesInDomainBrush(facesColor);

  QObject::connect(this, SIGNAL(changed()),
                   dgi, SLOT(modelChanged()));
  dgi->setVerticesPen(
    QPen(Qt::red, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  dgi->setVoronoiPen(
    QPen(Qt::darkGreen, 0, Qt::DashLine, Qt::RoundCap, Qt::RoundJoin));
  dgi->setSeedsPen(
    QPen(Qt::darkBlue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));

  dgi->setZValue(-1);
  scene.addItem(dgi);

  // Setup input handlers. They get events before the scene gets them
  // and the input they generate is passed to the triangulation with
  // the signal/slot mechanism
  pi = new CGAL::Qt::GraphicsViewPolylineInput<K>(this, &scene, 0, true); // inputs polylines which are not closed
  QObject::connect(pi, SIGNAL(generate(CGAL::Object)),
                   this, SLOT(processInput(CGAL::Object)));

  tcc = new CGAL::Qt::TriangulationCircumcircle<CDT>(&scene, &cdt, this);
  tcc->setPen(QPen(Qt::red, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));

  dms = new CGAL::Qt::DelaunayMeshInsertSeeds<CDT>(&scene, &cdt, this);//input seeds
  QObject::connect(dms, SIGNAL(generate(CGAL::Object)),
                   this, SLOT(processInput(CGAL::Object)));

  //
  // Manual handling of actions
  //
  QObject::connect(this->actionQuit, SIGNAL(triggered()),
                   this, SLOT(close()));

  // We put mutually exclusive actions in an QActionGroup
  QActionGroup* ag = new QActionGroup(this);
  ag->addAction(this->actionInsertPolyline);

  // Check two actions
  this->actionInsertPolyline->setChecked(true);
  this->actionShowDelaunay->setChecked(true);
  this->actionShowVertices->setChecked(true);
  this->actionShowTriangulationInDomain->setChecked(false);
  this->actionShow_faces_in_domain->setChecked(true);
  this->actionShow_constrained_edges->setChecked(true);
  this->actionShow_voronoi_edges->setChecked(false);
  this->actionShow_seeds->setChecked(false);
  this->actionInsertSeeds_OnOff->setChecked(false);

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);
  this->graphicsView->setMouseTracking(true);

  // Turn the vertical axis upside down
  this->graphicsView->scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Constrained_Delaunay_triangulation_2.html");
  this->addAboutCGAL();
  this->setupExportSVG(this->actionExport_SVG, this->graphicsView);

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
          this, SLOT(open(QString)));
}

void
MainWindow::processInput(CGAL::Object o)
{
  // Polygon
  std::list<Point_2> points;
  if(CGAL::assign(points, o))
  {
    if(points.size() == 1)
      cdt.insert(points.front());
    else
      insert_polyline(points.begin(), points.end());
    }
  else
  {
    // Seed (from Shift + left clic)
    Point_2 p;
    if(CGAL::assign(p, o))
    {
      m_seeds.push_back(p);
      if(actionShow_seeds->isChecked())
        dgi->setVisibleSeeds(true, m_seeds.begin(), m_seeds.end());
    }
  }

  discoverComponents(cdt, m_seeds);
  Q_EMIT( changed());
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */
void
MainWindow::on_actionInsertPolyline_toggled(bool checked)
{
  if(checked){
    scene.installEventFilter(pi);
  } else {
    scene.removeEventFilter(pi);
  }
}

void
MainWindow::on_actionInsertSeeds_OnOff_toggled(bool checked)
{
  if(checked){
    std::cout << "Insert seeds with Shift + Left click" << std::endl;
    scene.installEventFilter(dms);
  } else {
    scene.removeEventFilter(dms);
  }
}

void
MainWindow::on_actionShowDelaunay_toggled(bool checked)
{
  dgi->setVisibleEdges(checked);
  if(checked)
  {
    dgi->setVisibleInsideEdges(false);
    actionShowTriangulationInDomain->setChecked(false);
  }
  update();
}

void
MainWindow::on_actionShowVertices_toggled(bool checked)
{
  dgi->setVisibleVertices(checked);
  update();
}

void
MainWindow::on_actionShowTriangulationInDomain_toggled(bool checked)
{
  dgi->setVisibleInsideEdges(checked);
  if(checked)
  {
    dgi->setVisibleEdges(false);
    actionShowDelaunay->setChecked(false);
  }
  update();
}

void
MainWindow::on_actionShow_constrained_edges_toggled(bool checked)
{
  dgi->setVisibleConstraints(checked);
  update();
}

void
MainWindow::on_actionShow_voronoi_edges_toggled(bool checked)
{
  dgi->setVisibleVoronoiEdges(checked);
  update();
}

void
MainWindow::on_actionShow_faces_in_domain_toggled(bool checked)
{
  dgi->setVisibleFacesInDomain(checked);
  update();
}

void
MainWindow::on_actionShow_blind_faces_toggled(bool checked)
{
  dgi->setVisibleBlindFaces(checked);
  update();
}

void
MainWindow::on_actionShow_seeds_toggled(bool checked)
{
  dgi->setVisibleSeeds(checked, m_seeds.begin(), m_seeds.end());
  update();
}

void
MainWindow::on_actionCircumcenter_toggled(bool checked)
{
  if(checked){
    scene.installEventFilter(tcc);
    tcc->show();
  } else {
    scene.removeEventFilter(tcc);
    tcc->hide();
  }
}

void
MainWindow::on_actionClear_triggered()
{
  clear();
  Q_EMIT( changed());
}

void
MainWindow::clear()
{
  cdt.clear();
  m_seeds.clear();

  if(actionShow_seeds->isChecked())
    dgi->setVisibleSeeds(true, m_seeds.end(), m_seeds.end());
}

void
MainWindow::open(QString fileName)
{
  if(! fileName.isEmpty()){
    if(cdt.number_of_vertices() > 0)
    {
      QMessageBox msgBox(QMessageBox::Warning,
        "Open new polygon",
        "Do you really want to clear the current mesh?",
        (QMessageBox::Yes | QMessageBox::No),
        this);
      int ret = msgBox.exec();
      if(ret == QMessageBox::Yes)
        clear();
      else
        return;
    }
    if(fileName.endsWith(".polygons.cgal")){
      loadPolygonConstraints(fileName);
    } else if(fileName.endsWith(".cpts.cgal")){
      loadFile(fileName);
    } else if(fileName.endsWith(".edg")){
      loadEdgConstraints(fileName);
    } else if(fileName.endsWith(".poly")){
      loadPolyConstraints(fileName);
    } else if(fileName.endsWith(".wkt")){
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
      loadWKT(fileName);
#endif
    }
    this->addToRecentFiles(fileName);
  }
  Q_EMIT(changed());
  actionRecenter->trigger();
}

void
MainWindow::on_actionLoadConstraints_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
                                                  tr("Open Constraint File"),
                                                  ".",
                                                  tr("Edge files (*.edg);;"
                                                     "Polyline files (*.polygons.cgal);;"
                                                     "Poly files (*.poly);;"
                                                     "Plg files (*.plg);;"
                                                     "CGAL files (*.cpts.cgal);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files (*.WKT *.wkt);;"
                                                   #endif
                                                     "All (*)"));
  open(fileName);
}

void
MainWindow::loadWKT(QString
                    #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                    filename
                    #endif
                    )
{
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
  //Polygons todo : make it multipolygons
  std::ifstream ifs(qPrintable(filename));
  do
  {
    typedef CGAL::Polygon_with_holes_2<K> Polygon;
    typedef CGAL::Point_2<K> Point;
    std::vector<Polygon> mps;
    CGAL::read_multi_polygon_WKT(ifs, mps);
    for(const Polygon& p : mps)
    {
      if(p.outer_boundary().is_empty())
        continue;

      for(Point point : p.outer_boundary().container())
          cdt.insert(point);
      for(Polygon::General_polygon_2::Edge_const_iterator
          e_it=p.outer_boundary().edges_begin(); e_it != p.outer_boundary().edges_end(); ++e_it)
        cdt.insert_constraint(e_it->source(), e_it->target());

      for(Polygon::Hole_const_iterator h_it =
          p.holes_begin(); h_it != p.holes_end(); ++h_it)
      {
        for(Point point : h_it->container())
            cdt.insert(point);
        for(Polygon::General_polygon_2::Edge_const_iterator
            e_it=h_it->edges_begin(); e_it != h_it->edges_end(); ++e_it)
        {
          cdt.insert_constraint(e_it->source(), e_it->target());
        }
      }
    }
  }while(ifs.good() && !ifs.eof());
  //Edges
  ifs.clear();
  ifs.seekg(0, ifs.beg);
  do
  {
    typedef std::vector<K::Point_2> LineString;
    std::vector<LineString> mls;
    CGAL::read_multi_linestring_WKT(ifs, mls);
    for(const LineString& ls : mls)
    {
      if(ls.empty())
        continue;
      K::Point_2 p,q, qold(0,0); // initialize to avoid maybe-uninitialized warning from GCC6
      bool first = true;
      CDT::Vertex_handle vp, vq, vqold;
      LineString::const_iterator it =
          ls.begin();
      for(; it != ls.end(); ++it) {
        p = *it++;
        q = *it;
        if(p == q){
          continue;
        }
        if((!first) && (p == qold)){
          vp = vqold;
        } else {
          vp = cdt.insert(p);
        }
        vq = cdt.insert(q, vp->face());
        if(vp != vq) {
          cdt.insert_constraint(vp,vq);
        }
        qold = q;
        vqold = vq;
        first = false;
      }
    }
  }while(ifs.good() && !ifs.eof());

  //Points
  ifs.clear();
  ifs.seekg(0, ifs.beg);
  do
  {
    std::vector<K::Point_2> mpts;
    CGAL::read_multi_point_WKT(ifs, mpts);
    for(const K::Point_2& p : mpts)
    {
      cdt.insert(p);
    }
  }while(ifs.good() && !ifs.eof());

  discoverComponents(cdt, m_seeds);
  Q_EMIT( changed());
  actionRecenter->trigger();
#endif
}

void
MainWindow::loadFile(QString fileName)
{
  std::ifstream ifs(qPrintable(fileName));
  ifs >> cdt;
  if(!ifs) abort();
  discoverComponents(cdt, m_seeds);
  Q_EMIT( changed());
  actionRecenter->trigger();
}

void
MainWindow::loadPolyConstraints(QString fileName)
{
  std::ifstream ifs(qPrintable(fileName));
  read_triangle_poly_file(cdt,ifs);
  discoverComponents(cdt, m_seeds);
  Q_EMIT( changed());
  actionRecenter->trigger();
}

void
MainWindow::loadPolygonConstraints(QString fileName)
{
  K::Point_2 p,q, first;
  CDT::Vertex_handle vp, vq, vfirst;
  std::ifstream ifs(qPrintable(fileName));
  int n;
  // int counter = 0;
  while(ifs >> n){
    int poly_size = n;
    ifs >> first;
    p = first;
    vfirst = vp = cdt.insert(p);
    n--;
    while(n--){
      ifs >> q;
      vq = cdt.insert(q, vp->face());
      if(vp != vq) {
        cdt.insert_constraint(vp,vq);
        // std::cerr << "inserted constraint #" << counter++ << std::endl;
      }
      p = q;
      vp = vq;
    }
    if(poly_size != 2 && vp != vfirst) {
      cdt.insert_constraint(vp, vfirst);
    }
  }

  discoverComponents(cdt, m_seeds);
  Q_EMIT( changed());
  actionRecenter->trigger();
}

void
MainWindow::loadEdgConstraints(QString fileName)
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  CGAL::Timer tim;
  tim.start();
  std::ifstream ifs(qPrintable(fileName));
  bool first=true;
  int n;
  ifs >> n;

  K::Point_2 p,q, qold(0,0); // initialize to avoid maybe-uninitialized warning from GCC6

  CDT::Vertex_handle vp, vq, vqold;
  while(ifs >> p) {
    ifs >> q;
    if(p == q){
      continue;
    }
    if((!first) && (p == qold)){
      vp = vqold;
    } else {
      vp = cdt.insert(p);
    }
    vq = cdt.insert(q, vp->face());
    if(vp != vq) {
      cdt.insert_constraint(vp,vq);
    }
    qold = q;
    vqold = vq;
    first = false;
  }

  tim.stop();
  statusBar()->showMessage(QString("Insertion took %1 seconds").arg(tim.time()), 2000);
  discoverComponents(cdt, m_seeds);
  // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
  actionRecenter->trigger();
}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(dgi->boundingRect());
  this->graphicsView->fitInView(dgi->boundingRect(), Qt::KeepAspectRatio);
}

void
MainWindow::on_actionSaveConstraints_triggered()
{
  QString fileName = QFileDialog::getSaveFileName(this,

                                                  tr("Save Constraints"),
                                                  ".",
                                                  tr("CGAL files (*.cpts.cgal);;"
                                                     "VTU files (*.vtu);;"
                                                     "All (*)"));
  if(! fileName.isEmpty()){
      saveConstraints(fileName);
  }
}

void
MainWindow::saveConstraints(QString fileName)
{
  std::ofstream output(qPrintable(fileName));

  if(!fileName.endsWith("vtu") && output)
    output << cdt;
  else if (output)
  {
    CGAL::write_vtu(output, cdt);
  }
}

void
MainWindow::on_actionMakeGabrielConform_triggered()
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::size_t nv = cdt.number_of_vertices();
  CGAL::make_conforming_Gabriel_2(cdt);
  nv = cdt.number_of_vertices() - nv;
  discoverComponents(cdt, m_seeds);
  statusBar()->showMessage(QString("Added %1 vertices").arg(nv), 2000);
  // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
}

void
MainWindow::on_actionMakeDelaunayConform_triggered()
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::size_t nv = cdt.number_of_vertices();
  CGAL::make_conforming_Delaunay_2(cdt);
  discoverComponents(cdt, m_seeds);
  nv = cdt.number_of_vertices() - nv;
  statusBar()->showMessage(QString("Added %1 vertices").arg(nv), 2000);
   // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
}

void
MainWindow::on_actionMakeDelaunayMesh_triggered()
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  CGAL::Timer timer;
  timer.start();
  discoverComponents(cdt, m_seeds);
  timer.stop();
  QApplication::restoreOverrideCursor();

  bool ok;
  double shape = QInputDialog::getDouble(this, tr("Shape criterion"),
    tr("B = "), 0.125, 0.005, 100, 4, &ok);
  if(!ok) return;

  double edge_len = QInputDialog::getDouble(this, tr("Size criterion"),
    tr("S = "), 0., 0., (std::numeric_limits<double>::max)(), 5, &ok);
  if(!ok) return;

  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::size_t nv = cdt.number_of_vertices();
  timer.start();

  CGAL::refine_Delaunay_mesh_2(cdt,
      m_seeds.begin(), m_seeds.end(),
      Criteria(shape, edge_len),
      false);//mesh the subdomains including NO seed

  timer.stop();
  nv = cdt.number_of_vertices() - nv;
  statusBar()->showMessage(QString("Added %1 vertices in %2 seconds").arg(nv).arg(timer.time()), 2000);
  // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
}

void
MainWindow::on_actionMakeLipschitzDelaunayMesh_triggered()
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::set<Point_2> points;
  for(CDT::Finite_edges_iterator it = cdt.finite_edges_begin();
      it != cdt.finite_edges_end();
      ++it){
    if(cdt.is_constrained(*it)){
      Segment_2 s = cdt.segment(*it);
      points.insert(s.source());
      points.insert(s.target());
    }
  }

  discoverComponents(cdt, m_seeds);

  bool ok;
  double shape = QInputDialog::getDouble(this, tr("Shape criterion"),
    tr("B = "), 0.125, 0.005, 100, 4, &ok);
  if(!ok) return;
  double klip = QInputDialog::getDouble(this, tr("k-Lipschitz sizing field"),
    tr("k = "), 1., 0.01, 500, 5, &ok);
  if(!ok) return;

  Lipschitz_sizing_field field(points.begin(), points.end(), klip);
  Lipschitz_criteria criteria(shape, &field);
  Lipschitz_mesher mesher(cdt);
  mesher.set_criteria(criteria);

  std::size_t nv = cdt.number_of_vertices();
  mesher.init(true);
  mesher.set_seeds(m_seeds.begin(), m_seeds.end(),
                   false);//mesh the subdomains including NO seed

  mesher.refine_mesh();
  nv = cdt.number_of_vertices() - nv;
  statusBar()->showMessage(QString("Added %1 vertices").arg(nv), 2000);
  // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
}

void
MainWindow::on_actionInsertRandomPoints_triggered()
{
  QRectF rect = CGAL::Qt::viewportsBbox(&scene);
  CGAL::Qt::Converter<K> convert;
  Iso_rectangle_2 isor = convert(rect);
  CGAL::Random_points_in_iso_rectangle_2<Point_2> pg((isor.min)(), (isor.max)());
  bool ok = false;

  const int number_of_points =
      QInputDialog::getInt(this,
                           tr("Number of random points"),
                           tr("Enter number of random points"),
                           100,
                           0,
                           (std::numeric_limits<int>::max)(),
                           1,
                           &ok);

  if(!ok) {
    return;
  }

  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::vector<Point_2> points;
  points.reserve(number_of_points);
  for(int i = 0; i < number_of_points; ++i){
    points.push_back(*pg++);
  }
  cdt.insert(points.begin(), points.end());
  // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
}

void
MainWindow::on_actionTagBlindFaces_triggered()
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);

  CVD voronoi(cdt);
  voronoi.tag_faces_blind();

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

void
MainWindow::on_actionLloyd_optimization_triggered()
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);

  bool ok;
  int nb = QInputDialog::getInt(this, tr("QInputDialog::getInteger()"),
    tr("Number of iterations :"),
    1/*val*/, 0/*min*/, 1000/*max*/, 1/*step*/, &ok);
  if(!ok)
  {
    QApplication::restoreOverrideCursor();
    return;
  }

  CGAL::lloyd_optimize_mesh_2(cdt,
      max_iteration_number = nb,
      seeds_begin = m_seeds.begin(),
      seeds_end = m_seeds.end());

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

#include "Constrained_Delaunay_triangulation_2.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Constrained_Delaunay_triangulation_2 demo");

  // Import resources from libCGAL (Qt5).
  CGAL_QT_INIT_RESOURCES;

  MainWindow mainWindow;
  mainWindow.show();

  QStringList args = app.arguments();
  args.removeAt(0);
  Q_FOREACH(QString filename, args) {
    mainWindow.open(filename);
  }

  return app.exec();
}


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016, Guan-Horng Liu.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author:  Guan-Horng Liu
 *********************************************************************/

#include "reeds_shepp.h"
#include <boost/math/constants/constants.hpp>

namespace
{
// The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.

const double pi = boost::math::constants::pi<double>();
const double twopi = 2. * pi;
const double RS_EPS = 1e-6;
const double ZERO = 10 * std::numeric_limits<double>::epsilon();

inline double mod2pi(double x)
{
  double v = fmod(x, twopi);
  if (v < -pi)
    v += twopi;
  else if (v > pi)
    v -= twopi;
  return v;
}
inline void polar(double x, double y, double& r, double& theta)
{
  r = sqrt(x * x + y * y);
  theta = atan2(y, x);
}
inline void tauOmega(double u, double v, double xi, double eta, double phi, double& tau, double& omega)
{
  double delta = mod2pi(u - v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
  double t1 = atan2(eta * A - xi * B, xi * A + eta * B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
  tau = (t2 < 0) ? mod2pi(t1 + pi) : mod2pi(t1);
  omega = mod2pi(tau - u + v - phi);
}

// formula 8.1 in Reeds-Shepp paper
inline bool LpSpLp(double x, double y, double phi, double& t, double& u, double& v)
{
  polar(x - sin(phi), y - 1. + cos(phi), u, t);
  if (t >= -ZERO)
  {
    v = mod2pi(phi - t);
    if (v >= -ZERO)
    {
      assert(fabs(u * cos(t) + sin(phi) - x) < RS_EPS);
      assert(fabs(u * sin(t) - cos(phi) + 1 - y) < RS_EPS);
      assert(fabs(mod2pi(t + v - phi)) < RS_EPS);
      return true;
    }
  }
  return false;
}
// formula 8.2
inline bool LpSpRp(double x, double y, double phi, double& t, double& u, double& v)
{
  double t1, u1;
  polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
  u1 = u1 * u1;
  if (u1 >= 4.)
  {
    double theta;
    u = sqrt(u1 - 4.);
    theta = atan2(2., u);
    t = mod2pi(t1 + theta);
    v = mod2pi(t - phi);
    assert(fabs(2 * sin(t) + u * cos(t) - sin(phi) - x) < RS_EPS);
    assert(fabs(-2 * cos(t) + u * sin(t) + cos(phi) + 1 - y) < RS_EPS);
    assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
    return t >= -ZERO && v >= -ZERO;
  }
  return false;
}
void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
{
  double t, u, v, Lmin = path.length(), L;
  if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
    Lmin = L;
  }
  if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
    Lmin = L;
  }
  if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
    Lmin = L;
  }
  if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
    Lmin = L;
  }
  if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
    Lmin = L;
  }
  if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
    Lmin = L;
  }
  if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
    Lmin = L;
  }
  if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
}
// formula 8.3 / 8.4  *** TYPO IN PAPER ***
inline bool LpRmL(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
  polar(xi, eta, u1, theta);
  if (u1 <= 4.)
  {
    u = -2. * asin(.25 * u1);
    t = mod2pi(theta + .5 * u + pi);
    v = mod2pi(phi - t + u);
    assert(fabs(2 * (sin(t) - sin(t - u)) + sin(phi) - x) < RS_EPS);
    assert(fabs(2 * (-cos(t) + cos(t - u)) - cos(phi) + 1 - y) < RS_EPS);
    assert(fabs(mod2pi(t - u + v - phi)) < RS_EPS);
    return t >= -ZERO && u <= ZERO;
  }
  return false;
}
void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
{
  double t, u, v, Lmin = path.length(), L;
  if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
    Lmin = L;
  }
  if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
    Lmin = L;
  }
  if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
    Lmin = L;
  }
  if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
    Lmin = L;
  }

  // backwards
  double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
  if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
    Lmin = L;
  }
  if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
    Lmin = L;
  }
  if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
    Lmin = L;
  }
  if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
}
// formula 8.7
inline bool LpRupLumRm(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi * xi + eta * eta));
  if (rho <= 1.)
  {
    u = acos(rho);
    tauOmega(u, -u, xi, eta, phi, t, v);
    assert(fabs(2 * (sin(t) - sin(t - u) + sin(t - 2 * u)) - sin(phi) - x) < RS_EPS);
    assert(fabs(2 * (-cos(t) + cos(t - u) - cos(t - 2 * u)) + cos(phi) + 1 - y) < RS_EPS);
    assert(fabs(mod2pi(t - 2 * u - v - phi)) < RS_EPS);
    return t >= -ZERO && v <= ZERO;
  }
  return false;
}
// formula 8.8
inline bool LpRumLumRp(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi * xi - eta * eta) / 16.;
  if (rho >= 0 && rho <= 1)
  {
    u = -acos(rho);
    if (u >= -.5 * pi)
    {
      tauOmega(u, u, xi, eta, phi, t, v);
      assert(fabs(4 * sin(t) - 2 * sin(t - u) - sin(phi) - x) < RS_EPS);
      assert(fabs(-4 * cos(t) + 2 * cos(t - u) + cos(phi) + 1 - y) < RS_EPS);
      assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
      return t >= -ZERO && v >= -ZERO;
    }
  }
  return false;
}
void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
{
  double t, u, v, Lmin = path.length(), L;
  if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
    Lmin = L;
  }
  if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
    Lmin = L;
  }
  if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
    Lmin = L;
  }
  if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
    Lmin = L;
  }

  if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
    Lmin = L;
  }
  if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
    Lmin = L;
  }
  if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
    Lmin = L;
  }
  if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip + reflect
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
}
// formula 8.9
inline bool LpRmSmLm(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
  polar(xi, eta, rho, theta);
  if (rho >= 2.)
  {
    double r = sqrt(rho * rho - 4.);
    u = 2. - r;
    t = mod2pi(theta + atan2(r, -2.));
    v = mod2pi(phi - .5 * pi - t);
    assert(fabs(2 * (sin(t) - cos(t)) - u * sin(t) + sin(phi) - x) < RS_EPS);
    assert(fabs(-2 * (sin(t) + cos(t)) + u * cos(t) - cos(phi) + 1 - y) < RS_EPS);
    assert(fabs(mod2pi(t + pi / 2 + v - phi)) < RS_EPS);
    return t >= -ZERO && u <= ZERO && v <= ZERO;
  }
  return false;
}
// formula 8.10
inline bool LpRmSmRm(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
  polar(-eta, xi, rho, theta);
  if (rho >= 2.)
  {
    t = theta;
    u = 2. - rho;
    v = mod2pi(t + .5 * pi - phi);
    assert(fabs(2 * sin(t) - cos(t - v) - u * sin(t) - x) < RS_EPS);
    assert(fabs(-2 * cos(t) - sin(t - v) + u * cos(t) + 1 - y) < RS_EPS);
    assert(fabs(mod2pi(t + pi / 2 - v - phi)) < RS_EPS);
    return t >= -ZERO && u <= ZERO && v <= ZERO;
  }
  return false;
}
void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
{
  double t, u, v, Lmin = path.length() - .5 * pi, L;
  if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5 * pi, u, v);
    Lmin = L;
  }
  if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5 * pi, -u, -v);
    Lmin = L;
  }
  if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5 * pi, u, v);
    Lmin = L;
  }
  if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5 * pi, -u, -v);
    Lmin = L;
  }

  if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5 * pi, u, v);
    Lmin = L;
  }
  if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5 * pi, -u, -v);
    Lmin = L;
  }
  if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5 * pi, u, v);
    Lmin = L;
  }
  if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5 * pi, -u, -v);
    Lmin = L;
  }

  // backwards
  double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
  if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5 * pi, t);
    Lmin = L;
  }
  if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5 * pi, -t);
    Lmin = L;
  }
  if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5 * pi, t);
    Lmin = L;
  }
  if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5 * pi, -t);
    Lmin = L;
  }

  if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5 * pi, t);
    Lmin = L;
  }
  if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5 * pi, -t);
    Lmin = L;
  }
  if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5 * pi, t);
    Lmin = L;
  }
  if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5 * pi, -t);
}
// formula 8.11 *** TYPO IN PAPER ***
inline bool LpRmSLmRp(double x, double y, double phi, double& t, double& u, double& v)
{
  double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
  polar(xi, eta, rho, theta);
  if (rho >= 2.)
  {
    u = 4. - sqrt(rho * rho - 4.);
    if (u <= ZERO)
    {
      t = mod2pi(atan2((4 - u) * xi - 2 * eta, -2 * xi + (u - 4) * eta));
      v = mod2pi(t - phi);
      assert(fabs(4 * sin(t) - 2 * cos(t) - u * sin(t) - sin(phi) - x) < RS_EPS);
      assert(fabs(-4 * cos(t) - 2 * sin(t) + u * cos(t) + cos(phi) + 1 - y) < RS_EPS);
      assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
      return t >= -ZERO && v >= -ZERO;
    }
  }
  return false;
}
void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath& path)
{
  double t, u, v, Lmin = path.length() - pi, L;
  if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
  {
    path =
        ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5 * pi, u, -.5 * pi, v);
    Lmin = L;
  }
  if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
  {
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5 * pi, -u, .5 * pi,
                                                -v);
    Lmin = L;
  }
  if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
  {
    path =
        ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5 * pi, u, -.5 * pi, v);
    Lmin = L;
  }
  if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
    path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5 * pi, -u, .5 * pi,
                                                -v);
}

ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
{
  ReedsSheppStateSpace::ReedsSheppPath path;
  CSC(x, y, phi, path);
  CCC(x, y, phi, path);
  CCCC(x, y, phi, path);
  CCSC(x, y, phi, path);
  CCSCC(x, y, phi, path);
  return path;
}
}  // namespace

const ReedsSheppStateSpace::ReedsSheppPathSegmentType ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
  { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP },         // 0
  { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },        // 1
  { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },       // 2
  { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },       // 3
  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },    // 4
  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },   // 5
  { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },    // 6
  { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },   // 7
  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },   // 8
  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },    // 9
  { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },   // 10
  { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },    // 11
  { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },     // 12
  { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },     // 13
  { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },      // 14
  { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },    // 15
  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },  // 16
  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }   // 17
};

ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type, double t, double u,
                                                     double v, double w, double x)
  : type_(type)
{
  length_[0] = t;
  length_[1] = u;
  length_[2] = v;
  length_[3] = w;
  length_[4] = x;
  totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
}

double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
{
  return rho_ * reedsShepp(q0, q1).length();
}

ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
{
  double dx = q1[0] - q0[0], dy = q1[1] - q0[1], dth = q1[2] - q0[2];
  double c = cos(q0[2]), s = sin(q0[2]);
  double x = c * dx + s * dy, y = -s * dx + c * dy;
  return ::reedsShepp(x / rho_, y / rho_, dth);
}

void ReedsSheppStateSpace::type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data)
{
  ReedsSheppPath path = reedsShepp(q0, q1);
  for (int i = 0; i < 5; ++i)
    cb(path.type_[i], user_data);
  return;
}

void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb,
                                  void* user_data)
{
  ReedsSheppPath path = reedsShepp(q0, q1);
  double dist = rho_ * path.length();

  for (double seg = 0.0; seg <= dist; seg += step_size)
  {
    double qnew[3] = {};
    interpolate(q0, path, seg / rho_, qnew);
    cb(qnew, user_data);
  }
  return;
}

void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath& path, double seg, double s[3])
{
  if (seg < 0.0)
    seg = 0.0;
  if (seg > path.length())
    seg = path.length();

  double phi, v;

  s[0] = s[1] = 0.0;
  s[2] = q0[2];

  for (unsigned int i = 0; i < 5 && seg > 0; ++i)
  {
    if (path.length_[i] < 0)
    {
      v = std::max(-seg, path.length_[i]);
      seg += v;
    }
    else
    {
      v = std::min(seg, path.length_[i]);
      seg -= v;
    }
    phi = s[2];
    switch (path.type_[i])
    {
      case RS_LEFT:
        s[0] += (sin(phi + v) - sin(phi));
        s[1] += (-cos(phi + v) + cos(phi));
        s[2] = phi + v;
        break;
      case RS_RIGHT:
        s[0] += (-sin(phi - v) + sin(phi));
        s[1] += (cos(phi - v) - cos(phi));
        s[2] = phi - v;
        break;
      case RS_STRAIGHT:
        s[0] += (v * cos(phi));
        s[1] += (v * sin(phi));
        break;
      case RS_NOP:
        break;
    }
  }

  s[0] = s[0] * rho_ + q0[0];
  s[1] = s[1] * rho_ + q0[1];
}


#include <iostream>
#include <vector>
#include <iostream>
#include <iomanip>
#include <ctime>
#include <chrono>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

int roll_die( boost::random::mt19937 &gen, int begin, int end) {
    boost::random::uniform_int_distribution<> dist(begin, end);
    return dist(gen);
}

constexpr int iter = 100000;

std::vector<int> data(iter);

int main(int argc, char** argv){
	//once per process
	boost::random::mt19937 gen0;
	int random_seed1 = roll_die(gen0, 0,100000); // should be generated for every thread

	// per thread
	boost::random::mt19937 gen(random_seed1);//random_seed1 to have different starting conditions in diff threads

	std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();

	for(int i = 0; i < iter; ++i){
		data[i] = roll_die(gen,0, 20);
	}
	std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
	 for(int i = 0; i < iter; ++i){
			 std::cout << data[i]<< std::endl;
		 	}
	int total =  std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
	 std::cout << "Loop took " <<total << " ms"<< std::endl;
	 std::cout << "Iter took " << ((double)total  / double(iter)) * 1000<< " ns"<< std::endl;

 return 0;
}
/*
 * Iter took 14.63 ns
 */


/*******************************************************************
Author: David Ge (dge893@gmail.com, aka Wei Ge)
Last modified: 11/16/2020
Allrights reserved by David Ge

********************************************************************/
#include "Space.h"
#include "Curl.h"
#include "../MathTools/Matrix.h"
#include "../FileUtil/fileutil.h"
#include <stdlib.h>

#include "../boostLib/HiMatrix.h"
#include <boost/multiprecision/cpp_dec_float.hpp>

Space::Space()
{
	ds = 0.0;
	nx = ny = nz = 0;
	nx1 = ny1 = nz1 = 0;
	cellCount = 0;
	A = NULL;
	smax = M = 0;
	fieldMemorySize = 0;
	curlH = curlE = NULL;
}

Space::~Space()
{
	cleanup();
}

void Space::cleanup()
{
	tss_free3Darray(A, M + 1, M);
	if (curlH != NULL)
	{
		delete curlH;
		curlH = NULL;
	}
	if (curlE != NULL)
	{
		delete curlE;
		curlE = NULL;
	}
}

void Space::setCalculationMethod(CURLMETHOD method)
{
	curlE->setCalculationMethod(method);
	curlH->setCalculationMethod(method);
}

inline size_t Space::Idx(unsigned i, unsigned j, unsigned k)
{
	return k + nz1 * (j + ny1 * i);
}

/*
	generate space derivative estimation matrixes
*/
int Space::generateMatrix()
{
	int ret = ERR_OK;
	using namespace boost::multiprecision;
	cpp_dec_float_100 *W = (cpp_dec_float_100 *)malloc(M*M*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *Ai = (cpp_dec_float_100 *)malloc(M*M*sizeof(cpp_dec_float_100));
	double *AI = (double *)malloc(M*M*sizeof(double));
	A = tss_allocate3Darray(M + 1, M, M);
	if (AI == NULL || W == NULL || Ai == NULL || A == NULL)
	{
		ret = ERR_OUTOFMEMORY;
	}
	if (ret == ERR_OK)
	{
		HiMatrixTools HiMatrix;
		unsigned int P = smax;
		unsigned int N = smax;
		unsigned int mi;
		cpp_dec_float_100 X = 1.0;
		cpp_dec_float_100 Dn = 1.0;
		cpp_dec_float_100 Dx;
		unsigned int i;
		unsigned int w = 0;
		size_t k;
		for (w = 0; w <= 2 * smax; w++)
		{
			if (w <= smax)
			{
				P = smax + w;
				N = smax - w;
			}
			else
			{
				P = smax - (w - smax);
				N = smax + (w - smax);
			}
			mi = 0;
			k = 0;
			i = 0;
			while (i < P)
			{
				Dn = ((double)i + 1.0);
				Dx = Dn;
				X = 1.0;
				for (unsigned int j = 0; j < M; j++)
				{
					X = X*((double)j + 1.0);
					W[k] = Dx / X;
					Dx = Dx * Dn;
					k++;
				}
				mi += M;
				i++;
			}
			i = 0;
			while (i < N)
			{
				Dn = -((double)i + 1.0);
				Dx = Dn;
				X = 1.0;
				for (unsigned int j = 0; j < M; j++)
				{
					X = X*((double)j + 1.0);
					W[k] = Dx / X;
					Dx = Dx * Dn;
					k++;
				}
				mi += M;
				i++;
			}
			if (HiMatrix.HIinverse(W, Ai, (int)M))
			{
				k = 0;
				for (i = 0; i < M; i++)
				{
					for (unsigned int j = 0; j < M; j++)
					{
						AI[k] = Ai[k].convert_to<double>();
						k++;
					}
				}
				copy2Darray(AI, &(A->a[w]), M);
			}
			else
			{
				ret = ERR_INVERSEMATRIX;
				break;
			}
		}
	}
	if(AI != NULL) free(AI);
	if (W != NULL) free(W);
	if (Ai != NULL) free(Ai);
	return ret;
}

int Space::initializeSpace(double spaceStep, unsigned int inx, unsigned int iny, unsigned int inz, unsigned int ismax, const char *matrixFile)
{
	int ret = ERR_OK;
	ds = spaceStep;
	nx = inx; ny = iny; nz = inz;
	nx1 = inx+1; ny1 = iny+1; nz1 = inz+1;
	smax = ismax;
	M = 2 * smax;
	if (ds <= 0.0 || nx == 0 || ny == 0 || nz == 0 || smax == 0)
	{
		ret = ERR_INVALID_PARAM;
	}
	else
	{
		if (matrixFile != NULL && fileexists(matrixFile))
		{
			A = LoadEstimationMatrixesFromFile(matrixFile, M, &ret);
		}
		else
		{
			ret = generateMatrix();
		}
	}
	if (ret == ERR_OK)
	{
		cellCount = (nx + 1)*(ny + 1)*(nz + 1);
		fieldMemorySize = cellCount*sizeof(Point3Dstruct);
	}
	if (ret == ERR_OK)
	{
		curlH = new Curl();
		ret = curlH->initialize(this);
		if (ret == ERR_OK)
		{
			curlE = new Curl();
			ret = curlE->initialize(this);
			if (ret == ERR_OK)
			{
				//hard coded here as default
				setCalculationMethod(ByFastMethodFirstOrder);
			}
		}
	}
	return ret;
}

////////3 functions for dFx/dx,dy,dz/////////////////////////////////////
double Space::dx_Fx(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - i;
	if (w > 0) //near or at the lower boundary: w=1,2,...,smax; i=smax-1, smax-2,...,0
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = i + smax - nx;
		if (w > 0) //near or at the upper boundary: w=1,2,...,smax; i=nx-smax+1, nx-smax+2,...,nx
		{
			P = smax - w;
			N = smax + w;
			w = w + smax; //index into the array
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i + m,j,k)].x - F[w0].x);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i - m,j,k)].x - F[w0].x);
	}
	return d;
}

double Space::dy_Fx(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - j;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = j + smax - ny;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j + m,k)].x - F[w0].x);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j - m,k)].x - F[w0].x);
	}
	return d;
}

double Space::dz_Fx(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - k;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = k + smax - nz;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j,k + m)].x - F[w0].x);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j,k - m)].x - F[w0].x);
	}
	return d;
}
/////////--end of dFx /dx.dy.dz/////////////////////////////////////////
/////////--start of dFy /dx.dy.dz/////////////////////////////////////////
double Space::dx_Fy(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - i;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = i + smax - nx;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i + m,j,k)].y - F[w0].y);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i - m,j,k)].y - F[w0].y);
	}
	return d;
}

double Space::dy_Fy(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - j;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = j + smax - ny;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j + m,k)].y - F[w0].y);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j - m,k)].y - F[w0].y);
	}
	return d;
}

double Space::dz_Fy(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - k;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = k + smax - nz;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j,k + m)].y - F[w0].y);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j,k - m)].y - F[w0].y);
	}
	return d;
}
/////////--end of dFy /dx.dy.dz/////////////////////////////////////////
/////////--start of dFz /dx.dy.dz/////////////////////////////////////////
double Space::dx_Fz(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - i;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = i + smax - nx;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i + m,j,k)].z - F[w0].z);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i - m,j,k)].z - F[w0].z);
	}
	return d;
}

double Space::dy_Fz(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - j;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = j + smax - ny;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j + m,k)].z - F[w0].z);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j - m,k)].z - F[w0].z);
	}
	return d;
}

double Space::dz_Fz(Point3Dstruct *F, unsigned int h, unsigned int i, unsigned int j, unsigned int k)
{
	double d = 0.0;
	unsigned int P, N;
	unsigned int m;
	size_t w0 = Idx(i, j, k);
	int w = smax - k;
	if (w > 0)
	{
		P = smax + w;
		N = smax - w;
	}
	else
	{
		w = k + smax - nz;
		if (w > 0)
		{
			P = smax - w;
			N = smax + w;
			w = w + smax;
		}
		else
		{
			P = smax;
			N = smax;
			w = 0;
		}
	}
	//
	h = h - 1;
	for (m = 1; m <= P; m++)
	{
		d += A->a[w].r[h].c[m - 1] * (F[Idx(i,j,k + m)].z - F[w0].z);
	}
	for (m = 1; m <= N; m++)
	{
		d += A->a[w].r[h].c[P + m - 1] * (F[Idx(i,j,k - m)].z - F[w0].z);
	}
	return d;
}
/////////--end of dFz /dx.dy.dz/////////////////////////////////////////

void Space::SetFields(Point3Dstruct *fh, Point3Dstruct *fe)
{
	curlH->SetField(fh);
	curlE->SetField(fe);
}

int Space::CalculateNextCurlH(Point3Dstruct *pNext)
{
	return curlH->CalculateCurl(pNext);
}

int Space::CalculateNextCurlE(Point3Dstruct *pNext)
{
	return curlE->CalculateCurl(pNext);
}

Point3Dstruct *Space::GetCurrentCurlH()
{
	return curlH->GetCurrentCurl();
}

Point3Dstruct *Space::GetCurrentCurlE()
{
	return curlE->GetCurrentCurl();
}

int Space::writeMatrixesToFile(char *file)
{
	int ret = ERR_OK;
	FILE *retFileHandle = 0;
	ret = deleteFile(file);
	if (ret == ERR_OK)
	{
		ret = openTextfileWrite(file, &retFileHandle);
		if (ret == ERR_OK)
		{
			if (retFileHandle == 0)
			{
				ret = ERR_FILE_OPEN_WRIT_EINVAL;
			}
			else
			{
				unsigned int size = 1024;
				char buff[1024];
				Array3Dstruct *A = GetMaxtrixes(); //1+2smax,2smax,2smax
				sprintf_1(buff, size, "smax=%u, space estiation order = 2*smax = %u\r\n", smax, 2 * smax);
				writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
				for (unsigned int h = 0; h < 2 * smax + 1; h++)
				{
					if (h == 0)
					{
						sprintf_1(buff, size, "\r\nestimation matrix for spaces inside the boundary [%u x %u]=\n", 2 * smax, 2 * smax);
					}
					else if (h <= smax)
					{
						sprintf_1(buff, size, "\r\nestimation matrix for spaces near or at the lower boundary [%u x %u]=\n", 2 * smax, 2 * smax);
					}
					else
					{
						sprintf_1(buff, size, "\r\nestimation matrix for spaces near or at the upper boundary [%u x %u]=\n", 2 * smax, 2 * smax);
					}
					writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
					//
					for (unsigned int i = 0; i < 2 * smax; i++)
					{
						for (unsigned int j = 0; j < 2 * smax; j++)
						{
							if (j == 2 * smax - 1)
							{
								sprintf_1(buff, size, "%g\n", A->a[h].r[i].c[j]);
							}
							else
							{
								sprintf_1(buff, size, "%g,\t", A->a[h].r[i].c[j]);
							}
							writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
						}
					}
				}
				closefile(retFileHandle);
			}
		}
	}
	return ret;
}

int Space::saveMaxtrixToFile(char *file)
{
	int ret = ERR_OK;
	FILE *fh = 0;
	size_t w;
	size_t size = (M + 1)*M*M*sizeof(double);
	double *data = (double *)malloc(size);
	if (data == NULL)
	{
		ret = ERR_OUTOFMEMORY;
	}
	else
	{
		w = 0;
		for (unsigned int h = 0; h <= M; h++)
		{
			for (unsigned int i = 0; i < M; i++)
			{
				for (unsigned int j = 0; j < M; j++)
				{
					data[w] = A->a[h].r[i].c[j];
					w++;
				}
			}
		}
		ret = openfileWrite(file, &fh);
		if (ret == ERR_OK)
		{
			ret = writefile(fh, data, (unsigned int)size);
			closefile(fh);
		}
		free(data);
	}
	return ret;
}

//int Space::loadMaxtrixFromFile(const char *file)
Array3Dstruct *Space::LoadEstimationMatrixesFromFile(const char *file, unsigned int M, int *ret)
{
	*ret = ERR_OK;
	FILE *fh = 0;
	Array3Dstruct *A = NULL;
	size_t w;
	size_t size = (M + 1)*M*M*sizeof(double);
	double *data = (double *)malloc(size);
	if (data == NULL)
	{
		*ret = ERR_OUTOFMEMORY;
	}
	else
	{
		size_t fsize = fileSize(file);
		if (fsize != size)
		{
			*ret = ERR_INVALID_SIZE;
		}
	}
	if (*ret == ERR_OK)
	{
		*ret = openfileRead(file, &fh);
		if (*ret == ERR_OK)
		{
			*ret = readfile(fh, data, (unsigned int)size);
			closefile(fh);
		}
	}
	if (*ret == ERR_OK)
	{
		A = tss_allocate3Darray(M + 1, M, M);
		if (A == NULL)
		{
			*ret = ERR_OUTOFMEMORY;
		}
	}
	if (*ret == ERR_OK)
	{
		w = 0;
		for (unsigned int h = 0; h <= M; h++)
		{
			for (unsigned int i = 0; i < M; i++)
			{
				for (unsigned int j = 0; j < M; j++)
				{
					A->a[h].r[i].c[j] = data[w];
					w++;
				}
			}
		}
	}
	if (data != NULL)
		free(data);
	return A;
}

int Space::CreateMatrixFile(unsigned int smax0, char *file)
{
	int ret = ERR_OK;
	smax = smax0;
	M = 2 * smax;
	ret = generateMatrix();
	if (ret == ERR_OK)
	{
		ret = saveMaxtrixToFile(file);
	}
	return ret;
}


// Basic sanity check that header <boost/math/special_functions/ellint_1.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/special_functions/jacobi_theta.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

void compile_and_link_test()
{
    // Q parameter
   check_result<float>(boost::math::jacobi_theta1<float>(f, f));
   check_result<double>(boost::math::jacobi_theta1<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta1<long double>(l, l));
#endif

   check_result<float>(boost::math::jacobi_theta2<float>(f, f));
   check_result<double>(boost::math::jacobi_theta2<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta2<long double>(l, l));
#endif

   check_result<float>(boost::math::jacobi_theta3<float>(f, f));
   check_result<double>(boost::math::jacobi_theta3<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta3<long double>(l, l));
#endif

   check_result<float>(boost::math::jacobi_theta4<float>(f, f));
   check_result<double>(boost::math::jacobi_theta4<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta4<long double>(l, l));
#endif

    // Tau parameter
   check_result<float>(boost::math::jacobi_theta1tau<float>(f, f));
   check_result<double>(boost::math::jacobi_theta1tau<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta1tau<long double>(l, l));
#endif

   check_result<float>(boost::math::jacobi_theta2tau<float>(f, f));
   check_result<double>(boost::math::jacobi_theta2tau<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta2tau<long double>(l, l));
#endif

   check_result<float>(boost::math::jacobi_theta3tau<float>(f, f));
   check_result<double>(boost::math::jacobi_theta3tau<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta3tau<long double>(l, l));
#endif

   check_result<float>(boost::math::jacobi_theta4tau<float>(f, f));
   check_result<double>(boost::math::jacobi_theta4tau<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta4tau<long double>(l, l));
#endif

   // Minus 1 flavors
   check_result<float>(boost::math::jacobi_theta3m1<float>(f, f));
   check_result<double>(boost::math::jacobi_theta3m1<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta3m1<long double>(l, l));
#endif

   check_result<float>(boost::math::jacobi_theta4m1<float>(f, f));
   check_result<double>(boost::math::jacobi_theta4m1<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta4m1<long double>(l, l));
#endif

   check_result<float>(boost::math::jacobi_theta3m1tau<float>(f, f));
   check_result<double>(boost::math::jacobi_theta3m1tau<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta3m1tau<long double>(l, l));
#endif

   check_result<float>(boost::math::jacobi_theta4m1tau<float>(f, f));
   check_result<double>(boost::math::jacobi_theta4m1tau<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::jacobi_theta4m1tau<long double>(l, l));
#endif
}


//
// Created by krab1k on 13.11.18.
//

#include <cmath>
#include <Eigen/LU>

#include "delre.h"
#include "../structures/molecule.h"
#include "../structures/bond.h"
#include "../parameters.h"

CHARGEFW2_METHOD(DelRe)

std::vector<double> DelRe::calculate_charges(const Molecule &molecule) const {

    const size_t n = molecule.atoms().size();
    const size_t m = molecule.bonds().size();

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n, n);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n);

    for (size_t i = 0; i < n; i++) {
        auto &atom_i = molecule.atoms()[i];
        b(i) = -parameters_->atom()->parameter(atom::delta)(atom_i);
        A(i, i) = -1.0;
    }

    for (const auto &bond: molecule.bonds()) {
        size_t i = bond.first().index();
        size_t j = bond.second().index();
        A(i, j) = parameters_->bond()->parameter(bond::gammaA)(bond);
        A(j, i) = parameters_->bond()->parameter(bond::gammaB)(bond);
    }

    Eigen::VectorXd d = A.partialPivLu().solve(b);
    std::vector<double> q(n, 0);

    for (size_t k = 0; k < m; k++) {
        const auto &bond = molecule.bonds()[k];
        size_t i = bond.first().index();
        size_t j = bond.second().index();
        double dq = (d(i) - d(j)) / (2 * parameters_->bond()->parameter(bond::eps)(bond));
        q[i] -= dq;
        q[j] += dq;
    }

    return q;
}


//============================================================================
// Name        : polynomfit.cpp
// Author      :
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C, Ansi-style
//============================================================================

//#include <stdio.h>
//#include <stdlib.h>
#include <Eigen/Dense>
#include "include/treepoly_smooth.hh"
#include <random>

#include <iostream>

using namespace std;

int main(void) {
//	puts("Hello World!!!");
//	return EXIT_SUCCESS;

	cout << "Hello world" << endl;
	Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic > m_eigen_matrix;
	m_eigen_matrix.resize(2,2);

	m_eigen_matrix << 1, 2,
					3,4;
	cout << m_eigen_matrix << endl;

	std::vector<std::vector<double> > data(10000, std::vector<double>(2,0.0) );
	std::random_device rd;
	std::mt19937 gen(rd());

	std::normal_distribution<> d(0,1);
	std::uniform_int_distribution<> distrib(1, 6);
	for(int i = 0; i!= data.size(); i++) {
		const double x =  distrib(gen) + d(gen);
		const double y = -0.1*x*x*x + 0.7*x*x + 0.01*x + 0.5 + d(gen);
		data[i][0] = x;
		data[i][1] = y;
	}

	int n_levels = 5, n_input_vars =1;

	treepoly_smooth<long double> tree_poly(n_input_vars,n_levels);
	tree_poly.reserve(data.size());

	for(int i=0; i!= data.size(); i++) {
		//cout<< i << data[i][0] << " " << data[i][1] << endl;
		std::vector<double> xv(1);

		xv[0] = data[i][0];
		tree_poly.fill_1dim(data[i][0], data[i][1], 1.0);
	}
	tree_poly.train();

	for(double x=0; x< 1.0; x+=0.1) {
		double y = (-0.1*x*x*x + 0.7*x*x + 0.01*x + 0.5);
		std::vector<long double> xv(1);
		xv[0] = x;
		cout << "p("<<x<<")=" << tree_poly.eval(xv) << " vs " << y << endl;
	}

	//polynom.print("10 degree fit");

}


#pragma once

#include <Eigen/Core>
#include <limits>
#include <vector>

namespace collision2d {
template <typename N>
using Point = Eigen::Matrix<N, 2, 1>;

template <typename N>
using Polygon = std::vector<Point<N>>;

/**
 * Computes potential separating axes for a convex polygon.
 */
template <typename N>
void separatingAxes(const Polygon<N> &a, std::vector<Point<N>> &axes) {
  for (auto i = 0u; i < a.size(); ++i) {
    const auto current = a[i];
    const auto next = a[(i + 1) % a.size()];
    const auto edge = (next - current).normalized();
    // axis is the normal of an edge
    axes.emplace_back(Point<N>{-edge[1], edge[0]});
  }
}

/**
 * Projects the polygon onto the given axis and returns the maximum and minimum
 * coordinate on the axis. Note that the axis has to be normalized.
 */
template <typename N>
void project(const Polygon<N> &a, const Point<N> &axis, N &minProj,
             N &maxProj) {
  maxProj = -std::numeric_limits<N>::infinity();
  minProj = std::numeric_limits<N>::infinity();
  for (const Point<N> &v : a) {
    const N proj = axis.dot(v);
    if (proj < minProj) minProj = proj;
    if (proj > maxProj) maxProj = proj;
  }
}

/**
 * Check for collision between polygons a and b via the Separating Axis Theorem.
 */
template <typename N>
bool intersect(const Polygon<N> &a, const Polygon<N> &b) {
  // compute separating axes
  std::vector<Point<N>> axes;
  separatingAxes(a, axes);
  separatingAxes(b, axes);
  for (const auto &axis : axes) {
    N aMaxProj, aMinProj, bMaxProj, bMinProj;
    project(a, axis, aMinProj, aMaxProj);
    project(b, axis, bMinProj, bMaxProj);
    // check if projections overlap
    if (aMinProj > bMaxProj || bMinProj > aMaxProj) return false;
  }

  return true;
}

// /**
//  * Efficient test for a point to be in a convex polygon.
//  *
//  * Robert Nowak "An Efficient Test for a Point to Be in a Convex Polygon"
//  * http://demonstrations.wolfram.com/AnEfficientTestForAPointToBeInAConvexPolygon/
//  * Wolfram Demonstrations Project
//  * Published: March 7 2011
//  */
// template <typename N>
// bool intersect2(const Point<N> &point, const Polygon<N> &polygon,
//                 const N &epsilon = 1e-4) {
//   bool angle = false;  // stores the sign of the last angle
//   for (auto i = 0u; i < polygon.size(); ++i) {
//     const auto &a = polygon[i] - point;
//     const auto &b = polygon[(i + 1) % polygon.size()] - point;
//     const bool newAngle = b(0) * a(1) - a(0) * b(1) > -epsilon;
//     if (i > 0 && angle != newAngle) return false;
//     angle = newAngle;
//   }
//   return true;
// }

/**
 * Tests whether point intersects with convex polygon.
 * Source: https://stackoverflow.com/a/8721483
 */
template <typename N>
bool intersect(const Point<N> &point, const Polygon<N> &polygon,
               const N &epsilon = 1e-4) {
  bool result = false;
  std::size_t i, j;
  for (i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) {
    if ((polygon[i].y() > point.y()) != (polygon[j].y() > point.y()) &&
        (point.x() < (polygon[j].x() - polygon[i].x()) * (point.y() - polygon[i].y()) /
                          (polygon[j].y() - polygon[i].y() + epsilon) +
                      polygon[i].x())) {
      result = !result;
    }
  }
  return result;
}
}  // namespace collision2d


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/function/airy_ai.hpp>
#include <eve/function/airy_bi.hpp>
#include <eve/function/airy.hpp>
#include <eve/function/if_else.hpp>
#include <eve/function/prev.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>
#include <boost/math/special_functions/airy.hpp>

EVE_TEST_TYPES( "Check return types of airy_ai"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  using kv_t =  kumi::tuple<v_t, v_t>;
  using kT =    kumi::tuple<T, T>;
  TTS_EXPR_IS(eve::airy(T(0)), kT);
  TTS_EXPR_IS(eve::airy(v_t(0)), kv_t);
};

 EVE_TEST( "Check behavior of airy on wide"
         , eve::test::simd::ieee_reals
         , eve::test::generate(eve::test::randoms(-20.0, 0.0),
                               eve::test::randoms(0.0, 20.0)
                              )
         )
   <typename T>(T a0, T a1)
{
  using v_t = eve::element_type_t<T>;
  v_t abstol = 1000*eve::eps(eve::as<v_t>());
  auto eve_airy =  [](auto x) { return eve::airy(x); };
  auto std_airy_ai =  [](auto x)->v_t { return eve::airy_ai(x); };
  auto std_airy_bi =  [](auto x)->v_t { return eve::airy_bi(x); };

  {
    auto [ai, bi]= eve_airy(a0);
    TTS_ABSOLUTE_EQUAL(ai, map(std_airy_ai, a0), abstol);
    TTS_RELATIVE_EQUAL(bi, map(std_airy_bi, a0), 0.0004);
  }
  {
    auto [ai, bi]= eve_airy(a1);
    TTS_ABSOLUTE_EQUAL(ai, map(std_airy_ai, a1), abstol);
    TTS_RELATIVE_EQUAL(bi, map(std_airy_bi, a1), 0.0004);
  }

};


/*
 * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// check memory allocation in some method
#define EIGEN_RUNTIME_NO_MALLOC

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE PTransformd test
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;

const double TOL = 0.00001;

bool isUpperNull(const Eigen::Matrix3d & m)
{
  using namespace Eigen;
  return (Matrix3d(m.triangularView<StrictlyUpper>()).array() == 0.).all();
}

BOOST_AUTO_TEST_CASE(RotationMatrixTest)
{
  using namespace Eigen;
  using namespace sva;

  Vector2d theta2d = Vector2d::Random() * 10;
  double theta = theta2d(0);

  BOOST_CHECK_SMALL((RotX(theta) - AngleAxisd(-theta, Vector3d::UnitX()).matrix()).array().abs().sum(), TOL);
  BOOST_CHECK_SMALL((RotY(theta) - AngleAxisd(-theta, Vector3d::UnitY()).matrix()).array().abs().sum(), TOL);
  BOOST_CHECK_SMALL((RotZ(theta) - AngleAxisd(-theta, Vector3d::UnitZ()).matrix()).array().abs().sum(), TOL);
}

BOOST_AUTO_TEST_CASE(PTransformdTest)
{
  using namespace Eigen;
  using namespace sva;
  namespace constants = boost::math::constants;

  Matrix3d Em = AngleAxisd(constants::pi<double>() / 2., Vector3d(1., 0., 0.)).inverse().toRotationMatrix();
  Quaterniond Eq;
  Eq = AngleAxisd(constants::pi<double>() / 2., Vector3d(1., 0., 0.)).inverse();
  Vector3d r = Vector3d::Random() * 100.;

  // Identity
  PTransformd pt1 = PTransformd::Identity();

  BOOST_CHECK_EQUAL(pt1.rotation(), Matrix3d::Identity());
  BOOST_CHECK_EQUAL(pt1.translation(), Vector3d::Zero());

  // Matrix3d Vector3d constructor
  PTransformd pt2(Em, r);

  BOOST_CHECK_EQUAL(pt2.rotation(), Em);
  BOOST_CHECK_EQUAL(pt2.translation(), r);

  // Quaternion Vector3d constructor
  PTransformd pt3(Eq, r);

  BOOST_CHECK_EQUAL(pt3.rotation(), Eq.toRotationMatrix());
  BOOST_CHECK_EQUAL(pt3.translation(), r);

  // Quaternion constructor
  PTransformd pt4(Eq);

  BOOST_CHECK_EQUAL(pt4.rotation(), Eq.toRotationMatrix());
  BOOST_CHECK_EQUAL(pt4.translation(), Vector3d::Zero());

  // Matrix3d constructor
  PTransformd pt5(Em);

  BOOST_CHECK_EQUAL(pt5.rotation(), Em);
  BOOST_CHECK_EQUAL(pt5.translation(), Vector3d::Zero());

  // Vector3d constructor
  PTransformd pt6(r);

  BOOST_CHECK_EQUAL(pt6.rotation(), Matrix3d::Identity());
  BOOST_CHECK_EQUAL(pt6.translation(), r);

  // operator*(PTransformd)
  PTransformd pttmp(AngleAxisd(constants::pi<double>() / 4., Vector3d(0., 1., 0.)).toRotationMatrix(),
                    Vector3d::Random() * 100.);

  PTransformd pt7 = pt2 * pttmp;
  Matrix6d ptm(pt2.matrix() * pttmp.matrix());

  BOOST_CHECK_SMALL((pt7.matrix() - ptm).array().abs().sum(), TOL);

  // inv
  PTransformd pt8 = pt2.inv();
  Matrix6d pt8_minus_pt2_inv = pt8.matrix() - pt2.matrix().inverse();
  BOOST_CHECK_SMALL(pt8_minus_pt2_inv.array().abs().sum(), TOL);

  // ==
  BOOST_CHECK_EQUAL(pt2, pt2);
  BOOST_CHECK_NE(pt2, pt8);

  // !=
  BOOST_CHECK(pt2 != pt8);
  BOOST_CHECK(!(pt2 != pt2));
}

BOOST_AUTO_TEST_CASE(PTransformdLeftOperatorsTest)
{
  using namespace Eigen;
  using namespace sva;
  namespace constants = boost::math::constants;

  Quaterniond Eq;
  Eq = AngleAxisd(constants::pi<double>() / 2., Vector3d(1., 0., 0.));
  Vector3d r = Vector3d::Random() * 100.;
  PTransformd pt(Eq, r);
  PTransformd ptInv = pt.inv();
  Matrix6d pt6d = pt.matrix();
  Matrix6d ptInv6d = ptInv.matrix();
  Matrix6d ptDual6d = pt.dualMatrix();

  Matrix3d M, H, I;
  M << 1., 2., 3., 2., 1., 4., 3., 4., 1.;
  H = Matrix3d::Random() * 100.;
  I << 1., 2., 3., 2., 1., 4., 3., 4., 1.;

  ABInertiad ab(M, H, I);
  Matrix6d ab6d = ab.matrix();

  double mass = 1.;
  Vector3d h = Vector3d::Random() * 100.;
  RBInertiad rb(mass, h, I);
  Matrix6d rb6d = rb.matrix();

  Vector3d w, v, n, f;
  w = Vector3d::Random() * 100.;
  v = Vector3d::Random() * 100.;
  n = Vector3d::Random() * 100.;
  f = Vector3d::Random() * 100.;

  sva::MotionVecd mVec(w, v);
  Vector6d mVec6d = mVec.vector();

  sva::ForceVecd fVec(n, f);
  Vector6d fVec6d = fVec.vector();

  // PTransformd * MotionVecd
  MotionVecd mvRes1 = pt * mVec;
  Vector6d mvRes16d(pt6d * mVec6d);

  BOOST_CHECK_SMALL((mvRes1.vector() - mvRes16d).array().abs().sum(), TOL);

  // test the angular and linear version
  BOOST_CHECK_SMALL((mvRes1.angular() - pt.angularMul(mVec)).array().abs().sum(), TOL);
  BOOST_CHECK_SMALL((mvRes1.linear() - pt.linearMul(mVec)).array().abs().sum(), TOL);

  // test the vectorized version
  Matrix6Xd mv1Vec6Xd(6, 2);
  Matrix6Xd mvRes1Vec6Xd(6, 2);
  mv1Vec6Xd << mVec.vector(), mVec.vector();

  Eigen::internal::set_is_malloc_allowed(false);
  pt.mul(mv1Vec6Xd, mvRes1Vec6Xd);
  Eigen::internal::set_is_malloc_allowed(true);

  BOOST_CHECK_SMALL((mvRes1.vector() - mvRes1Vec6Xd.col(0)).norm(), TOL);
  BOOST_CHECK_EQUAL(mvRes1Vec6Xd.col(0), mvRes1Vec6Xd.col(1));

  // PTransformd^-1 * MotionVecd
  MotionVecd mvRes2 = pt.invMul(mVec);
  Vector6d mvRes26d(ptInv6d * mVec6d);

  BOOST_CHECK_SMALL((mvRes2.vector() - mvRes26d).array().abs().sum(), TOL);

  // test the angular and linear version
  BOOST_CHECK_SMALL((mvRes2.angular() - pt.angularInvMul(mVec)).array().abs().sum(), TOL);
  BOOST_CHECK_SMALL((mvRes2.linear() - pt.linearInvMul(mVec)).array().abs().sum(), TOL);

  // test the vectorized version
  Matrix6Xd mv2Vec6Xd(6, 2);
  Matrix6Xd mvRes2Vec6Xd(6, 2);
  mv2Vec6Xd << mVec.vector(), mVec.vector();

  Eigen::internal::set_is_malloc_allowed(false);
  pt.invMul(mv2Vec6Xd, mvRes2Vec6Xd);
  Eigen::internal::set_is_malloc_allowed(true);

  BOOST_CHECK_SMALL((mvRes2.vector() - mvRes2Vec6Xd.col(0)).norm(), TOL);
  BOOST_CHECK_EQUAL(mvRes2Vec6Xd.col(0), mvRes2Vec6Xd.col(1));

  // PTransformd* * ForceVecd
  ForceVecd fvRes1 = pt.dualMul(fVec);
  Vector6d fvRes16d(ptDual6d * fVec6d);

  BOOST_CHECK_SMALL((fvRes1.vector() - fvRes16d).array().abs().sum(), TOL);

  // test the couple and force version
  BOOST_CHECK_SMALL((fvRes1.couple() - pt.coupleDualMul(fVec)).array().abs().sum(), TOL);
  BOOST_CHECK_SMALL((fvRes1.force() - pt.forceDualMul(fVec)).array().abs().sum(), TOL);

  // test the vectorized version
  Matrix6Xd fv1Vec6Xd(6, 2);
  Matrix6Xd fvRes1Vec6Xd(6, 2);
  fv1Vec6Xd << fVec.vector(), fVec.vector();

  Eigen::internal::set_is_malloc_allowed(false);
  pt.dualMul(fv1Vec6Xd, fvRes1Vec6Xd);
  Eigen::internal::set_is_malloc_allowed(true);

  BOOST_CHECK_SMALL((fvRes1.vector() - fvRes1Vec6Xd.col(0)).norm(), TOL);
  BOOST_CHECK_EQUAL(fvRes1Vec6Xd.col(0), fvRes1Vec6Xd.col(1));

  // PTransformd T * ForceVecd
  ForceVecd fvRes2 = pt.transMul(fVec);
  Vector6d fvRes26d(pt6d.transpose() * fVec6d);

  BOOST_CHECK_SMALL((fvRes2.vector() - fvRes26d).array().abs().sum(), TOL);

  // test the couple and force version
  BOOST_CHECK_SMALL((fvRes2.couple() - pt.coupleTransMul(fVec)).array().abs().sum(), TOL);
  BOOST_CHECK_SMALL((fvRes2.force() - pt.forceTransMul(fVec)).array().abs().sum(), TOL);

  // test the vectorized version
  Matrix6Xd fv2Vec6Xd(6, 2);
  Matrix6Xd fvRes2Vec6Xd(6, 2);
  fv2Vec6Xd << fVec.vector(), fVec.vector();

  Eigen::internal::set_is_malloc_allowed(false);
  pt.transMul(fv2Vec6Xd, fvRes2Vec6Xd);
  Eigen::internal::set_is_malloc_allowed(true);

  BOOST_CHECK_SMALL((fvRes2.vector() - fvRes2Vec6Xd.col(0)).norm(), TOL);
  BOOST_CHECK_EQUAL(fvRes2Vec6Xd.col(0), fvRes2Vec6Xd.col(1));

  // PTransformd* * RBInertiad * PTransformd^-1
  RBInertiad rbRes1 = pt.dualMul(rb);
  Matrix6d rbRes16d(ptDual6d * rb6d * ptInv6d);

  BOOST_CHECK_SMALL((rbRes1.matrix() - rbRes16d).array().abs().sum(), TOL);

  // PTransformd T * RBInertiad * PTransformd
  RBInertiad rbRes2 = pt.transMul(rb);
  Matrix6d rbRes26d(pt6d.transpose() * rb6d * pt6d);

  BOOST_CHECK_SMALL((rbRes2.matrix() - rbRes26d).array().abs().sum(), TOL);

  // PTransformd* * ABInertiad * PTransformd^-1
  ABInertiad abRes1 = pt.dualMul(ab);
  Matrix6d abRes16d(ptDual6d * ab6d * ptInv6d);

  BOOST_CHECK_SMALL((abRes1.matrix() - abRes16d).array().abs().sum(), TOL);

  // PTransformd T * ABInertiad * PTransformd
  ABInertiad abRes2 = pt.transMul(ab);
  Matrix6d abRes26d(pt6d.transpose() * ab6d * pt6d);

  BOOST_CHECK_SMALL((abRes2.matrix() - abRes26d).array().abs().sum(), TOL);
}

BOOST_AUTO_TEST_CASE(EulerAngleTest)
{
  using namespace Eigen;
  using namespace sva;
  namespace cst = boost::math::constants;

  Vector3d res;

  res = rotationError<double>(Matrix3d::Identity(), RotX(cst::pi<double>() / 2.));
  BOOST_CHECK_SMALL((res - Vector3d(cst::pi<double>() / 2., 0., 0.)).norm(), TOL);

  res = rotationError<double>(Matrix3d::Identity(), RotY(cst::pi<double>() / 2.));
  BOOST_CHECK_SMALL((res - Vector3d(0., cst::pi<double>() / 2., 0.)).norm(), TOL);

  res = rotationError<double>(Matrix3d::Identity(), RotZ(cst::pi<double>() / 2.));
  BOOST_CHECK_SMALL((res - Vector3d(0., 0., cst::pi<double>() / 2.)).norm(), TOL);

  res = rotationError<double>(RotZ(cst::pi<double>() / 4.), RotZ(cst::pi<double>() / 2.));
  BOOST_CHECK_SMALL((res - Vector3d(0., 0., cst::pi<double>() / 4.)).norm(), TOL);
}

BOOST_AUTO_TEST_CASE(InterpolateTest)
{
  using namespace Eigen;
  using namespace sva;
  namespace cst = boost::math::constants;

  PTransformd from(Matrix3d::Identity(), Vector3d(0., 0., 0.));
  PTransformd to(AngleAxisd(cst::pi<double>(), Vector3d::UnitZ()).toRotationMatrix(), Vector3d(1., 2., -3.));

  PTransformd res = interpolate<double>(from, to, 0.5);

  BOOST_CHECK_SMALL((res.rotation() - AngleAxisd(cst::pi<double>() / 2., Vector3d::UnitZ()).toRotationMatrix()).norm(),
                    TOL);
  BOOST_CHECK_SMALL((res.translation() - Vector3d(0.5, 1., -1.5)).norm(), TOL);

  res = interpolate<double>(from, to, 0);
  BOOST_CHECK_SMALL((res.rotation() - from.rotation()).norm(), TOL);
  BOOST_CHECK_SMALL((res.translation() - from.translation()).norm(), TOL);

  res = interpolate<double>(from, to, 1);
  BOOST_CHECK_SMALL((res.rotation() - to.rotation()).norm(), TOL);
  BOOST_CHECK_SMALL((res.translation() - to.translation()).norm(), TOL);
}

BOOST_AUTO_TEST_CASE(TransformError)
{
  using namespace Eigen;
  using namespace sva;
  namespace cst = boost::math::constants;

  PTransformd X_a_b(Quaterniond(Vector4d::Random()).normalized(), Vector3d::Random());
  PTransformd X_a_c(Quaterniond(Vector4d::Random()).normalized(), Vector3d::Random());

  MotionVecd V_a_b = transformVelocity(X_a_b);
  MotionVecd V_a_c = transformVelocity(X_a_c);

  BOOST_CHECK_SMALL((V_a_b.angular() - rotationVelocity(X_a_b.rotation())).norm(), TOL);
  BOOST_CHECK_SMALL((V_a_b.linear() - X_a_b.translation()).norm(), TOL);
  BOOST_CHECK_SMALL((V_a_c.angular() - rotationVelocity(X_a_c.rotation())).norm(), TOL);
  BOOST_CHECK_SMALL((V_a_c.linear() - X_a_c.translation()).norm(), TOL);

  MotionVecd V_b_c_a = transformError(X_a_b, X_a_c);
  Vector3d w_b_c_a = rotationError(X_a_b.rotation(), X_a_c.rotation());
  Vector3d v_b_c_a = X_a_c.translation() - X_a_b.translation();

  BOOST_CHECK_SMALL((V_b_c_a.angular() - w_b_c_a).norm(), TOL);
  BOOST_CHECK_SMALL((V_b_c_a.linear() - v_b_c_a).norm(), TOL);
}

BOOST_AUTO_TEST_CASE(sincTest)
{
  auto dummy_sinc = [](double x) { return std::sin(x) / x; };
  double eps = std::numeric_limits<double>::epsilon();

  // test equality between -1 and 1 (avoid 0)
  double t = -1.;
  const int nrIter = 333;
  for(int i = 0; i < nrIter; ++i)
  {
    BOOST_CHECK_EQUAL(dummy_sinc(t), sva::sinc(t));
    t += 2. / nrIter;
  }
  BOOST_CHECK(std::isnan(dummy_sinc(0.)));
  BOOST_CHECK_EQUAL(sva::sinc(0.), 1.);

  // not sure thoses test will work on all architectures
  BOOST_CHECK_EQUAL(dummy_sinc(eps), sva::sinc(eps));
  BOOST_CHECK_EQUAL(dummy_sinc(std::sqrt(eps)), sva::sinc(std::sqrt(eps)));
  BOOST_CHECK_EQUAL(dummy_sinc(std::sqrt(std::sqrt(eps))), sva::sinc(std::sqrt(std::sqrt(eps))));
}

BOOST_AUTO_TEST_CASE(sinc_invTest)
{
  auto dummy_sinc_inv = [](double x) { return x / std::sin(x); };
  double eps = std::numeric_limits<double>::epsilon();

  // test equality between -1 and 1 (avoid 0)
  double t = -1.;
  const int nrIter = 333;
  for(int i = 0; i < nrIter; ++i)
  {
    BOOST_CHECK_EQUAL(dummy_sinc_inv(t), sva::sinc_inv(t));
    t += 2. / nrIter;
  }
  BOOST_CHECK(std::isnan(dummy_sinc_inv(0.)));
  BOOST_CHECK_EQUAL(sva::sinc_inv(0.), 1.);

  // not sure thoses test will work on all architectures
  BOOST_CHECK_EQUAL(dummy_sinc_inv(eps), sva::sinc_inv(eps));
  BOOST_CHECK_EQUAL(dummy_sinc_inv(std::sqrt(eps)), sva::sinc_inv(std::sqrt(eps)));
  BOOST_CHECK_EQUAL(dummy_sinc_inv(std::sqrt(std::sqrt(eps))), sva::sinc_inv(std::sqrt(std::sqrt(eps))));
}

template<typename T>
inline Eigen::Vector3<T> oldRotationVelocity(const Eigen::Matrix3<T> & E_a_b, double prec)
{
  Eigen::Vector3<T> w;
  T acosV = (E_a_b(0, 0) + E_a_b(1, 1) + E_a_b(2, 2) - 1.) * 0.5;
  T theta = std::acos(acosV);

  if(E_a_b.isIdentity(prec))
  {
    w.setZero();
  }
  else
  {
    w = Eigen::Vector3<T>(-E_a_b(2, 1) + E_a_b(1, 2), -E_a_b(0, 2) + E_a_b(2, 0), -E_a_b(1, 0) + E_a_b(0, 1));
    w *= theta / (2. * std::sin(theta));
  }

  return w;
}

BOOST_AUTO_TEST_CASE(oldVsNewRotationVelocity)
{
  using namespace Eigen;
  using namespace sva;

  Matrix3d r1(RotZ(1.) * RotX(1.5) * RotZ(2.));
  Matrix3d r2(RotZ(1.043) * RotY(0.3422) * RotX(-0.30943));
  Matrix3d r3(RotX(-0.8348) * RotY(-0.2344) * RotZ(0.2344));
  Matrix3d r4(Matrix3d::Identity());

  BOOST_CHECK(oldRotationVelocity(r1, 1e-7).isApprox(rotationVelocity(r1)));
  BOOST_CHECK(oldRotationVelocity(r2, 1e-7).isApprox(rotationVelocity(r2)));
  BOOST_CHECK(oldRotationVelocity(r3, 1e-7).isApprox(rotationVelocity(r3)));
  BOOST_CHECK(oldRotationVelocity(r4, 1e-7).isApprox(rotationVelocity(r4)));
}