#include <fstream>
#include <boost/config.hpp>
#include <boost/version.hpp>
// CGAL headers
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_2.h>

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

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/PointsInKdTreeGraphicsItem.h>
#include <CGAL/Qt/utility.h>
#include <CGAL/IO/WKT.h>

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

#include "NearestNeighbor.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Vector_2 Vector_2;
typedef K::Segment_2 Segment_2;
typedef K::Iso_rectangle_2 Iso_rectangle_2;
typedef CGAL::Search_traits_2<K> TreeTraits;
typedef CGAL::Orthogonal_k_neighbor_search<TreeTraits> Neighbor_search;
typedef Neighbor_search::Tree Tree;

typedef CGAL::Qt::NearestNeighbor<Neighbor_search> NearestNeighbor;

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

private:
  Tree tree;

  CGAL::Qt::Converter<K> convert;
  QGraphicsScene scene;

  CGAL::Qt::PointsInKdTreeGraphicsItem<Tree> * pgi;
  NearestNeighbor * nearest_neighbor;

public:
  MainWindow();

  template <typename G>
  void
  on_actionGenerate_triggered()
  {

    QRectF rect = CGAL::Qt::viewportsBbox(&scene);
    CGAL::Qt::Converter<K> convert;
    Iso_rectangle_2 isor = convert(rect);
    Point_2 center = CGAL::midpoint(isor[0], isor[2]);
    Vector_2 offset = center - CGAL::ORIGIN;
    double w = isor.xmax() - isor.xmin();
    double h = isor.ymax() - isor.ymin();
    double radius = (w<h) ? w/2 : h/2;

    G pg(radius);
    bool ok = false;

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

    if(!ok) {
      return;
    }

    // wait cursor
    QApplication::setOverrideCursor(Qt::WaitCursor);

    std::vector<Point_2> points;

    points.reserve(number_of_points);
    for(int i = 0; i < number_of_points; ++i){
      points.push_back(*pg + offset);
      ++pg;
    }
    tree.insert(points.begin(), points.end());

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

public Q_SLOTS:

  virtual void open(QString fileName);
  void N_changed(int i);
  void on_actionClear_triggered();
  void on_actionLoadPoints_triggered();
  void on_actionRecenter_triggered();
  void on_actionGeneratePointsOnCircle_triggered();
  void on_actionGeneratePointsInSquare_triggered();
  void on_actionGeneratePointsInDisc_triggered();

  void clear();

Q_SIGNALS:
  void changed();
};

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

  this->graphicsView->setAcceptDrops(false);
  this->graphicsView->setCursor(Qt::CrossCursor);

  // Add a GraphicItem for the point set
  pgi = new CGAL::Qt::PointsInKdTreeGraphicsItem<Tree>(&tree);

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

  pgi->setVerticesPen(QPen(Qt::black, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(pgi);

  nearest_neighbor = new NearestNeighbor(&scene, &tree, this, this->nn->value());
  nearest_neighbor->setPen(QPen(Qt::red, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.installEventFilter(nearest_neighbor);

  //
  // Manual handling of actions
  //

  QObject::connect(this->nn, SIGNAL(valueChanged(int)),
                   this, SLOT(N_changed(int)));

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

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

  // Uncomment the following line to get antialiasing by default.
//   actionUse_Antialiasing->setChecked(true);

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

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

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Spatial_searching_2.html");
  this->addAboutCGAL();

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

void MainWindow::N_changed(int i)
{
  nearest_neighbor->setN(i);
  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()
{
  clear();
  Q_EMIT( changed());
}

void
MainWindow::on_actionRecenter_triggered()
{
  if(tree.empty()){
    return;
  }
  this->graphicsView->setSceneRect(pgi->boundingRect());
  this->graphicsView->fitInView(pgi->boundingRect(), Qt::KeepAspectRatio);
}

void
MainWindow::on_actionGeneratePointsOnCircle_triggered()
{
  typedef CGAL::Random_points_on_circle_2<Point_2> Generator;
  on_actionGenerate_triggered<Generator>();
}

void
MainWindow::on_actionGeneratePointsInSquare_triggered()
{
  typedef CGAL::Random_points_in_square_2<Point_2> Generator;
  on_actionGenerate_triggered<Generator>();
}

void
MainWindow::on_actionGeneratePointsInDisc_triggered()
{
  typedef CGAL::Random_points_in_disc_2<Point_2> Generator;
  on_actionGenerate_triggered<Generator>();
}

void
MainWindow::on_actionLoadPoints_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
                                                  tr("Open Points file"),
                                                  ".",
                                                  tr("CGAL files (*.pts.cgal);;"
                                                     "WKT files (*.wkt *.WKT);;"
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::ifstream ifs(qPrintable(fileName));

  K::Point_2 p;
  std::vector<K::Point_2> points;
  if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
  {
    CGAL::IO::read_multi_point_WKT(ifs, points);
  }
  else{
    while(ifs >> p) {
      points.push_back(p);
    }
  }
  tree.insert(points.begin(), points.end());

  // default cursor
  QApplication::restoreOverrideCursor();
  this->addToRecentFiles(fileName);
  actionRecenter->trigger();
  Q_EMIT( changed());

}

void
MainWindow::clear()
{
  tree.clear();
}

#include "Spatial_searching_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("Spatial_searching_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(Spatial_searching_2);

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


#include "goodnessfunction.h"

#include <QImage>
#include <Eigen/Dense>
#include "eigenutility.h"
#include "image.h"

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;

namespace
{
double computeGaussian(const VectorXd& x, const VectorXd& c, const MatrixXd& S_inv)
{
    const unsigned n = x.rows();
    const VectorXd r = x - c;
    const double   a = 1.0 / sqrt(pow(2.0 * M_PI, static_cast<double>(n)) * (1.0 / S_inv.determinant()));
    const double   b = exp(- 0.5 * r.transpose() * S_inv * r);
    return a * b;
}

template<typename Scalar> Scalar clamp(Scalar x, Scalar m, Scalar M) { return std::max(std::min(x, M), m); }

double computeVar(const MatrixXd &w_mean, const double &o_mean, const vector<MatrixXd> &w, const VectorXd &o, unsigned s, unsigned t, unsigned N)
{
    double t1 = 0.0;
    for (unsigned i = 0; i < N; ++ i)
    {
        t1 += (o[i] * w[i](s, t) - o_mean * w_mean(s, t)) * (o[i] * w[i](s, t) - o_mean * w_mean(s, t));
    }

    double t2 = 0.0;
    for (unsigned i = 0; i < N; ++ i)
    {
        t2 += (o[i] - o_mean) * (o[i] * w[i](s, t) - o_mean * w_mean(s, t));
    }

    double t3 = 0.0;
    for (unsigned i = 0; i < N; ++ i)
    {
        t3 += (o[i] - o_mean) * (o[i] - o_mean);
    }

    return (static_cast<double>(N) / static_cast<double>(N - 1)) * (t1 - 2.0 * w_mean(s, t) * t2 + w_mean(s, t) * w_mean(s, t) * t3);
}
}

GoodnessFunction::GoodnessFunction() :
    alpha(0.0020),
    epsilon(0.020)
{

}

// A pure implementation of the adaptive kernel density estimation algorithms described in [Talton et al. 2009] (Section 5)
void GoodnessFunction::computeCovariance()
{
    const unsigned N = getFeatureList().size();
    const unsigned n = getFeatureList()[0].rows() + getParameterList()[0].rows();

    SList.clear();
    SList.resize(N);

    // Exception
    assert (N != 0);
    if (N == 1)
    {
        SList[0] = MatrixXd::Identity(n, n);
        return;
    }

    // Prepare the data points
    vector<VectorXd> x(N);
    for (unsigned i = 0; i < N; ++ i)
    {
        x[i] = getJointVector(getParameterList()[i], getFeatureList()[i]);
    }

    // Compute the distances from the k-nearest neighborhoods
    const unsigned k = std::min(N - 1, n);
    VectorXd squaredDistance_k = VectorXd::Zero(N);
    for (unsigned i = 0; i < N; ++ i)
    {
        vector<double> d(N);
        for (unsigned j = 0; j < N; ++ j)
        {
            d[j] = (x[i] - x[j]).squaredNorm();
        }
        partial_sort(d.begin(), d.begin() + (k + 1), d.end());  // Note: d[0] is always 0.0
        squaredDistance_k(i) = d[k];
    }

    // For each Gaussian center, ...
    for (unsigned center = 0; center < N; ++ center)
    {
        // Compute weights (omega in the paper) for this data point
        VectorXd o = VectorXd::Zero(N);
        for (unsigned j = 0; j < N; ++ j)
        {
            const double   alpha = 1.0;
            const MatrixXd S_inv = (1.0 / (alpha * squaredDistance_k[center])) * MatrixXd::Identity(n, n);
            o(j) = computeGaussian(x[j], x[center], S_inv);
        }
        o /= o.sum();

        // Compute weights (w in the paper) for this data point
        vector<MatrixXd> w(N);
        for (unsigned j = 0; j < N; ++ j)
        {
            w[j] = MatrixXd::Zero(n, n);
            for (unsigned s = 0; s < n; ++ s) for (unsigned t = s; t < n; ++ t)
            {
                w[j](s, t) = (x[j].row(s) - x[center].row(s)) * (x[j].row(t) - x[center].row(t));
                w[j](t, s) = (x[j].row(s) - x[center].row(s)) * (x[j].row(t) - x[center].row(t));
            }
        }

        // Compute a covariance matrix for this data point
        SList[center] = MatrixXd::Zero(n, n);
        for (unsigned s = 0; s < n; ++ s) for (unsigned t = s; t < n; ++ t)
        {
            double elem = 0.0;
            for (unsigned j = 0; j < N; ++ j)
            {
                elem += o[j] * w[j](s, t);
            }
            SList[center](s, t) = elem;
            SList[center](t, s) = elem;
        }

        // Compute the bandwidth shrinkage algorithm
        const MatrixXd& Sigma = SList[center];

        MatrixXd Phi = MatrixXd::Zero(n, n);
        for (unsigned j = 0; j < n; ++ j) Phi(j, j) = Sigma(j, j);

        const MatrixXd& w_mean = Sigma;
        const double    o_mean = o.sum() / static_cast<double>(N);

        double t1 = 0.0;
        for (unsigned s = 0; s < n; ++ s) for (unsigned t = 0; t < n; ++ t)
        {
            if (s == t) continue;
            t1 += computeVar(w_mean, o_mean, w, o, s, t, N);
        }

        double t2 = 0.0;
        for (unsigned s = 0; s < n; ++ s) for (unsigned t = 0; t < n; ++ t)
        {
            if (s == t) continue;
            t2 += w_mean(s, t) * w_mean(s, t);
        }

        double lambda = t1 / t2;
        lambda = isnan(lambda) ? 0.0 : clamp(lambda, 0.0, 1.0);

        SList[center] = lambda * Phi + (1.0 - lambda) * Sigma;
    }
}

void GoodnessFunction::regularizeCovariance()
{
    for (MatrixXd &Sigma : SList)
    {
        const unsigned n = Sigma.rows();
        for (unsigned i = 0; i < n; ++ i)
        {
            Sigma(i, i) = max(Sigma(i, i), epsilon);
        }
    }
}

VectorXd GoodnessFunction::applyGradientAscent(const VectorXd& x, const VectorXd& f, double scale) const
{
    const VectorXd grad = computeGradient(x, f).block(0, 0, x.rows(), 1);
    const VectorXd xNew = x + scale * alpha * grad;

    return getClippedParameters(xNew);
}

double GoodnessFunction::getValue(const VectorXd& j) const
{
#if 0
    const double   sig = 0.10;
    const unsigned N   = getFeatureList().size();

    double sum = 0.0;
    for (unsigned i = 0; i < N; ++ i) {
        const VectorXd& f_i = getFeatureList()[i];
        const VectorXd& x_i = getParameterList()[i];
        const VectorXd  j_i = getJointVector(x_i, f_i);
        const double    d_i = (j - j_i).squaredNorm();
        sum += exp(- d_i / sig);
    }
    return sum / static_cast<double>(N);
#else
    const unsigned N = getFeatureList().size();
    double sum = 0.0;
    for (unsigned i = 0; i < N; ++ i) {
        const MatrixXd& S     = SList[i];
        const MatrixXd  S_inv = S.inverse();
        const VectorXd& f_i   = getFeatureList()[i];
        const VectorXd& x_i   = getParameterList()[i];
        const VectorXd  j_i   = getJointVector(x_i, f_i);
        sum += computeGaussian(j, j_i, S_inv);
    }
    return sum / static_cast<double>(N);
#endif
}

double GoodnessFunction::getValue(const VectorXd& x, const VectorXd &f) const
{
    const VectorXd j = getJointVector(x, f);
    return getValue(j);
}

VectorXd GoodnessFunction::computeGradient(const VectorXd &j) const
{
    const unsigned n = j.rows();
    const unsigned N = getFeatureList().size();
    VectorXd sum = VectorXd::Zero(n);
    for (unsigned i = 0; i < N; ++ i)
    {
        const MatrixXd& S     = SList[i];
        const MatrixXd  S_inv = S.inverse();
        const VectorXd& f_i   = getFeatureList()[i];
        const VectorXd& x_i   = getParameterList()[i];
        const VectorXd  j_i   = getJointVector(x_i, f_i);
        sum += - computeGaussian(j, j_i, S_inv) * S_inv * (j - j_i);
    }

    // NaN check
    for (unsigned i = 0; i < sum.rows(); ++ i) sum(i) = std::isnan(sum(i)) ? 0.0 : sum(i);

    return sum / static_cast<double>(N);
}

VectorXd GoodnessFunction::computeGradient(const VectorXd &x, const VectorXd &f) const
{
    const VectorXd j = getJointVector(x, f);
    return computeGradient(j);
}

VectorXd GoodnessFunction::getClippedParameters(VectorXd x)
{
    for (int i = 0; i < x.rows(); ++ i)
    {
        if (x(i) < 0.0) x(i) = 0.0;
        if (x(i) > 1.0) x(i) = 1.0;
    }
    return x;
}

VectorXd GoodnessFunction::getJointVector(const VectorXd &x, const Eigen::VectorXd& f)
{
    return EigenUtility::join(x, f);
}

VectorXd GoodnessFunction::getAverageParameterSet() const
{
    assert(!pList.empty());

    VectorXd ave = VectorXd::Zero(pList[0].rows());
    for (const VectorXd& v : pList)
    {
        ave += v;
    }
    return (1.0 / static_cast<double>(pList.size())) * ave;
}


#include "problemes.h"
#include "utilitaires.h"

#include <boost/math/constants/constants.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

ENREGISTRER_PROBLEME(317, "Firecracker") {
    // A firecracker explodes at a height of 100 m above level ground. It breaks into a large number of very small
    // fragments, which move in every direction; all of them have the same initial velocity of 20 m/s.
    //
    // We assume that the fragments move without air resistance, in a uniform gravitational field with g=9.81 m/s2.
    //
    // Find the volume (in m3) of the region through which the fragments move before reaching the ground. Give your
    // answer rounded to four decimal places.
    const long double v = 20;
    const long double h = 100;
    const long double g = 9.81L;

    long double resultat = M_PIl * (2 * g * v * h + v * v * v) * (2 * g * v * h + v * v * v) / (4 * g * g * g);
    return std::to_fixed(resultat, 4);
}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 * All rights reserved.
 *
 * See LICENSE file in the root of the mrob library.
 *
 *
 * SO3.cpp
 *
 *  Created on: Feb 12, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include "mrob/SO3.hpp"
#include <cmath>
#include <iostream>
#include <Eigen/LU> // for determinant
#include <Eigen/Geometry> // for quaternions and rotations

using namespace mrob;

SO3::SO3(const Mat3 &R) :
        R_(R)
{
}

SO3::SO3(const Mat31 &w) :
        R_(Mat3::Identity())
{
    //std::cout << "SO3 with Mat31" << std::endl;
    this->exp(hat3(w));
}

SO3::SO3(const SO3 &R) :
        R_(R.R())
{
}

template<typename OtherDerived>
SO3::SO3(const Eigen::MatrixBase<OtherDerived>& rhs)  :
    R_(rhs)
{    //std::cout << "SE3 MAT4" << std::endl;
}

SO3& SO3::operator=(const SO3 &rhs)
{
    //std::cout << "SO3 operator equal" << std::endl;
    // check for self assignment
    if (this == &rhs)
        return *this;
    R_ = rhs.R();
    return *this;
}

SO3 SO3::operator*(const SO3& rhs) const
{
    Mat3 res = R_ * rhs.R();
    return SO3(res);
}

SO3 SO3::mul(const SO3& rhs) const
{
    return (*this) * rhs;
}

void SO3::update_lhs(const Mat31 &dw)
{
    SO3 dR(dw);
    R_ = dR.R() * R_;
}

void SO3::update_rhs(const Mat31 &dw)
{
    SO3 dR(dw);
    R_ = R_ * dR.R();
}

Mat31 mrob::vee3(const Mat3 &w_hat)
{
    Mat31 w;
    w << -w_hat(1,2), w_hat(0,2), -w_hat(0,1);
    return w;
}

Mat3 mrob::hat3(const Mat31 &w)
{
    Mat3 w_hat;
    w_hat <<     0.0, -w(2),  w(1),
                w(2),   0.0, -w(0),
               -w(1),  w(0),   0.0;
    return w_hat;
}

void SO3::exp(const Mat3 &w_hat)
{
    Mat31 w = vee3(w_hat);
    double o = w.norm();
    double c1,c2;
    // See numerical_test.cpp to justify this thershold
    if ( o < 1e-5){
        // sin(o)/o = 1 - x^2/3! + x^4/5! + O(x^6)
        c1 =  1 - o*o/6.0;
        // (1-cos(o))/o^2 = 0.5 + 1/2*f''*x^2/ + ... , where f'' = 1/12
        c2 = 0.5 - o*o/24.0;
    }
    else
    {
        // Standard case with the well-known Rodriguez formula
        c1 = std::sin(o)/o;
        c2 = (1 - std::cos(o))/o/o;
    }
    R_ << Mat3::Identity() + c1 * w_hat + c2 * w_hat *w_hat;
}

Mat3 SO3::ln(double *ro) const
{
    // Logarithmic mapping of the rotations
    Mat3 res;
    double tr = (R_.trace()-1)*0.5;
    double o = std::acos(tr); //image in [0,pi]
    // We choose a tolerance of this small value since the solution involves R being almost symetric
    if ( tr > -1.0 + 1e0)
    {
        // We will evaluate 3 cases:
        // 1) o (angle) is NaN because trace was exactly 1 plus a small round off, so acos is not defined
        double d1;
        // Special case tr =1  and theta -> 0
        if ( std::isnan(o) )
        {
            d1 = 0.0;
            o = 0.0;
        }
        // 2) incorrect x/sin when approaching 0. The other problematic point o = pi is handled below
        // We choose this value since Taylor improves over the numerical result of the program (see numericap_test.cpp)
        else if( o < 1e-5)
        {
            // Taylor expansion around 0
            d1 = 0.5 + o*o/12;
        }
        // 3) normal case
        else
        {
            d1 = 0.5 * o / std::sin(o);
        }
        res << d1 * ( R_ - R_.transpose());
    }
    else
    {
        // Special case tr = -1  so theta = + pi or multiples
        // Again, we handle nan's assuming they express exact +-pi plus a numerical error
        // The second condition stand for the error in acos. Very close to pi it is better (in error) to assume
        // a rotation of exactly pi
        if ( std::isnan(o) || M_PI - o < 6e-8 )
        {
            o = M_PI;// exact case for theta
        }
        // In this case we assume theta is well calculated, but given the almost symetry conditions
        // on the expansion that allow to solve the problem this way.
        else
        {
            // The result of acos is good enough
            //std::cout << std::setprecision(20) << M_PI - o << std::endl;
        }
        // As we approach pi, the exponent(theta) becomes:
        // R = I + 0 + (1-cos)/o^2)W^2, which evaluated at +pi = 2/pi^2
        // This rotation is almost symmetric R = Rt and W = hat(w)
        // We can consider the first order term sin/o negligible.
        //
        // From here, we know that W^2 = ww^t - theta^2I, (you can span W^2 to see this)
        // which leaves R = I + 2/pi2 (wwt - pi2 I)
        // R+I = 2/pi2 wwt
        // wwt = pi2 / 2 (R+I)
        // so we find the maximum row and apply that formula
        // knowing that norm(w) = pi (theta)
        Mat31 w;
        double d = std::cos(o);

        if( R_(0,0) > R_(1,1) && R_(0,0) > R_(2,2) )
        {
            // For stability, we average the two elements since it is almost symetric
            w << R_(0,0) - d,
                 0.5 * ( R_(0,1) + R_(1,0)),
                 0.5 * ( R_(0,2) + R_(2,0));
        }
        else if( R_(1,1) > R_(0,0) && R_(1,1) > R_(2,2) )
        {
            w << 0.5 * ( R_(1,0) + R_(0,1)),
                 R_(1,1) - d,
                 0.5 * ( R_(1,2) + R_(2,1));
        }
        else
        {
            w << 0.5 * ( R_(2,0) + R_(0,2)),
                 0.5 * ( R_(2,1) + R_(1,2)),
                 R_(2,2) - d;
        }
        // normalize the vector w, such that norm(w) = theta
        double length = w.norm();
        if (length > 0.0)
        {
            w *= o / length;
        }
        else
        {
            w << 0.0, 0.0, 0.0;
        }
        res = hat3(w);

        // NOTE: this does not work for very very small theta, which is expectedsince at pi the axis is undefined
        // The problem of this approach is that we loose the sign of rotation, so we try to estimate it
        // by comparing R with the 1st order of Exp(w) and Exp(-w).
        if( (Mat3::Identity() + res - R_ ).norm() >  (Mat3::Identity() - res - R_).norm() )
        {
            res *= -1.0;
            // Note that o is the absolute value of the angle of rotation (don't flip)
        }
    }
    if (ro != nullptr) *ro = o;
    return res;
}

Mat31 SO3::ln_vee() const
{
    Mat3 w_hat = this->ln();
    return vee3(w_hat);
}

SO3 SO3::inv(void) const
{
    return SO3(R_.transpose());
}

Mat3 SO3::adj() const
{
    return R_;
}

Mat3 SO3::R() const
{
    return R_;
}

Mat3& SO3::ref2R()
{
    return R_;
}

double SO3::distance(const SO3 &rhs) const
{
    return (*this * rhs.inv()).ln_vee().norm();
}

void SO3::print(void) const
{
    std::cout << R_ << std::endl;
}

void SO3::print_lie(void) const
{

    Mat31 w =  this->ln_vee();
    std::cout << w << std::endl;
}

bool mrob::isSO3(Mat3 R)
{
    matData_t det = R.determinant();
    if (det  < 0)
        return false;
    if ( fabs(det - 1.0) > 1e-6)
        return false;
    return true;

}

Mat3 mrob::quat_to_so3(const Eigen::Ref<const Mat41> v)
{

    Eigen::Quaternion<matData_t> q(v);
    //std::cout << "Initial vector : " << v << ", transformed quaternion" << q.vec() << "\n and w = \n" << q.toRotationMatrix() << std::endl;
    return q.normalized().toRotationMatrix();
}

// quaternion q = [qx, qy, qz, qw](Eigen convention)
Mat41 mrob::so3_to_quat(const Eigen::Ref<const Mat3> R)
{
    Eigen::Quaternion<matData_t> q(R);
    Mat41 res;
    res << q.x(), q.y(), q.z(), q.w();
    return res;
}

Mat3 mrob::rpy_to_so3(const Eigen::Ref<const Mat31> v)
{
    Mat3 R;
    R = Eigen::AngleAxisd(v(0), Eigen::Vector3d::UnitX())
          * Eigen::AngleAxisd(v(1), Eigen::Vector3d::UnitY())
          * Eigen::AngleAxisd(v(2), Eigen::Vector3d::UnitZ());
    return R;
}


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

#ifndef RW_MATH_POLYNOMIALND_HPP_
#define RW_MATH_POLYNOMIALND_HPP_

/**
 * @file PolynomialND.hpp
 *
 * \copydoc rw::math::PolynomialND
 */
#if !defined(SWIG)
#include <rw/core/macros.hpp>

#include <Eigen/Core>
#include <sstream>
#include <vector>
#endif

namespace rw { namespace math {
    //! @addtogroup math
#if !defined(SWIG)
    //! @{

#endif
    /**
     * @brief Representation of a polynomial that can have non-scalar coefficients (polynomial
     * matrix).
     *
     * Representation of a polynomial of the following form:
     *
     * @f$
     *  f(x) = C_n x^n + C_(n-1) x^(n-1) + C_2 x^2 + C_1 x + C_0
     * @f$
     *
     * The polynomial is represented as a list of coefficients ordered from lowest-order term to
     * highest-order term, \f$ {c_0,c_1,...,c_n}\f$.
     */
    template< typename Coef, typename Scalar = double > class PolynomialND
    {
      public:
        /**
         * @brief Create polynomial with uninitialized coefficients.
         * @param order [in] the order of the polynomial.
         */
        explicit PolynomialND (std::size_t order) : _coef (std::vector< Coef > (order + 1)) {}

        /**
         * @brief Create polynomial from vector.
         * @param coefficients [in] the coefficients ordered from lowest-order term to highest-order
         * term.
         */
        PolynomialND (const std::vector< Coef >& coefficients) : _coef (coefficients) {}

        /**
         * @brief Create polynomial from other polynomial.
         * @param p [in] the polynomial to copy.
         */
        PolynomialND (const PolynomialND< Coef, Scalar >& p) :
            _coef (std::vector< Coef > (p.order () + 1))
        {
            for (std::size_t i = 0; i <= p.order (); i++)
                _coef[i] = p[i];
        }

        /**
         * @brief Destructor
         */
        virtual ~PolynomialND () {}

        /**
         * @brief Get the order of the polynomial (the highest power).
         * @return the order.
         */
        std::size_t order () const { return _coef.size () - 1; }

        /**
         * @brief Increase the order of this polynomial.
         * @param increase [in] how much to increase the order (default is 1).
         * @param value [in] initialize new coefficients to this value.
         */
        void increaseOrder (std::size_t increase, const Coef& value)
        {
            const std::size_t size = _coef.size ();
            _coef.resize (size + increase);
            for (std::size_t i = size; i < size + increase; i++) {
                _coef[i] = value;
            }
        }
#if !defined(SWIGJAVA)

        /**
         * @brief Increase the order of this polynomial.
         * @param increase [in] how much to increase the order (default is 1).
         * @see increaseOrder(std::size_t,const Coef&) for a version that initializes the new
         * coefficients to a certain value.
         */
#endif
        void increaseOrder (std::size_t increase = 1)
        {
            const std::size_t size = _coef.size ();
            _coef.resize (size + increase);
        }

        /**
         * @brief Evaluate the polynomial using Horner's Method.
         * @param x [in] the input parameter.
         * @return the value \f$ f(x)\f$.
         */
        Coef evaluate (const Scalar& x) const
        {
            // Horner's Method
            Coef res = _coef.back ();
            for (int i = static_cast< int > (_coef.size () - 2); i >= 0; i--)
                res = _coef[i] + res * x;
            return res;
        }

        /**
         * @brief Evaluate the first \b n derivatives of the polynomial using Horner's Method.
         * @param x [in] the input parameter.
         * @param n [in] the number of derivatives to find (default is the first derivative only)
         * @return a vector of values \f$ {f(x),\dot{f}(x),\ddot{f}(x),\cdots}\f$.
         */
        std::vector< Coef > evaluateDerivatives (const Scalar& x, std::size_t n = 1) const
        {
            // Horner's Method
            std::vector< Coef > res (n + 1);
            res[0] = _coef.back ();
            for (int i = static_cast< int > (_coef.size () - 2); i >= 0; i--) {
                int minJ = static_cast< int > (std::min< std::size_t > (n, _coef.size () - 1 - i));
                for (int j = minJ; j > 0; j--) {
                    res[j] = res[j - 1] + res[j] * x;
                }
                res[0] = _coef[i] + res[0] * x;
            }
            Scalar k = 1;
            for (std::size_t i = 2; i <= n; i++) {
                const Scalar kInit = k;
                for (std::size_t j = 0; j < i - 1; j++) {
                    k += kInit;
                }
                res[i] *= k;
            }
            return res;
        }

        /**
         * @brief Perform deflation of polynomial.
         * @param x [in] a root of the polynomial.
         * @return a new polynomial of same order minus one.
         * @note There is no check that the given root is in fact a root of the polynomial.
         */
        PolynomialND< Coef, Scalar > deflate (const Scalar& x) const
        {
            // Horner Method
            std::size_t no = order () - 1;
            PolynomialND< Coef, Scalar > res (no);
            res[no] = _coef.back ();
            for (int i = (int) no - 1; i >= 0; i--) {
                res[i] = x * res[i + 1] + _coef[i + 1];
            }
            return res;
        }

        /**
         * @brief Get the derivative polynomial.
         * @param n [in] gives the n'th derivative (default is n=1).
         * @return a new polynomial of same order minus one.
         * @note To evaluate derivatives use the evaluate derivative method which is more precise.
         */
        PolynomialND< Coef, Scalar > derivative (std::size_t n = 1) const
        {
            if (n == 0)
                return *this;
            std::size_t no = order () - 1;
            PolynomialND< Coef, Scalar > der (no);
            for (std::size_t i = 1; i <= order (); i++)
                der[i - 1] = (Coef) (_coef[i] * double(i));
            return der.derivative (n - 1);
        }

        /**
         * @name Coefficient access operators.
         * Operators used to access coefficients.
         */
#if !defined(SWIG)
        ///@{

        /**
         * @brief Get specific coefficient.
         * @param i [in] the power of the term to get coefficient for.
         * @return the coefficient.
         */
        const Coef& operator() (std::size_t i) const
        {
            if (i > order ()) {
                std::stringstream str;
                str << "Polynomial of order " << order () << " has no coefficient with index " << i;
                RW_THROW (str.str ());
            }
            return _coef[i];
        }

        /**
         * @brief Get specific coefficient.
         * @param i [in] the power of the term to get coefficient for.
         * @return the coefficient.
         */
        Coef& operator() (size_t i)
        {
            if (i > order ()) {
                std::stringstream str;
                str << "Polynomial of order " << order () << " has no coefficient with index " << i;
                RW_THROW (str.str ());
            }
            return _coef[i];
        }

        /**
         * @brief Get specific coefficient.
         * @param i [in] the power of the term to get coefficient for.
         * @return the coefficient.
         */
        const Coef& operator[] (size_t i) const
        {
            if (i > order ()) {
                std::stringstream str;
                str << "Polynomial of order " << order () << " has no coefficient with index " << i;
                RW_THROW (str.str ());
            }
            return _coef[i];
        }

        /**
         * @brief Get specific coefficient.
         * @param i [in] the power of the term to get coefficient for.
         * @return the coefficient.
         */
        Coef& operator[] (size_t i)
        {
            if (i > order ()) {
                std::stringstream str;
                str << "Polynomial of order " << order () << " has no coefficient with index " << i;
                RW_THROW (str.str ());
            }
            return _coef[i];
        }
#else

        ARRAYOPERATOR (Coef);

#endif
#if !defined(SWIG)
///@}
#endif

        /**
         * @name Arithmetic operators between polynomial and scalars.
         * Operators used to do arithmetic with scalars.
         */

#if !defined(SWIG)
        ///@{
#endif
        /**
         * @brief Scalar multiplication
         * @param s [in] scalar to multiply with.
         * @return new polynomial after multiplication.
         */
        const PolynomialND< Coef, Scalar > operator* (Scalar s) const
        {
            PolynomialND< Coef, Scalar > pol (order ());
            for (std::size_t i = 0; i <= order (); i++) {
                pol[i] = _coef[i] * s;
            }
            return pol;
        }

        /**
         * @brief Scalar division
         * @param s [in] scalar to divide with.
         * @return new polynomial after division.
         */
        const PolynomialND< Coef, Scalar > operator/ (Scalar s) const
        {
            PolynomialND< Coef, Scalar > pol (order ());
            for (std::size_t i = 0; i <= order (); i++) {
                pol[i] = _coef[i] / s;
            }
            return pol;
        }

        /**
         * @brief Scalar multiplication
         * @param s [in] the scalar to multiply with.
         * @return reference to same polynomial with changed coefficients.
         */
        PolynomialND< Coef, Scalar >& operator*= (Scalar s)
        {
            for (std::size_t i = 0; i <= order (); i++) {
                _coef[i] *= s;
            }
            return *this;
        }

        /**
         * @brief Scalar division
         * @param s [in] the scalar to divide with.
         * @return reference to same polynomial with changed coefficients.
         */
        PolynomialND< Coef, Scalar >& operator/= (Scalar s)
        {
            for (std::size_t i = 0; i <= order (); i++) {
                _coef[i] /= s;
            }
            return *this;
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar multiplication
         * @param s [in] scalar to multiply with.
         * @param p [in] polynomial to multiply with.
         * @return new polynomial after multiplication.
         */
        friend const PolynomialND< Coef, Scalar > operator* (Scalar s,
                                                             const PolynomialND< Coef, Scalar >& p)
        {
            PolynomialND< Coef, Scalar > pol (p.order ());
            for (std::size_t i = 0; i <= p.order (); i++) {
                pol[i] = p[i] * s;
            }
            return pol;
        }
#endif
#if !defined(SWIG)
        ///@}
#endif

        /**
         * @name Arithmetic operators between polynomials.
         * Operators used to do arithmetic between two polynomials.
         */

#if !defined(SWIG)
        ///@{
#endif

        /**
         * @brief Polynomial subtraction.
         * @param b [in] polynomial of to subtract.
         * @return new polynomial after subtraction.
         */
        const PolynomialND< Coef, Scalar > operator- (const PolynomialND< Coef, Scalar >& b) const
        {
            const std::size_t thisOrder = order ();
            const std::size_t bOrder    = b.order ();
            if (bOrder > thisOrder) {
                PolynomialND< Coef, Scalar > pol (bOrder);
                for (std::size_t i = 0; i <= thisOrder; i++) {
                    pol[i] = (*this)[i] - b[i];
                }
                for (std::size_t i = thisOrder + 1; i <= bOrder; i++) {
                    pol[i] = -b[i];
                }
                return pol;
            }
            else {
                PolynomialND< Coef, Scalar > pol (thisOrder);
                for (std::size_t i = 0; i <= bOrder; i++) {
                    pol[i] = (*this)[i] - b[i];
                }
                for (std::size_t i = bOrder + 1; i <= thisOrder; i++) {
                    pol[i] = (*this)[i];
                }
                return pol;
            }
        }

        /**
         * @brief Polynomial subtraction.
         * @param b [in] polynomial to subtract.
         * @return same polynomial with different coefficients after subtraction.
         */
        PolynomialND< Coef, Scalar >& operator-= (const PolynomialND< Coef, Scalar >& b)
        {
            const std::size_t thisOrder = order ();
            const std::size_t bOrder    = b.order ();
            if (bOrder > thisOrder) {
                increaseOrder (bOrder - thisOrder);
                for (std::size_t i = 0; i <= thisOrder; i++) {
                    (*this)[i] -= b[i];
                }
                for (std::size_t i = thisOrder + 1; i <= bOrder; i++) {
                    (*this)[i] = -b[i];
                }
            }
            else {
                for (std::size_t i = 0; i <= bOrder; i++) {
                    (*this)[i] -= b[i];
                }
            }
            return *this;
        }

        /**
         * @brief Polynomial addition.
         * @param b [in] polynomial to add.
         * @return new polynomial after addition.
         */
        const PolynomialND< Coef, Scalar > operator+ (const PolynomialND< Coef, Scalar >& b) const
        {
            const std::size_t thisOrder = order ();
            const std::size_t bOrder    = b.order ();
            if (bOrder > thisOrder) {
                PolynomialND< Coef, Scalar > pol (bOrder);
                for (std::size_t i = 0; i <= thisOrder; i++) {
                    pol[i] = (*this)[i] + b[i];
                }
                for (std::size_t i = thisOrder + 1; i <= bOrder; i++) {
                    pol[i] = b[i];
                }
                return pol;
            }
            else {
                PolynomialND< Coef, Scalar > pol (thisOrder);
                for (std::size_t i = 0; i <= bOrder; i++) {
                    pol[i] = (*this)[i] + b[i];
                }
                for (std::size_t i = bOrder + 1; i <= thisOrder; i++) {
                    pol[i] = (*this)[i];
                }
                return pol;
            }
        }

        /**
         * @brief Polynomial addition.
         * @param b [in] polynomial to add.
         * @return same polynomial with different coefficients after addition.
         */
        PolynomialND< Coef, Scalar >& operator+= (const PolynomialND< Coef, Scalar >& b)
        {
            const std::size_t thisOrder = order ();
            const std::size_t bOrder    = b.order ();
            if (bOrder > thisOrder) {
                increaseOrder (bOrder - thisOrder);
                for (std::size_t i = 0; i <= thisOrder; i++) {
                    (*this)[i] += b[i];
                }
                for (std::size_t i = thisOrder + 1; i <= bOrder; i++) {
                    (*this)[i] = b[i];
                }
            }
            else {
                for (std::size_t i = 0; i <= bOrder; i++) {
                    (*this)[i] += b[i];
                }
            }
            return *this;
        }

        /**
         * @brief Polynomial multiplication.
         *
         * A convolution of the coefficients is used.
         * Notice that more efficient algorithms exist for polynomials with scalar coefficients.
         *
         * @param b [in] polynomial to multiply. Post-multiplication is used - the dimensions must
         * match.
         * @return new polynomial after multiplication.
         */
        template< typename OutCoef, typename Coef2 = Coef >
        PolynomialND< OutCoef, Scalar > multiply (const PolynomialND< Coef2, Scalar >& b) const
        {
            const std::size_t ord = order () + b.order ();
            PolynomialND< OutCoef, Scalar > pol (ord);
            for (std::size_t k = 0; k <= ord; k++) {
                const std::size_t firstJ = (k < b.order ()) ? 0 : k - b.order ();
                pol[ord - k]             = _coef[order () - firstJ] * b[b.order () - (k - firstJ)];
                for (std::size_t j = firstJ + 1; j <= std::min (k, order ()); j++) {
                    pol[ord - k] += _coef[order () - j] * b[b.order () - (k - j)];
                }
            }
            return pol;
        }

        /**
         * @brief Multiply with a coefficient.
         *
         * Each coefficient is post-multiplied with the given coefficient.
         *
         * @param b [in] coefficient to multiply with. Post-multiplication is used - the dimensions
         * must match.
         * @return new polynomial after multiplication.
         */
        template< typename OutCoef, typename Coef2 = Coef >
        PolynomialND< OutCoef, Scalar > multiply (const Coef2& b) const
        {
            PolynomialND< OutCoef, Scalar > pol (order ());
            for (std::size_t i = 0; i <= order (); i++) {
                pol[i] = _coef[i] * b;
            }
            return pol;
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Assignment.
         * @param b [in] the polynomial to take coefficients from.
         */
        void operator= (const PolynomialND< Coef, Scalar >& b)
        {
            _coef.resize (b.order () + 1);
            for (size_t i = 0; i <= b.order (); i++)
                _coef[i] = b[i];
        }
#endif
#if !defined(SWIG)
///@}
#endif

        /**
         * @brief Negate coefficients.
         * @return new polynomial with coefficients negated.
         */
        const PolynomialND< Coef, Scalar > operator- () const
        {
            PolynomialND< Coef, Scalar > pol (order ());
            for (std::size_t i = 0; i <= order (); i++) {
                pol[i] = (Coef) (_coef[i] * double(-1));
            }
            return pol;
        }
#if !defined(SWIG)
        /**
         * @brief Printing polynomial to stream.
         * @param out [in/out] the stream to write to.
         * @param p [in] the polynomail to print.
         * @return the same ostream as out parameter.
         */
        friend std::ostream& operator<< (std::ostream& out, const PolynomialND< Coef, Scalar >& p)
        {
            out << "Polynomial: ";
            for (size_t i = p.order (); i > 0; i--)
                out << p[i] << " x^" << i << " + ";
            out << p[0];
            return out;
        }
#else
#define tmp Coef, Scalar
        TOSTRING (rw::math::PolynomialND< tmp >);
#endif

        /**
         * @brief Check if polynomials are equal.
         * @param b [in] the polynomial to compare with.
         * @return true if equal, false if not.
         */
        bool operator== (const PolynomialND< Coef, Scalar >& b) const
        {
            for (size_t i = 0; i <= order (); i++)
                if (_coef[i] != b[i])
                    return false;
            return true;
        }

      protected:
        //! @brief The coefficient vector.
        std::vector< Coef > _coef;
    };

    /**
     * @brief Multiply 3D polynomial matrix with 3D polynomial vector.
     * @param A [in] the matrix expression.
     * @param b [in] the vector expression.
     * @return a 3D polynomial vector.
     */
    PolynomialND< Eigen::Vector3d > operator* (const PolynomialND< Eigen::Matrix3d >& A,
                                               const PolynomialND< Eigen::Vector3d >& b);

    /**
     * @brief Multiply 3D polynomial vector with 3D polynomial matrix.
     * @param a [in] the vector expression.
     * @param A [in] the matrix expression.
     * @return a 3D polynomial vector.
     */
    PolynomialND< Eigen::Matrix< double, 1, 3 > >
    operator* (const PolynomialND< Eigen::Matrix< double, 1, 3 > >& a,
               const PolynomialND< Eigen::Matrix3d >& A);
# if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix3d>&, const
    //! PolynomialND<Eigen::Vector3d>&)

    #endif
    PolynomialND< Eigen::Vector3d > operator* (const PolynomialND< Eigen::Matrix3d >& A,
                                               const Eigen::Vector3d& b);
#if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix<double,1,3> >&, const
    //! PolynomialND<Eigen::Matrix3d>&)

    #endif
    PolynomialND< Eigen::Matrix< double, 1, 3 > >
    operator* (const PolynomialND< Eigen::Matrix< double, 1, 3 > >& a, const Eigen::Matrix3d& A);

#if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix3d>&, const
    //! PolynomialND<Eigen::Vector3d>&)

    #endif
    PolynomialND< Eigen::Vector3f, float >
    operator* (const PolynomialND< Eigen::Matrix3f, float >& A,
               const PolynomialND< Eigen::Vector3f, float >& b);
#if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix<double,1,3> >&, const
    //! PolynomialND<Eigen::Matrix3d>&)

    #endif
    PolynomialND< Eigen::Matrix< float, 1, 3 >, float >
    operator* (const PolynomialND< Eigen::Matrix< float, 1, 3 >, float >& a,
               const PolynomialND< Eigen::Matrix3f, float >& A);
#if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix3d>&, const Eigen::Vector3d&)

    #endif
    PolynomialND< Eigen::Vector3f, float >
    operator* (const PolynomialND< Eigen::Matrix3f, float >& A, const Eigen::Vector3f& b);

#if !defined(SWIGJAVA)
    //! @copydoc operator*(const PolynomialND<Eigen::Matrix<double,1,3> >&, const Eigen::Matrix3d&)

    #endif
    PolynomialND< Eigen::Matrix< float, 1, 3 >, float >
    operator* (const PolynomialND< Eigen::Matrix< float, 1, 3 >, float >& a,
               const Eigen::Matrix3f& A);

    //! @}
}}    // namespace rw::math

#endif /* RW_MATH_POLYNOMIALND_HPP_ */


#ifndef BFGS_HPP
#define BFGS_HPP

#include <Eigen/Dense>
#include <limits>

/** Damped BFGS update
 * Implements "Procedure 18.2 Damped BFGS updating for SQP" form Numerical Optimization by Nocedal.
 *
 * @param[in,out]   B hessian matrix, is updated by this function
 * @param[in]       s step vector (x - x_prev)
 * @param[in]       y gradient change (grad - grad_prev)
 */
template <typename Mat, typename Vec>
void BFGS_update(Mat& B, const Vec& s, const Vec& y) {
    using Scalar = typename Mat::Scalar;
    Scalar sy, sr, sBs;
    Vec Bs, r;

    Bs.noalias() = B * s;
    sBs = s.dot(Bs);
    sy = s.dot(y);

    if (sy < 0.2 * sBs) {
        // damped update to enforce positive definite B
        Scalar theta;
        theta = 0.8 * sBs / (sBs - sy);
        r.noalias() = theta * y + (1 - theta) * Bs;
        sr = theta * sy + (1 - theta) * sBs;
    } else {
        // unmodified BFGS
        r = y;
        sr = sy;
    }

    if (sr < std::numeric_limits<Scalar>::epsilon()) {
        return;
    }

    B.noalias() += -Bs * Bs.transpose() / sBs + r * r.transpose() / sr;
}

// extern template void BFGS_update<Eigen::MatrixXd, Eigen::VectorXd>(Eigen::MatrixXd& B,
//                                                                    const Eigen::VectorXd& s,
//                                                                    const Eigen::VectorXd& y)

#endif /* BFGS_HPP */


#include "../Util/MercatorUtil.h"
#include "../Util/ComputeAngle.h"
#include "../Util/SimpleLogger.h"

#include <osrm/Coordinate.h>

#include "ConcaveHull.h"

#include <algorithm>
#include <set>
#include <vector>
#include <limits>
#include <valarray>
#include <boost/foreach.hpp>

float getAngle(const FixedPointCoordinate& prev, const FixedPointCoordinate& curr, const FixedPointCoordinate& next, float *outAngle) {
	int prev_x = prev.lon , prev_y = prev.lat;
	int curr_x = curr.lon , curr_y = curr.lat;
	int next_x = next.lon , next_y = next.lat;
	if (next_x == curr_x && next_y == curr_y)
		return -9000.f;
	if (next_x == prev_x && next_y == prev_y)
		return -360.f;

	int a_x = curr_x - prev_x;
	int a_y = curr_y - prev_y;
	int b_x = next_x - curr_x;
	int b_y = next_y - curr_y;
	float vect = a_x*b_y - b_x*a_y;
	float scal = a_x*b_x + a_y*b_y;
	float angle = 0;
	if (scal == 0){
		if (vect > 0)
			angle = 90.f;
		if (vect < 0)
			angle = -90.f;
	}
	else {
		angle = atan(vect / scal) * 180.f / M_PI;
		if (scal < 0){
			if (vect >= 0)
				angle += 180.f;
			if (vect < 0)
				angle -= 180.f;
		}
	}
	if (angle == 360.f)
		angle = 0;
	*outAngle = 180.f - angle;
	return *outAngle;
}

int ccw(const FixedPointCoordinate& p0, const FixedPointCoordinate& p1, const FixedPointCoordinate& p2) {
	int dx1 = p1.lon - p0.lon;
	int dy1 = p1.lat - p0.lat;
	int dx2 = p2.lon - p0.lon;
	int dy2 = p2.lat - p0.lat;
	int d = dx1*dy2 - dy1*dx2;
	if (d > 0) return 1;
	if (d < 0) return -1;
	if ((dx1*dx2 < 0) || (dy1*dy2 < 0)) return -1;
	if ((dx1*dx1 + dy1*dy1) < (dx2*dx2 + dy2*dy2)) return 1;
	return 0;
}

bool intersect(const FixedPointCoordinate& p1, const FixedPointCoordinate& p2, const FixedPointCoordinate& q1, const FixedPointCoordinate& q2) {
	if ((p1.lon == q1.lon && p1.lat == q1.lat || p2.lon == q2.lon && p2.lat == q2.lat) ||
		(p1.lon == q2.lon && p1.lat == q2.lat || p2.lon == q1.lon && p2.lat == q1.lat))
		return false;

	return (ccw(p1, p2, q1) * ccw(p1, p2, q2) <= 0) && (ccw(q1, q2, p1) * ccw(q1, q2, p2) <= 0);
}

bool intersect(const std::vector<FixedPointCoordinate>& hull, const FixedPointCoordinate& next) {
	int last = hull.size() - 1;
	for (int i = 1; i < last; i++){
		if (intersect(hull[i - 1], hull[i], hull[last], next))
			return true;
	}
	return false;
}

inline bool isNear(const FixedPointCoordinate& a, const FixedPointCoordinate& b, int maxLat, int maxLon){
	return (abs(a.lat - b.lat) < maxLat && abs(a.lon - b.lon) < maxLon);
}

void calculateHull(const std::vector<FixedPointCoordinate>& points, std::vector<FixedPointCoordinate>& hull, int maxLat, int maxLon) {

	hull.clear();

	if (points.empty())
		return;

	//hull = points;
	//return;
	std::vector<int> counts(points.size(), 0);

	int start = 0;
	for (int i = 1; i < points.size(); i++) {
		if (points[i].lat < points[start].lat)
			start = i;
	}
	hull.push_back(points[start]);

	int curr = start;
	int next = -1;
	const FixedPointCoordinate init(points[start].lat - 10, points[start].lon);
	const FixedPointCoordinate *prev = &init;

#ifdef MyDEBUG
	SimpleLogger().Write(logDEBUG) << "Init: " << init;
	SimpleLogger().Write(logDEBUG) << "Start: " << start << ": " << points[start];
#endif
	while (next != start) {
		next = -1;
		float max_angle = -10000.f;
		int min_rot = std::numeric_limits<int>::max();
		for (int i = 0; i < points.size(); i++) {
#ifdef MyDEBUG
			SimpleLogger().Write(logDEBUG) << i << ": " << points[i]
				<< " dist: " << FixedPointCoordinate::ApproximateEuclideanDistance(points[i], points[curr])
				<< " [" << (FixedPointCoordinate::ApproximateEuclideanDistance(points[i], points[curr]) < 1.2 * MAX_POINTS_DIST ? "v" : " ") << "]"
				<< " rough: lat " << abs(points[i].lat - points[curr].lat) << " lon " << abs(points[i].lon - points[curr].lon)
				<< " [" << (isNear(points[i], points[curr], maxLat, maxLon) ? "v" : " ") << "]"

				<< " angle: " << getAngle(*prev, points[curr], points[i])
				//<< " rot: " << getRot(*prev, points[curr], points[i])
				<< " intersect:" << intersect(hull, points[i]);
#endif
			float angle = -10000.f;
			if (i != curr && isNear(points[i], points[curr], maxLat, maxLon)
				&& getAngle(*prev, points[curr], points[i], &angle) > max_angle
				&& !intersect(hull, points[i]))
			{
				max_angle = angle;
				next = i;
			}
		}
		//BOOST_ASSERT(next >= 0);
		if (next < 0) {
			SimpleLogger().Write(logWARNING) << "Error in hull calculations.";
			hull.push_back(points[start]);
			return;
		}
		hull.push_back(points[next]);
		if (++counts[next] > 4){
			SimpleLogger().Write(logWARNING) << "Something goes wrong in hull calculations.";
			hull.push_back(points[start]);
			return;
		}

#ifdef MyDEBUG
		SimpleLogger().Write(logDEBUG) << curr << " -> " << next;
#endif
		prev = &points[curr];
		curr = next;
	}
	return;
};

void concaveHull(const std::set<FixedPointCoordinate>& coordinates, std::vector<FixedPointCoordinate>& hull) {

	if (coordinates.empty())
		return;

	const FixedPointCoordinate& start = *coordinates.begin();

	// calculate scale by lat and scale by lon for current geographic area
	FixedPointCoordinate south(start.lat - 100, start.lon);
	FixedPointCoordinate east(start.lat, start.lon - 100);
	int maxLat = 100 * MAX_POINTS_DIST / FixedPointCoordinate::ApproximateDistance(south, start);
	int maxLon = 100 * MAX_POINTS_DIST / FixedPointCoordinate::ApproximateDistance(east, start);

	std::vector<FixedPointCoordinate> points;

	points.reserve(coordinates.size() + 1);
	int min_point = 0, i = 0;
	BOOST_FOREACH(FixedPointCoordinate coord, coordinates)
	{
		points.push_back(coord);
		if (points[i++].lat < points[min_point].lat)
			min_point = i;
	}
	south.lat = points[min_point].lat - maxLat;
	south.lon = points[min_point].lon;
	points.push_back(south);

	calculateHull(points, hull, maxLat*1.2, maxLon*1.2);
	hull[0] = hull[1];
	if (hull.size() > 1)
		hull[hull.size() - 1] = hull[hull.size() - 2];
	SimpleLogger().Write(logINFO) << "First hull: " << hull.size() << " points. Expanding...";

	int dLat = maxLat / 1.4142135623730950488016887242097;
	int dLon = maxLon / 1.4142135623730950488016887242097;

	std::vector<FixedPointCoordinate> expanded;
	expanded.reserve((hull.size() - 1) * 8);
	for (int i = 0; i < hull.size() - 1; i++) {
		FixedPointCoordinate p0(hull[i].lat + maxLat, hull[i].lon);
		expanded.push_back(p0);
		FixedPointCoordinate p1(hull[i].lat - maxLat, hull[i].lon);
		expanded.push_back(p1);
		FixedPointCoordinate p2(hull[i].lat, hull[i].lon + maxLon);
		expanded.push_back(p2);
		FixedPointCoordinate p3(hull[i].lat, hull[i].lon - maxLon);
		expanded.push_back(p3);
		FixedPointCoordinate p4(hull[i].lat + dLat, hull[i].lon + dLon);
		expanded.push_back(p4);
		FixedPointCoordinate p5(hull[i].lat - dLat, hull[i].lon - dLon);
		expanded.push_back(p5);
		FixedPointCoordinate p6(hull[i].lat - dLat, hull[i].lon + dLon);
		expanded.push_back(p6);
		FixedPointCoordinate p7(hull[i].lat + dLat, hull[i].lon - dLon);
		expanded.push_back(p7);
	}
	calculateHull(expanded, hull, maxLat*1.2, maxLon*1.2);

}


#include <state_estimation/filters/kalman_filter_vs.h>
#include <state_estimation/utilities/data_subset_utilities.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

void KalmanFilterVS::myPredict(const Eigen::VectorXd& u, double dt) {
    system_model_->update(filter_state_.x, u, dt);

    // Get our sub matrices and vectors
    const Eigen::VectorXd x_subset = getSubset(filter_state_.x, system_model_->activeStates());
    const Eigen::VectorXd u_subset = getSubset(u, system_model_->activeControls());
    const Eigen::MatrixXd cov_subset =
        getSubset(filter_state_.covariance, system_model_->activeStates());
    const Eigen::MatrixXd A_subset = getSubset(system_model_->A(), system_model_->activeStates());
    const Eigen::MatrixXd B_subset = getSubset(system_model_->B(), system_model_->activeStates(),
                                               system_model_->activeControls());
    const Eigen::MatrixXd Rc_subset =
        getSubset(system_model_->Rc(), system_model_->activeControls());
    const Eigen::MatrixXd P_subset =
        getSubset(system_model_->P(), system_model_->activeStates(), {});
    const Eigen::MatrixXd V_subset = getSubset(system_model_->V(), system_model_->activeStates(),
                                               system_model_->activeControls());

    // Update the state
    const Eigen::VectorXd Ax_subset = A_subset * x_subset;
    const Eigen::VectorXd Bu_subset = B_subset * u_subset;
    const Eigen::VectorXd Ax_full = convertSubsetToFullZeroed(
        Ax_subset, system_model_->activeStates(), system_model_->stateSize());
    const Eigen::VectorXd Bu_full = convertSubsetToFullZeroed(
        Bu_subset, system_model_->activeStates(), system_model_->stateSize());

    filter_state_.x = system_model_->addVectors(Ax_full, Bu_full);

    // Update the covariance
    const Eigen::MatrixXd cov_prime_subset = A_subset * cov_subset * A_subset.transpose() +
                                             P_subset * system_model_->Rp() * P_subset.transpose() +
                                             V_subset * Rc_subset * V_subset.transpose();

    convertSubsetToFull(cov_prime_subset, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "KF predicition update:" << std::endl
              << "A=" << std::endl
              << printMatrix(system_model_->A()) << std::endl
              << "B=" << std::endl
              << printMatrix(system_model_->B()) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << filter_state_.x.transpose() << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void KalmanFilterVS::myCorrect(const Eigen::VectorXd& z,
                               measurement_models::LinearMeasurementModel* model, double dt) {
    // Update our measurement model
    model->update(filter_state_.x, dt);

    // Get our sub matrices/vectors
    const Eigen::VectorXd x_subset = getSubset(filter_state_.x, system_model_->activeStates());
    const Eigen::MatrixXd cov_subset =
        getSubset(filter_state_.covariance, system_model_->activeStates());
    const Eigen::MatrixXd C_subset =
        getSubset(model->C(), model->activeMeasurements(), system_model_->activeStates());
    const Eigen::MatrixXd meas_cov_subset =
        getSubset(model->covariance(), model->activeMeasurements());

    // Compute the Kalman gain
    const Eigen::MatrixXd cov_C_T = cov_subset * C_subset.transpose();
    const Eigen::MatrixXd K = cov_C_T * (C_subset * cov_C_T + meas_cov_subset).inverse();

    // Update the state
    const Eigen::VectorXd z_prime_subset = C_subset * x_subset;
    const Eigen::VectorXd z_prime_full = convertSubsetToFullZeroed(
        z_prime_subset, model->activeMeasurements(), model->measurementSize());
    const Eigen::VectorXd dz_full = model->subtractVectors(z, z_prime_full);
    const Eigen::VectorXd dz_subset = getSubset(dz_full, model->activeMeasurements());
    const Eigen::VectorXd dx_subset = K * dz_subset;
    const Eigen::VectorXd dx_full = convertSubsetToFullZeroed(
        dx_subset, system_model_->activeStates(), system_model_->stateSize());

    filter_state_.x = system_model_->addVectors(filter_state_.x, dx_full);

    // Update the covariance
    const Eigen::MatrixXd I = Eigen::MatrixXd::Identity(cov_subset.rows(), cov_subset.rows());
    const Eigen::MatrixXd cov_prime = (I - K * C_subset) * cov_subset;

    convertSubsetToFull(cov_prime, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "KF measurement update:" << std::endl
              << "C=" << std::endl
              << printMatrix(model->C()) << std::endl
              << "z'=" << printMatrix(z_prime_full) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "K=" << std::endl
              << printMatrix(K) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation


//----------------------------------*-C++-*-----------------------------------//
/**
 *  @file  CLP.cc
 *  @brief CLP member definitions
 *  @note  Copyright (C) 2013 Jeremy Roberts
 */
//----------------------------------------------------------------------------//

#include "CLP.hh"
#include "utilities/DBC.hh"
#ifdef DETRAN_ENABLE_BOOST
#include <boost/math/special_functions/legendre.hpp>
#endif

namespace detran_orthog
{

//----------------------------------------------------------------------------//
CLP::CLP(const Parameters &p)
  : ContinuousOrthogonalBasis(p)
{
#ifndef DETRAN_ENABLE_BOOST
  THROW("CLP needs boost to be enabled.");
#else

  // Allocate the basis matrix
  d_basis = new callow::MatrixDense(d_size, d_order + 1, 0.0);

  // Allocate the normalization array
  d_a = Vector::Create(d_order + 1, 0.0);

  // The weights are just the qw's.
  double L = d_upper_bound - d_lower_bound;
  for (size_t i = 0; i < d_w->size(); ++i)
  {
    d_x[i] =  2.0*(d_x[i] - d_lower_bound)/L - 1.0;
    (*d_w)[i] = d_qw[i];
  }

  // Build the basis
  for (size_t l = 0; l <= d_order; ++l)
  {
    for (size_t i = 0; i < d_size; ++i)
    {
      (*d_basis)(i, l) = boost::math::legendre_p(l, d_x[i]);
    }
    // Inverse of normalization coefficient.
    (*d_a)[l] = (2.0 * l + 1.0) / 2.0;
  }
  //d_orthonormal = true;
  //compute_a();
#endif

}

} // end namespace detran_orthog

//----------------------------------------------------------------------------//
//              end of file CLP.cc
//----------------------------------------------------------------------------//


#include <iostream>
#include <fstream>
#include <vector>
#include <random>
#include <Eigen/Dense>
#include "../include/GaussianHMM.h"
using namespace Eigen;

int main() {
  std::cout << "Start ...\n";
  double var = 1.0;
  std::random_device seed;
  std::mt19937 random_number_generator(seed());
  std::normal_distribution<double> white_noise(0.0, var);

  ArrayXd clusters(2);
  ArrayXd start(2);
  ArrayXXd trans(2, 2);
  clusters << 10, 1;
  start << 1, 0;
  trans << 0.88, 0.12, 0.4, 0.6;

  GaussianHMM hmm(2, var, 47, 1000);
  std::cout << "Max epoch - " << hmm.max_epoch << "\n";
  hmm.pi = start;
  hmm.means_ = clusters;
  hmm.A = trans;
  hmm.covars_ = ArrayXd::Ones(2) * var;

  int n_sample = 1000;
  ArrayXi state_seqence(n_sample);
  std::vector<double> X;
  hmm.sample(n_sample, 100, X, state_seqence);

  std::ofstream signal_writer("signal.dat", std::ios_base::out | std::ios_base::trunc);
  signal_writer << "state\tobservation\n";
  for (size_t i = 0; i < X.size(); i++) {
    signal_writer << state_seqence(i) << '\t' << X[i] << '\n';
  }
  signal_writer.close();

  std::cout << "******************* Fitting test *******************\n";
  GaussianHMM fit_hmm(2, 1e-2, -1, 100);
  std::vector<size_t> lengths;
  std::cout << "Start to fit X ...\n";
  fit_hmm.fit(X, lengths);
  std::cout << "Fitted result:\n"
            << "Mean:\n" << fit_hmm.means_ << "\n"
            << "Var: \n" << fit_hmm.covars_ << "\n"
            << "A: \n" << fit_hmm.A << "\n";

  return 0;
}


#include "get_floor_fHf.h"
#include "ransac_floor_fHf.h"
#include "ransac_data.h"
#include "randperm.h"
#include <Eigen/Geometry>
#include <vector>

using namespace Eigen;

RansacData ransac_floor_fHf(MatrixXd &p1, MatrixXd &p2, Matrix3d &R1, Matrix3d &R2, int nbr_iter, double thresh)
{
    // Init RANSAC loop
    PoseData best_posedata;
    int best_nbr_inliers = 0;
    VectorXi best_inliers;
    best_inliers.setZero();
    int nbr_pts = p1.cols();

    MatrixXd reproj1(2, nbr_pts);
    MatrixXd reproj2(2, nbr_pts);
    MatrixXd reproj(4, nbr_pts);
    VectorXd reproj_mean(nbr_pts);
    MatrixXd H(3,3);
    VectorXi inliers(nbr_pts);
    int nbr_inliers;
    VectorXi history(nbr_iter);

    int nbr_pts_minimal = 3;

    for (int i = 0; i < nbr_iter; i++) {
        std::vector<int> rands = randperm(nbr_pts_minimal, nbr_pts);
        MatrixXd x1(3, nbr_pts_minimal);
        MatrixXd x2(3, nbr_pts_minimal);
        for (int j = 0; j < nbr_pts_minimal; j++) {
            x1.col(j) << p1.col(rands[j]);
            x2.col(j) << p2.col(rands[j]);
        }

        // Get pose
        MatrixXd x1h(2,3);
        MatrixXd x2h(2,3);
        x1h << x1.colwise().hnormalized();
        x2h << x2.colwise().hnormalized();

        PoseData posedata = get_floor_fHf(x1h, x2h, R1, R2);

        // Compute reprojection error, and compare to other solutions.
        H = posedata.homography;
        reproj1 = p2.colwise().hnormalized() - (H * p1).colwise().hnormalized();
        reproj2 = p1.colwise().hnormalized() - (H.lu().solve(p2)).colwise().hnormalized();
        reproj << reproj1.cwiseAbs(), reproj2.cwiseAbs();
        reproj_mean = reproj.colwise().mean().array();

        inliers = (reproj_mean.array() < thresh).cast<int>();
        nbr_inliers = inliers.sum();

        if (best_nbr_inliers < nbr_inliers) {
            best_posedata = posedata;
            best_nbr_inliers = nbr_inliers;
            best_inliers = inliers;
        }
        history(i) = best_nbr_inliers;

    }

    RansacData ransac_data;
    ransac_data.posedata = best_posedata;
    ransac_data.inliers = best_inliers;
    ransac_data.history = history;

    return ransac_data;
}

// ---------------- //
// MATLAB interface //
// ---------------- //
#ifdef MATLAB_MEX_FILE
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    if (nrhs != 6) {
        mexErrMsgIdAndTxt("ransac_floor_fHf:nrhs", "Six input arguments are required.");
    }
    if (nlhs != 4) {
        mexErrMsgIdAndTxt("ransac_floor_fHf:nlhs", "Four output arguments are required.");
    }
    if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0])) {
        mexErrMsgIdAndTxt("ransac_floor_fHf:notDouble", "Input data must be type double.");
    }
    if(mxGetNumberOfElements(prhs[0]) % 3 != 0
       && mxGetNumberOfElements(prhs[0]) != mxGetNumberOfElements(prhs[1])
       && mxGetNumberOfElements(prhs[2]) != 9 && mxGetNumberOfElements(prhs[3]) != 9
       && mxGetNumberOfElements(prhs[4]) != 1 && mxGetNumberOfElements(prhs[5]) != 1)
    {
        mexErrMsgIdAndTxt("ransac_floor_fHf:incorrectSize", "Input dimensions incorrect.");
    }
    // Convert to expected input
    int nbr_pts = mxGetNumberOfElements(prhs[0]) / 3;
    VectorXd x1_tmp = Map<VectorXd>(mxGetPr(prhs[0]), mxGetNumberOfElements(prhs[0]));
    VectorXd x2_tmp = Map<VectorXd>(mxGetPr(prhs[1]), mxGetNumberOfElements(prhs[1]));
    MatrixXd x1 = Map<MatrixXd>(x1_tmp.data(), 3, nbr_pts);
    MatrixXd x2 = Map<MatrixXd>(x2_tmp.data(), 3, nbr_pts);

    VectorXd R1_tmp = Map<VectorXd>(mxGetPr(prhs[2]), 9);
    VectorXd R2_tmp = Map<VectorXd>(mxGetPr(prhs[3]), 9);
    Matrix3d R1 = Map<Matrix3d>(R1_tmp.data(), 3, 3);
    Matrix3d R2 = Map<Matrix3d>(R2_tmp.data(), 3, 3);

    double *nbr_iter_p = mxGetPr(prhs[4]);
    double *thresh = mxGetPr(prhs[5]);
    int nbr_iter = (int) nbr_iter_p[0];

    // Compute output
    RansacData ransac_data = ransac_floor_fHf(x1, x2, R1, R2, nbr_iter, thresh[0]);
    PoseData posedata = ransac_data.posedata;

    // Wrap it up to Matlab compatible output
    plhs[0] = mxCreateDoubleMatrix(3, 3, mxREAL);
    double* zr = mxGetPr(plhs[0]);
    for (Index i = 0; i < posedata.homography.size(); i++) {
        zr[i] = posedata.homography(i);
    }
    plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
    zr = mxGetPr(plhs[1]);
    zr[0] = posedata.focal_length;

    plhs[2] = mxCreateDoubleMatrix(nbr_pts, 1, mxREAL);
    zr = mxGetPr(plhs[2]);
    for (Index i = 0; i < nbr_pts; i++) {
        zr[i] = ransac_data.inliers(i);
    }

    plhs[3] = mxCreateDoubleMatrix(nbr_iter, 1, mxREAL);
    zr = mxGetPr(plhs[3]);
    for (Index i = 0; i < nbr_iter; i++) {
        zr[i] = ransac_data.history(i);
    }
}
#endif


#include <math.h>
#include <chrono>
#include <stdexcept>
#include <iostream>
#include <Eigen/Dense>
#include "kepler.h"
#include "elements.h"

using std::function;
using std::runtime_error;
using Eigen::Vector3d;

namespace kepler {
    double period(double sma, double mu) {
        return M_PI * 2 * sqrt(pow(sma, 3) / mu);
    }

    double newton(
            double p0,
            function<double(double)> const &func,
            function<double(double)> const &deriv,
            int maxiter = 50,
            double tol = 1e-8
    ) {
        for (auto i = 1; i < maxiter; i++) {
            auto p = p0 - func(p0) / deriv(p0);
            if (fabs(p - p0) < tol) {
                return p;
            }
            p0 = p;
        }
        throw runtime_error("Not converged.");
    }

    double mean2ecc(double M, double ecc) {
        auto E = newton(M, [ecc, M](double E) -> double {
            return E - ecc * sin(E) - M;
        }, [ecc](double E) -> double {
            return 1 - ecc * cos(E);
        });
        return E;
    }

    double ecc2true(double E, double ecc) {
        return 2 * atan2(sqrt(1 + ecc) * sin(E / 2), sqrt(1 - ecc) * cos(E / 2));
    }

    void benchmark(int times) {
        auto mu = 3.986004418e5;
        Vector3d r(8.59072560e+02, -4.13720368e+03, 5.29556871e+03);
        Vector3d v(7.37289205e+00, 2.08223573e+00, 4.39999794e-01);
        auto el = elements::elements(r, v, mu);

        auto best = std::numeric_limits<double>::infinity();
        auto worst = -std::numeric_limits<double>::infinity();
        double all = 0;
        for (auto i=0; i < times; i++) {
            auto begin = std::chrono::high_resolution_clock::now();

            mean2ecc(M_PI, el[1]);

            auto end = std::chrono::high_resolution_clock::now();
            auto current = std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count()/1e9;
            all += current;
            if (current < best) {
                best = current;
            }
            if (current > worst) {
                worst = current;
            }
        }
        std::cout << "[" << all/times << "," << best << "," << worst << "]" << std::endl;
    }
}


#include "common.hpp"
#include "PPXTF.hpp"
#include <sys/stat.h>
#include <Eigen/LU>

using namespace Eigen;
using namespace std;

/*
  #################################### PPMTF ####################################
  # [input1]: R1_observed ([mode1]{mode2, mode3: value})
  # [input2]: R2_observed ([mode2]{mode1, mode3: value})
  # [input3]: R3_observed ([mode3]{mode1, mode2: value})
  # [input4]: A (L1 x K matrix)
  # [input5]: B (L2 x K matrix)
  # [input6]: C (L3 x K matrix)
  # [input7]: L1 -- Length of mode1
  # [input8]: L2 -- Length of mode2
  # [input9]: L3 -- Length of mode3
  # [input10]: X -- tensor no.
  # [output]: A, B, C, mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C
 */
tuple<mat_t, mat_t, mat_t, mat_t, mat_t, mat_t>
pptf(const r_t& r1, const r_t& r2, const r_t& r3,
     vec_mat_t& A, vec_mat_t& B, vec_mat_t& C,
     int L1, int L2, int L3, int K, int ItrNum, double Alp,
     const string& outfile_prefix, const string& X){
  // Hyper-hyper parameters (same as [Salakhutdinov+, ICML08])
  double alpha = Alp;
  double beta0 = 2;
  double mu0 = 0;
  int nu0 = K;
  mat_t W0 = mat_t::Identity(K,K);

  mat_t Lam_A, mu_A, Lam_B, mu_B, Lam_C, mu_C;
  puts("Gibbs Sampling:");
  for(int itr = 0; itr < ItrNum; ++itr){
    if(itr % 10 == 0) printf("itr:%d\n", itr);

    // Sample Lam_{A,B,C,D} & mu_{A,B,C,D}
    tie(Lam_A, mu_A) = sample_Lam_X_mu_X(A[itr], L1, nu0, W0, beta0, mu0, K);
    tie(Lam_B, mu_B) = sample_Lam_X_mu_X(B[itr], L2, nu0, W0, beta0, mu0, K);
    tie(Lam_C, mu_C) = sample_Lam_X_mu_X(C[itr], L3, nu0, W0, beta0, mu0, K);

    // Sample A
    A.push_back(mat_t::Zero(L1, K));
    for(int n = 0; n < L1; ++n){
      mat_t BC_BC, BC_R;
      tie(BC_BC, BC_R) = calc_XYXY_XYR(K, B[itr], C[itr], r1[n]);
      mat_t lam_ast_inv = (Lam_A + alpha * BC_BC).inverse();
      mat_t mu_ast = lam_ast_inv * (alpha * BC_R + Lam_A * mu_A);
      A[itr+1].row(n) = multivariate_normal(mu_ast, lam_ast_inv).transpose();
    }

    // Sample B
    B.push_back(mat_t::Zero(L2, K));
    for(int i = 0; i < L2; ++i){
      mat_t AC_AC, AC_R;
      tie(AC_AC, AC_R) = calc_XYXY_XYR(K, A[itr+1], C[itr], r2[i]);
      mat_t lam_ast_inv = (Lam_B + alpha * AC_AC).inverse();
      mat_t mu_ast = lam_ast_inv * (alpha * AC_R + Lam_B * mu_B);
      B[itr+1].row(i) = multivariate_normal(mu_ast, lam_ast_inv).transpose();
    }

    // Sample C
    C.push_back(mat_t::Zero(L3, K));
    for(int j = 0; j < L3; ++j){
      mat_t AB_AB, AB_R;
      tie(AB_AB, AB_R) = calc_XYXY_XYR(K, A[itr+1], B[itr+1], r3[j]);
      mat_t lam_ast_inv = (Lam_C + alpha * AB_AB).inverse();
      mat_t mu_ast = lam_ast_inv * (alpha * AB_R + Lam_C * mu_C);
      C[itr+1].row(j) = multivariate_normal(mu_ast, lam_ast_inv).transpose();
    }

    // Output model parameters and hyper-parameters
    if(itr == 0 || itr % 10 == 9){
      string pre = outfile_prefix;
      save_parameters(K, itr+1, pre, "A", A, mu_A, Lam_A, X);
      save_parameters(K, itr+1, pre, "B", B, mu_B, Lam_B, X);
      save_parameters(K, itr+1, pre, "C", C, mu_C, Lam_C, X);
    }
  }
  return forward_as_tuple(mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

int main(int argc, char *argv[]){
  if(argc < 3){
      printf("Usage: PPITF [Dataset] [City] ([Alp (default:200)] [MaxNumTrans (default:100)] [MaxNumVisit (default:100)] [ItrNum (default:100)])\n");
      return -1;
  }

  // Fix a seed
  seeding();

  // ################################# Parameters ##################################
  // Dataset
  const string Dataset = argv[1];
  // City
//  const string City = "TK";
  const string City = argv[2];
  // Hyper-hyper parameter alpha
  double Alp = 200;
  string AlpStr = "200";
  if(argc >= 4){
	  Alp = atof(argv[3]);
	  AlpStr = argv[3];
  }
  // Maximum number of transitions per user (-1: infinity)
  int MaxNumTrans = 100;
  if(argc >= 5){
	  MaxNumTrans = atoi(argv[4]);
  }
  // Maximum number of POI visits per user (-1: infinity)
  int MaxNumVisit = 100;
  if(argc >= 6){
	  MaxNumVisit = atoi(argv[5]);
  }
  // Number of iterations in Gibbs sampling
  int ItrNum = 100;
  if(argc >= 7){
	  ItrNum = atoi(argv[6]);
  }

  // Training user index file (input)
  string TUserIndexFile = "../data/" + Dataset + "/tuserindex_%s.csv";
  // POI index file (input)
  string POIIndexFile = "../data/" + Dataset + "/POIindex_%s.csv";
  // Training transition tensor file (input)
  string TrainTransTensorFile = "../data/" + Dataset + "/traintranstensor_%s_mnt" + std::to_string(MaxNumTrans) + ".csv";
  // Training visit tensor file (input)
  string TrainVisitTensorFile = "../data/" + Dataset + "/trainvisittensor_%s_mnv" + std::to_string(MaxNumVisit) + ".csv";
  // Prefix of the model parameter file (output)
  const string OutDir = "../data/" + Dataset + "/PPITF_" + City + "_alp" + AlpStr + "_mnt" + std::to_string(MaxNumTrans) + "_mnv" + std::to_string(MaxNumVisit);
  mkdir(OutDir.c_str(), 0755);
  const string ModelParameterFile = OutDir + "/modelparameter";

  // Number of time slots
  int T;
  if(Dataset.find("PF") == 0){
	  T = 30;
  }else if(Dataset.find("FS") == 0){
	  T = 12;
  }else{
	  cout << "Wrong Dataset\n";
	  exit(-1);
  }

  // Number of columns in model parameters (A, B, C)
//  const int K = 32;
  const int K = 16;
  // Number of zero elements for each user in a training tensor (-1: all)
  const int ZeroNum = 1000;

  // Replace %s with City
  TUserIndexFile = string_replace(TUserIndexFile, City);
  POIIndexFile = string_replace(POIIndexFile, City);
  TrainTransTensorFile = string_replace(TrainTransTensorFile, City);
  TrainVisitTensorFile = string_replace(TrainVisitTensorFile, City);

  // Number of training users --> N
  int N = line_num(TUserIndexFile) - 1;
  // Number of POIs --> M
  int M = line_num(POIIndexFile) - 1;

  // Read a training transition tensor
  puts("Reading a training transition tensor.");
  r_t RT1_observed, RT2_observed, RT3_observed;
  tie(RT1_observed, RT2_observed, RT3_observed) = ReadTrainTransTensor(N, M, ZeroNum, TrainTransTensorFile);
  // Read a training visit tensor
  puts("Reading a training visit tensor.");
  r_t RV1_observed, RV2_observed, RV3_observed;
  tie(RV1_observed, RV2_observed, RV3_observed) = ReadTrainVisitTensor(N, M, T, ZeroNum, TrainVisitTensorFile);

  {
    // Initialize model parameters (AT, BT, CT) of the training transition tensor by random values in [0,1)
    vec_mat_t A, B, C;
    A.push_back(random_mat(N, K));
    B.push_back(random_mat(M, K));
    C.push_back(random_mat(M, K));
    // PPTF for the training transition tensor --> AT, BT, CT, mu_AT, Lam_AT, mu_BT, Lam_BT, mu_CT, Lam_CT
    mat_t mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C;
    tie(mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C) = pptf(RT1_observed, RT2_observed, RT3_observed,
						      A, B, C, N, M, M, K, ItrNum, Alp, ModelParameterFile, "T");
  }
  {
    // Initialize model parameters (AV, BV, CV) of the training visit tensor by random values in [0,1)
    vec_mat_t A, B, C;
    A.push_back(random_mat(N, K));
    B.push_back(random_mat(M, K));
    C.push_back(random_mat(T, K));
    // PPTF for the training visit tensor --> AV, BV, CV, mu_AV, Lam_AV, mu_BV, Lam_BV, mu_CV, Lam_CV
    mat_t mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C;
    tie(mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C) = pptf(RV1_observed, RV2_observed, RV3_observed,
						      A, B, C, N, M, T, K, ItrNum, Alp, ModelParameterFile, "V");
  }
  return 0;
}


/*
 * test_propagator.cpp
 *
 * Created on: Mar 21, 2018 15:28
 * Description:
 *
 * Copyright (c) 2018 Ruixiang Du (rdu)
 */

#include <iostream>

#include <boost/numeric/odeint.hpp>

#include "model/bicycle_model.hpp"

using namespace robosw;
using namespace boost::numeric::odeint;

int main() {
  double t0 = 0;
  double tf = 10;
  double dt = 0.01;

  BicycleKinematics::state_type x = {0.0, 0.0, 0.0, 0.0};
  BicycleKinematics model({0.8, 0});

  boost::numeric::odeint::integrate_const(
      boost::numeric::odeint::runge_kutta4<BicycleKinematics::state_type>(),
      model, x, 0.0, 10.0, 0.01);

  //   runge_kutta4<std::vector<double>> rk4;
  //   double t = t0;
  //   for (size_t i = 0; i < 1000; ++i, t += dt) {
  //     rk4.do_step(model, x, t, dt);
  //   }

  std::cout << "final state: " << x[0] << " , " << x[1] << std::endl;

  return 0;
}


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <iomanip>
#include <iostream>

#define DIRECTLAYER 2
#define PI314 (static_cast<double>(3.1415926535897932384626433))
#define E271 (static_cast<double>(2.7182818284590452354))

namespace Laplace3D3DDipole {

using EVec3 = Eigen::Vector3d;
using EMat3 = Eigen::Matrix3d;

inline double ERFC(double x) { return std::erfc(x); }
inline double ERF(double x) { return std::erf(x); }

inline double f(double r, double eta) {
    return ERFC(sqrt(PI314 / eta) * r) / r;
}

inline double fp(double r, double eta) {
    return -ERFC(sqrt(PI314 / eta) * r) / (r * r) -
           2 * exp(-PI314 * r * r / eta) / (r * sqrt(eta));
}

// real and wave sum of 2D Laplace kernel Ewald

// xm: target, xn: source
inline EVec3 realSum(const double eta, const EVec3 &xn, const EVec3 &xm) {
    EVec3 rmn = xm - xn;
    double rnorm = rmn.norm();
    if (rnorm < 1e-14) {
        return EVec3(0, 0, 0);
    }
    return -fp(rnorm, eta) / rnorm * rmn;
}

inline EVec3 gKernelEwald(const EVec3 &xm, const EVec3 &xn) {
    const double eta = 1.0; // recommend for box=1 to get machine precision
    EVec3 target = xm;
    EVec3 source = xn;
    target[0] = target[0] - floor(target[0]); // periodic BC
    target[1] = target[1] - floor(target[1]);
    target[2] = target[2] - floor(target[2]);
    source[0] = source[0] - floor(source[0]);
    source[1] = source[1] - floor(source[1]);
    source[2] = source[2] - floor(source[2]);

    // real sum
    int rLim = 6;
    EVec3 Kreal(0, 0, 0);
    for (int i = -rLim; i <= rLim; i++) {
        for (int j = -rLim; j <= rLim; j++) {
            for (int k = -rLim; k <= rLim; k++) {
                EVec3 rmn = target - source + EVec3(i, j, k);
                if (rmn.norm() < 1e-13) {
                    continue;
                }
                Kreal += realSum(eta, EVec3(0, 0, 0), rmn);
            }
        }
    }

    // wave sum
    int wLim = 6;
    EVec3 Kwave(0, 0, 0);
    EVec3 rmn = target - source;

    for (int i = -wLim; i <= wLim; i++) {
        for (int j = -wLim; j <= wLim; j++) {
            for (int k = -wLim; k <= wLim; k++) {
                if (i == 0 && j == 0 && k == 0) {
                    continue;
                }
                EVec3 kvec = EVec3(i, j, k);
                double knorm = kvec.norm();
                Kwave += 2 * PI314 * sin(2 * PI314 * kvec.dot(rmn)) *
                         exp(-eta * PI314 * knorm * knorm) /
                         (PI314 * knorm * knorm) * kvec;
            }
        }
    }

    return Kreal + Kwave;
}

inline EVec3 gKernel(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < 1e-14) {
        return EVec3(0, 0, 0);
    } else {
        return rst / pow(rnorm, 3);
    }
}

inline Eigen::Matrix3d gKernelGrad(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < 1e-14) {
        return Eigen::Matrix3d::Zero();
    } else {
        return Eigen::Matrix3d::Identity() / pow(rnorm, 3) -
               3 * rst * rst.transpose() / pow(rnorm, 5);
    }
}

// Out of Direct Sum Layer, far field part
inline EVec3 gKernelFF(const EVec3 &target, const EVec3 &source) {
    EVec3 fEwald = gKernelEwald(target, source);
    const int N = DIRECTLAYER;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                EVec3 gFree = gKernel(target, source - EVec3(i, j, k));
                fEwald -= gFree;
            }
        }
    }

    // {
    //   std::cout << "source:" << source << std::endl
    //             << "target:" << target << std::endl
    //             << "gKernalFF" << fEwald << std::endl;
    // }
    return fEwald;
}

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */

template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);

    // testing Ewald routine
    double zeroTest = gKernelEwald(EVec3(0.5, 0.5, 0.5), EVec3(0.5, 0.5, 0.5))
                          .dot(EVec3(1, 1, 1));
    std::cout << std::setprecision(16) << "zeroTest: " << zeroTest << std::endl;

    double centerTest = gKernelEwald(EVec3(0, 0, 0), EVec3(0.5, 0.5, 0.5))
                            .dot(EVec3(0.5, 0.5, 0.5));
    std::cout << std::setprecision(16) << "centerTest: " << centerTest
              << " error: " << centerTest - 0 << std::endl;

    centerTest = gKernelEwald(EVec3(0.2, 0.3, 0.4), EVec3(0.3, 0.6, 0.5))
                     .dot(EVec3(3, 2, 1));
    std::cout << std::setprecision(16) << "centerTest2: " << centerTest
              << " error: " << centerTest + 23.48660380315382667504
              << std::endl;

    centerTest = gKernelEwald(EVec3(0.7, 0.9, 0.7), EVec3(0.2, 0.3, 0.4))
                     .dot(EVec3(0.1, 2, 0.3));
    std::cout << std::setprecision(16) << "centerTest2: " << centerTest
              << " error: " << centerTest + 0.83918927151112920892 << std::endl;

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {
        -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {
        -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    const double scaleLEquiv = 1.05;
    const double scaleLCheck = 2.95;
    const double pCenterLEquiv[3] = {
        -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2};
    const double pCenterLCheck[3] = {
        -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2};

    auto pointMEquiv =
        surface(pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(
        pCheck, (double *)&(pCenterCheck[0]), scaleCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth =0

    auto pointLEquiv = surface(
        pEquiv, (double *)&(pCenterLCheck[0]), scaleLCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth =  0
    auto pointLCheck = surface(
        pCheck, (double *)&(pCenterLEquiv[0]), scaleLEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd M2L(3 * equivN, 3 * equivN); // Laplace, 1->1

    Eigen::MatrixXd A(3 * checkN, 3 * equivN);
    A.setZero();
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                               pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l],
                                         pointLEquiv[3 * l + 1],
                                         pointLEquiv[3 * l + 2]);
            EVec3 temp = gKernel(Cpoint, Lpoint);
            A(3 * k, 3 * l) = temp[0];
            A(3 * k + 1, 3 * l + 1) = temp[1];
            A(3 * k + 2, 3 * l + 2) = temp[2];
            // A.block(k, l, 1, 3) = gKernel(Cpoint, Lpoint).transpose();
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1],
                                     pointMEquiv[3 * i + 2]);
        //		std::cout << "debug:" << Mpoint << std::endl;

        // assemble linear system
        Eigen::VectorXd f0(3 * checkN);
        Eigen::VectorXd f1(3 * checkN);
        Eigen::VectorXd f2(3 * checkN);
        f0.setZero();
        f1.setZero();
        f2.setZero();
        for (int k = 0; k < checkN; k++) {
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                                   pointLCheck[3 * k + 2]);
            //			std::cout<<"debug:"<<k<<std::endl;
            // sum the images
            EVec3 temp = gKernelFF(Cpoint, Mpoint);
            f0[3 * k] = temp[0];
            f1[3 * k + 1] = temp[1];
            f2[3 * k + 2] = temp[2];
        }
        // std::cout << "debug:" << f0 << std::endl;

        M2L.block(0, 3 * i, 3 * equivN, 1) =
            (ApinvU.transpose() * (ApinvVT.transpose() * f0));
        M2L.block(0, 3 * i + 1, 3 * equivN, 1) =
            (ApinvU.transpose() * (ApinvVT.transpose() * f1));
        M2L.block(0, 3 * i + 2, 3 * equivN, 1) =
            (ApinvU.transpose() * (ApinvVT.transpose() * f2));
    }

    // dump M2L
    for (int i = 0; i < 3 * equivN; i++) {
        for (int j = 0; j < 3 * equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific
                      << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        dipolePoint(1);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        dipoleValue(1);
    dipolePoint[0] = Eigen::Vector3d(0.2, 0.3, 0.4);
    dipoleValue[0] = Eigen::Vector3d(0.1, 0.2, 0.3);

    // solve M
    A.resize(3 * checkN, 3 * equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
    Eigen::VectorXd f(3 * checkN);
    for (int k = 0; k < checkN; k++) {
        EVec3 sum(0, 0, 0);
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1],
                               pointMCheck[3 * k + 2]);
        for (size_t p = 0; p < dipolePoint.size(); p++) {
            EVec3 temp = gKernel(Cpoint, dipolePoint[p]);
            sum[0] += temp[0] * dipoleValue[p][0];
            sum[1] += temp[1] * dipoleValue[p][1];
            sum[2] += temp[2] * dipoleValue[p][2];
        }
        f[3 * k] = sum[0];
        f[3 * k + 1] = sum[1];
        f[3 * k + 2] = sum[2];
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1],
                                   pointMEquiv[3 * l + 2]);
            // A(k, l) = gKernel(Mpoint, Cpoint);
            EVec3 temp = gKernel(Cpoint, Mpoint);
            A(3 * k, 3 * l) = temp[0];
            A(3 * k + 1, 3 * l + 1) = temp[1];
            A(3 * k + 2, 3 * l + 2) = temp[2];
        }
    }
    pinv(A, ApinvU, ApinvVT);
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));

    std::cout << "Msource: " << Msource << std::endl;

    Eigen::VectorXd M2Lsource = M2L * (Msource);

    Eigen::Vector3d samplePoint(0.7, 0.9, 0.7);
    //    Eigen::Vector3d samplePoint = dipolePoint[0];
    double Usample = 0;
    double UsampleSP = 0;

    for (int i = -DIRECTLAYER; i < 1 + DIRECTLAYER; i++) {
        for (int j = -DIRECTLAYER; j < 1 + DIRECTLAYER; j++) {
            for (int k = -DIRECTLAYER; k < 1 + DIRECTLAYER; k++) {
                for (size_t p = 0; p < dipolePoint.size(); p++) {
                    Usample +=
                        gKernel(samplePoint, dipolePoint[p] + EVec3(i, j, k))
                            .dot(dipoleValue[p]);
                }
            }
        }
    }

    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1],
                               pointLEquiv[3 * p + 2]);
        EVec3 M2Lsp(M2Lsource[3 * p], M2Lsource[3 * p + 1],
                    M2Lsource[3 * p + 2]);
        UsampleSP += gKernel(samplePoint, Lpoint).dot(M2Lsp);
    }

    std::cout << "samplePoint:" << samplePoint << std::endl;
    std::cout << "Usample NF:" << Usample << std::endl;
    std::cout << "Usample FF:" << UsampleSP << std::endl;
    std::cout << "Usample FF+NF total:" << UsampleSP + Usample << std::endl;

    std::cout
        << "Error : "
        << UsampleSP + Usample -
               gKernelEwald(samplePoint, dipolePoint[0]).dot(dipoleValue[0])
        << std::endl;

    return 0;
}

} // namespace Laplace3D3DDipole

#undef DIRECTLAYER
#undef PI314
#undef E271


#pragma once

#include <Eigen/Dense>
#include <vector>
#include "../flux_worker.hpp"
#include "../outflow_helper.hpp"
#include "aux/eigen2hdf.hpp"
#include "base/numbers.hpp"

namespace boltzmann {
namespace impl_mls {

class DiffusiveReflection : public boltzmann::impl::flux_worker
{
 public:
  /**
   * @param hw Hermite quad. weights (from @see QHermiteW)
   * @param hx Hermite quad. nodes
   * @param vt tangential velocity (moving wall)
   * @param Tw wall temperature
   * @param rho scaling factor
   *
   */
  DiffusiveReflection(const vec_t& hw, const vec_t& hx, double vt, double Tw, double rho = 1.0);

  virtual void apply(mat_t& out,
                     const mat_t& in,
                     const dealii::Point<2>& dummy = dealii::Point<2>(0, 0)) const;

 private:
  boltzmann::impl::outflow_helper outflow_;
  const vec_t w_;
  const vec_t x_;
  double rho_;

  // Eigen::MatrixXd rho_minus_;
  Eigen::MatrixXd Mw_;
};

// --------------------------------------------------------------------------------
DiffusiveReflection::DiffusiveReflection(
    const vec_t& hw, const vec_t& hx, double vt, double Tw, double rho)
    : outflow_(hw, hx)
    , w_(hw)
    , x_(hx)
    , rho_(rho)
{
  unsigned int K = hx.size();
  Mw_.resize(K, K);

  unsigned int khalf = K / 2;

  // normalization factor
  const double fMw = rho_ / std::sqrt(2 * numbers::PI) / std::pow(Tw, 1.5);

  // initialize maxwellian
  for (unsigned int i = 0; i < K; ++i) {
    for (unsigned int j = 0; j < K; ++j) {
      if (i < khalf) {  // inflow boundary
        double x2h = std::pow(x_[i], 2) + std::pow(x_[j] - vt, 2);
        Mw_(i, j) = fMw * std::exp(0.5 * x2h * (-1. / Tw)) * std::sqrt(w_[i] * w_[j]) * x_[i];
      } else {  // outflow
        Mw_(i, j) = 0;
      }
    }
  }

}

// --------------------------------------------------------------------------------
void
DiffusiveReflection::apply(mat_t& out, const mat_t& in, const dealii::Point<2>& dummy) const
{
  int K = out.cols();

  // compute outflow
  double rhom = 0;
  for (int i = 0; i < K; ++i) {
    for (int j = 0; j < K; ++j) {
      rhom += in(i, j) * outflow_.get_y(i) * outflow_.get_x(j);
    }
  }

  int khalf = K / 2;

  out.setZero();

  // outflow
  for (int i = khalf; i < K; ++i) {
    for (int j = 0; j < K; ++j) {
      out(i, j) = in(i, j) * x_[i];
    }
  }

  // inflow M_w
  out += Mw_ * rhom;
}

}  // namespace impl_mls
}  // end namespace boltzmann