#include <fstream>
#include <boost/config.hpp>
#include <boost/version.hpp>
// CGAL headers
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Apollonius_graph_2.h>
#include <CGAL/Apollonius_graph_hierarchy_2.h>
#include <CGAL/Apollonius_graph_filtered_traits_2.h>
#include <CGAL/point_generators_2.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_real.hpp>

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

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/ApolloniusGraphGraphicsItem.h>
#include <CGAL/Qt/GraphicsViewCircleInput.h>

// for viewportsBbox
#include <CGAL/Qt/utility.h>

// the two base classes
#include "ui_Apollonius_graph_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

typedef CGAL::Simple_cartesian<double> K;
typedef K::Point_2 Point_2;
typedef K::Iso_rectangle_2 Iso_rectangle_2;

typedef CGAL::Apollonius_graph_filtered_traits_2<K,CGAL::Integral_domain_without_division_tag>  Gt;

typedef Gt::Point_2                           Point_2;
typedef K::Circle_2                           Circle_2;
typedef Gt::Site_2                            Apollonius_site_2;
typedef Gt::Site_2::Weight                    Weight;

typedef CGAL::Apollonius_graph_2<Gt> Apollonius;

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

private:
  Apollonius ag;
  QGraphicsScene scene;

  CGAL::Qt::ApolloniusGraphGraphicsItem<Apollonius,K> * agi;
  CGAL::Qt::GraphicsViewCircleInput<K> * ci;

public:
  MainWindow();

public Q_SLOTS:

  void processInput(CGAL::Object o);

  void on_actionInsertRandomPoints_triggered();

  void on_actionLoadPoints_triggered();

  void on_actionSavePoints_triggered();

  void on_actionClear_triggered();

  void on_actionRecenter_triggered();

  virtual void open(QString fileName);

Q_SIGNALS:
  void changed();
};

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

  this->graphicsView->setAcceptDrops(false);

  agi = new CGAL::Qt::ApolloniusGraphGraphicsItem<Apollonius, K>(&ag);

  QObject::connect(this, SIGNAL(changed()),
                   agi, SLOT(modelChanged()));

  agi->setSitesPen(QPen(Qt::red, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  agi->setEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(agi);

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

  ci = new CGAL::Qt::GraphicsViewCircleInput<K>(this, &scene);
  QObject::connect(ci, SIGNAL(generate(CGAL::Object)),
                   this, SLOT(processInput(CGAL::Object)));

  scene.installEventFilter(ci);

  //
  // Manual handling of actions
  //

  QObject::connect(this->actionQuit, SIGNAL(triggered()),
                   this, SLOT(close()));

  //
  // 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->transform().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_Apollonius_graph_2.html");
  this->addAboutCGAL();

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

void
MainWindow::processInput(CGAL::Object o)
{
  std::pair<Point_2, double> center_and_sr;
  if(CGAL::assign(center_and_sr, o)){
    ag.insert(Apollonius_site_2(center_and_sr.first, sqrt(center_and_sr.second)));
    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_actionClear_triggered()
{
  ag.clear();
  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);
  double width = isor.xmax() - isor.xmin();

  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<Apollonius_site_2> points;
  points.reserve(number_of_points);
  boost::rand48 rng;
  boost::uniform_real<> dist(0.005*width, 0.05*width);
  boost::variate_generator<boost::rand48&, boost::uniform_real<> > radius(rng,dist);

  for(int i = 0; i < number_of_points; ++i){
    points.push_back(Apollonius_site_2(*pg++,radius()));
  }
      ag.insert(points.begin(), points.end());
  // default cursor
  QApplication::restoreOverrideCursor();
  Q_EMIT( changed());
}

void
MainWindow::on_actionLoadPoints_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
                                                  tr("Open Points file"),
                                                  ".",
                                                  tr("CGAL files (*.wpts.cgal);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files (*.wkt *.WKT);;"
                                                   #endif
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{

  QApplication::setOverrideCursor(Qt::WaitCursor);
  if(! fileName.isEmpty()){
    std::ifstream ifs(qPrintable(fileName));

    std::vector<Apollonius_site_2> points;
    if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
    {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
      std::vector<K::Point_3> point_3_s;
      CGAL::read_multi_point_WKT(ifs, point_3_s);
      for(const K::Point_3& point_3 : point_3_s)
      {
        points.push_back(Apollonius_site_2(K::Point_2(point_3.x(), point_3.y()), point_3.z()));
      }
#endif
    } else{
      K::Weighted_point_2 p;
      while(ifs >> p) {
        points.push_back(Apollonius_site_2(p.point(),p.weight()));
      }
    }
    ag.insert(points.begin(), points.end());
    this->addToRecentFiles(fileName);
    actionRecenter->trigger();
    Q_EMIT( changed());
  }
  QApplication::restoreOverrideCursor();
}

void
MainWindow::on_actionSavePoints_triggered()
{
  QString fileName = QFileDialog::getSaveFileName(this,
                                                  tr("Save points"),
                                                  ".reg.cgal",
                                                  tr("Weighted Points (*.wpts.cgal);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files(*.wkt *.WKT);;"
                                                   #endif
                                                     "All (*)"));
  if(! fileName.isEmpty()){
    std::ofstream ofs(qPrintable(fileName));
    if(fileName.endsWith(".wkt",Qt::CaseInsensitive))
    {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
      std::vector<K::Point_3> points;
      for(Apollonius::Sites_iterator
          vit = ag.sites_begin(),
          end = ag.sites_end();
          vit!= end; ++vit)
      {
        points.push_back(K::Point_3(vit->point().x(),
                                    vit->point().y(),
                                    vit->weight()));
      }
      CGAL::write_multi_point_WKT(ofs, points);
#endif
    }
    else
      for(Apollonius::Sites_iterator
          vit = ag.sites_begin(),
          end = ag.sites_end();
          vit!= end; ++vit)
      {
        ofs << vit->point()<<" "<<vit->weight()<<std::endl;
      }
  }
}

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

#include "Apollonius_graph_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("Apollonius_graph_2 demo");

  // Import resources from libCGAL (Qt5).
  // See https://doc.qt.io/qt-5/qdir.html#Q_INIT_RESOURCE
  CGAL_QT_INIT_RESOURCES;
  Q_INIT_RESOURCE(Apollonius_graph_2);

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}


//
// Created by Yohsuke Murase on 2020/06/04.
//

#include <iostream>
#include <ostream>
#include <vector>
#include <array>
#include <random>
#include <cassert>
#include <mpi.h>
#include <Eigen/Dense>
#include <fstream>
#include "StrategyN3M5.hpp"

// calculate the distribution of fixation probability rho
// against randomly selected N2M3 deterministic strategies

std::array<double,3> CalcPayoffs(const std::array<double,StrategyN3M5::N>& stationary_state, double benefit) {
  const double cost = 1.0;
  std::array<double,3> ans = {0.0, 0.0, 0.0};
  for (size_t i = 0; i < StrategyN3M5::N; i++) {
    StateN3M5 s(i);
    double pa = 0.0, pb = 0.0, pc = 0.0;
    if (s.ha[0] == false) { pa -= cost; pb += benefit / 2.0; pc += benefit / 2.0; }
    if (s.hb[0] == false) { pb -= cost; pa += benefit / 2.0; pc += benefit / 2.0; }
    if (s.hc[0] == false) { pc -= cost; pa += benefit / 2.0; pb += benefit / 2.0; }
    ans[0] += stationary_state[i] * pa;
    ans[1] += stationary_state[i] * pb;
    ans[2] += stationary_state[i] * pc;
  }
  return ans;
}

double FixationProb(size_t N, double sigma, double e, double benefit, const StrategyN3M5 &res, const StrategyN3M5 &mut, double s_yyy) {
  auto a_xxx = mut.StationaryState(e);
  auto a_xxy = mut.StationaryState(e, &mut, &res);
  auto a_xyy = mut.StationaryState(e, &res, &res);

  double s_xxx = CalcPayoffs(a_xxx, benefit)[0];
  auto _xxy = CalcPayoffs(a_xxy, benefit);
  double s_xxy = _xxy[0];
  double s_yxx = _xxy[2];
  auto _xyy = CalcPayoffs(a_xyy, benefit);
  double s_xyy = _xyy[0];
  double s_yyx = _xyy[1];

  // rho_inv = \sum_{i=0}^{N-1} exp(sigma[S]),
  // where S is defined as
  // S =  i/6(i^2−3iN+6i+3N^2−12N+11)s_{yyy}
  //     −i/6(i+1)(2i−3N+4)s_{yyx}
  //     +i/6(i−1)(i+1)s_{yxx}
  //     −i/6(i^2−3i(N−1)+3N^2−6N+2)s_{xyy}
  //     +i/6(i−1)(2i−3N+2))s_{xxy}
  //     −i/6(i^2−3i+2)s_{xxx}

  double num_games = (N-1) * (N-2) / 2.0;
  double rho_inv = 0.0;
  for (int i=0; i < N; i++) {
    double x = sigma / num_games * (i / 6.0) * (
        (i*i - 3.0*i*N + 6.0*i + 3.0*N*N - 12.0*N + 11.0) * s_yyy
            - (i+1.0) * (2.0*i - 3.0*N + 4) * s_yyx
            + (i-1.0) * (i+1.0) * s_yxx
            - (i*i - 3.0*i*(N-1.0) + 3.0*N*N - 6.0*N + 2.0) * s_xyy
            + (i-1.0) * (2.0*i-3.0*N+2.0) * s_xxy
            - (i*i - 3.0*i + 2.0) * s_xxx
    );
    rho_inv += std::exp(x);
  }

  return 1.0 / rho_inv;
}

std::bitset<StrategyN3M5::N> DrawRandomBit32768(std::mt19937_64 & rnd) {
  std::uniform_int_distribution<uint64_t > dist(0, std::numeric_limits<uint64_t>::max() );

  std::bitset<StrategyN3M5::N> ans;
  int rep = StrategyN3M5::N / 64;
  assert(StrategyN3M5::N % 64 == 0);
  for (int i = 0; i < rep; i++) {
    std::bitset<StrategyN3M5::N> mask = dist(rnd);
    mask <<= (64 * i);
    ans |= mask;
  }
  return ans;
}

int main(int argc, char *argv[]) {
  MPI_Init(&argc, &argv);
  Eigen::initParallel();
  if( argc != 8 ) {
    std::cerr << "Error : invalid argument" << std::endl;
    std::cerr << "  Usage : " << argv[0] << " <N> <sigma> <e> <benefit> <resident 0:caprin, +:num_resident_samples> <num_mutants> <seed>" << std::endl;
    MPI_Finalize();
    return 1;
  }

  size_t N = std::strtoul(argv[1], nullptr,0);
  double sigma = std::strtod(argv[2], nullptr);
  double e = std::strtod(argv[3], nullptr);
  double benefit = std::strtod(argv[4], nullptr);

  long n_resident = std::strtol(argv[5], nullptr, 0);
  long n_mutants = std::strtol(argv[6], nullptr, 0);
  int seed = std::strtol(argv[7], nullptr, 0);

  const size_t NUM_BINS = 1000;
  std::vector<size_t> counts(NUM_BINS, 0ul);
  size_t robust_count = 0ul;

  int my_rank = 0;
  MPI_Comm_rank(MPI_COMM_WORLD, &my_rank);
  int num_procs = 0;
  MPI_Comm_size(MPI_COMM_WORLD, &num_procs);

  if (n_resident > 0) {
    {
      std::seed_seq seq = {seed, my_rank};
      std::mt19937_64 rnd(seq);
      size_t my_n_resident = n_resident / num_procs;
      if (my_rank < n_resident % num_procs) { my_n_resident++; }
      // std::cerr << "my_n_resident: " << my_n_resident << " " << my_rank << ' ' << th << std::endl;

      for (size_t i = 0; i < my_n_resident; i++) {
        std::cerr << "i: " << i << " " << my_rank << std::endl;
        StrategyN3M5 res( DrawRandomBit32768(rnd) );
        auto a_yyy = res.StationaryState(e);
        double s_yyy = CalcPayoffs(a_yyy, benefit)[0];
        for (size_t j = 0; j < n_mutants; j++) {
          std::cerr << "j: " << j << " " << my_rank << std::endl;
          StrategyN3M5 mut( DrawRandomBit32768(rnd) );
          double rho = FixationProb(N, sigma, e, benefit, res, mut, s_yyy);
          std::cerr << "  rho: " << rho << std::endl;
          size_t b = static_cast<size_t>(rho * NUM_BINS);
          counts[b]++;
          if (rho <= 1.0 / N) { robust_count++; }
        }
      }
    }
  }
  else {
    {
      std::seed_seq seq = {seed, my_rank};
      std::mt19937_64 rnd(seq);

      StrategyN3M5 res = StrategyN3M5::CAPRI3();
      auto a_yyy = res.StationaryState(e);
      double s_yyy = CalcPayoffs(a_yyy, benefit)[0];

      size_t my_n_mutants = n_mutants / num_procs;
      if (my_rank < n_mutants % num_procs) { my_n_mutants++; }
      //std::cerr << "my_n_mutants: " << my_n_mutants << ' ' << my_rank << ' ' << th << std::endl;

      for (size_t j = 0; j < my_n_mutants; j++) {
        StrategyN3M5 mut( DrawRandomBit32768(rnd) );
        double rho = FixationProb(N, sigma, e, benefit, res, mut, s_yyy);
        size_t b = static_cast<size_t>(rho * NUM_BINS);
        counts[b] += 1;
        if (rho <= 1.0 / N) { robust_count++; }
      }
    }
  }

  // reduce counts
  std::vector<size_t> all_counts(NUM_BINS, 0ul);
  size_t all_robust_count = 0ul;
  MPI_Reduce(counts.data(), all_counts.data(), all_counts.size(), MPI_UNSIGNED_LONG, MPI_SUM, 0, MPI_COMM_WORLD);
  MPI_Reduce(&robust_count, &all_robust_count, 1, MPI_UNSIGNED_LONG, MPI_SUM, 0, MPI_COMM_WORLD);

  // print counts
  if (my_rank == 0) {
    std::ofstream fout("dist.dat");
    double dx = 1.0 / NUM_BINS;
    double total = (n_resident == 0) ? n_mutants : n_resident * n_mutants;
    for (size_t i = 0; i < NUM_BINS; i++) {
      fout << i * dx << ' ' << (double)all_counts[i]/total << std::endl;
    }

    std::cerr << "robust_count/total: " << all_robust_count << " / " << total << " : " << (double)all_robust_count/total << std::endl;
  }

  MPI_Finalize();
  return 0;
}


//
// Created by xiang on 1/4/18.
// this program shows how to perform direct bundle adjustment
//
#include <iostream>

using namespace std;

#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

#include <Eigen/Core>
#include <sophus/se3.hpp>
#include <opencv2/opencv.hpp>

#include <pangolin/pangolin.h>
#include <boost/format.hpp>

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> VecSE3;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVec3d;

// global variables
string pose_file = "../poses.txt";
string points_file = "../points.txt";

// intrinsics
float fx = 277.34;
float fy = 291.402;
float cx = 312.234;
float cy = 239.777;

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
            (1 - xx) * (1 - yy) * data[0] +
            xx * (1 - yy) * data[1] +
            (1 - xx) * yy * data[img.step] +
            xx * yy * data[img.step + 1]
    );
}

// g2o vertex that use sophus::SE3 as pose
class VertexSophus : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    VertexSophus() {}

    ~VertexSophus() {}

    bool read(std::istream &is) {}

    bool write(std::ostream &os) const {}

    virtual void setToOriginImpl() {
        _estimate = Sophus::SE3d();
    }

    virtual void oplusImpl(const double *update_) {
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> update(update_);
        setEstimate(Sophus::SE3d::exp(update) * estimate());
    }
};

// TODO edge of projection error, implement it
// 16x1 error, which is the errors in patch
typedef Eigen::Matrix<double,16,1> Vector16d;
class EdgeDirectProjection : public g2o::BaseBinaryEdge<16, Vector16d, g2o::VertexSBAPointXYZ, VertexSophus> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeDirectProjection(float *color, cv::Mat &target) {
        this->origColor = color;
        this->targetImg = target;
    }

    ~EdgeDirectProjection() {}

    virtual void computeError() override {
        // TODO START YOUR CODE HERE
        // compute projection error ...
        const g2o::VertexSBAPointXYZ *vertexPw = static_cast<const g2o::VertexSBAPointXYZ * >(vertex(0));
        const VertexSophus *vertexTcw = static_cast<const VertexSophus * >(vertex(1));
        Eigen::Vector3d p_nsp = vertexTcw->estimate() * vertexPw->estimate();
        p_nsp /= p_nsp[2];
        double u = p_nsp[0]*fx+cx;
        double v = p_nsp[1]*fy+cy;
        if (u-2>=0 && v-2>=0 &&u+1<=targetImg.cols-1 && v+1<=targetImg.rows-1){
            int index=0;
            for (int du=-2;du<=1;du++){
                for (int dv=-2;dv<=1;dv++)
                {
                    _error[index] = origColor[index]-GetPixelValue(targetImg, u+du, v+dv);
                    index++;
                }
            }
        }
        else{
            this->setLevel(1);
            for(int i=0;i<16;i++) _error[i]=0;
        }
        // END YOUR CODE HERE
    }

    // Let g2o compute jacobian for you

    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}

private:
    cv::Mat targetImg;  // the target image
    float *origColor = nullptr;   // 16 floats, the color of this point
};

// plot the poses and points for you, need pangolin
void Draw(const VecSE3 &poses, const VecVec3d &points);

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

    // read poses and points
    VecSE3 poses;
    VecVec3d points;
    ifstream fin(pose_file);

    while (!fin.eof()) {
        double timestamp = 0;
        fin >> timestamp;
        if (timestamp == 0) break;
        double data[7];
        for (auto &d: data) fin >> d;
        poses.push_back(Sophus::SE3d(
                Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
                Eigen::Vector3d(data[0], data[1], data[2])
        ));
        if (!fin.good()) break;
    }
    fin.close();

    vector<float *> color;
    fin.open(points_file);
    while (!fin.eof()) {
        double xyz[3] = {0};
        for (int i = 0; i < 3; i++) fin >> xyz[i];
        if (xyz[0] == 0) break;
        points.push_back(Eigen::Vector3d(xyz[0], xyz[1], xyz[2]));
        float *c = new float[16];
        for (int i = 0; i < 16; i++) fin >> c[i];
        color.push_back(c);

        if (fin.good() == false) break;
    }
    fin.close();

    cout << "poses: " << poses.size() << ", points: " << points.size() << endl;

    // read images
    vector<cv::Mat> images;
    boost::format fmt("../%d.png");
    for (int i = 0; i < 7; i++) {
        images.push_back(cv::imread((fmt % i).str(), 0));
    }

    // build optimization problem
    // typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> DirectBlock;  // 求解的向量是6＊1的
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // 求解的向量是6＊1的
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    // TODO add vertices, edges into the graph optimizer
    // START YOUR CODE HERE
    vector<VertexSophus *> vertex_poses;
    for (int i=0; i<poses.size(); i++)
    {
        VertexSophus *v = new VertexSophus();
        v->setId(i);
        v->setEstimate(poses[i]);
        optimizer.addVertex(v);
        vertex_poses.push_back(v);
    }

    vector<g2o::VertexSBAPointXYZ *> vertex_points;
    for (int i=0;i<points.size();i++){
        g2o::VertexSBAPointXYZ *v = new g2o::VertexSBAPointXYZ();
        v->setId(i+poses.size());
        v->setEstimate(points[i]);

        v->setMarginalized(true);
        optimizer.addVertex(v);
        vertex_points.push_back(v);
    }

    for(int i=0;i<vertex_poses.size();i++){
        for (int j=0;j<vertex_points.size();j++)
        {
            EdgeDirectProjection *edge = new EdgeDirectProjection(color[j], images[i]);
            edge->setVertex(0, vertex_points[j]);
            edge->setVertex(1, vertex_poses[i]);
            edge->setInformation(Eigen::Matrix<double, 16, 16>::Identity());
            edge->setRobustKernel(new g2o::RobustKernelHuber());
            optimizer.addEdge(edge);
        }
    }

    // END YOUR CODE HERE

    // perform optimization
    optimizer.initializeOptimization(0);
    optimizer.optimize(200);

    // TODO fetch data from the optimizer
    // START YOUR CODE HERE
    for (int i=0;i<poses.size();i++){
        poses[i] = vertex_poses[i]->estimate();
    }
    for (int i=0;i<points.size();i++){
        points[i] = vertex_points[i]->estimate();
    }
    // END YOUR CODE HERE

    // plot the optimized points and poses
    Draw(poses, points);

    // delete color data
    for (auto &c: color) delete[] c;
    return 0;
}

void Draw(const VecSE3 &poses, const VecVec3d &points) {
    if (poses.empty() || points.empty()) {
        cerr << "parameter is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

        // draw poses
        float sz = 0.1;
        int width = 640, height = 480;
        for (auto &Tcw: poses) {
            glPushMatrix();
            Sophus::Matrix4f m = Tcw.inverse().matrix().cast<float>();
            glMultMatrixf((GLfloat *) m.data());
            glColor3f(1, 0, 0);
            glLineWidth(2);
            glBegin(GL_LINES);
            glVertex3f(0, 0, 0);
            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
            glVertex3f(0, 0, 0);
            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(0, 0, 0);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(0, 0, 0);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
            glEnd();
            glPopMatrix();
        }

        // points
        glPointSize(2);
        glBegin(GL_POINTS);
        for (size_t i = 0; i < points.size(); i++) {
            glColor3f(0.0, points[i][2]/4, 1.0-points[i][2]/4);
            glVertex3d(points[i][0], points[i][1], points[i][2]);
        }
        glEnd();

        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
}


#include "Day13-ShuttleSearch.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>

#include <vector>
#include <cmath>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2020
{
namespace Day13
{

struct BusDeparture
{
    int busID;
    int offset;
};

std::vector<BusDeparture> parseScheduleDescription(const std::string& scheduleDescriptionString)
{
    std::vector<std::string> tokens;
    boost::split(tokens, scheduleDescriptionString, boost::is_any_of(","));

    std::vector<BusDeparture> busDepartures;
    for (int i = 0; i < tokens.size(); ++i)
    {
        if (tokens.at(i) == "x")
        {
            continue;
        }
        const int busID = std::stoi(tokens.at(i));
        const int offset = i % busID;
        BusDeparture busDeparture{busID, offset};
        busDepartures.push_back(std::move(busDeparture));
    }

    return busDepartures;
}

int64_t getWaitTime(int64_t earliestDepartureTimestamp, int busID)
{
    int64_t earliestDepartureTimeViaThisBus = (earliestDepartureTimestamp / busID) * busID;
    if (earliestDepartureTimeViaThisBus < earliestDepartureTimestamp)
    {
        earliestDepartureTimeViaThisBus += busID;
    }
    return earliestDepartureTimeViaThisBus - earliestDepartureTimestamp;
}

int earliestBusIDMultipliedByWaitTime(int earliestDepartureTimestamp, const std::string& scheduleDescriptionString)
{
    std::vector<BusDeparture> busDepartures = parseScheduleDescription(scheduleDescriptionString);

    const auto minWaitTimeBusDepartureIter = std::min_element(busDepartures.cbegin(), busDepartures.cend(), [earliestDepartureTimestamp](const auto& lhs, const auto& rhs)
                                                              {
                                                                  return getWaitTime(earliestDepartureTimestamp, lhs.busID) < getWaitTime(earliestDepartureTimestamp, rhs.busID);
                                                              });

    const int minWaitTimeBusID = minWaitTimeBusDepartureIter->busID;

    return getWaitTime(earliestDepartureTimestamp, minWaitTimeBusID) * minWaitTimeBusID;
}

int64_t earliestTimestampWithMatchingDepartures(const std::string& scheduleDescriptionString)
{
    std::vector<BusDeparture> busDepartures = parseScheduleDescription(scheduleDescriptionString);

    int64_t delta = 1;
    int64_t candidate = 0;

    for (const auto& busDeparture : busDepartures)
    {
        const int busID = busDeparture.busID;
        const int offset = busDeparture.offset;
        while (getWaitTime(candidate, busID) != offset)
        {
            candidate += delta;
        }
        delta *= busID;
    }

    return candidate;
}

}
}
}


#include <shift/math/vector.hpp>
//#include <shift/math/matrix.hpp>
//#include <shift/core/algorithm.hpp>
#include <shift/core/boost_disable_warnings.hpp>
#include <boost/test/unit_test.hpp>
#include <shift/core/boost_restore_warnings.hpp>

using namespace shift::math;

template <typename T>
using vector5 = vector<5, T>;

BOOST_AUTO_TEST_CASE(vector_details)
{
  BOOST_CHECK_EQUAL(detail::components_count_v<float>, 1);
  BOOST_CHECK_EQUAL(detail::components_count_v<vector2<float>>, 2);
  BOOST_CHECK_EQUAL(detail::components_count_v<vector3<float>>, 3);
  BOOST_CHECK_EQUAL(detail::components_count_v<vector4<float>>, 4);

  BOOST_CHECK((std::is_same_v<detail::select_type_t<float>, float>));
  BOOST_CHECK((std::is_same_v<detail::select_type_t<int, float>, float>));
  BOOST_CHECK(
    (std::is_same_v<detail::select_type_t<int, float, double>, double>));
  BOOST_CHECK(
    (std::is_same_v<detail::select_type_t<std::uint32_t, std::int16_t>,
                    std::uint32_t>));
  BOOST_CHECK(
    (std::is_same_v<detail::select_type_t<std::int32_t, std::uint16_t>,
                    std::int32_t>));
}

BOOST_AUTO_TEST_CASE(vector_is_trivial)
{
  static_assert(std::is_trivial_v<vector2<float>>);
  static_assert(std::is_trivial_v<vector3<float>>);
  static_assert(std::is_trivial_v<vector4<float>>);
  BOOST_CHECK(std::is_trivial_v<vector2<float>>);
  BOOST_CHECK(std::is_trivial_v<vector3<float>>);
  BOOST_CHECK(std::is_trivial_v<vector4<float>>);
}

BOOST_AUTO_TEST_CASE(vector_constexpr_construction)
{
  constexpr vector2<float> v2f1{1.0f, 2.0f};
  constexpr vector2<float> v2f2(1.0f, 2.0f);
  constexpr vector2<float> v2f3 = {1.0f, 2.0f};
  constexpr vector2<float> v2f4{v2f1};
  constexpr vector2<float> v2f5(v2f2);
  constexpr vector2<float> v2f6 = v2f3;

  BOOST_STATIC_ASSERT(v2f1.x == 1.0f);
  BOOST_STATIC_ASSERT(v2f1.y == 2.0f);
  BOOST_STATIC_ASSERT(v2f2.x == 1.0f);
  BOOST_STATIC_ASSERT(v2f2.y == 2.0f);
  BOOST_STATIC_ASSERT(v2f3.x == 1.0f);
  BOOST_STATIC_ASSERT(v2f3.y == 2.0f);
  BOOST_STATIC_ASSERT(v2f4.x == 1.0f);
  BOOST_STATIC_ASSERT(v2f4.y == 2.0f);
  BOOST_STATIC_ASSERT(v2f5.x == 1.0f);
  BOOST_STATIC_ASSERT(v2f5.y == 2.0f);
  BOOST_STATIC_ASSERT(v2f6.x == 1.0f);
  BOOST_STATIC_ASSERT(v2f6.y == 2.0f);
}

BOOST_AUTO_TEST_CASE(vector_dynamic_construction)
{
  vector2<float> v2f1{1.0f, 2.0f};
  vector2<float> v2f2(1.0f, 2.0f);
  vector2<float> v2f3 = {1.0f, 2.0f};
  vector2<float> v2f4{v2f1};
  vector2<float> v2f5(v2f2);
  vector2<float> v2f6 = v2f3;

  BOOST_CHECK_EQUAL(v2f1.x, 1.0f);
  BOOST_CHECK_EQUAL(v2f1.y, 2.0f);
  BOOST_CHECK_EQUAL(v2f2.x, 1.0f);
  BOOST_CHECK_EQUAL(v2f2.y, 2.0f);
  BOOST_CHECK_EQUAL(v2f3.x, 1.0f);
  BOOST_CHECK_EQUAL(v2f3.y, 2.0f);
  BOOST_CHECK_EQUAL(v2f4.x, 1.0f);
  BOOST_CHECK_EQUAL(v2f4.y, 2.0f);
  BOOST_CHECK_EQUAL(v2f5.x, 1.0f);
  BOOST_CHECK_EQUAL(v2f5.y, 2.0f);
  BOOST_CHECK_EQUAL(v2f6.x, 1.0f);
  BOOST_CHECK_EQUAL(v2f6.y, 2.0f);
}

BOOST_AUTO_TEST_CASE(vector_accessors)
{
  auto v2f = make_vector_from(1.0f, 2.0f);
  BOOST_CHECK_EQUAL(v2f(0), 1.0f);
  BOOST_CHECK_EQUAL(v2f(1), 2.0f);
  BOOST_CHECK_EQUAL(v2f.x, 1.0f);
  BOOST_CHECK_EQUAL(v2f.y, 2.0f);

  auto v3f = make_vector_from(1.0f, 2.0f, 3.0f);
  BOOST_CHECK_EQUAL(v3f(0), 1.0f);
  BOOST_CHECK_EQUAL(v3f(1), 2.0f);
  BOOST_CHECK_EQUAL(v3f(2), 3.0f);
  BOOST_CHECK_EQUAL(v3f.x, 1.0f);
  BOOST_CHECK_EQUAL(v3f.y, 2.0f);
  BOOST_CHECK_EQUAL(v3f.z, 3.0f);

  auto v4f = make_vector_from(1.0f, 2.0f, 3.0f, 4.0f);
  BOOST_CHECK_EQUAL(v4f(0), 1.0f);
  BOOST_CHECK_EQUAL(v4f(1), 2.0f);
  BOOST_CHECK_EQUAL(v4f(2), 3.0f);
  BOOST_CHECK_EQUAL(v4f(3), 4.0f);
  BOOST_CHECK_EQUAL(v4f.x, 1.0f);
  BOOST_CHECK_EQUAL(v4f.y, 2.0f);
  BOOST_CHECK_EQUAL(v4f.z, 3.0f);
  BOOST_CHECK_EQUAL(v4f.w, 4.0f);
}

BOOST_AUTO_TEST_CASE(vector_construction_using_initializer_list)
{
  vector3<float> v = {1.0f, 2.0f, 3.0f};
  BOOST_CHECK_EQUAL(v.x, 1.0f);
  BOOST_CHECK_EQUAL(v.y, 2.0f);
  BOOST_CHECK_EQUAL(v.z, 3.0f);
}

BOOST_AUTO_TEST_CASE(vector_explicit_construction)
{
  vector4<float> v(1.0f, 2.0f, 3.0f, 4.0f);
  BOOST_CHECK_EQUAL(v.x, 1.0f);
  BOOST_CHECK_EQUAL(v.y, 2.0f);
  BOOST_CHECK_EQUAL(v.z, 3.0f);
  BOOST_CHECK_EQUAL(v.w, 4.0f);
}

BOOST_AUTO_TEST_CASE(vector_make_construction)
{
  auto v1 = make_vector_from(1.0f, 2.0, 3.0f);
  BOOST_STATIC_ASSERT((std::is_same_v<decltype(v1)::value_type, double>));
  BOOST_CHECK_EQUAL(v1.x, 1.0);
  BOOST_CHECK_EQUAL(v1.y, 2.0);
  BOOST_CHECK_EQUAL(v1.z, 3.0);

  auto v2 = make_vector_from(make_vector_from(1.0f, 2.0f), 3.0);
  BOOST_STATIC_ASSERT((std::is_same_v<decltype(v2)::value_type, double>));
  BOOST_CHECK_EQUAL(v2.x, 1.0);
  BOOST_CHECK_EQUAL(v2.y, 2.0);
  BOOST_CHECK_EQUAL(v2.z, 3.0);

  auto v3 = make_vector_from(1.0, make_vector_from(2.0f, 3.0f));
  BOOST_STATIC_ASSERT((std::is_same_v<decltype(v3)::value_type, double>));
  BOOST_CHECK_EQUAL(v3.x, 1.0);
  BOOST_CHECK_EQUAL(v3.y, 2.0);
  BOOST_CHECK_EQUAL(v3.z, 3.0);
}

BOOST_AUTO_TEST_CASE(vector_copy_construction)
{
  auto v1 = make_vector_from(1.0f, 2.0f, 3.0f, 4.0f);
  vector4<float> v2(v1);
  BOOST_CHECK_EQUAL(v2.x, 1.0f);
  BOOST_CHECK_EQUAL(v2.y, 2.0f);
  BOOST_CHECK_EQUAL(v2.z, 3.0f);
  BOOST_CHECK_EQUAL(v2.w, 4.0f);
}

BOOST_AUTO_TEST_CASE(vector_carray_construction)
{
  const float values[4] = {1.0f, 2.0f, 3.0f, 4.0f};
  auto v1 = make_vector_from(values);
  BOOST_CHECK_EQUAL(v1.x, 1.0f);
  BOOST_CHECK_EQUAL(v1.y, 2.0f);
  BOOST_CHECK_EQUAL(v1.z, 3.0f);
  BOOST_CHECK_EQUAL(v1.w, 4.0f);
}

BOOST_AUTO_TEST_CASE(vector_array_construction)
{
  const std::array<float, 4> values{{1.0f, 2.0f, 3.0f, 4.0f}};
  auto v1 = make_vector_from(values);
  BOOST_CHECK_EQUAL(v1.x, 1.0f);
  BOOST_CHECK_EQUAL(v1.y, 2.0f);
  BOOST_CHECK_EQUAL(v1.z, 3.0f);
  BOOST_CHECK_EQUAL(v1.w, 4.0f);
}

BOOST_AUTO_TEST_CASE(vector_arithmetic_operators)
{
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = make_vector_from(1.0f, 2.0f, 3.0f);
    auto v3 = v1 + v2;
    BOOST_CHECK_EQUAL(v3.x, 5.0f);
    BOOST_CHECK_EQUAL(v3.y, 7.0f);
    BOOST_CHECK_EQUAL(v3.z, 9.0f);
  }
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = make_vector_from(1.0f, 2.0f, 3.0f);
    auto v3 = v1 - v2;
    BOOST_CHECK_EQUAL(v3.x, 3.0f);
    BOOST_CHECK_EQUAL(v3.y, 3.0f);
    BOOST_CHECK_EQUAL(v3.z, 3.0f);
  }
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = make_vector_from(1.0f, 2.0f, 3.0f);
    auto v3 = v1 * v2;
    BOOST_CHECK_EQUAL(v3.x, 4.0f);
    BOOST_CHECK_EQUAL(v3.y, 10.0f);
    BOOST_CHECK_EQUAL(v3.z, 18.0f);
  }
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = make_vector_from(1.0f, 2.0f, 3.0f);
    auto v3 = v1 / v2;
    BOOST_CHECK_EQUAL(v3.x, 4.0f);
    BOOST_CHECK_EQUAL(v3.y, 2.5f);
    BOOST_CHECK_EQUAL(v3.z, 2.0f);
  }
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = v1 * 2.0f;
    BOOST_CHECK_EQUAL(v2.x, 8.0f);
    BOOST_CHECK_EQUAL(v2.y, 10.0f);
    BOOST_CHECK_EQUAL(v2.z, 12.0f);
  }
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = 2.0f * v1;
    BOOST_CHECK_EQUAL(v2.x, 8.0f);
    BOOST_CHECK_EQUAL(v2.y, 10.0f);
    BOOST_CHECK_EQUAL(v2.z, 12.0f);
  }
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = v1 / 2.0f;
    BOOST_CHECK_EQUAL(v2.x, 2.0f);
    BOOST_CHECK_EQUAL(v2.y, 2.5f);
    BOOST_CHECK_EQUAL(v2.z, 3.0f);
  }
}

BOOST_AUTO_TEST_CASE(vector_arithmetic_assignment_operators)
{
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = make_vector_from(1.0f, 2.0f, 3.0f);
    v1 += v2;
    BOOST_CHECK_EQUAL(v1.x, 5.0f);
    BOOST_CHECK_EQUAL(v1.y, 7.0f);
    BOOST_CHECK_EQUAL(v1.z, 9.0f);
  }
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = make_vector_from(1.0f, 2.0f, 3.0f);
    v1 -= v2;
    BOOST_CHECK_EQUAL(v1.x, 3.0f);
    BOOST_CHECK_EQUAL(v1.y, 3.0f);
    BOOST_CHECK_EQUAL(v1.z, 3.0f);
  }
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = make_vector_from(1.0f, 2.0f, 3.0f);
    v1 *= v2;
    BOOST_CHECK_EQUAL(v1.x, 4.0f);
    BOOST_CHECK_EQUAL(v1.y, 10.0f);
    BOOST_CHECK_EQUAL(v1.z, 18.0f);
  }
  {
    auto v1 = make_vector_from(4.0f, 5.0f, 6.0f);
    auto v2 = make_vector_from(1.0f, 2.0f, 3.0f);
    v1 /= v2;
    BOOST_CHECK_EQUAL(v1.x, 4.0f);
    BOOST_CHECK_EQUAL(v1.y, 2.5f);
    BOOST_CHECK_EQUAL(v1.z, 2.0f);
  }
  {
    auto v = make_vector_from(4.0f, 5.0f, 6.0f);
    v *= 2.0f;
    BOOST_CHECK_EQUAL(v.x, 8.0f);
    BOOST_CHECK_EQUAL(v.y, 10.0f);
    BOOST_CHECK_EQUAL(v.z, 12.0f);
  }
  {
    auto v = make_vector_from(4.0f, 5.0f, 6.0f);
    v /= 2.0f;
    BOOST_CHECK_EQUAL(v.x, 2.0f);
    BOOST_CHECK_EQUAL(v.y, 2.5f);
    BOOST_CHECK_EQUAL(v.z, 3.0f);
  }
}

BOOST_AUTO_TEST_CASE(vector_swizzle)
{
  auto v1 = make_vector_from(1.0f, 2.0f, 3.0f);
  auto v2 = swizzle<2, 1, 0>(v1);
  BOOST_CHECK_EQUAL(v2.x, v1.z);
  BOOST_CHECK_EQUAL(v2.y, v1.y);
  BOOST_CHECK_EQUAL(v2.z, v1.x);

  auto v3 = swizzle<1, 2, 1, 0>(v1);
  BOOST_CHECK_EQUAL(v3.x, v1.y);
  BOOST_CHECK_EQUAL(v3.y, v1.z);
  BOOST_CHECK_EQUAL(v3.z, v1.y);
  BOOST_CHECK_EQUAL(v3.w, v1.x);
}

BOOST_AUTO_TEST_CASE(vector_norm)
{
  using std::abs;
  using std::norm;
  {
    const auto v1 = make_vector_from(1.0f, 2.0f, 3.0f);
    const auto v2 = make_vector_from(1, 2, 3);
    BOOST_CHECK_EQUAL(norm(v1), 14.0f);
    BOOST_CHECK_EQUAL(norm(v2), 14);
  }
}

BOOST_AUTO_TEST_CASE(vector_abs)
{
  using std::abs;
  using std::norm;
  {
    const auto v1 = make_vector_from(3.0f, 4.0f, 12.0f);
    const auto v2 = make_vector_from(3, 4, 12);
    BOOST_CHECK_EQUAL(abs(v1), 13.0f);
    BOOST_CHECK_EQUAL(abs(v2), 13.0);
  }
}

BOOST_AUTO_TEST_CASE(vector_std_min_max)
{
  const auto v1 = make_vector_from(1.0f, 0.0f, 0.0f);
  const auto v2 = make_vector_from(0.0f, 2.0f, 0.0f);
  BOOST_CHECK_EQUAL(std::min(v1, v2), v1);
  BOOST_CHECK_EQUAL(std::min(v2, v1), v1);
  BOOST_CHECK_EQUAL(std::max(v1, v2), v2);
  BOOST_CHECK_EQUAL(std::max(v2, v1), v2);
}

BOOST_AUTO_TEST_CASE(vector_math_min_max)
{
  const auto v1 = make_vector_from(1.0f, 5.0f, 3.0f);
  const auto v2 = make_vector_from(2.0f, 4.0f, 6.0f);
  BOOST_CHECK_EQUAL(min(v1), 1.0f);
  BOOST_CHECK_EQUAL(max(v1), 5.0f);
  BOOST_CHECK_EQUAL(min(v2), 2.0f);
  BOOST_CHECK_EQUAL(max(v2), 6.0f);
  BOOST_CHECK_EQUAL(min(v1, v2), make_vector_from(1.0f, 4.0f, 3.0f));
  BOOST_CHECK_EQUAL(min(v2, v1), make_vector_from(1.0f, 4.0f, 3.0f));
  BOOST_CHECK_EQUAL(max(v1, v2), make_vector_from(2.0f, 5.0f, 6.0f));
  BOOST_CHECK_EQUAL(max(v2, v1), make_vector_from(2.0f, 5.0f, 6.0f));
}

BOOST_AUTO_TEST_CASE(vector_dot)
{
  BOOST_CHECK_EQUAL(
    dot(make_vector_from(1.0f, 2.0f), make_vector_from(3.0f, 4.0f)), 11.0f);
  BOOST_CHECK_EQUAL(
    dot(make_vector_from(1.0f, 2.0f, 3.0f), make_vector_from(4.0f, 5.0f, 6.0f)),
    32.0f);
  BOOST_CHECK_EQUAL(dot(make_vector_from(1.0f, 2.0f, 3.0f, 4.0f),
                        make_vector_from(5.0f, 6.0f, 7.0f, 8.0f)),
                    70.0f);
}

BOOST_AUTO_TEST_CASE(vector_cross)
{
  {
    constexpr auto v1 = make_vector_from(1.0f, 2.0f);
    constexpr auto v2 = make_vector_from(3.0f, 4.0f);
    BOOST_CHECK_EQUAL(cross(v1, v2), -2.0f);
  }
  {
    constexpr auto x_axis = make_vector_from(1.0f, 0.0f, 0.0f);
    constexpr auto y_axis = make_vector_from(0.0f, 1.0f, 0.0f);
    constexpr auto z_axis = make_vector_from(0.0f, 0.0f, 1.0f);
    BOOST_CHECK_EQUAL(cross(x_axis, y_axis), z_axis);
    BOOST_CHECK_EQUAL(cross(y_axis, z_axis), x_axis);
    BOOST_CHECK_EQUAL(cross(z_axis, x_axis), y_axis);
  }
}

BOOST_AUTO_TEST_CASE(vector_floor)
{
  const auto v1 = make_vector_from(-1.7f, -1.5f, -1.2f);
  const auto v2 = make_vector_from(-0.0f, 0.0f);
  const auto v3 = make_vector_from(1.2f, 1.5f, 1.7f);
  BOOST_CHECK_EQUAL(floor(v1), make_vector_from(-2.0f, -2.0f, -2.0f));
  BOOST_CHECK_EQUAL(floor(v2), make_vector_from(-0.0f, 0.0f));
  BOOST_CHECK_EQUAL(floor(v3), make_vector_from(1.0f, 1.0f, 1.0f));
}

BOOST_AUTO_TEST_CASE(vector_ceil)
{
  const auto v1 = make_vector_from(-1.7f, -1.5f, -1.2f);
  const auto v2 = make_vector_from(-0.0f, 0.0f);
  const auto v3 = make_vector_from(1.2f, 1.5f, 1.7f);
  BOOST_CHECK_EQUAL(ceil(v1), make_vector_from(-1.0f, -1.0f, -1.0f));
  BOOST_CHECK_EQUAL(ceil(v2), make_vector_from(-0.0f, 0.0f));
  BOOST_CHECK_EQUAL(ceil(v3), make_vector_from(2.0f, 2.0f, 2.0f));
}

BOOST_AUTO_TEST_CASE(vector_round)
{
  const auto v1 = make_vector_from(-1.7f, -1.5f, -1.2f);
  const auto v2 = make_vector_from(-0.0f, 0.0f);
  const auto v3 = make_vector_from(1.2f, 1.5f, 1.7f);
  BOOST_CHECK_EQUAL(round(v1), make_vector_from(-2.0f, -2.0f, -1.0f));
  BOOST_CHECK_EQUAL(round(v2), make_vector_from(-0.0f, 0.0f));
  BOOST_CHECK_EQUAL(round(v3), make_vector_from(1.0f, 2.0f, 2.0f));
}

BOOST_AUTO_TEST_CASE(vector_clamp)
{
  BOOST_CHECK_EQUAL(clamp(make_vector_from(1.5f, 1.5f, 1.5f),
                          make_vector_from(2.0f, 1.0f, 0.0f),
                          make_vector_from(3.0f, 2.0f, 1.0f)),
                    make_vector_from(2.0f, 1.5f, 1.0f));
}

BOOST_AUTO_TEST_CASE(vector_normalize)
{
  {
    const auto v1 = make_vector_from(2.0f, 0.0f, 0.0f);
    const auto v2 = make_vector_from(1.0f, 0.0f, 0.0f);
    const auto v3 = make_vector_from(0.1f, 0.0f, 0.0f);
    BOOST_CHECK_EQUAL(normalize(v1), v2);
    BOOST_CHECK_EQUAL(normalize(v2), v2);
    BOOST_CHECK_EQUAL(normalize(v3), v2);
  }
}

BOOST_AUTO_TEST_CASE(vector_reflect)
{
  using std::norm;
  {
    const auto direction = make_vector_from(0.0f, -1.0f);
    const auto normal = normalize(make_vector_from(1.0f, 2.0f));
    const auto expected_result = make_vector_from(0.8f, 0.6f);
    auto result = reflect(direction, normal);
    BOOST_CHECK(almost_equal(result, expected_result));
    BOOST_CHECK(norm(direction) == 1.0f);
    BOOST_CHECK(norm(expected_result) == 1.0f);
    BOOST_CHECK(almost_equal(reflect(result, normal), direction));
  }
  {
    const auto direction = make_vector_from(-1.0f, 0.0f);
    const auto normal = normalize(make_vector_from(1.0f, 2.0f));
    const auto expected_result = make_vector_from(-0.6f, 0.8f);
    auto result = reflect(direction, normal);
    BOOST_CHECK(almost_equal(result, expected_result));
    BOOST_CHECK(norm(direction) == 1.0f);
    BOOST_CHECK(norm(expected_result) == 1.0f);
    BOOST_CHECK(almost_equal(reflect(-result, normal), -direction));
  }
}

BOOST_AUTO_TEST_CASE(vector_step)
{
  BOOST_CHECK_EQUAL(step(0.5f, make_vector_from(0.0f, 0.5f, 2.0f)),
                    make_vector_from(0.0f, 1.0f, 1.0f));
  BOOST_CHECK_EQUAL(step(make_vector_from(0.5f, 0.5f, 0.5f),
                         make_vector_from(0.0f, 0.5f, 2.0f)),
                    make_vector_from(0.0f, 1.0f, 1.0f));
}

BOOST_AUTO_TEST_CASE(vector_mix)
{
  using shift::core::mix;
  BOOST_CHECK_EQUAL(mix(make_vector_from(0.0f, 1.0f, 2.0f),
                        make_vector_from(4.0f, 5.0f, 6.0f), 0.25f),
                    make_vector_from(1.0f, 2.0f, 3.0f));
  BOOST_CHECK_EQUAL(
    mix(make_vector_from(0.0f, 1.0f, 2.0f), make_vector_from(4.0f, 5.0f, 6.0f),
        make_vector_from(0.5f, 0.25f, 0.75f)),
    make_vector_from(2.0f, 2.0f, 5.0f));
}

/// A dummy type used for testing. The template argument has to be a
/// compile-time constant and be equal to 2 in order to hit the template
/// specialization.
template <int Const>
struct constexpr_test
{
  static constexpr bool value = false;
};

template <>
struct constexpr_test<2>
{
  static constexpr bool value = true;
};

BOOST_AUTO_TEST_CASE(vector_contexpr)
{
  constexpr vector3<int> v1(1, 2, 3);
  BOOST_STATIC_ASSERT((constexpr_test<v1(1)>::value));

  constexpr auto v2 = make_vector_from(1, 2, 3);
  BOOST_STATIC_ASSERT((constexpr_test<v2(1)>::value));
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

// Note: Matrices unit tests have been split in different files since
// building them with eigen3 eats a lot of RAM and may be a problem while
// compiling in small systems.

#include <gtest/gtest.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/random.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace std;

TEST(Matrices, loadFromArray)
{
	alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
		const double nums[3 * 4] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};

	CMatrixFixed<double, 3, 4> mat;
	mat.loadFromArray(nums);

	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 4; c++)
			EXPECT_EQ(nums[4 * r + c], mat(r, c));
}

alignas(MRPT_MAX_STATIC_ALIGN_BYTES) static double test_nums[3 * 4] = {
	1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};

TEST(Matrices, CMatrixFixedNumeric_loadWithEigenMap)
{
	// Row major
	const auto mat =
		CMatrixFixed<double, 3, 4>(Eigen::Map<
								   Eigen::Matrix<double, 3, 4, Eigen::RowMajor>,
								   MRPT_MAX_STATIC_ALIGN_BYTES>(test_nums));

	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 4; c++)
			EXPECT_EQ(test_nums[4 * r + c], mat(r, c));
}

TEST(Matrices, EigenMatrix_loadWithEigenMap)
{
	// Col major
	const Eigen::Matrix<double, 3, 4> mat =
		Eigen::Map<Eigen::Matrix<double, 3, 4>, MRPT_MAX_STATIC_ALIGN_BYTES>(
			test_nums);

	for (int r = 0; r < 3; r++)	 // Transposed!!
		for (int c = 0; c < 4; c++)
			EXPECT_EQ(test_nums[3 * c + r], mat(r, c));
}


#ifndef MULTIAGENT_UTILS_COALITION_STRUCTURE_GENERATION_HPP
#define MULTIAGENT_UTILS_COALITION_STRUCTURE_GENERATION_HPP

#include <limits>
#include <boost/function.hpp>
#include <boost/thread.hpp>
#include <numeric/IntegerPartitioning.hpp>
#include <base/Time.hpp>
#include <base-logging/Logging.hpp>

namespace multiagent {
namespace utils {

typedef std::string Agent;
typedef std::vector<Agent> AgentList;
typedef AgentList Coalition;
typedef std::vector< Coalition > CoalitionStructure;

std::ostream& operator<<(std::ostream& os, const AgentList& list);
std::ostream& operator<<(std::ostream& os, const CoalitionStructure& list);

struct Bounds
{
    Bounds(double exact);
    Bounds();

    double maximum;
    double average;
    double minimum;

    std::string toString() const;
};

std::ostream& operator<<(std::ostream& os, const Bounds& bounds);

/**
 * This is an implementation of the coalition structure generation as described in:
 * "An Anytime Algorithm for Optimal Coalition Structure Generation", (Rahwan et al., 2009)
 *
 * Please note that the current implementation uses recursion
 */
class CoalitionStructureGeneration
{
public:
    typedef boost::function1<double, const Coalition&> CoalitionValueFunction;
    typedef boost::function1<double, const CoalitionStructure&> CoalitionStructureValueFunction;

    struct Statistics
    {
        std::vector<numeric::IntegerPartition> allIntegerPartitions;
        std::vector<numeric::IntegerPartition> prunedIntegerPartitions;
        std::vector<numeric::IntegerPartition> searchedIntegerPartitions;

        std::vector<numeric::IntegerPartition> remainingIntegerPartitions() const;

        std::string toString() const;
    };

private:
    AgentList mAgents;
    Statistics mStatistics;

    // Agents listed by size of the coalition
    typedef std::map<size_t, std::set< Coalition > > AgentCoalitionMap;
    AgentCoalitionMap mAgentCoalitionMap;

    typedef std::map<size_t, Bounds> CoalitionBoundMap;
    CoalitionBoundMap mCoalitionBoundMap;

    CoalitionValueFunction mCoalitionValueFunction;
    CoalitionStructureValueFunction mCoalitionStructureValueFunction;

    typedef std::map<numeric::IntegerPartition, Bounds> IntegerPartitionBoundsMap;
    IntegerPartitionBoundsMap mIntegerPartitionBoundsMap;

    mutable boost::mutex mSolutionMutex;
    mutable boost::mutex mStatisticsMutex;
    boost::thread mThread;
    base::Time mStartTime;
    base::Time mCompletionTime;
    CoalitionStructure mCurrentBestCoalitionStructure;
    double mCurrentBestCoalitionStructureValue;
    double mCurrentSolutionQuality;
    double mGlobalUpperBound;

    /**
     * Compute the integer partitions and the agent coalition map for coalition size up to
     * the maximum number of agents
     */
    void prepare();

    double bestLowerBound(const CoalitionBoundMap& boundMap);
    double bestLowerBound(const IntegerPartitionBoundsMap& boundMap);
    double bestUpperBound(const IntegerPartitionBoundsMap& boundMap);

    Bounds computeIntegerPartitionBounds(const numeric::IntegerPartition& partition);
    IntegerPartitionBoundsMap prune(const IntegerPartitionBoundsMap& boundsMap);
    CoalitionBoundMap prune(const CoalitionBoundMap& boundMap);

    numeric::IntegerPartition selectIntegerPartition(const IntegerPartitionBoundsMap& boundMap, double maximumBound) const;

    /**
     *
     * \param globalUpperBound
     * \param bestStar Quality of the solution, i.e. 1.05 means 95% percent of the optimal solution
     * \return true if this subspace contained a better solution than already existed
     */
    bool searchSubspace(const numeric::IntegerPartition& partition, size_t k, size_t alpha, const AgentList& agents, const CoalitionStructure& currentStructure, double betaStar);

    bool updateCurrentBestCoalitionStructure(const CoalitionStructure& coalitionStructure, double value);

public:
    /**
     * Find best coalitionstructure -- will block until coalition structure is found
     */
    CoalitionStructure findBest(double quality = 1.0);

    /**
     * Reset in order to restart a new search
     */
    void reset();

    /**
     * Search for a solution and allow retrieval of intermediate results via currentBestSolution
     */
    void anytimeSearch(double quality = 1.0);

    /**
     * Check if anytimeSearch completed
     * \return true, upon completition, false otherwise -- if search has not been started at all, it will return false
     */
    bool anytimeSearchCompleted() const { return mCompletionTime != base::Time(); }

    /**
     * Compute total elapsed time since last search start
     * \return time object representing the elapsed time, i.e. use toSeconds() or corresponding function to retrieve detailled value
     */
    base::Time elapsed() const { return base::Time::now() - mStartTime; }

    /**
     * Stop the search before completion
     */
    void stopSearch();

    /**
     * Retrieve the current best solution
     * return the current best solution, if none has been found it will return an empty CoalitionStructure
     */
    CoalitionStructure currentBestSolution() const;

    double currentBestSolutionValue() const;
    double currentBestSolutionQuality() const;

    /**
     * \params agents List of agents that are available
     * \param coalitionValueFunction Function that allows to compute the value of an individual coalition
     * \param coalitionStructureValueFunction Function that allows to compute the value of a coalition structure
     */
    CoalitionStructureGeneration(const AgentList& agents, CoalitionValueFunction coalitionValueFunction, CoalitionStructureValueFunction coalitionStructureValueFunction);

    /**
     * Stringify status of this CoalitionStructureGeneration object
     * \return stringified instance
     */
    std::string toString() const;

    /**
     * Stringify a CoalitionStructure
     * \return stringified instance of CoalitionStructure
     */
    static std::string toString(const CoalitionStructure& c);

    Statistics getStatistics() const;
};

} // end namespace utils
} // end namespace multiagent
#endif // MULTIAGENT_UTILS_COALITION_STRUCTURE_GENERATION_HPP


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

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

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

//[assign_box_corners
//` Shows how four point can be assigned from a 2D box

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

using namespace boost::geometry;

int main()
{
    typedef model::d2::point_xy<double> point;
    typedef model::box<point> box;

    box b;
    assign_values(b, 2, 2, 5, 5);

    point ll, lr, ul, ur;
    assign_box_corners(b, ll, lr, ul, ur);

    std::cout << "box: " << dsv(b) << std::endl << std::endl;

    std::cout << dsv(ul) << " --- " << dsv(ur) << std::endl;
    for (int i = 0; i < 3; i++)
    {
        std::cout << "  |          |" << std::endl;
    }
    std::cout << dsv(ll) << " --- " << dsv(lr) << std::endl;

    return 0;
}

//]

//[assign_box_corners_output
/*`
Output:
[pre
box: ((2, 2), (5, 5))

(2, 5) --- (5, 5)
  |          |
  |          |
  |          |
(2, 2) --- (5, 2)
]
*/
//]


#include <Eigen/Geometry>
#include <aslam/backend/DesignVariableGenericVector.hpp>
#include <aslam/backend/DesignVariableVector.hpp>
#include <aslam/backend/GenericMatrixExpression.hpp>
#include <aslam/backend/VectorExpression.hpp>
#include <aslam/backend/VectorExpressionToGenericMatrixTraits.hpp>
#include <aslam/backend/test/ExpressionTests.hpp>
#include <sm/eigen/NumericalDiff.hpp>
#include <sm/eigen/gtest.hpp>
#include <sm/kinematics/rotations.hpp>

using namespace aslam::backend;
using namespace std;

TEST(GenericMatrixExpressionNodeTestSuites, testGenericMatrixBasicOperations) {
    try {
        const int VEC_ROWS = 5;

        typedef GenericMatrixExpression<4, 2> GMAT;
        typedef GenericMatrixExpression<2, VEC_ROWS> GMAT2;

        auto identity = Eigen::Matrix<double, VEC_ROWS, VEC_ROWS>::Identity();
        GenericMatrixExpression<VEC_ROWS, VEC_ROWS> identityExp(identity);

        GMAT::matrix_t mat = GMAT::matrix_t::Random();
        GMAT2::matrix_t mat2 = GMAT2::matrix_t::Random();
        GMAT matExp(mat);
        GMAT2 matExp2(mat2);

        sm::eigen::assertNear(matExp.evaluate(), mat, 1e-14, SM_SOURCE_FILE_POS,
                              "Testing evaluation fits initialization.");
        sm::eigen::assertNear(matExp2.evaluate(), mat2, 1e-14, SM_SOURCE_FILE_POS,
                              "Testing evaluation fits initialization.");

        sm::eigen::assertNear(identityExp.evaluate(), identity, 1e-14, SM_SOURCE_FILE_POS,
                              "Testing the transpose method.");
        sm::eigen::assertNear((matExp.transpose()).evaluate(), mat.transpose(), 1e-14, SM_SOURCE_FILE_POS,
                              "Testing the transpose method.");
        sm::eigen::assertNear((matExp * matExp2).evaluate(), mat * mat2, 1e-14, SM_SOURCE_FILE_POS,
                              "Testing the product.");
        sm::eigen::assertNear((matExp + matExp).evaluate(), 2 * mat, 1e-14, SM_SOURCE_FILE_POS, "Testing the sum.");
        sm::eigen::assertNear((matExp - matExp).evaluate(), 0 * mat, 1e-14, SM_SOURCE_FILE_POS,
                              "Testing the difference.");

        // vector design variable
        typedef GenericMatrixExpression<VEC_ROWS, 1, double> GV;
        typedef DesignVariableGenericVector<VEC_ROWS> DGvec;
        GV::matrix_t vec = GV::matrix_t::Random();
        DGvec dv(vec);
        dv.setActive(true);
        dv.setBlockIndex(0);
        GV vecExp(&dv);

        sm::eigen::assertNear((matExp2 * vecExp).evaluate(), mat2 * vec, 1e-14, SM_SOURCE_FILE_POS,
                              "Testing the matrix vector design variable product.");

        {
            SCOPED_TRACE("");
            JacobianContainer jc(vec.rows());
            sm::eigen::assertEqual(dv.value(), vec, SM_SOURCE_FILE_POS, "Testing evaluation fits initialization.");
            sm::eigen::assertEqual(vecExp.evaluate(), vec, SM_SOURCE_FILE_POS,
                                   "Testing evaluation fits initialization.");
            vecExp.evaluateJacobians(jc);
            sm::eigen::assertNear(jc.asDenseMatrix(), identity, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }

        {
            SCOPED_TRACE("");
            JacobianContainer jc(vec.rows());
            auto exp = identityExp * vecExp;
            exp.evaluateJacobians(jc);
            sm::eigen::assertNear(exp.evaluate(), vec, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the transposed design variable matrix product.");
            sm::eigen::assertNear(jc.asDenseMatrix(), identity, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }

        {
            SCOPED_TRACE("");
            JacobianContainer jc(VEC_ROWS);
            auto exp = -vecExp;
            (exp).evaluateJacobians(jc);
            sm::eigen::assertNear(exp.evaluate(), -(vecExp.evaluate()), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the negated design variable.");
            sm::eigen::assertNear(jc.asDenseMatrix(), -identity, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }
        {
            SCOPED_TRACE("");
            JacobianContainer jc(mat2.rows());
            auto exp = matExp2 * vecExp;
            (exp).evaluateJacobians(jc);
            sm::eigen::assertNear(exp.evaluate(), matExp2.evaluate() * vecExp.evaluate(), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the design variable matrix product.");
            sm::eigen::assertNear(jc.asDenseMatrix(), mat2, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }
        {
            SCOPED_TRACE("");
            JacobianContainer jc(mat2.rows());
            auto exp = (vecExp.transpose() * matExp2.transpose()).transpose();
            exp.evaluateJacobians(jc);
            sm::eigen::assertNear(exp.evaluate(), matExp2.evaluate() * vecExp.evaluate(), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the transposed design variable matrix product.");
            sm::eigen::assertNear(jc.asDenseMatrix(), mat2, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }

        {
            SCOPED_TRACE("");
            JacobianContainer jc(1);
            auto exp = (vecExp.transpose() * vecExp);
            exp.evaluateJacobians(jc);
            sm::eigen::assertNear(exp.evaluate(), vecExp.transpose().evaluate() * vecExp.evaluate(), 1e-14,
                                  SM_SOURCE_FILE_POS, "Testing the design variable vector square.");
            sm::eigen::assertNear(jc.asDenseMatrix(), 2 * vec.transpose(), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }

        {
            JacobianContainer jc(1);
            auto exp = (vecExp.transpose() * vecExp).inverse();
            Eigen::Matrix<double, 1, 1> val;
            val(0, 0) = 1 / vec.dot(vec);
            sm::eigen::assertNear(exp.evaluate(), val, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the inverse operation on a 1 x 1 matrix.");
            exp.evaluateJacobians(jc);
            sm::eigen::assertNear(jc.asDenseMatrix(), -2 * vec.transpose() * (1 / (vec.dot(vec) * vec.dot(vec))), 1e-14,
                                  SM_SOURCE_FILE_POS, "Testing evaluationJacobian fits theoretical value.");
        }

        {
            auto inv = vecExp * vecExp.transpose();
            auto invValue = (vec * vec.transpose()).eval();
            sm::eigen::assertNear(inv.evaluate(), invValue, 1e-14, SM_SOURCE_FILE_POS, "Testing the dyadic product.");

            JacobianContainer jc(VEC_ROWS);
            for (int i = 0; i < VEC_ROWS; i++) {
                jc.clear();
                GV::matrix_t testVectorValue = GV::matrix_t::Zero();
                testVectorValue[i] = 1;
                GV testVector(testVectorValue);
                auto exp = inv * testVector;
                sm::eigen::assertNear(exp.evaluate(), invValue * testVectorValue, 1e-14, SM_SOURCE_FILE_POS,
                                      "Testing the dyadic product.");
                testJacobian(exp);
                exp.evaluateJacobians(jc);
                sm::eigen::assertNear(jc.asDenseMatrix(),
                                      identity * vec.dot(testVectorValue) + vec * (testVectorValue).transpose(), 1e-14,
                                      SM_SOURCE_FILE_POS, "Testing evaluationJacobian fits theoretical value.");
            }
        }

        {
            auto inv = (identityExp + vecExp * vecExp.transpose()).inverse();
            auto invValue = (identity + vec * vec.transpose()).inverse().eval();
            sm::eigen::assertNear(inv.evaluate(), invValue, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the inverse operation.");

            JacobianContainer jc(VEC_ROWS);
            for (int i = 0; i < VEC_ROWS; i++) {
                jc.clear();
                GV::matrix_t testVectorValue = GV::matrix_t::Zero();
                testVectorValue[i] = 1;
                GV testVector(testVectorValue);
                auto exp = inv * testVector;
                sm::eigen::assertNear(exp.evaluate(), invValue * testVectorValue, 1e-14, SM_SOURCE_FILE_POS,
                                      "Testing the inverse operation.");
                testJacobian(exp);
                exp.evaluateJacobians(jc);
                sm::eigen::assertNear(jc.asDenseMatrix(),
                                      -invValue * (identity * vec.dot(invValue * testVectorValue) +
                                                   vec * (invValue * testVectorValue).transpose()),
                                      1e-14, SM_SOURCE_FILE_POS, "Testing evaluationJacobian fits theoretical value.");
            }
        }

        {
            SCOPED_TRACE("");
            JacobianContainer jc(VEC_ROWS);
            auto exp = vecExp * 3;
            GV exp2 = exp;
            exp2.evaluateJacobians(jc);
            sm::eigen::assertNear(exp2.evaluate(), vec * 3, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the design variable times a scalar.");
            sm::eigen::assertNear(jc.asDenseMatrix(), identity * 3, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }

        typedef GenericMatrixExpression<VEC_ROWS, VEC_ROWS> GM3;
        GM3::matrix_t mat3 = GM3::matrix_t::Random();
        GM3 matExp3(mat3);
        {
            SCOPED_TRACE("");
            JacobianContainer jc(1);
            (vecExp.transpose() * matExp3 * vecExp).evaluateJacobians(jc);
            sm::eigen::assertNear(jc.asDenseMatrix(), vec.transpose() * (mat3 + mat3.transpose()), 1e-14,
                                  SM_SOURCE_FILE_POS, "Testing evaluationJacobian fits theoretical value.");
        }

        // second vector design variable
        GV::matrix_t vec2 = GV::matrix_t::Random();
        DGvec dv2(vec2);
        dv2.setActive(true);
        dv2.setBlockIndex(1);
        GV vecExp2(&dv2);

        {
            SCOPED_TRACE("");
            JacobianContainer jc(1);
            (vecExp2.transpose() * matExp3 * vecExp).evaluateJacobians(jc);

            Eigen::Matrix<double, 1, VEC_ROWS * 2> result;
            result.head<VEC_ROWS>() = vec2.transpose() * mat3;
            result.tail<VEC_ROWS>() = vec.transpose() * mat3.transpose();

            sm::eigen::assertNear(jc.asDenseMatrix(), result, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }

        {
            SCOPED_TRACE("");
            JacobianContainer jc(VEC_ROWS);
            (vecExp + vecExp2).evaluateJacobians(jc);
            Eigen::Matrix<double, VEC_ROWS, 2 * VEC_ROWS> result;
            result.block<VEC_ROWS, VEC_ROWS>(0, 0).setIdentity();
            result.block<VEC_ROWS, VEC_ROWS>(0, VEC_ROWS).setIdentity();

            sm::eigen::assertNear(jc.asDenseMatrix(), result, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }

        {
            SCOPED_TRACE("");
            JacobianContainer jc(VEC_ROWS);
            (vecExp - vecExp2).evaluateJacobians(jc);
            Eigen::Matrix<double, VEC_ROWS, 2 * VEC_ROWS> result;
            result.block<VEC_ROWS, VEC_ROWS>(0, 0).setIdentity();
            result.block<VEC_ROWS, VEC_ROWS>(0, VEC_ROWS).setIdentity() *= -1;

            sm::eigen::assertNear(jc.asDenseMatrix(), result, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }
    } catch (std::exception const& e) {
        FAIL() << e.what();
    }
}

TEST(GenericMatrixExpressionNodeTestSuites, testVectorExpressionToGenericMatrixExpression) {
    try {
        const int VEC_ROWS = 5;

        DesignVariableVector<VEC_ROWS> dvec;
        dvec.setActive(true);
        dvec.setBlockIndex(0);
        Eigen::MatrixXd vec = Eigen::MatrixXd::Random(VEC_ROWS, 1);
        dvec.setParameters(vec);

        VectorExpression<VEC_ROWS> vecExp(&dvec);
        auto gVecExp = convertToGME(vecExp);

        SCOPED_TRACE("");
        sm::eigen::assertNear(gVecExp.evaluate(), vec, 1e-14, SM_SOURCE_FILE_POS,
                              "Testing evaluation fits initialization.");
        sm::eigen::assertNear(gVecExp.transpose().evaluate(), vec.transpose(), 1e-14, SM_SOURCE_FILE_POS,
                              "Testing evaluation fits initialization.");

        typedef GenericMatrixExpression<VEC_ROWS, 1, double> GV;
        GV gVec(vec);
        {
            SCOPED_TRACE("");
            JacobianContainer jc(1);
            auto exp = (gVecExp.transpose() * gVecExp);
            sm::eigen::assertNear(exp.evaluate(), (vec.transpose() * vec).eval(), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the design variable matrix square.");
            exp.evaluateJacobians(jc);
            sm::eigen::assertNear(jc.asDenseMatrix(), (2 * vec.transpose()).eval(), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }

        auto identity = Eigen::Matrix<double, VEC_ROWS, VEC_ROWS>::Identity();
        GenericMatrixExpression<VEC_ROWS, VEC_ROWS> identityExp(identity);

        {
            SCOPED_TRACE("");
            JacobianContainer jc(VEC_ROWS);
            auto exp = (identityExp * gVecExp);
            sm::eigen::assertNear(exp.evaluate(), vec.eval(), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the identity times the converted vector expression.");
            exp.evaluateJacobians(jc);
            sm::eigen::assertNear(jc.asDenseMatrix(), identity, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }
        {
            SCOPED_TRACE("");
            JacobianContainer jc(VEC_ROWS);
            auto exp = (gVecExp.transpose() * identityExp).transpose();
            sm::eigen::assertNear(exp.evaluate(), vec.eval(), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the identity times the converted vector expression.");
            exp.evaluateJacobians(jc);
            sm::eigen::assertNear(jc.asDenseMatrix(), identity, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }
        {
            SCOPED_TRACE("");
            JacobianContainer jc(VEC_ROWS);
            auto exp = gVecExp.transpose().transpose();
            sm::eigen::assertNear(exp.evaluate(), vec.eval(), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing the identity times the converted vector expression.");
            exp.evaluateJacobians(jc);
            sm::eigen::assertNear(jc.asDenseMatrix(), identity, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }
    } catch (std::exception const& e) {
        FAIL() << e.what();
    }
}

TEST(GenericMatrixExpressionNodeTestSuites, testCrossProduct) {
    try {
        const int VEC_ROWS = 3;

        typedef Eigen::Matrix<double, VEC_ROWS, 1> vector_t;
        DesignVariableVector<VEC_ROWS> dvec, dvec2;
        dvec.setActive(true);
        dvec.setBlockIndex(0);
        dvec2.setActive(true);
        dvec2.setBlockIndex(1);
        vector_t vec = vector_t::Random();
        vector_t vec2 = vector_t::Random();

        dvec.setParameters(vec);
        VectorExpression<VEC_ROWS> vecExp(&dvec);
        dvec2.setParameters(vec2);
        VectorExpression<VEC_ROWS> vec2Exp(&dvec2);

        auto gVecExp = convertToGME(vecExp);
        auto gVec2Exp = convertToGME(vec2Exp);

        typedef GenericMatrixExpression<VEC_ROWS, 2, double> GMAT;
        typename GMAT::matrix_t mat = GMAT::matrix_t::Random();
        GMAT gMat(mat);
        {
            SCOPED_TRACE("");
            auto exp = (gVecExp.cross(gMat));
            sm::eigen::assertNear(exp.evaluate().col(0), vec.cross(mat.col(0)), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing cross product evaluation.");
            sm::eigen::assertNear(exp.evaluate().col(1), vec.cross(mat.col(1)), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing cross product evaluation.");
        }
        {
            SCOPED_TRACE("");
            auto exp = (gVecExp.cross(gVec2Exp));
            sm::eigen::assertNear(exp.evaluate(), vec.cross(vec2), 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing cross product evaluation.");
            JacobianContainer jc(VEC_ROWS);
            exp.evaluateJacobians(jc);
            Eigen::Matrix<double, VEC_ROWS, 2 * VEC_ROWS> result;
            result.block<VEC_ROWS, VEC_ROWS>(0, 0) = sm::kinematics::crossMx(-vec2);
            result.block<VEC_ROWS, VEC_ROWS>(0, VEC_ROWS) = sm::kinematics::crossMx(vec);
            sm::eigen::assertNear(jc.asDenseMatrix(), result, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }
        {
            SCOPED_TRACE("");
            auto exp = (gVecExp.cross(gMat) * GenericMatrixExpression<2, 1>(Eigen::Matrix<double, 2, 1>::Ones()));
            sm::eigen::assertNear(exp.evaluate(), vec.cross(mat.col(0)) + vec.cross(mat.col(1)), 1e-14,
                                  SM_SOURCE_FILE_POS, "Testing cross product evaluation.");
            JacobianContainer jc(VEC_ROWS);
            exp.evaluateJacobians(jc);
            Eigen::Matrix<double, VEC_ROWS, VEC_ROWS> result;
            result = sm::kinematics::crossMx(-mat.col(0)) + sm::kinematics::crossMx(-mat.col(1));
            sm::eigen::assertNear(jc.asDenseMatrix(), result, 1e-14, SM_SOURCE_FILE_POS,
                                  "Testing evaluationJacobian fits theoretical value.");
        }
    } catch (std::exception const& e) {
        FAIL() << e.what();
    }
}


/*    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
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *      Montenbruck, O., Gill, E. Satellite Orbits: Models, Methods, Applications, Springer, 2005.
 *      The MathWorks, Inc. RKF54b, Symbolic Math Toolbox, 2012.
 *
 *    Notes
 *      For the tests using data from the Symbolic Math Toolbox (MathWorks, 2012), the single step
 *      and full integration error tolerances were picked to be as small as possible, without
 *      causing the tests to fail. These values are not deemed to indicate any bugs in the code;
 *      however, it is important to take these discrepancies into account when using this numerical
 *      integrator.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/reinitializableNumericalIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTests.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/burdenAndFairesNumericalIntegratorTest.h"

#include "Tudat/InputOutput/matrixTextFileReader.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"

#include <limits>
#include <string>

#include <Eigen/Core>

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_runge_kutta_fehlberg_45_integrator )

using linear_algebra::flipMatrixRows;

using numerical_integrators::NumericalIntegratorXdPointer;
using numerical_integrators::ReinitializableNumericalIntegratorXdPointer;
using numerical_integrators::RungeKuttaVariableStepSizeIntegratorXd;
using numerical_integrators::RungeKuttaCoefficients;

using numerical_integrator_test_functions::computeNonAutonomousModelStateDerivative;

//! Test Runge-Kutta-Fehlberg 45 integrator using benchmark data from (Burden and Faires, 2001).
BOOST_AUTO_TEST_CASE( testRungeKuttaFehlberg45IntegratorUsingBurdenAndFairesData )
{
    // Read in benchmark data (Table 5.9 from (Burden and Faires, 2001)).
    std::string pathToBenchmarkDatafile = input_output::getTudatRootPath( )
            + "/Mathematics/NumericalIntegrators/UnitTests/table5_6BurdenAndFaires.txt";

    // Store benchmark data in matrix.
    Eigen::MatrixXd table5_9BurdenAndFaires
            = input_output::readMatrixFromFile( pathToBenchmarkDatafile );

    // Declare constants related to the benchmark file.
    const int FINAL_ROW = table5_9BurdenAndFaires.rows( ) - 1;
    const int TIME_COLUMN_INDEX = 0;
    const int EXPECTED_LOWER_ORDER_STATE_COLUMN_INDEX = 2;
    const int EXPECTED_STEP_SIZE_COLUMN_INDEX = 3;
    const int EXPECTED_RELATIVE_ERROR_COLUMN_INDEX = 4;
    const int EXPECTED_HIGHER_ORDER_STATE_COLUMN_INDEX = 6;

    // Set parameters of integration taken from (Burden and Faires, 2001).
    // This should to be added to the benchmark data file and parsed accordingly once the Tudat
    // parser architecture has been added to the library.
    const double initialTime = 0.0;
    const double finalTime = 2.0;
    const Eigen::VectorXd initialState = Eigen::VectorXd::Constant( 1, 0.5 );
    const double initialStepSize = 0.25;
    const double minimumStepSize = 0.01;
    const double maximumStepSize = 0.25;
    const double relativeErrorTolerance = 0.0;
    const double absoluteErrorTolerance = 1.0e-5;
    const double safetyFactorForNextStepSize = 0.84;
    const double maximumFactorIncreaseForNextStepSize = 4.0;
    const double minimumFactorDecreaseForNextStepSize = 0.1;

    // Declare Burden and Faires class object, containing new step size and state derivative
    // functions.
    BurdenAndFairesNumericalIntegratorTest burdenAndFairesNumericalIntegratorTest;

    // Case 1: Use integrateTo() to integrate to final time in one step and check results against
    // benchmark data from Burden and Faires.
    {
        // Declare integrator with all necessary settings.
        RungeKuttaVariableStepSizeIntegratorXd integrator(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                    std::bind( &BurdenAndFairesNumericalIntegratorTest::computeStateDerivative,
                                 &burdenAndFairesNumericalIntegratorTest, std::placeholders::_1, std::placeholders::_2 ),
                    initialTime,
                    initialState,
                    minimumStepSize,
                    maximumStepSize,
                    relativeErrorTolerance,
                    absoluteErrorTolerance,
                    safetyFactorForNextStepSize,
                    maximumFactorIncreaseForNextStepSize,
                    minimumFactorDecreaseForNextStepSize,
                    std::bind( &BurdenAndFairesNumericalIntegratorTest::computeNewStepSize,
                                 &burdenAndFairesNumericalIntegratorTest,
                                 std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4,
                               std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, std::placeholders::_8 ) );

        // Integrator to final time.
        Eigen::VectorXd finalState = integrator.integrateTo( finalTime, initialStepSize );

        // Check that the computed final time matches the required final time.
        BOOST_CHECK_CLOSE_FRACTION( table5_9BurdenAndFaires( FINAL_ROW, TIME_COLUMN_INDEX ),
                                    integrator.getCurrentIndependentVariable( ),
                                    std::numeric_limits< double >::epsilon( ) );

        // Check that computed final state matches the expected final state.
        BOOST_CHECK_CLOSE_FRACTION(
                    table5_9BurdenAndFaires( FINAL_ROW, EXPECTED_LOWER_ORDER_STATE_COLUMN_INDEX ),
                    finalState( 0 ), 1.0e-8 );

        // Roll back to the previous step. This should be possible since the integrateTo() function
        // was called above.
        BOOST_CHECK( integrator.rollbackToPreviousState( ) );

        // Check that the rolled back time is as required.
        BOOST_CHECK_CLOSE_FRACTION(
                    table5_9BurdenAndFaires( FINAL_ROW - 1, TIME_COLUMN_INDEX ),
                    integrator.getCurrentIndependentVariable( ), 1.0e-8 );

        // Check that the rolled back state is as required. This test should be exact.
        BOOST_CHECK_CLOSE_FRACTION(
                    table5_9BurdenAndFaires( FINAL_ROW - 1,
                                             EXPECTED_LOWER_ORDER_STATE_COLUMN_INDEX ),
                    integrator.getCurrentState( )( 0 ), 1.0e-8 );

        // Check that it is now not possible to roll back.
        BOOST_CHECK( !integrator.rollbackToPreviousState( ) );
    }

    // Case 2: Use integrateTo() to integrate to final time in multiple steps and check results
    // against benchmark data from Burden and Faires.
    {
        // Declare integrator with all necessary settings.
        RungeKuttaVariableStepSizeIntegratorXd integrator(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                    std::bind( &BurdenAndFairesNumericalIntegratorTest::computeStateDerivative,
                                 &burdenAndFairesNumericalIntegratorTest, std::placeholders::_1, std::placeholders::_2 ),
                    initialTime,
                    initialState,
                    minimumStepSize,
                    maximumStepSize,
                    relativeErrorTolerance,
                    absoluteErrorTolerance,
                    safetyFactorForNextStepSize,
                    maximumFactorIncreaseForNextStepSize,
                    minimumFactorDecreaseForNextStepSize,
                    std::bind( &BurdenAndFairesNumericalIntegratorTest::computeNewStepSize,
                                 &burdenAndFairesNumericalIntegratorTest,
                                 std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4,
                               std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, std::placeholders::_8 ) );

        // Store the initial step size as the step size to perform the first integration step.
        double stepSize = initialStepSize;

        for ( int i = 1; i < table5_9BurdenAndFaires.rows( ) - 1; i++ )
        {
            // Perform integration step using stored step size.
            integrator.performIntegrationStep( stepSize );

            // Check that the computed intermediate time matches the required intermediate time.
            BOOST_CHECK_CLOSE_FRACTION( table5_9BurdenAndFaires( i, TIME_COLUMN_INDEX ),
                                        integrator.getCurrentIndependentVariable( ),
                                        1.0e-8 );

            // Check that the computed intermediate state matches the required intermediate state.
            // Note that for some reason the check for table5_9BurdenAndFaires( 2, 2 ) failed
            // against a tolerance of 1.0e-8: this seems to come from the fact that the input data
            // from the file is read in incorrectly, introducing an error in the last significant
            // digit. All the other values satisfy a tolerance of 1.0e-8.
            BOOST_CHECK_CLOSE_FRACTION(
                        table5_9BurdenAndFaires( i, EXPECTED_LOWER_ORDER_STATE_COLUMN_INDEX ),
                        integrator.getCurrentState( )( 0 ),
                        1.0e-7 );

            // Check that the computed step size matches the required step size state.
            BOOST_CHECK_CLOSE_FRACTION(
                        table5_9BurdenAndFaires( i, EXPECTED_STEP_SIZE_COLUMN_INDEX ),
                        stepSize, 1.0e-7 );

            // Check that the computed relative error matches the required relative error.
            BOOST_CHECK_CLOSE_FRACTION(
                        table5_9BurdenAndFaires( i, EXPECTED_RELATIVE_ERROR_COLUMN_INDEX ),
                        burdenAndFairesNumericalIntegratorTest.relativeError_( 0 ), 1.0e-1 );

            // Check that the computed lower order estimate matches the required lower order
            // estimate. Note that this is the order that is integrated for the RFK-45 integrator.
            BOOST_CHECK_CLOSE_FRACTION(
                        table5_9BurdenAndFaires( i, EXPECTED_LOWER_ORDER_STATE_COLUMN_INDEX ),
                        burdenAndFairesNumericalIntegratorTest.lowerOrderEstimate_( 0 ),
                        1.0e-7 );

            // Check that the computed higher order estimate matches the required higher order
            // estimate.
            BOOST_CHECK_CLOSE_FRACTION(
                        table5_9BurdenAndFaires( i, EXPECTED_HIGHER_ORDER_STATE_COLUMN_INDEX ),
                        burdenAndFairesNumericalIntegratorTest.higherOrderEstimate_( 0 ),
                        1.0e-7 );

            // Update the step size for the next step based on the computed value in the
            // integrator.
            stepSize = integrator.getNextStepSize( );
        }

        // Store last time and state.
        const double lastTime = integrator.getCurrentIndependentVariable( );
        const Eigen::VectorXd lastState = integrator.getCurrentState( );

        // Integrate to final time.
        const Eigen::VectorXd finalState = integrator.integrateTo( finalTime,
                                                                   integrator.getNextStepSize( ) );

        // Check that the computed final time matches the required final time.
        BOOST_CHECK_CLOSE_FRACTION( finalTime,
                                    integrator.getCurrentIndependentVariable( ),
                                    std::numeric_limits< double >::epsilon( ) );

        // Check that computed final state matches the expected final state.
        BOOST_CHECK_CLOSE_FRACTION(
                    table5_9BurdenAndFaires( FINAL_ROW, EXPECTED_LOWER_ORDER_STATE_COLUMN_INDEX ),
                    finalState( 0 ), 1.0e-8 );

        // Check that the final state outputted by the integrator is the same as obtained from the
        // get-function.
        BOOST_CHECK_EQUAL( integrator.getCurrentState( ), finalState );

        // Roll back to the previous step. This should be possible since the
        // performIntegrationStep() function was called above.
        BOOST_CHECK( integrator.rollbackToPreviousState( ) );

        // Check that the rolled back time is as required. This test should be exact.
        BOOST_CHECK_EQUAL( lastTime, integrator.getCurrentIndependentVariable( ) );

        // Check that the rolled back state is as required. This test should be exact.
        BOOST_CHECK_EQUAL( lastState( 0 ), integrator.getCurrentState( )( 0 ) );

        // Check that it is now not possible to roll back.
        BOOST_CHECK( !integrator.rollbackToPreviousState( ) );
    }
}

//! Test Runge-Kutta-Fehlberg 45 integrator using benchmark data from (The MathWorks, 2012).
BOOST_AUTO_TEST_CASE( testRungeKuttaFehlberg45IntegratorUsingMatlabData )
{
    using namespace numerical_integrator_tests;

    // Read in benchmark data (generated using Symbolic Math Toolbox in Matlab
    // (The MathWorks, 2012)). This data is generated using the RKF54b numerical integrator.
    const std::string pathToForwardIntegrationOutputFile = input_output::getTudatRootPath( )
            + "/Mathematics/NumericalIntegrators/UnitTests"
            + "/matlabOutputRungeKuttaFehlberg45Forward.txt";
    const std::string pathToDiscreteEventIntegrationOutputFile = input_output::getTudatRootPath( )
            + "/Mathematics/NumericalIntegrators/UnitTests"
            + "/matlabOutputRungeKuttaFehlberg45DiscreteEvent.txt";

    // Store benchmark data in matrix.
    const Eigen::MatrixXd matlabForwardIntegrationData =
            input_output::readMatrixFromFile( pathToForwardIntegrationOutputFile, "," );
    Eigen::MatrixXd matlabBackwardIntegrationData = matlabForwardIntegrationData;
    flipMatrixRows( matlabBackwardIntegrationData );
    const Eigen::MatrixXd matlabDiscreteEventIntegrationData =
            input_output::readMatrixFromFile( pathToDiscreteEventIntegrationOutputFile, "," );

    // Set integrator parameters.

    // All of the following parameters are set such that the input data is fully accepted by the
    // integrator, to determine the steps to be taken.
    const double zeroMinimumStepSize = std::numeric_limits< double >::epsilon( );
    const double infiniteMaximumStepSize = std::numeric_limits< double >::infinity( );
    const double infiniteRelativeErrorTolerance = std::numeric_limits< double >::infinity( );
    const double infiniteAbsoluteErrorTolerance = std::numeric_limits< double >::infinity( );

    // The following parameters set how the error control mechanism should work.
    const double relativeErrorTolerance = 1.0e-15;
    const double absoluteErrorTolerance = 1.0e-15;

    // Case 1: Execute integrateTo() to integrate one step forward in time.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        executeOneIntegrateToStep( matlabForwardIntegrationData, 1.0e-15, integrator );
    }

    // Case 2: Execute performIntegrationStep() to perform multiple integration steps until final
    //         time.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTime( matlabForwardIntegrationData,
                                               1.0e-15, 1.0e-14, integrator );
    }

    // Case 3: Execute performIntegrationStep() to perform multiple integration steps until initial
    //         time (backwards).
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabBackwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabBackwardIntegrationData( FIRST_ROW,
                                                        STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTime( matlabBackwardIntegrationData,
                                               1.0e-15, 1.0e-14, integrator );
    }

    // Case 4: Execute integrateTo() to integrate to specified time in one step.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    relativeErrorTolerance,
                    absoluteErrorTolerance );

        executeIntegrateToToSpecifiedTime( matlabForwardIntegrationData, 1.0e-12, integrator,
                                           matlabForwardIntegrationData(
                                               matlabForwardIntegrationData.rows( ) - 1,
                                               TIME_COLUMN_INDEX ) );
    }

    // Case 5: Execute performIntegrationstep() to integrate to specified time in multiple steps,
    //         including discrete events.
    {
        // Declare integrator with all necessary settings.
        ReinitializableNumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKuttaVariableStepSizeIntegratorXd >(
                    RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                    &computeNonAutonomousModelStateDerivative,
                    matlabDiscreteEventIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabDiscreteEventIntegrationData( FIRST_ROW,
                                                             STATE_COLUMN_INDEX ) ).finished( ),
                    zeroMinimumStepSize,
                    infiniteMaximumStepSize,
                    infiniteRelativeErrorTolerance,
                    infiniteAbsoluteErrorTolerance );

        performIntegrationStepToSpecifiedTimeWithEvents( matlabDiscreteEventIntegrationData,
                                                         1.0e-15, 1.0e-12, integrator );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//  (C) Copyright Matt Borland 2021.
//  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/ccmath/ldexp.hpp>
#include "test_compile_result.hpp"

void compile_and_link_test()
{
   check_result<float>(boost::math::ccmath::ldexp(1.0f, 1));
   check_result<double>(boost::math::ccmath::ldexp(1.0, 1));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::ccmath::ldexp(1.0l, 1));
#endif
}


#pragma once

#include <vector>
#include <type_traits>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include "Bounds.hh"
#include "LineSegment/linesegment.hh"
#include "LineSegment/LineSegment2/linesegment2.hh"
#include "../../util/assert.hh"
#include "../../util/log.hh"
#include "../../util/Maybe.hh"

namespace bold
{
  typedef unsigned int uint;

  template<typename T>
  class Polygon2
  {
  public:
    typedef Eigen::Matrix<T, 2, 1> Point;
    typedef std::vector<Point,Eigen::aligned_allocator<Point>> PointVector;

    Polygon2(PointVector const& vertices)
    : d_vertices(vertices)
    {
      if (vertices.size() < 3)
      {
        log::error("Polygon2::Polygon2") << "Cannot create a polygon with number of vertices: " << vertices.size();
        throw std::runtime_error("A polygon must have at least three vertices");
      }
    }

    Polygon2(Bounds<T,2> bounds)
    : d_vertices()
    {
      d_vertices.push_back(bounds.min());
      d_vertices.emplace_back(Point(bounds.max().x(), bounds.min().y()));
      d_vertices.push_back(bounds.max());
      d_vertices.emplace_back(Point(bounds.min().x(), bounds.max().y()));
    }

    bool contains(Point const& point)
    {
      bool isInside = false;
      for (unsigned i = 0, j = d_vertices.size() - 1; i < d_vertices.size(); j = i++)
      {
        Point const& a = d_vertices[i];
        Point const& b = d_vertices[j];

        if (((a.y() > point.y()) != (b.y() > point.y()))
          && (point.x() < (b.x() - a.x()) * (point.y() - a.y()) / (b.y() - a.y()) + a.x()))
        {
          isInside = !isInside;
        }
      }
      return isInside;
    }

    uint vertexCount() const { return d_vertices.size(); }

    Point operator[](uint i)
    {
      return d_vertices[i];
    }

    typename PointVector::iterator begin() { return d_vertices.begin(); }
    typename PointVector::iterator end() { return d_vertices.end(); }
    typename PointVector::const_iterator begin() const { return d_vertices.begin(); }
    typename PointVector::const_iterator end() const { return d_vertices.end(); }

    /// Returns the subsection of the provided that that resides within this polygon.
    /// Assumes the poly is convex. Returns an empty result if no intersection exists.
    Maybe<LineSegment2<T>> clipLine(LineSegment2<T> const& line)
    {
      bool contains1 = contains(line.p1());
      bool contains2 = contains(line.p2());

      // Line is completely inside the polygon.
      // As we're convex, there cannot be any intersection.
      if (contains1 && contains2)
        return line;

      PointVector intersectionPoints;
      for (unsigned i = 0, j = d_vertices.size() - 1; i < d_vertices.size(); j = i++)
      {
        Point const& a = d_vertices[i];
        Point const& b = d_vertices[j];
        LineSegment2<T> l(a, b);
        auto result = l.tryIntersect(line);
        if (result.hasValue())
        {
          // Ensure unique members of the list
          if (intersectionPoints.size() > 0 && intersectionPoints[0] == result.value())
            continue;
          if (intersectionPoints.size() > 1 && intersectionPoints[1] == result.value())
            continue;

          intersectionPoints.push_back(std::move(result.value()));

          // In a convex polygon, we should never see more than two intersections
          if (intersectionPoints.size() == 2)
            break;
        }
      }

      if (intersectionPoints.size() == 0)
      {
        // Line is completely outside and does not intersect with polygon
        return Maybe<LineSegment2<T>>::empty();
      }

      if (intersectionPoints.size() == 2)
      {
        // Line is completely outside and intersects with polygon
        return LineSegment2<T>(intersectionPoints[0], intersectionPoints[1]);
      }

      // One end of the line is outside, and we have a single intersection
      ASSERT(intersectionPoints.size() == 1);
      if (contains1)
        return LineSegment2<T>(line.p1(), intersectionPoints[0]);
      else
        return LineSegment2<T>(intersectionPoints[0], line.p2());
    }

  private:
    PointVector d_vertices;
  };

  typedef Polygon2<int> Polygon2i;
  typedef Polygon2<float> Polygon2f;
  typedef Polygon2<double> Polygon2d;

  template<typename T>
  Maybe<Polygon2<T>> make_polygon2(typename Polygon2<T>::PointVector const& vertices)
  {
    return vertices.size() < 3
      ? Maybe<Polygon2<T>>::empty()
      : Polygon2<T>(vertices);
  }

  inline Maybe<Polygon2d> make_polygon2d(typename Polygon2d::PointVector const& vertices)
  {
    return make_polygon2<double>(vertices);
  }
}