#ifndef INCLUDED_STDDEFX
#include "stddefx.h"
#define INCLUDED_STDDEFX
#endif

#ifndef INCLUDED_GEO_CIRCULARNEIGHBOURHOOD
#include "geo_circularneighbourhood.h"
#define INCLUDED_GEO_CIRCULARNEIGHBOURHOOD
#endif

// Library headers.
#ifndef INCLUDED_BOOST_NONCOPYABLE
#include <boost/noncopyable.hpp>
#define INCLUDED_BOOST_NONCOPYABLE
#endif
#ifndef INCLUDED_BOOST_MATH_SPECIAL_FUNCTIONS_ROUND
#include <boost/math/special_functions/round.hpp>
#define INCLUDED_BOOST_MATH_SPECIAL_FUNCTIONS_ROUND
#endif

// PCRaster library headers.

// Module headers.
#ifndef INCLUDED_GEO_SCANCONVERSION
#include "geo_scanconversion.h"
#define INCLUDED_GEO_SCANCONVERSION
#endif

/*!
  \file
  This file contains the implementation of the CircularNeighbourhood class.
*/

//------------------------------------------------------------------------------

/*
namespace geo {

class CircularNeighbourhoodPrivate
{
public:

  CircularNeighbourhoodPrivate()
  {
  }

  ~CircularNeighbourhoodPrivate()
  {
  }

};

} // namespace geo
*/

//------------------------------------------------------------------------------
// DEFINITION OF STATIC CIRCULARNEIGHBOURHOOD MEMBERS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF CIRCULARNEIGHBOURHOOD MEMBERS
//------------------------------------------------------------------------------

geo::CircularNeighbourhood::CircularNeighbourhood(double radius)

  : Neighbourhood(radius)

{
  init();
}

geo::CircularNeighbourhood::CircularNeighbourhood(double fromRadius,
         double toRadius)

  : Neighbourhood(fromRadius, toRadius)

{
  init();
}

geo::CircularNeighbourhood::~CircularNeighbourhood()
{
}

namespace geo {

class SetRaster: public boost::noncopyable {

private:

  SimpleRaster<double>& d_raster;

public:

  SetRaster(SimpleRaster<double>& raster)
    : d_raster(raster)
  {
  }

  bool operator()(size_t col, size_t row) {
    d_raster.cell(row, col) = 1.0;
    return true;
  }
};

}

void geo::CircularNeighbourhood::init()
{
  SetRaster setRaster(*this);
  using namespace boost::math;

  if(isOutline()) {
    midpointCircle<int, SetRaster>(
         static_cast<size_t>(round(toRadius())),
         static_cast<size_t>(round(toRadius())),
         static_cast<size_t>(round(toRadius())),
         setRaster);
  }
  else {
    midpointCircle<int, SetRaster>(
         static_cast<size_t>(round(toRadius())),
         static_cast<size_t>(round(toRadius())),
         static_cast<size_t>(round(fromRadius())),
         static_cast<size_t>(round(toRadius())),
         setRaster);
  }
}

//------------------------------------------------------------------------------
// DEFINITION OF FREE OPERATORS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF FREE FUNCTIONS
//------------------------------------------------------------------------------


#ifndef INCLUDE_MIMKL_MODELS_EASY_MKL_HPP_
#define INCLUDE_MIMKL_MODELS_EASY_MKL_HPP_

#include <Eigen/StdVector>
#include <mimkl/data_structures.hpp>
#include <mimkl/definitions.hpp>
#include <mimkl/linear_algebra.hpp>
#include <mimkl/models/model.hpp>
#include <mimkl/solvers.hpp>
#include <spdlog/fmt/ostr.h>
#include <spdlog/spdlog.h>

using mimkl::definitions::Indexing;

namespace mimkl
{
namespace models
{

static std::shared_ptr<spdlog::logger> logger_easy_mkl =
spdlog::stdout_color_mt("EasyMKL");

template <typename Scalar, typename Kernel>
class EasyMKL : public Model<Scalar, Kernel>
{

    private:
    // inheritance of templatized base members
    using Model<Scalar, Kernel>::_kernels_handler;
    using Model<Scalar, Kernel>::_trained;
    using Model<Scalar, Kernel>::_precompute;
    using Model<Scalar, Kernel>::_trace_normalization;
    using Model<Scalar, Kernel>::_number_of_support_vectors;
    using Model<Scalar, Kernel>::_number_of_kernels;

    // logger instance
    std::shared_ptr<spdlog::logger> _logger = spdlog::get("EasyMKL");
    // typedefs
    typedef MATRIX(Scalar) Matrix;
    typedef COLUMN(Scalar) Column;
    typedef ROW(Scalar) Row;
    typedef Eigen::Map<Column> MapColumn;

    // parameters
    double _lambda = 0.8;
    double _epsilon = 0.0001;
    bool _regularization_factor = false;

    Matrix _gammas;
    Matrix _etas;
    Row _biases;
    Indexing _class_map;

    std::vector<std::string> _unique_labels;
    Index _number_of_classes;
    Index _number_of_dichotomies;
    bool _binary;
    Matrix _kernels_sum;

    Column get_dichotomy(const std::string &);
    std::pair<Column, Column>
    optimize_gamma(const bool &, const Column &, const Column & = Column());
    Column compute_weights(const Column &);
    Scalar compute_bias(const Column &, const Column &, const Column &);

    void setup(std::vector<std::string>);
    void train(const Index &);
    void fit(const std::vector<std::string> &);
    Matrix decision_function();
    Column
    distances(const Column &, const Column &, const Column &, const Scalar &);

    public:
    EasyMKL(const std::vector<Kernel> & = std::vector<Kernel>(),
            const bool precompute = true,
            const bool trace_normalization = true,
            const double lambda = 0.8,
            const double epsilon = 0.0001,
            const bool regularization_factor = false);
    EasyMKL(const std::vector<Matrix> & = std::vector<Matrix>(),
            const bool precompute = true,
            const bool trace_normalization = true,
            const double lambda = 0.8,
            const double epsilon = 0.0001,
            const bool regularization_factor = false);

    // Functional, call with data matrix
    void fit(const Matrix &, const std::vector<std::string> &);
    std::vector<std::string> predict(const Matrix &);
    Matrix predict_proba(const Matrix &);
    Matrix decision_function(const Matrix &);
    // Matricial, call with kernel matrices
    void fit(const std::vector<Matrix> &, const std::vector<std::string> &);
    std::vector<std::string> predict(const std::vector<Matrix> &);
    Matrix predict_proba(const std::vector<Matrix> &);
    Matrix decision_function(const std::vector<Matrix> &);

    double get_lambda() const;
    double get_epsilon() const;
    bool get_regularization_factor() const;
    Matrix get_gammas() const;
    Matrix get_etas() const;
    Row get_biases() const;
    Indexing get_class_map() const;
    std::vector<std::string> get_one_versus_rest_order() const;
    Matrix get_optimal_kernel();
    Matrix get_optimal_kernel_by_class_index(const Index i);
    std::vector<Matrix> get_optimal_kernels();

    void set_lambda(const double);
    void set_epsilon(const double);
    void set_regularization_factor(const bool);
    void set_parameters(const double,
                        const double epsilon = 0.0001,
                        const bool regularization_factor = false);
    void set_gammas(const Matrix &);
    void set_etas(const Matrix &);
    void set_biases(const Row &);
    void set_class_map(const Indexing &);
};

template <typename Scalar, typename Kernel>
EasyMKL<Scalar, Kernel>::EasyMKL(const std::vector<Kernel> &kernel_functions,
                                 const bool precompute,
                                 const bool trace_normalization,
                                 const double lambda,
                                 const double epsilon,
                                 const bool regularization_factor)
: Model<Scalar, Kernel>(kernel_functions, precompute, trace_normalization)
{
    set_parameters(lambda, epsilon, regularization_factor);
}

template <typename Scalar, typename Kernel>
EasyMKL<Scalar, Kernel>::EasyMKL(const std::vector<Matrix> &kernel_matrices,
                                 const bool precompute,
                                 const bool trace_normalization,
                                 const double lambda,
                                 const double epsilon,
                                 const bool regularization_factor)
: Model<Scalar, Kernel>(kernel_matrices, precompute, trace_normalization)
{
    set_parameters(lambda, epsilon, regularization_factor);
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Column
EasyMKL<Scalar, Kernel>::get_dichotomy(const std::string &label)
{
    auto range = _class_map.equal_range(label);
    Column y = Column::Constant(_number_of_support_vectors, 1, -1);
    for (auto i = range.first; i != range.second; ++i)
    {
        y(i->second) = 1;
    }
    return y;
}

template <typename Scalar, typename Kernel>
std::pair<typename EasyMKL<Scalar, Kernel>::Column,
          typename EasyMKL<Scalar, Kernel>::Column>
EasyMKL<Scalar, Kernel>::optimize_gamma(const bool &regularization_factor,
                                        const Column &y,
                                        const Column &eta)
{
    mimkl::solvers::KOMD<Scalar> optimizer((eta.rows() > 0) ?
                                           _kernels_handler.sum(eta) :
                                           _kernels_sum,
                                           _lambda, _epsilon,
                                           regularization_factor);
    optimizer.solve(y);
    Column gamma = optimizer.get_result();
    return std::make_pair(gamma, y.cwiseProduct(gamma));
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Column
EasyMKL<Scalar, Kernel>::compute_weights(const Column &directed_gamma)
{
    // TODO maybe after all gammas are known and then compute etas rowwise, so
    // we
    // compute each kernel once? ->cycle through directed_gamma instead of
    // through
    // kernels
    Column eta = Column::Constant(_number_of_kernels, 1, 0.0);
    for (Index eta_index = 0; eta_index < _number_of_kernels; ++eta_index)
    {
        // compute current kernel matrix K
        _logger->trace("base kernel :\n{}", _kernels_handler[eta_index]);
        eta(eta_index) = directed_gamma.transpose() *
                         _kernels_handler[eta_index] * directed_gamma;

        _logger->trace("d(y)_r :\n{}", eta(eta_index));
    }
    _logger->trace("pre norming eta :\n{}", eta);
    eta /=
    eta.sum(); // l1-norm, as eta_i is >=0 by construction ( kernels are SDP)
    _logger->trace("eta :\n{}", eta);
    _logger->debug("compute_weights() done");
    return eta;
}

template <typename Scalar, typename Kernel>
Scalar EasyMKL<Scalar, Kernel>::compute_bias(const Column &gamma,
                                             const Column &directed_gamma,
                                             const Column &eta)
{
    return 0.5 * gamma.transpose() * (_kernels_handler.sum(eta)) * directed_gamma;
}

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::setup(std::vector<std::string> labels)
{
    _logger->debug("setup() start");
    _number_of_support_vectors = _kernels_handler.get_lhs_size();
    _number_of_kernels = _kernels_handler.get_number_of_kernels();
    if (_number_of_support_vectors != labels.size())
        throw std::length_error("sample size and lables size do not match");

    // manage labels
    _class_map = mimkl::data_structures::indexing_from_vector_of_strings(labels);

    std::sort(labels.begin(), labels.end()); // pass labels as value
    _unique_labels = labels;
    auto last = std::unique(_unique_labels.begin(), _unique_labels.end());
    _unique_labels.erase(last, _unique_labels.end());
    _number_of_classes = _unique_labels.size();
    _binary = _number_of_classes < 3;

    // treat the binary classification case
    _number_of_dichotomies = _binary ? 1 : _number_of_classes;
    _logger->debug("is binary : {}", _binary);
    _logger->debug("numer of dichotomies: {}", _number_of_dichotomies);

    _gammas.resize(_number_of_support_vectors, _number_of_dichotomies);
    _etas.resize(_number_of_kernels, _number_of_dichotomies);
    _biases.resize(1, _number_of_dichotomies);

    _kernels_sum = _kernels_handler.sum();
    _logger->debug("setup() done");
}

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::train(const Index &class_index)
{
    Column y = get_dichotomy(_unique_labels[class_index]);
    _logger->debug("get_dichotomy() done");
    // gamma on plain sum
    std::pair<Column, Column> gamma_pair =
    optimize_gamma(_regularization_factor, y);
    _logger->debug("optimize_gamma() done");
    Column eta = compute_weights(gamma_pair.second);
    _logger->debug("compute_weights() done");
    // gamma on weighted sum, like KOMD on given kernel
    gamma_pair = optimize_gamma(false, y, eta);
    _logger->debug("optimize_gamma() weighted done");
    Scalar bias = compute_bias(gamma_pair.first, gamma_pair.second, eta);
    _logger->debug("compute_bias() done");

    // update state for the given dichotomy
    _gammas.col(class_index) << gamma_pair.first;
    _etas.col(class_index) << eta;
    _biases.col(class_index) << bias;
}

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::fit(const std::vector<std::string> &labels)
{
    _logger->debug("fit() start");
    setup(labels);
    // or iterate over class_index?
    for (Index i = 0; i < _number_of_dichotomies; ++i)
        train(i);
    _trained = true;
    _logger->debug("fit() done");
}

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::fit(const Matrix &X,
                                  const std::vector<std::string> &labels)
{
    _logger->debug("fit() data start");
    _kernels_handler.set_lhs(X);
    fit(labels);
    _logger->debug("fit() data done");
}

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::fit(const std::vector<Matrix> &kernel_matrices,
                                  const std::vector<std::string> &labels)
{
    _logger->debug("fit() kernel matrices start");
    _kernels_handler.set_matrices(kernel_matrices, true);
    fit(labels);
    _logger->debug("fit() kernel matrices done");
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Column EasyMKL<Scalar, Kernel>::distances(
const Column &y, const Column &gamma, const Column &eta, const Scalar &bias)
{
    _logger->debug("about to break?");
    return (_kernels_handler.sum(eta).transpose() * y.cwiseProduct(gamma)).array() -
           bias;
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Matrix
EasyMKL<Scalar, Kernel>::decision_function()
{
    Matrix D(_kernels_handler.get_rhs_size(), _number_of_classes);
    _logger->debug("decision_function() start");
    for (Index i = 0; i < _number_of_dichotomies; ++i)
    {
        D.col(i) << distances(get_dichotomy(_unique_labels[i]), _gammas.col(i),
                              _etas.col(i), _biases(i));
    }
    if (_binary)
    {
        D.col(1) << -D.col(0);
    }
    _logger->debug("decision_function() done");
    return D;
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Matrix
EasyMKL<Scalar, Kernel>::decision_function(const Matrix &X)
{
    if (!_trained)
        throw std::logic_error("The model should be trained first (after "
                               "instantiation or change in parameters)");
    _kernels_handler.set_rhs(X);
    return decision_function();
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Matrix
EasyMKL<Scalar, Kernel>::decision_function(const std::vector<Matrix> &kernel_matrices)
{
    if (!_trained)
        throw std::logic_error("The model should be trained first (after "
                               "instantiation or change in parameters)");
    if (kernel_matrices[0].rows() != _number_of_support_vectors)
        throw std::length_error("Similarities must be provided for all support "
                                "vectors; matrices have wrong number of rows.");
    if (kernel_matrices.size() != _number_of_kernels)
        throw std::length_error(
        "Same number of kernels as on training is required");
    _kernels_handler.set_matrices(kernel_matrices);
    return decision_function();
}

template <typename Scalar, typename Kernel>
std::vector<std::string>
EasyMKL<Scalar, Kernel>::predict(const std::vector<Matrix> &kernel_matrices)
{
    _logger->debug("predict() matricial start");
    Matrix D = decision_function(kernel_matrices);
    std::vector<std::string> prediction;
    prediction.reserve(_kernels_handler.get_rhs_size());
    // argmax
    typename Matrix::Index max_index;
    for (Index i = 0; i < _kernels_handler.get_rhs_size(); ++i)
    {
        D.row(i).maxCoeff(&max_index);
        _logger->trace("max_index[{}]:\t {}", i, max_index);
        prediction.push_back(_unique_labels[max_index]);
    }
    _logger->debug("predict() matricial done");
    return prediction;
}

template <typename Scalar, typename Kernel>
std::vector<std::string> EasyMKL<Scalar, Kernel>::predict(const Matrix &X)
{
    _logger->debug("predict() functional start");
    Matrix D = decision_function(X);
    std::vector<std::string> prediction;
    prediction.reserve(_kernels_handler.get_rhs_size());
    // argmax
    typename Matrix::Index max_index;
    for (Index i = 0; i < _kernels_handler.get_rhs_size(); ++i)
    {
        D.row(i).maxCoeff(&max_index);
        _logger->trace("max_index[{}]:\t {}", i, max_index);
        prediction.push_back(_unique_labels[max_index]);
    }
    _logger->debug("predict() functional done");
    return prediction;
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Matrix
EasyMKL<Scalar, Kernel>::predict_proba(const std::vector<Matrix> &kernel_matrices)
{
    _logger->debug("predict_proba() matricial done");
    Matrix D = decision_function(kernel_matrices);
    _logger->debug("predict_proba() matricial done");
    return mimkl::linear_algebra::rowwise_soft_max(D);
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Matrix
EasyMKL<Scalar, Kernel>::predict_proba(const Matrix &X)
{
    _logger->debug("predict_proba() functional done");
    Matrix D = decision_function(X);
    _logger->debug("predict_proba() functional done");
    return mimkl::linear_algebra::rowwise_soft_max(D);
}

template <typename Scalar, typename Kernel>
double EasyMKL<Scalar, Kernel>::get_lambda() const
{
    return _lambda;
}

template <typename Scalar, typename Kernel>
double EasyMKL<Scalar, Kernel>::get_epsilon() const
{
    return _epsilon;
}

template <typename Scalar, typename Kernel>
bool EasyMKL<Scalar, Kernel>::get_regularization_factor() const
{
    return _regularization_factor;
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Matrix EasyMKL<Scalar, Kernel>::get_gammas() const
{
    return _gammas;
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Matrix EasyMKL<Scalar, Kernel>::get_etas() const
{
    return _etas;
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Row EasyMKL<Scalar, Kernel>::get_biases() const
{
    return _biases;
}

template <typename Scalar, typename Kernel>
Indexing EasyMKL<Scalar, Kernel>::get_class_map() const
{
    return _class_map;
}

template <typename Scalar, typename Kernel>
std::vector<std::string> EasyMKL<Scalar, Kernel>::get_one_versus_rest_order() const
{
    return _unique_labels;
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Matrix
EasyMKL<Scalar, Kernel>::get_optimal_kernel()
{
    if (!_trained)
        throw std::logic_error(
        "The model has not been fit, kernel weights are undetermined");
    return _kernels_handler.sum(
    _etas.rowwise().mean()); // TODO treat exception for binary problem
}

template <typename Scalar, typename Kernel>
typename EasyMKL<Scalar, Kernel>::Matrix
EasyMKL<Scalar, Kernel>::get_optimal_kernel_by_class_index(const Index i)
{
    if (!_trained)
        throw std::logic_error(
        "The model has not been fit, kernel weights are undetermined");
    return _kernels_handler.sum(_etas.col(i));
}

template <typename Scalar, typename Kernel>
std::vector<typename EasyMKL<Scalar, Kernel>::Matrix>
EasyMKL<Scalar, Kernel>::get_optimal_kernels()
{
    if (!_trained)
        throw std::logic_error(
        "The model has not been fit, kernel weights are undetermined");
    std::vector<Matrix> all_optimal_kernels;
    all_optimal_kernels.reserve(_number_of_dichotomies);
    for (Index i = 0; i < _number_of_dichotomies; ++i)
        all_optimal_kernels.push_back(_kernels_handler.sum(_etas.col(i)));
    return all_optimal_kernels;
}

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::set_lambda(const double lambda)
{
    _lambda = lambda;
    _trained = false;
    _logger->debug("changing parameters requires refitting before prediction");
}

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::set_epsilon(const double epsilon)
{
    _epsilon = epsilon;
    _trained = false;
    _logger->debug("changing parameters requires refitting before prediction");
}

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::set_regularization_factor(
const bool regularization_factor)
{
    _regularization_factor = regularization_factor;
    _trained = false;
    _logger->debug("changing parameters requires refitting before prediction");
}

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::set_parameters(const double lambda,
                                             const double epsilon,
                                             const bool regularization_factor)
{
    _lambda = lambda;
    _epsilon = epsilon;
    _regularization_factor = regularization_factor;
    _trained = false;
    _logger->debug("changing parameters requires refitting before prediction");
    _logger->debug("lambda:\n{}", _lambda);
    _logger->debug("epsilon:\n{}", _epsilon);
}

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::set_gammas(const Matrix &gammas)
{
    _gammas = gammas;
};

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::set_etas(const Matrix &etas)
{
    _etas = etas;
};

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::set_biases(const Row &biases)
{
    _biases = biases;
};

template <typename Scalar, typename Kernel>
void EasyMKL<Scalar, Kernel>::set_class_map(const Indexing &class_map)
{
    _class_map = class_map;
    // get companion objects
    std::vector<std::string> labels;
    labels.reserve(class_map.size());
    // keep linear complexity
    for (Indexing::const_iterator it = _class_map.begin();
         it != _class_map.end();)
    {
        const auto key = it->first;
        labels.push_back(key);
        do
        {
            ++it;
        } while (it != _class_map.end() && key == it->first);
    }
    std::sort(labels.begin(), labels.end()); // pass labels as value
    _unique_labels = labels;
    auto last = std::unique(_unique_labels.begin(), _unique_labels.end());
    _unique_labels.erase(last, _unique_labels.end());

    _number_of_classes = _unique_labels.size();
    _binary = _number_of_classes < 3;
    _number_of_dichotomies = _binary ? 1 : _number_of_classes;
};

} // namespace models
} // namespace mimkl

#endif /* INCLUDE_MIMKL_MODELS_EASY_MKL_HPP_ */


#include <path_follower/controller/robotcontroller_ackermann_stanley.h>

#include <path_follower/utils/pose_tracker.h>
#include <ros/ros.h>
#include <path_follower/utils/visualizer.h>

#include <cslibs_navigation_utilities/MathHelper.h>
#include <deque>

#include <limits>
#include <boost/algorithm/clamp.hpp>

#include <path_follower/factory/controller_factory.h>

REGISTER_ROBOT_CONTROLLER(RobotController_Ackermann_Stanley, ackermann_stanley, ackermann);

RobotController_Ackermann_Stanley::RobotController_Ackermann_Stanley():
    RobotController()
{

	ROS_INFO("Parameters: k_forward=%f, k_backward=%f\n"
				"vehicle_length=%f\n"
				"factor_steering_angle=%f\n"
				"goal_tolerance=%f",
				params_.k_forward(), params_.k_backward(),
				params_.vehicle_length(),
				params_.factor_steering_angle(),
				params_.goal_tolerance());

}

void RobotController_Ackermann_Stanley::stopMotion() {

	move_cmd_.setVelocity(0.f);
	move_cmd_.setDirection(0.f);

	MoveCommand cmd = move_cmd_;
	publishMoveCommand(cmd);
}

void RobotController_Ackermann_Stanley::start() {

}

void RobotController_Ackermann_Stanley::reset() {

    RobotController::reset();
}

void RobotController_Ackermann_Stanley::setPath(Path::Ptr path) {
    RobotController::setPath(path);
}

RobotController::MoveCommandStatus RobotController_Ackermann_Stanley::computeMoveCommand(
		MoveCommand* cmd) {

	if(path_interpol.n() <= 2)
		return RobotController::MoveCommandStatus::ERROR;

    const Eigen::Vector3d pose = pose_tracker_->getRobotPose();

    RobotController::findOrthogonalProjection();
    double d = -orth_proj_;

    if(RobotController::isGoalReached(cmd)){
       return RobotController::MoveCommandStatus::REACHED_GOAL;
    }

	// draw a line to the orthogonal projection
	geometry_msgs::Point from, to;
	from.x = pose[0]; from.y = pose[1];
    to.x = path_interpol.p(proj_ind_); to.y = path_interpol.q(proj_ind_);
    visualizer_->drawLine(12341234, from, to, getFixedFrame(), "kinematic", 1, 0, 0, 1, 0.01);

	// theta_e = theta_vehicle - theta_path (orientation error)
    double theta_e = MathHelper::AngleDelta(pose[2], path_interpol.theta_p(proj_ind_));

    // if we drive backwards invert d and set theta_e to the complementary angle
    if (getDirSign() < 0.) {
        theta_e = MathHelper::NormalizeAngle(M_PI + theta_e);
    }

	const double k = getDirSign() > 0. ? params_.k_forward() : params_.k_backward();

	const double phi = theta_e + atan2(k * d, velocity_);

	// This is the accurate steering angle for 4 wheel steering
	const float phi_actual = (float) asin(params_.factor_steering_angle() * sin(phi));

    double exp_factor = RobotController::exponentialSpeedControl();
	move_cmd_.setDirection(phi_actual);
    move_cmd_.setVelocity(getDirSign() * (float) velocity_ * exp_factor);

	*cmd = move_cmd_;

	return RobotController::MoveCommandStatus::OKAY;
}

void RobotController_Ackermann_Stanley::publishMoveCommand(
		const MoveCommand& cmd) const {

	geometry_msgs::Twist msg;
	msg.linear.x  = cmd.getVelocity();
	msg.linear.y  = 0;
	msg.angular.z = cmd.getDirectionAngle();

	cmd_pub_.publish(msg);
}


#define BOOST_TEST_MODULE 04 test 1
#include <boost/test/included/unit_test.hpp>

#define GLM_ENABLE_EXPERIMENTAL
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/string_cast.hpp>

#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <utility>

BOOST_AUTO_TEST_CASE(first_test)
{
    glm::vec4 vec(1.0f, 0.0f, 0.0f, 1.0f);
    glm::mat4 trans;
    trans = glm::translate(trans, glm::vec3(1.0f, 1.0f, 0.0f));
    vec = trans * vec;
    BOOST_TEST(vec.x == 2.0f);
    BOOST_TEST(vec.y == 1.0f);
    BOOST_TEST(vec.z == 0.0f);

    trans = glm::rotate(trans, 90.0f, glm::vec3(0.0, 0.0, 1.0));
    std::cout << glm::to_string(trans);
    trans = glm::scale(trans, glm::vec3(0.5, 0.5, 0.5));
    vec = glm::vec4(1.0f, 0.0f, 0.0f, 1.0f);
    vec = trans * vec;
    BOOST_TEST(vec.x == 0.775963187f);
    BOOST_TEST(vec.y == 1.44699836f);
    BOOST_TEST(vec.z == 0.0f);
}


#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/auto_differentiation.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/slam3d/vertex_pointxyz.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <iostream>

#include "common.h"
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <Eigen/Dense>

using namespace Sophus;
using namespace Eigen;
using namespace std;

struct Camera
{
    Camera() {}

    Camera(double* data)
    {
        Rt = g2o::SE3Quat::exp(Eigen::Map<Eigen::Matrix<double, 6, 1>>(data));

        f = data[6];
        k1 = data[7];
        k2 = data[8];
    }

    void set_to(double* data) const
    {
        Eigen::Matrix<double, 6, 1> rt = Rt.log();

        for (int i = 0; i < 6; ++i)
            data[i] = rt[i];

        data[6] = f;
        data[7] = k1;
        data[8] = k2;
    }

    g2o::SE3Quat Rt; // g2o::SE3Quat stores rotation first and then translation
    double f = 0.0, k1 = 0.0, k2 = 0.0;
};

class VertexCamera: public g2o::BaseVertex<9, Camera>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {

        _estimate = Camera();
    }

    virtual void oplusImpl(const double *update) override {
        _estimate.Rt = g2o::SE3Quat::exp(Eigen::Map<const Eigen::Matrix<double, 6, 1>>(update)) * _estimate.Rt;
        _estimate.f += update[6];
        _estimate.k1 += update[7];
        _estimate.k2 += update[8];
    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
};

class VertexLandmark: public g2o::BaseVertex<3, Eigen::Vector3d>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {
        _estimate = Eigen::Vector3d::Zero();
    }

    virtual void oplusImpl(const double *update) override {
        _estimate += Eigen::Map<const Eigen::Vector3d>(update);
    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
};

class EdgeReprojection: public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexLandmark, VertexCamera>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeReprojection(double f, double k1, double k2) : f_(f), k1_(k1), k2_(k2) { }

    virtual void computeError() override {
        const VertexCamera* v_cam = static_cast<VertexCamera*>(_vertices[1]);
        const VertexLandmark* v_point = static_cast<VertexLandmark*>(_vertices[0]);
        auto cam = v_cam->estimate();
        Eigen::Vector3d X = v_point->estimate();
        Eigen::Vector3d X_cam = cam.Rt * X;
        X_cam /= X_cam.z();

        auto p2 = X_cam.x() * X_cam.x() + X_cam.y() * X_cam.y();
        auto r = 1.0 + p2 * (cam.k1 + (p2 * cam.k2));

        Eigen::Vector2d uv = -X_cam.head<2>() *  cam.f * r; // minus because of the dataset projection
        _error = _measurement - uv;
    }

    virtual void linearizeOplus()  override  {
        const VertexCamera *v_cam = static_cast<VertexCamera*>(_vertices[1]);
        const VertexLandmark *v_landmark = static_cast<VertexLandmark*>(_vertices[0]);
        Eigen::Vector3d X3d = v_landmark->estimate();
        auto cam = v_cam->estimate();
        Eigen::Vector3d Xc = cam.Rt * X3d;

        double X = Xc.x();
        double Y = Xc.y();
        double Z = Xc.z();
        double Z_2 = Z * Z;
        double f = cam.f;
        double k1 = cam.k1;
        double k2 = cam.k2;

        double x = X / Z;
        double y = Y / Z;
        double x2 = x*x;
        double y2 = y*y;
        double n2 = x*x + y*y;
        double n4 = n2 * n2;
        double r = 1.0 + n2 * k1 + n4 * k2;

        Eigen::Matrix2d dedxd = -Eigen::Matrix2d::Identity() * f; // minus because of the dataset projection

        Eigen::Matrix2d dxddxp = Eigen::Matrix2d::Identity();

        dxddxp(0, 0) = (2*k1*x+4*k2*x2*x+4*y2*x*k2)*x + r;
        dxddxp(0, 1) = x*(2*k1*y+4*x2*k2*y+4*k2*y2*y);
        dxddxp(1, 0) = y*(2*k1*x+4*k2*x2*x+4*y2*k2*x);
        dxddxp(1, 1) = (2*k1*y+4*x2*y*k2+4*k2*y2*y)*y + r;

        Eigen::Matrix<double, 2, 3> dxpdXc;
        dxpdXc << 1.0/Z, 0.0, -X / (Z_2),
                  0.0, 1.0/Z, -Y / (Z_2);

        Eigen::Matrix<double, 2, 3> dedXc = dedxd * dxddxp * dxpdXc;

        Eigen::Matrix3d dXcdR;
        dXcdR << 0.0, -Z, Y,
                 Z, 0.0, -X,
                 -Y, X, 0.0;

        Eigen::Matrix3d dXcdt = Eigen::Matrix3d::Identity();

        Eigen::Vector2d dedf(-r*x, -r*y);   // minus because of the dataset projection
        Eigen::Vector2d dxddk1(x * n2, y * n2);
        Eigen::Vector2d dxddk2(x * n4, y * n4);
        Eigen::Vector2d dedk1 = dedxd * dxddk1;
        Eigen::Vector2d dedk2 = dedxd * dxddk2;

        _jacobianOplusXj.block<2, 3>(0, 0) = dedXc * -dXcdR; // -dXcdR <=> -Xc^
        _jacobianOplusXj.block<2, 3>(0, 3) = dedXc * dXcdt;
        _jacobianOplusXj.col(6) = dedf;
        _jacobianOplusXj.col(7) = dedk1;
        _jacobianOplusXj.col(8) = dedk2;

        _jacobianOplusXi = dedXc * cam.Rt.rotation().matrix();

         _jacobianOplusXi *= -1; // take the negative jacobian because the error is defined as (measurement - projection)
         _jacobianOplusXj *= -1;

    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
    private:
        double f_, k1_, k2_;

};

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

    if (argc != 2) {
        cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;
        return 1;
    }

    BALProblem dataset(argv[1]);
    dataset.Normalize();
    dataset.Perturb(0.1, 0.5, 0.5);
    dataset.WriteToPLYFile("initial_pc.ply");

    std::cout << "\n";
    std::cout << "nb cameras: " << dataset.num_cameras() << std::endl;
    std::cout << "nb landmarks: " << dataset.num_points() << std::endl;
    std::cout << "nb observations: " << dataset.num_observations() << std::endl;
    std::cout << "nb parameters: " << dataset.num_parameters() << std::endl;
    std::cout << "check: " << dataset.num_cameras() * 9 + dataset.num_points()*3 << std::endl;

    // pose dimension 9, landmark is 3
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;

    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
    );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    auto* cameras = dataset.mutable_cameras();
    std::vector<VertexCamera*> camera_vertices;
    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        auto *c = new VertexCamera();
        c->setId(i);
        c->setEstimate(Camera(cameras + (i*dataset.camera_block_size())));
        optimizer.addVertex(c);
        camera_vertices.push_back(c);
    }

    auto* landmarks = dataset.mutable_points();
    std::vector<VertexLandmark*> landmark_vertices;
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        auto* l = new VertexLandmark();
        l->setId(dataset.num_cameras() + i);
        l->setEstimate(Eigen::Map<Eigen::Vector3d>(landmarks + i*dataset.point_block_size()));
        l->setMarginalized(true);
        optimizer.addVertex(l);
        landmark_vertices.push_back(l);
    }

    auto* observations = dataset.observations();
    auto* cam_indices = dataset.camera_index();
    auto* landmark_indices = dataset.point_index();
    for (int i = 0; i < dataset.num_observations(); ++i)
    {
        auto c = Camera(cameras + (cam_indices[i]*dataset.camera_block_size()));
        auto* e = new EdgeReprojection(c.f, c.k1, c.k2);
        e->setVertex(1, camera_vertices[cam_indices[i]]);
        e->setVertex(0, landmark_vertices[landmark_indices[i]]);
        e->setMeasurement(Eigen::Map<const Eigen::Vector2d>(observations + i*2));
        e->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(e);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(40);

    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        camera_vertices[i]->estimate().set_to(cameras + (i * dataset.camera_block_size()));
    }
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        Eigen::Vector3d X = landmark_vertices[i]->estimate();
        landmarks[i*3] = X.x();
        landmarks[i*3+1] = X.y();
        landmarks[i*3+2] = X.z();
    }

    dataset.WriteToPLYFile("after_ba_g2o.ply");

    return 0;
}


#include <gmp_lib/CanonicalSystem/CanonicalSystem.h>

#include <cstdlib>
#include <cmath>
#include <array>
#include <exception>

#include <boost/numeric/odeint.hpp>

namespace as64_
{

namespace gmp_
{

typedef std::array<double,2> can_sys_state;
typedef boost::numeric::odeint::runge_kutta_cash_karp54< can_sys_state > can_sys_error_stepper_type;
typedef boost::numeric::odeint::controlled_runge_kutta< can_sys_error_stepper_type > can_sys_controlled_stepper_type;

CanonicalSystem::CanonicalSystem(double T, double Ds)
{
  this->s0 = 0;
  this->sf = 1;
  this->Ds = Ds;

  this->sd_dot = 1/T;

  this->s = 0;
  this->s_dot = this->sd_dot;
}

void CanonicalSystem::integrate(double t0, double tf)
{
  static can_sys_controlled_stepper_type ctrl_stepper;

  if (tf < t0) throw std::runtime_error("Cannot integrate backwards in time: t0 > tf");

  if (std::isinf(this->Ds))
  {
    this->s_dot = this->sd_dot;
    this->s = this->s + this->s_dot*(tf - t0);

    if (this->s > this->sf) this->s = this->sf;
    else if (this->s < this->s0) this->s = this->s0;

    return;
  }

  auto ode_fun = [this](const can_sys_state &state, can_sys_state &state_dot, double t)
  {
    state_dot[0] = state[1];
    state_dot[1] = this->getPhaseDDot(state[0], state[1]);
  };

  can_sys_state state = {s, s_dot};
  boost::numeric::odeint::integrate_adaptive(ctrl_stepper, ode_fun, state, t0, tf, tf-t0);
  s = state[0];
  s_dot = state[1];
}

void CanonicalSystem::setDuration(double T, double t)
{
  if (T < t) throw std::runtime_error("The current time has already exceeded the duration");

  this->sd_dot = (this->sf - this->s) / (T - t);
}

void CanonicalSystem::setRemainingDuration(double T)
{
  this->sd_dot = (this->sf - this->s) / T;
}

double CanonicalSystem::getPhaseDDot(double s, double s_dot)
{
  double s_ddot = 0;
  if (s>=this->s0 && s < this->sf) s_ddot = -this->Ds*(s_dot - this->sd_dot);
  else if (s>=1)                   s_ddot = -400*s_dot - 1000*(s-this->sf);
  else                             s_ddot = -400*s_dot - 1000*(s-this->s0);

  return s_ddot;
}

void CanonicalSystem::reset()
{
  this->s = this->s0;
  this->s_dot = this->sd_dot;
}

} // namespace gmp_

} // namespace as64_


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

#include <algorithm>
#include <limits>

#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/math/tools/minima.hpp>
#include <boost/math/tools/roots.hpp>

#include "geometry/spline_curve_3D.hpp"
#include "geometry/cubic_spline_interp_3D.hpp"

/*!
 * Constructor for creating a spline curve of given degree from a knot vector
 * and associated control points.
 */
SplineCurve3D::SplineCurve3D(
        int degree,
        std::vector<real> knotVector,
        std::vector<gmx::RVec> ctrlPoints)
{
    // TODO: probably better to put some of these things into the initialiser
    // list

    nCtrlPoints_ = ctrlPoints.size();
    nKnots_ = knotVector.size();
    degree_ = degree;
    arcLengthTableAvailable_ = false;

    // ensure minimal number of control points for given degree:
    if( nCtrlPoints_ < degree_ + 1 )
    {
        std::cerr<<"ERROR: Need at least d + 1 control points!"<<std::endl;
        std::cerr<<"d = "<<degree_
                 <<" and n = "<<nCtrlPoints_
                 <<std::endl;
        std::abort();
    }

    // ensure minimal number of knots:
    if( nKnots_ < nCtrlPoints_ + degree_ + 1 )
    {
        std::cerr<<"ERROR: Need at least n + d + 1 knots!"<<std::endl;
        std::cerr<<"d = "<<degree_
                 <<" and n = "<<nCtrlPoints_
                 <<" and k = "<<nKnots_
                 <<std::endl;
        std::abort();
    }

    // assign knot vector and control points:
    knots_ = knotVector;
    ctrlPoints_ = ctrlPoints;
}

/*!
 * Default constructor for initialiser lists. Does not set any members!
 */
SplineCurve3D::SplineCurve3D()
{

}

/*!
 * Public interface for evaluation of spline curve. Returns the value of the
 * spline curve or its derivative at the given evaluation point. If the
 * evaluation point lies outside the knot range, linear extrapolation is used.
 */
gmx::RVec
SplineCurve3D::evaluate(
        const real &eval,
        unsigned int deriv)
{
    // extrapolation or interpolation?
    if( eval < knots_.front() || eval > knots_.back() )
    {
        return evaluateExternal(eval, deriv);
    }
    else
    {
        return evaluateInternal(eval, deriv);
    }
}

/*!
 * Auxiliary function for evaluating the spline curve at points inside the
 * range covered by knots.
 */
gmx::RVec
SplineCurve3D::evaluateInternal(const real &eval, unsigned int deriv)
{
    // container for basis functions or derivatives:
    SparseBasis basis;

    // derivative required?
    if( deriv == 0 )
    {
        // evaluate B-spline basis:
        basis = B_(eval, knots_, degree_);
    }
    else
    {
        // evaluate B-spline basis derivatives:
        basis = B_(eval, knots_, degree_, deriv);
    }

    // return value of spline curve (derivative) at given evaluation point:
    return computeLinearCombination(basis);
}

/*!
 * Auxiliary function for evaluating the spline curve at points outside the
 * range covered by knots. Linear extrapolation is used in this case.
 */
gmx::RVec
SplineCurve3D::evaluateExternal(const real &eval, unsigned int deriv)
{
    // which boundary is extrapolation based on?
    real boundary;
    if( eval < knots_.front() )
    {
        boundary = knots_.front();
    }
    else
    {
        boundary = knots_.back();
    }

    // derivative required?
    if( deriv == 0 )
    {
        // compute slope and offset:
        // TODO: this can be made more efficient by evaluating basis and derivs
        // in one go!
        SparseBasis basis = B_(boundary, knots_, degree_);
        gmx::RVec offset = computeLinearCombination(basis);
        basis = B_(boundary, knots_, degree_, 1);
        gmx::RVec slope = computeLinearCombination(basis);

        // return extrapolation point:
        svmul(eval - boundary, slope, slope);
        rvec_add(slope, offset, slope);
        return slope;

    }
    else if( deriv == 1 )
    {
        // simply return the slope at the endpoint:
        SparseBasis basis = B_(boundary, knots_, degree_, 1);
        return computeLinearCombination(basis);
    }
    else
    {
        // for linear extrapolation, second and higher order deriv are zero:
        return gmx::RVec(0.0, 0.0, 0.0);
    }
}

/*!
 * Evaluates the linear combination of basis functions weighted by control
 * points, i.e. computes
 *
 * \f[
 *      \sum_i \mathbf{c}_i B_{i,p}(x)
 * \f]
 *
 * where the sum only goes over the nonzero elements of the basis for
 * efficiency.
 */
gmx::RVec
SplineCurve3D::computeLinearCombination(const SparseBasis &basis)
{
    gmx::RVec value(gmx::RVec(0.0, 0.0, 0.0));
    for(auto b : basis)
    {
        gmx::RVec tmp;
        svmul(b.second, ctrlPoints_[b.first], tmp);
        rvec_add(value, tmp, value);
    }

    return value;
}

/*!
 * Change the internal representation of the curve such that it is
 * parameterised in terms of arc length.
 */
void
SplineCurve3D::arcLengthParam()
{
    // number of uniformly spaced arc length parameters:
    int nNew = 10*nCtrlPoints_;

    // create lookup table for arc length at knots:
    prepareArcLengthTable();

    // determine uniform arc length spacing:
    real arcLenStep = arcLengthTable_.back() / (nNew - 1);

    // initialise new control points and parameters:
    std::vector<real> newParams;
    std::vector<gmx::RVec> newPoints;

    // loop over uniformly spaced arc length intervals:
    for(int i = 0; i < nNew; i++)
    {
        // calculate target arc length:
        real newParam = i*arcLenStep;
        newParams.push_back(newParam);

        // find parameter value corresponding to arc length value:
        real oldParam = arcLengthToParam(newParam);

        //  evaluate spline to get new point:
        newPoints.push_back(this -> evaluate(oldParam, 0));
    }

    // interpolate new points to get arc length parameterised curve:
    CubicSplineInterp3D Interp;
    SplineCurve3D newSpl = Interp(newParams,
                                  newPoints,
                                  eSplineInterpBoundaryHermite);

    // update own parameters:
    this -> knots_ = newSpl.knots_;
    this -> ctrlPoints_ = newSpl.ctrlPoints_;
    this -> nKnots_ = newSpl.nKnots_;
    this -> nCtrlPoints_ = newSpl.nCtrlPoints_;
    this -> arcLengthTableAvailable_ = false;

    // reset reference points for mapping:
    refPoints_.clear();
}

/*!
 * Calculates the length along the arc of the curve between the two parameter
 * values a and b.
 */
real
SplineCurve3D::length(const real &lo, const real &hi)
{
    // do we need to form a lookup table:
    if( arcLengthTableAvailable_ == false || arcLengthTable_.size() == 0 )
    {
        prepareArcLengthTable();
    }

    // initialise length as zero::
    real length = 0.0;

    // find intervals of evaluation points:
    int idxLo = findInterval(lo);
    int idxHi = findInterval(hi);

    // add distance in endpoint intervals:
    if( idxHi == idxLo )
    {
        length += arcLengthBoole(lo, hi);
    }
    else
    {
        length += arcLengthBoole(lo, knots_[idxLo + 1]);
        length += arcLengthBoole(knots_[idxHi], hi);
    }

    // if necessary, loop over intermediate spline segments and sum up lengths:
    if( idxHi - idxLo > 1 )
    {
        for(int i = idxLo + 1; i < idxHi; i++)
        {
            // add length of current segment from lookup table:
            length += arcLengthTable_[i + 1] - arcLengthTable_[i];
        }
    }

    return length;
}

/*!
 * Convenience function to calculate arc length of curve between first and last
 * support point.
 */
real
SplineCurve3D::length()
{
    return length(knots_.front(), knots_.back());
}

/*!
 * Returns the tangent vector at the given evaluation point. Simply a wrapper
 * around the general evaluation function requesting the first derivative at
 * the given point.
 */
gmx::RVec
SplineCurve3D::tangentVec(const real &eval)
{
    return evaluate(eval, 1);
}

/*!
 * Returns the normal vector at the evaluation point.
 */
gmx::RVec
SplineCurve3D::normalVec(const real &eval)
{
    return evaluate(eval, 2);
}

/*!
 * Returns the speed of the curve at the given evaluation point, where speed
 * refers to the magnitude of the tangent vector.
 */
real
SplineCurve3D::speed(const real &eval)
{
    // return magnitude of tangent vector:
    return norm( this -> tangentVec(eval) );
}

/*!
 * Takes point in Cartesian coordinates and returns that points coordinates in
 * the curvilinear system defined by the spline curve. Return value is an RVec,
 * which contains the following information:
 *
 *      [0] - distance along the arc of the curve
 *      [1] - squared (!) distance from the curve at closest point
 *      [2] - angular coordinate; this is not yet implemented
 *
 * Note that this function assumes that the curve is parameterised by arc
 * length!
 *
 * \todo implement angular coordinate!
 */
gmx::RVec
SplineCurve3D::cartesianToCurvilinear(const gmx::RVec &cartPoint)
{
    // find index of interval containing closest point on spline curve:
    unsigned int idx = closestSplinePoint(cartPoint);

    // find closest point on this interval:
    gmx::RVec proj = projectionInInterval(
            cartPoint,
            knots_[idx + degree_],
            knots_[idx + degree_ + 1]);

    // check neighbouring knot intervals and extrapolate if necessary:
    gmx::RVec altProj;

    // next lower knot interval:
    if( idx == 0 )
    {
        altProj = projectionInExtrapRange(cartPoint, -1.0);
    }
    else
    {
        altProj = projectionInInterval(
                cartPoint,
                knots_[idx + degree_ - 1],
                knots_[idx + degree_]);
    }

    // does alternative projection give closer point:
    if( altProj[RR] < proj[RR] )
    {
        proj = altProj;
    }

    // next higher knot interval:
    if( idx == refPoints_.size() - 2 )
    {
        altProj = projectionInExtrapRange(cartPoint, 1.0);
    }
    else
    {
        altProj = projectionInInterval(
                cartPoint,
                knots_[idx + degree_ + 1],
                knots_[idx + degree_ + 2]);
    }

    // does alternative projection give closer point:
    if( altProj[RR] < proj[RR] )
    {
        proj = altProj;
    }

    // TODO: calculate angular coordinate!
    proj[PP] = 0.0;

    // return point in curvilinear coordinates:
    return proj;
}

/*!
 * Auxiliary function for finding the closest point on a spline curve that
 * returns the corresponding spline interval index. First, a set of reference
 * points is sampled from the spline curve at the location of the unique knots
 * (this step is skipped if the reference points have already been computed in
 * a previous call to this function). Secondly, a linear search over these
 * reference points is carried out to find the point closest to a given test
 * point.
 *
 * The return value is the index of the closest reference point, except for the
 * case where the closest reference point is the last point, which is mapped to
 * the last interval, i.e. the index of the penultimate reference point is
 * returned in this case.
 *
 * \todo Linear search may not be the most efficient.
 */
unsigned int
SplineCurve3D::closestSplinePoint(const gmx::RVec &point)
{
    // build lookup table:
    if( refPoints_.empty() )
    {
        refPoints_.reserve(uniqueKnots().size());
        for(auto s : uniqueKnots())
        {
            refPoints_.push_back( this -> evaluate(s, 0) );
        }
    }

    // find index of closest reference point on spline curve:
    unsigned int idxMinDist = 0;
    real minDist = std::numeric_limits<real>::infinity();
    for(unsigned int i = 0; i < refPoints_.size(); i++)
    {
        // find dist to control point:
        real dist = distance2(point, refPoints_[i]);

        // closer than previous closest point:
        if( dist < minDist )
        {
            minDist = dist;
            idxMinDist = i;
        }
    }

    // special case of last control point:
    if( idxMinDist == refPoints_.size() - 1 )
    {
        // simply map this to last interval:
        idxMinDist--;
    }

    // return index of interval of closest point:
    return idxMinDist;
}

/*!
 * Auxiliary function that maps a point in Cartesian coordinates onto an
 * internal segment of the spline curve. This is accomplished by iteratively
 * minimising the distance between the test point and a base point sampled from
 * the spline curve using the (derivative free) method of Brent.
 *
 * \throws A logic error is thrown if the minimum can not be converged within
 * a hardcoded number of 100 Brent iterations.
 */
gmx::RVec
SplineCurve3D::projectionInInterval(
        const gmx::RVec &point,
        const real &lo,
        const real &hi)
{
    // internal parameters:
    const boost::uintmax_t maxIter = 100;
    boost::uintmax_t iter = maxIter;

    // objective function binding:
    boost::function<real(real)> objFun;
    objFun = boost::bind(
            &SplineCurve3D::pointSqDist,
            this,
            point,
            _1);

    // find minimum via Brent's method:
    int bits = std::numeric_limits<double>::digits;
    std::pair<double, double> result;
    result = boost::math::tools::brent_find_minima(
            objFun,
            lo,
            hi,
            bits,
            iter);

    // make sure convergence has been reached:
    if( iter >= maxIter )
    {
        throw std::logic_error("Could not converge Brent iteration in "
                               "Cartesian to curvilinear mapping!");
    }

    // return curvilinear coordinates of point:
    // TODO: implement angular coordinate
    gmx::RVec curvPoint;
    curvPoint[SS] = result.first;
    curvPoint[RR] = result.second;
    return curvPoint;
}

/*!
 * Auxiliary function that projects a point in Cartesian coordinates onto the
 * extrapolation range beyond its two endpoints. As the curve is known to be a
 * line in this range the projection is solved for analytically as a projection
 * onto a ray. To achieve this, a ray is constructed by sampling two point
 * from the spline curve (its endpoint and a second point \f$ ds \f$  beyond
 * this endpoint.
 */
gmx::RVec
SplineCurve3D::projectionInExtrapRange(
        const gmx::RVec &point,
        const real &ds)
{
    // return variable:
    gmx::RVec proj;

    // lower or upper extrapolation range?
    gmx::RVec extrapPointA;
    gmx::RVec extrapPointB;
    real arcLenOffset;
    real arcLenSign;
    if( ds < 0.0 )
    {
        // lower range:
        extrapPointA = ctrlPoints_.front();
        arcLenOffset = knots_.front();
        arcLenSign = -1.0;
        extrapPointB = this -> evaluate(arcLenOffset + ds, 0);
    }
    else if( ds > 0.0 )
    {
        // upper range:
        extrapPointA = ctrlPoints_.back();
        arcLenOffset = knots_.back();
        arcLenSign = 1.0;
        extrapPointB = this -> evaluate(arcLenOffset + ds, 0);
    }
    else
    {
        throw std::logic_error("Parameter dt may not be zero!");
    }

    // (non-normalised) direction vector for the extrapolating line:
    gmx::RVec lineDirVector;
    rvec_sub(extrapPointB, extrapPointA, lineDirVector);

    // vector between the test point and the ray's endpoint:
    gmx::RVec endpointVector;
    rvec_sub(point, extrapPointA, endpointVector);

    // is ray endpoint the closest point?
    // NOTE: in this case the angle between line direction vector and endpoint
    // vector will be >= 90 degrees and the scalar product <= 0.0!
    real cosOfAngle = iprod(endpointVector, lineDirVector);
    if( cosOfAngle <= 0.0 )
    {
        proj[SS] = arcLenOffset;
        proj[RR] = distance2(point, extrapPointA);

        return proj;
    }

    // projection of test point position onto ray and base point:
    real b = cosOfAngle/iprod(lineDirVector, lineDirVector);
    gmx::RVec basePoint;
    svmul(b, lineDirVector, basePoint);
    rvec_add(basePoint, extrapPointA, basePoint);

    // curvilinear coordinates around this line:
    proj[SS] = arcLenOffset + arcLenSign*b;
    proj[RR] = distance2(point, basePoint);

    // return points in curvilinear coordinates:
    return proj;
}

/*!
 * Getter function for access to the spline curves control points.
 */
std::vector<gmx::RVec>
SplineCurve3D::ctrlPoints() const
{
    return ctrlPoints_;
}

/*!
 * Uses Newton-Cotes quadrature of curve speed to determine the length of the
 * arc between two given parameter values. The specific quadrature rule applied
 * is Boole's rule.
 */
real
SplineCurve3D::arcLengthBoole(const real &lo, const real &hi)
{
    // determine intermediate evaluation points:
    real h = (hi - lo)/4.0;
    real t2 = lo + 1.0*h;
    real t3 = lo + 2.0*h;
    real t4 = lo + 3.0*h;

    // evaluate speed at support points:
    real s1 = speed(lo);
    real s2 = speed(t2);
    real s3 = speed(t3);
    real s4 = speed(t4);
    real s5 = speed(hi);

    // evaluate Boole's law:
    return 2.0*h/45.0*(7.0*s1 + 32.0*s2 + 12.0*s3 + 32.0*s4 + 7.0*s5);
}

/*!
 * Prepares a lookup table that associates an arc length with each knot, where
 * the first knot is assigned an arc length of zero.
 */
void
SplineCurve3D::prepareArcLengthTable()
{
    arcLengthTable_.resize(knots_.size());
    real segmentLength;
    for(unsigned int i = 0; i < knots_.size() - 1; i++)
    {
        // calculate length of current segment:
        segmentLength = arcLengthBoole(knots_[i], knots_[i+1]);

        // add to arc length table:
        arcLengthTable_[i + 1] = arcLengthTable_[i] + segmentLength;
    }

    // set flag:
    arcLengthTableAvailable_ = true;
}

/*!
 * Returns arc length value at the control points by simply removing the
 * repeated knot values from the arc length lookup lookup table.
 */
std::vector<real>
SplineCurve3D::ctrlPointArcLength()
{
    // check availability:
    if( arcLengthTableAvailable_ == false )
    {
        prepareArcLengthTable();
    }

    // create copy of table and remove redundant elements:
    std::vector<real> arcLength(arcLengthTable_.begin() + degree_,
                                arcLengthTable_.end() - degree_);

    return arcLength;
}

/*!
 * Returns arc length value at first control point.
 */
real
SplineCurve3D::frstPointArcLength()
{
    // check availability:
    if( arcLengthTableAvailable_ == false )
    {
        prepareArcLengthTable();
    }

    return arcLengthTable_.front();
}

/*!
 * Returns arc length value at last control point.
 */
real
SplineCurve3D::lastPointArcLength()
{
    // check availability:
    if( arcLengthTableAvailable_ == false )
    {
        prepareArcLengthTable();
    }

    return arcLengthTable_.back();
}

/*!
 * Returns the parameter value (in the current parameterisation, typically
 * chord length) that corresponds to a given value of arc length. This is done
 * via bisection.
 */
real
SplineCurve3D::arcLengthToParam(real &arcLength)
{
    boost::uintmax_t maxIter = 100;
    real absTol = 0.01*std::sqrt(std::numeric_limits<real>::epsilon());

    // sanity check for arc length table:
    if( arcLengthTableAvailable_ != true )
    {
        prepareArcLengthTable();
    }

    // handle upper endpoint:
    if( arcLength == arcLengthTable_.back() )
    {
        return knots_.back();
    }

    // find appropriate interval:
    std::pair<std::vector<real>::iterator, std::vector<real>::iterator> bounds;
    bounds = std::equal_range(arcLengthTable_.begin(), arcLengthTable_.end(), arcLength);
    int idxLo = bounds.second - arcLengthTable_.begin() - 1;
    int idxHi = bounds.second - arcLengthTable_.begin();

    // handle query outside table range:
    // TODO: add case for query below lower bound and test!
    if( bounds.second == arcLengthTable_.end() )
    {
        return knots_.back() + arcLength - arcLengthTable_.back();
    }
    if( bounds.second == arcLengthTable_.begin() )
    {
        std::cerr<<"ERROR: arc length below table value range!"<<std::endl;
        std::abort();
    }

    // initialise bisection interval and lower limit:
    real tLo = knots_[idxLo];
    real tHi = knots_[idxHi];
    real tLi = tLo;

    // target arc length within this interval:
    real targetIntervalLength = arcLength - arcLengthTable_[idxLo];

    // termination condition binding:
    boost::function<bool(real, real)> termCond;
    termCond = boost::bind(&SplineCurve3D::arcLengthToParamTerm,
                           this,
                           _1,
                           _2,
                           absTol);

    // objective function binding:
    boost::function<real(real)> objFun;
    objFun = boost::bind(&SplineCurve3D::arcLengthToParamObj,
                         this,
                         tLi,
                         _1,
                         targetIntervalLength);

    std::pair<real, real> result;
    result = boost::math::tools::toms748_solve(objFun,
                                               tLo,
                                               tHi,
                                               termCond,
                                               maxIter);

    // best guess if middle of bracketing interval:
    return 0.5*(result.first + result.second);
}

/*!
 * Termination condition for re-parameterisation optimisation.
 */
bool
SplineCurve3D::arcLengthToParamTerm(real lo, real hi, real tol)
{
    return std::abs(hi - lo) <= tol;
}

/*!
 * Objective function for re-parameterisation optimisation.
 */
real
SplineCurve3D::arcLengthToParamObj(real lo, real hi, real target)
{
    return arcLengthBoole(lo, hi) - target;
}

/*!
 * Computes the squared Euclidean distance between some point and the point
 * on the spline curve with the given parameter value. For efficiency, the
 * square root is not drawn, hence the squared distance is returned.
 */
double
SplineCurve3D::pointSqDist(gmx::RVec point, double eval)
{
    // evaluate spline:
    gmx::RVec splPoint = evaluate(eval, 0);

    // return squared distance:
    return (splPoint[XX] - point[XX])*(splPoint[XX] - point[XX]) +
           (splPoint[YY] - point[YY])*(splPoint[YY] - point[YY]) +
           (splPoint[ZZ] - point[ZZ])*(splPoint[ZZ] - point[ZZ]);
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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 NETKET_ISING1D_HPP
#define NETKET_ISING1D_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <complex>
#include <iostream>
#include <vector>
#include "Utils/random_utils.hpp"
#include "abstract_hamiltonian.hpp"

namespace netket {

/**
  Transverse field Ising model on an arbitrary graph.
*/
template <class G>
class Ising : public AbstractHamiltonian {
  const int nspins_;
  double h_;
  double J_;

  const G &graph_;

  /**
    List of bonds for the interaction part.
  */
  std::vector<std::vector<int>> bonds_;

  /**
    Hilbert space descriptor for this hamiltonian.
  */
  Hilbert hilbert_;

 public:
  /**
    Json constructor.
    @param G is a graph from which the number of spins and the bonds are
    obtained.
    @param pars is a json list of parameters. The default value of J is 1.0
  */
  explicit Ising(const G &graph, const json &pars)
      : nspins_(graph.Nsites()),
        h_(FieldVal(pars["Hamiltonian"], "h")),
        J_(FieldOrDefaultVal(pars["Hamiltonian"], "J", 1.0)),
        graph_(graph) {
    Init();
  }

  void Init() {
    GenerateBonds();

    // Specifying the hilbert space
    json hil;
    hil["Hilbert"]["Name"] = "Spin";
    hil["Hilbert"]["Nspins"] = nspins_;
    hil["Hilbert"]["S"] = 0.5;

    hilbert_.Init(hil);

    InfoMessage() << "Transverse-Field Ising model created " << std::endl;
    InfoMessage() << "h = " << h_ << std::endl;
    InfoMessage() << "J = " << J_ << std::endl;
  }

  /**
    Member function generating the bonds on the lattice.
    bonds[i][k] contains the k-th bond for site i.
  */
  void GenerateBonds() {
    auto adj = graph_.AdjacencyList();

    bonds_.resize(nspins_);

    for (int i = 0; i < nspins_; i++) {
      for (auto s : adj[i]) {
        if (s > i) {
          bonds_[i].push_back(s);
        }
      }
    }
  }

  /**
  Member function finding the connected elements of the Hamiltonian.
  Starting from a given visible state v, it finds all other visible states v'
  such that the hamiltonian matrix element H(v,v') is different from zero.
  In general there will be several different connected visible units satisfying
  this condition, and they are denoted here v'(k), for k=0,1...N_connected.
  @param v a constant reference to the visible configuration.
  @param mel(k) is modified to contain matrix elements H(v,v'(k)).
  @param connector(k) for each k contains a list of sites that should be changed
  to obtain v'(k) starting from v.
  @param newconfs(k) is a vector containing the new values of the visible units
  on the affected sites, such that: v'(k,connectors(k,j))=newconfs(k,j). For the
  other sites v'(k)=v, i.e. they are equal to the starting visible
  configuration.
  */
  void FindConn(const Eigen::VectorXd &v,
                std::vector<std::complex<double>> &mel,
                std::vector<std::vector<int>> &connectors,
                std::vector<std::vector<double>> &newconfs) const override {
    connectors.clear();
    connectors.resize(nspins_ + 1);
    newconfs.clear();
    newconfs.resize(nspins_ + 1);
    mel.resize(nspins_ + 1);

    mel[0] = 0;
    connectors[0].resize(0);
    newconfs[0].resize(0);

    for (int i = 0; i < nspins_; i++) {
      // spin flips
      mel[i + 1] = -h_;
      connectors[i + 1].push_back(i);
      newconfs[i + 1].push_back(-v(i));

      // interaction part
      for (auto bond : bonds_[i]) {
        mel[0] -= J_ * v(i) * v(bond);
      }
    }
  }

  const Hilbert &GetHilbert() const override { return hilbert_; }
};

}  // namespace netket

#endif


#pragma once
#include <boost/serialization/access.hpp>

#include "Node.hxx"
#include "Tensor.hxx"

class Eta : public Tensor {
 private:
  char i1, i2;

  friend class boost::serialization::access;

 public:
  Eta() = default;
  Eta(char const i1, char const i2);

  char order () const override;

  void exchangeTensorIndices (std::map<char, char> const & exchange_map) override;
  int sortIndices () override;

  int evaluate(std::map <char, char> const & eval_map) const override;
  mpq_class symmetrize() override;
  bool containsIndex (char i) const override;
  std::string print () const override;
  std::string printMaple () const override;
  int applyTensorSymmetries (int parity) override;

  bool lessThan(Node const * other) const override;
  bool equals(Node const * other) const override;

  std::unique_ptr<Node> clone () const override;

  char getOther (char i) const;

  template<class Archive>
  void serialize (Archive & ar, unsigned int const version);

  ~Eta() = default;
};


// Copyright Aleksey Gurtovoy 2003-2004
// Copyright Jaap Suter 2003
//
// Distributed under the Boost Software License,Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/bitwise.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/aux_/test.hpp>

typedef integral_c<unsigned int, 0> _0;
typedef integral_c<unsigned int, 1> _1;
typedef integral_c<unsigned int, 2> _2;
typedef integral_c<unsigned int, 8> _8;
typedef integral_c<unsigned int, 0xffffffff> _ffffffff;

MPL_TEST_CASE()
{
    MPL_ASSERT_RELATION( (bitand_<_0,_0>::value), ==, 0 );
    MPL_ASSERT_RELATION( (bitand_<_1,_0>::value), ==, 0 );
    MPL_ASSERT_RELATION( (bitand_<_0,_1>::value), ==, 0 );
    MPL_ASSERT_RELATION( (bitand_<_0,_ffffffff>::value), ==, 0 );
    MPL_ASSERT_RELATION( (bitand_<_1,_ffffffff>::value), ==, 1 );
    MPL_ASSERT_RELATION( (bitand_<_8,_ffffffff>::value), ==, 8 );
}

MPL_TEST_CASE()
{
    MPL_ASSERT_RELATION( (bitor_<_0,_0>::value), ==, 0 );
    MPL_ASSERT_RELATION( (bitor_<_1,_0>::value), ==, 1 );
    MPL_ASSERT_RELATION( (bitor_<_0,_1>::value), ==, 1 );
    MPL_ASSERT_RELATION( (bitor_<_0,_ffffffff>::value), ==, 0xffffffff );
    MPL_ASSERT_RELATION( (bitor_<_1,_ffffffff>::value), ==, 0xffffffff );
    MPL_ASSERT_RELATION( (bitor_<_8,_ffffffff>::value), ==, 0xffffffff );
}

MPL_TEST_CASE()
{
    MPL_ASSERT_RELATION( (bitxor_<_0,_0>::value), ==, 0 );
    MPL_ASSERT_RELATION( (bitxor_<_1,_0>::value), ==, 1 );
    MPL_ASSERT_RELATION( (bitxor_<_0,_1>::value), ==, 1 );
    MPL_ASSERT_RELATION( (bitxor_<_0,_ffffffff>::value), ==, (0xffffffff ^ 0) );
    MPL_ASSERT_RELATION( (bitxor_<_1,_ffffffff>::value), ==, (0xffffffff ^ 1) );
    MPL_ASSERT_RELATION( (bitxor_<_8,_ffffffff>::value), ==, (0xffffffff ^ 8) );
}

MPL_TEST_CASE()
{
    MPL_ASSERT_RELATION( (shift_right<_0,_0>::value), ==, 0 );
    MPL_ASSERT_RELATION( (shift_right<_1,_0>::value), ==, 1 );
    MPL_ASSERT_RELATION( (shift_right<_1,_1>::value), ==, 0 );
    MPL_ASSERT_RELATION( (shift_right<_2,_1>::value), ==, 1 );
    MPL_ASSERT_RELATION( (shift_right<_8,_1>::value), ==, 4 );
}

MPL_TEST_CASE()
{
    MPL_ASSERT_RELATION( (shift_left<_0,_0>::value), ==, 0 );
    MPL_ASSERT_RELATION( (shift_left<_1,_0>::value), ==, 1 );
    MPL_ASSERT_RELATION( (shift_left<_1,_1>::value), ==, 2 );
    MPL_ASSERT_RELATION( (shift_left<_2,_1>::value), ==, 4 );
    MPL_ASSERT_RELATION( (shift_left<_8,_1>::value), ==, 16 );
}


///////////////////////////////////////////////////////////////////////////////
// value.hpp                                                                 //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_EMPIRICAL_DISTRIBUTION_KOLMOGOROV_SMIRNOV_STATISTIC_VALUE_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_EMPIRICAL_DISTRIBUTION_KOLMOGOROV_SMIRNOV_STATISTIC_VALUE_HPP_ER_2010
#include <cmath>
#include <utility>

#include <boost/type_traits.hpp>
#include <boost/range.hpp>

#include <boost/numeric/conversion/converter.hpp>

#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/if.hpp>

#include <boost/foreach.hpp>
#include <boost/parameter/name.hpp>
#include <boost/parameter/binding.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics/count.hpp>

#include <boost/statistics/detail/non_parametric/empirical_distribution/ordered_sample.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace empirical_distribution{
namespace kolmogorov_smirnov_statistic{

BOOST_PARAMETER_NAME(benchmark_distribution);

namespace impl{

    // T can be an integer or a float
    template<typename T,typename T1,typename Comp = std::less<T> >
	class value : public boost::accumulators::accumulator_base
    {
        typedef Comp comp_;
        typedef boost::accumulators::dont_care dont_care_;

        public:

        typedef T1 result_type;
        typedef T sample_type;

        value(dont_care_){}

        void operator()(dont_care_){}

        template<typename Args>
        result_type result(const Args& args)const{
            namespace ac = boost::accumulators;
            namespace ed = detail::empirical_distribution;
            namespace ks = ed::kolmogorov_smirnov_statistic;
            typedef T1 val_;
            typedef std::size_t size_;
            typedef ac::tag::count tag_n_;
            typedef ed::tag::ordered_sample tag_os_;
            typedef typename parameter::binding<
                Args, ac::tag::accumulator>::type cref_acc_set_;
            typedef typename boost::remove_cv<
               typename boost::remove_reference<
                   cref_acc_set_
                >::type
            >::type acc_set_;
            typedef typename ed::result_of::ordered_sample<
                acc_set_>::type ref_os_;
            typedef typename boost::remove_const< //in case ref changed to cref
            	typename boost::remove_reference<ref_os_>::type
            >::type os_;
            typedef typename boost::range_reference<os_>::type ref_elem_;
            typedef boost::numeric::converter<val_,size_> converter_;

            ref_os_ ref_os = ac::extract_result<tag_os_>(
                 args[ ac::accumulator ] );
            val_ m1 = converter_::convert( 0 );
            size_ i = 0;
            size_ n = boost::accumulators::extract::count(
                args[ ac::accumulator ] );

            BOOST_FOREACH(ref_elem_ e,ref_os){
                i += e.second;
                val_ ecdf = converter_::convert( i ) / converter_::convert( n );
                val_ true_cdf = cdf(
                    args[ ks::_benchmark_distribution ] ,
                    e.first
                );
                val_ m2
                	= (true_cdf > ecdf)?(true_cdf - ecdf) : (ecdf - true_cdf);
                if(m2 > m1){ m1 = m2; }
            }

            return m1;
        }

    };

}// impl
namespace tag
{
    template<typename T1 = double>
    struct value: boost::accumulators::depends_on<
        empirical_distribution::tag::ordered_sample,
        accumulators::tag::count
    >
    {
        struct impl{
            template<typename T,typename W>
            struct apply{
                typedef empirical_distribution
                    ::kolmogorov_smirnov_statistic::impl::value<T,T1> type;
            };
        };
    };
}// tag
namespace result_of{

    template<typename T1,typename AccSet,typename D>
    struct value{
    	typedef empirical_distribution
            ::kolmogorov_smirnov_statistic::tag::value<T1> tag_;
        typedef typename
            boost::accumulators::detail::template
            	extractor_result<AccSet,tag_>::type type;
    };

}
namespace extract
{

    template<typename T1,typename AccSet,typename D>
    typename kolmogorov_smirnov_statistic::result_of::template
        value<T1,AccSet,D>::type
  	value(AccSet const& acc,const D& dist)
    {
        namespace ed = detail::empirical_distribution;
        namespace ks = ed::kolmogorov_smirnov_statistic;
    	typedef ks::tag::value<T1> tag_;
        return boost::accumulators::extract_result<tag_>(
            acc,
            (ks::_benchmark_distribution = dist)
        );
  	}

}// extract
}// kolmogorov_smirnov_statistic
}// empirical_distribution
}// detail
}// statistics
}// boost

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_EXPRECNEGC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_EXPRECNEGC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-exponential
    Function object implementing exprecnegc capabilities

    Computes the  function: \f$1-e^{-\frac1x}\f$

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = exprecnegc(x);
    @endcode

    is equivalent to
    @code
    T r = oneminus(exp(-rec((x))));
    @endcode

    @see exp, exprecneg

  **/
  Value exprecnegc(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/exprecnegc.hpp>
#include <boost/simd/function/simd/exprecnegc.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_COMBINATORIAL_FUNCTIONS_EXPR_IS_PRIME_HPP_INCLUDED
#define NT2_COMBINATORIAL_FUNCTIONS_EXPR_IS_PRIME_HPP_INCLUDED

#include <nt2/combinatorial/functions/is_prime.hpp>
#include <nt2/include/functions/scalar/bitwise_cast.hpp>
#include <nt2/include/functions/globalall.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/primes.hpp>
#include <nt2/include/functions/scalar/saturate.hpp>
#include <nt2/include/functions/scalar/rem.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/functions/scalar/is_flint.hpp>
#include <nt2/include/functions/scalar/is_less.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/false.hpp>
#include <nt2/sdk/simd/logical.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>
#include <nt2/core/container/table/table.hpp>
#include <boost/mpl/if.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::is_prime_, tag::cpu_,
                             (A0),
                             (unspecified_<A0>)
                             )
  {
    typedef typename A0::value_type                                                   value_type;
    typedef typename meta::as_logical<value_type>::type                              bvalue_type;
    typedef nt2::container::table<bvalue_type>                                       result_type;
    typedef typename nt2::meta::as_integer<value_type>::type                         ivalue_type;
    typedef typename boost::mpl::if_<meta::is_floating_point<value_type>,
                                     uint32_t, ivalue_type>::type           itype;
    NT2_FUNCTOR_CALL(1)
      {
        itype m = nt2::oneplus(nt2::sqrt(nt2::globalmax(a0)));
        nt2::container::table<itype> p = nt2::primes(m);
        result_type r(nt2::of_size(1, nt2::numel(a0)));

        for(size_t i=1; i <= numel(a0) ; i++)
          {
            r(i) = is_prime(a0(i), p);
          }
        return r;
      }
  };
} }
#endif


/** A simple neuron / genetic algorithm implemented in pipeline language */

#include <job_stream/job_stream.h>

#include <boost/lexical_cast.hpp>
#include <cmath>
#include <memory>
#include <random>

using std::unique_ptr;

std::mt19937 rngEngine;
std::uniform_real_distribution<float> rng(-1, 1);
float getRandom() {
    return rng(rngEngine);
}

class NeuralLayer {
public:
    NeuralLayer() {}
    NeuralLayer(const NeuralLayer& other) {
        this->inputs = other.inputs;
        this->neurons = other.neurons;
        this->weights = other.weights;
    }
    NeuralLayer(int inputs, int neurons) : inputs(inputs), neurons(neurons) {
        for (int i = 0; i < neurons * inputs; i++) {
            this->weights.push_back(getRandom());
        }
    }

    int inputs;
    int neurons;

    float* eval(float* inputs) {
        if (!this->lastResults) {
            this->lastResults.reset(new float[this->neurons]);
        }

        for (int i = 0; i < this->neurons; i++) {
            float v = 0.0;
            for (int j = 0; j < this->inputs; j++) {
                v += this->weights[i * this->inputs + j] * inputs[j];
            }
            this->lastResults.get()[i] = 1.f / (1.f + exp(-v));
        }

        return this->lastResults.get();
    }

    void initFrom(NeuralLayer* a, NeuralLayer* b) {
        this->inputs = a->inputs;
        this->neurons = a->neurons;
        std::uniform_int_distribution<> dist(1, a->weights.size() - 2);
        int split = dist(rngEngine);
        for (int i = 0; i < split; i++) {
            this->weights.push_back(a->weights[i]);
        }
        for (int i = split, m = a->weights.size(); i < m; i++) {
            this->weights.push_back(b->weights[i]);
        }

        //Mutate!
        std::uniform_int_distribution<> wdist(0, a->weights.size() / 2);
        std::uniform_int_distribution<> mut(0, this->weights.size());
        for (int j = 0, k = wdist(rngEngine); j < k; j++) {
            this->weights[mut(rngEngine)] += 0.2 * getRandom();
        }
    }

private:
    std::vector<float> weights;
    unique_ptr<float[]> lastResults;

    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive& ar, const unsigned int version) {
        ar & this->inputs;
        ar & this->neurons;
        ar & this->weights;
    }
};

class NeuralNet {
public:
    NeuralNet() {}
    NeuralNet(int neurons, int inputs, int outputs) {
        unique_ptr<NeuralLayer> layer;
        layer.reset(new NeuralLayer(inputs, neurons));
        this->layers.push_back(std::move(layer));
        layer.reset(new NeuralLayer(neurons, neurons));
        this->layers.push_back(std::move(layer));
        layer.reset(new NeuralLayer(neurons, outputs));
        this->layers.push_back(std::move(layer));
    }
    NeuralNet(const NeuralNet& other) {
        unique_ptr<NeuralLayer> layer;
        for (int i = 0, m = other.layers.size(); i < m; i++) {
            layer.reset(new NeuralLayer(*other.layers[i]));
            this->layers.push_back(std::move(layer));
        }
        this->score = other.score;
    }
    NeuralNet(NeuralNet& a, NeuralNet& b) {
        //Cross a and b
        unique_ptr<NeuralLayer> layer;
        for (int i = 0, m = a.layers.size(); i < m; i++) {
            layer.reset(new NeuralLayer());
            layer->initFrom(a.layers[i].get(), b.layers[i].get());
            this->layers.push_back(std::move(layer));
        }
    }

    float score;

    float getError(YAML::Node& array) {
        unique_ptr<float[]> inputs(new float[array.size()]);
        for (int i = 0, m = array.size(); i < m; i++) {
            inputs[i] = array[i].as<float>();
        }

        float* layerInput = inputs.get();
        for (int i = 0, m = this->layers.size(); i < m; i++) {
            layerInput = this->layers[i]->eval(layerInput);
        }

        //layerInput now == output
        float score = 0;
        for (int base = this->layers[0]->inputs, i = 0,
                m = array.size() - base; i < m; i++) {
            score += (layerInput[i] - inputs[base + i])
                    * (layerInput[i] - inputs[base + i]);
        }
        return score;
    }

private:
    std::vector<unique_ptr<NeuralLayer>> layers;

    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive& ar, const unsigned int version) {
        ar & this->score;
        int m = this->layers.size();
        ar & m;
        for (int i = 0; i < m; i++) {
            if (Archive::is_loading::value) {
                unique_ptr<NeuralLayer> ptr(new NeuralLayer());
                ar & *ptr;
                this->layers.push_back(std::move(ptr));
            }
            else {
                ar & *this->layers[i];
            }
        }
    }
};

class NetworkPopulace {
public:
    NetworkPopulace() {
    }

    NetworkPopulace(const NetworkPopulace& other) {
        for (size_t i = 0, m = other.networks.size(); i < m; i++) {
            this->networks.push_back(unique_ptr<NeuralNet>(
                    new NeuralNet(*other.networks[i])));
        }
    }

    void addNetwork(unique_ptr<NeuralNet> network) {
        this->networks.push_back(std::move(network));
    }

    void joinNetwork(unique_ptr<NetworkPopulace> other) {
        for (auto& mem : other->networks) {
            this->networks.push_back(std::move(mem));
        }
    }

    NeuralNet& bestNetwork() {
        float best = this->bestScore();
        for (int i = 0, m = this->networks.size(); i < m; i++) {
            auto* net = this->networks[i].get();
            if (net->score == best) {
                return *net;
            }
        }
        throw std::runtime_error("No best network?");
    }

    float bestScore() {
        float min = 1e35f;
        for (int i = 0, m = this->networks.size(); i < m; i++) {
            auto* net = this->networks[i].get();
            if (net->score < min) {
                min = net->score;
            }
        }
        return min;
    }

    void clear() {
        this->networks.clear();
    }

    void cross() {
        std::vector<NeuralNet*> newNets;
        for (int i = 0, m = this->networks.size() - 1; i < m; i++) {
            NeuralNet& src1 = this->monteCarlo(0);
            NeuralNet& src2 = this->monteCarlo(&src1);
            newNets.push_back(new NeuralNet(src1, src2));
        }
        //Elite!
        for (int i = 0, m = this->networks.size(); i < m; i++) {
            if (this->networks[i]->score == this->bestScore()) {
                newNets.push_back(this->networks[i].release());
                break;
            }
        }
        this->networks.clear();
        for (int i = 0, m = newNets.size(); i < m; i++) {
            unique_ptr<NeuralNet> ptr(newNets[i]);
            this->networks.push_back(std::move(ptr));
        }
    }

    NeuralNet& monteCarlo(NeuralNet* dontChoose) {
        float max = 0.0;
        float total = 0.0;
        int m = this->networks.size();
        for (int i = 0; i < m; i++) {
            if (this->networks[i].get() == dontChoose) continue;
            float score = this->networks[i]->score;
            if (score > max) max = score;
        }
        max *= 1.01;
        for (int i = 0; i < m; i++) {
            if (this->networks[i].get() == dontChoose) continue;
            total += max - this->networks[i]->score;
        }

        std::uniform_real_distribution<float> rngMonte(0, total);
        float slice = rngMonte(rngEngine);
        for (int i = 0; i < m; i++) {
            if (this->networks[i].get() == dontChoose) continue;
            slice -= max - this->networks[i]->score;
            if (slice <= 0.0) {
                return *this->networks[i];
            }
        }

        throw std::runtime_error("monteCarlo fell out bottom");
    }

    int size() {
        return this->networks.size();
    }

    NeuralNet& operator[](int index) {
        return *this->networks[index];
    }

private:
    std::vector<unique_ptr<NeuralNet>> networks;

    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive& ar, const unsigned int version) {
        int m = this->networks.size();
        ar & m;
        for (int i = 0; i < m; i++) {
            if (Archive::is_loading::value) {
                unique_ptr<NeuralNet> ptr(new NeuralNet());
                ar & *ptr;
                this->networks.push_back(std::move(ptr));
            }
            else {
                ar & *this->networks[i];
            }
        }
    }
};

class MakeNetworks : public job_stream::Job<MakeNetworks, int> {
public:
    static const char* NAME() { return "makeNetworks"; }

    void handleWork(unique_ptr<int> networkCount) {
        //Initialize networkCount networks
        auto conf = this->config;
        for (int i = 0; i < *networkCount; i++) {
            this->emit(NeuralNet(conf["neurons"].as<int>(),
                    conf["numInputs"].as<int>(),
                    conf["numOutputs"].as<int>()));
        }
    }
} makeNetworks;

class EvalNetwork : public job_stream::Job<EvalNetwork, NeuralNet> {
public:
    static const char* NAME() { return "evalNetwork"; }

    void handleWork(unique_ptr<NeuralNet> network) {
        float score = 0.0;
        auto tests = this->globalConfig["tests"].as<
                std::vector<YAML::Node>>();
        for (int i = 0, m = tests.size(); i < m; i++) {
            score += network->getError(tests[i]);
        }
        network->score = score;
        this->emit(*network);
    }
} evalNetwork;

class CheckErrorAndBreed : public job_stream::Reducer<CheckErrorAndBreed, NetworkPopulace,
        NeuralNet> {
public:
    static const char* NAME() { return "checkErrorAndBreed"; }

    void handleAdd(NetworkPopulace& current, unique_ptr<NeuralNet> work) {
        current.addNetwork(std::move(work));
    }

    void handleJoin(NetworkPopulace& current,
            unique_ptr<NetworkPopulace> other) {
        current.joinNetwork(std::move(other));
    }

    void handleDone(NetworkPopulace& current) {
        float bestScore = current.bestScore();
        NeuralNet& best = current.bestNetwork();
        if (bestScore < this->config["error"].as<float>()) {
            std::ostringstream ss;
            ss << "Done!  Best error: " << boost::lexical_cast<std::string>(
                    current.bestScore());
            ss << ".";
            this->emit(ss.str());
            return;
        }
        else {
            printf("Best score: %.3f (", bestScore);
            auto tests = this->globalConfig["tests"]
                    .as<std::vector<YAML::Node>>();
            for (int i = 0, m = tests.size(); i < m; i++) {
                if (i != 0) {
                    printf(", ");
                }
                printf("%f", best.getError(tests[i]));
            }
            printf(")\n");
        }

        current.cross();
        for (int i = 0, m = current.size(); i < m; i++) {
            this->recur(current[i]);
        }

        //Wait for those crossed networks to come through, so we maintain
        //population size.
        current.clear();
    }
} checkErrorAndBreed;

int main(int argc, char* argv[]) {
    job_stream::runProcessor(argc, argv);
    return 0;
}


#include <GazeEstimation.h>
#include <RotationHelpers.h>
#include <opencv2/core/core.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <LandmarkDetectorFunc.h>
#include <LandmarkDetectorModel.h>
#include "MyGazeEstimation.h"

using namespace Eigen;

void MyGazeEstimation::EstimateGaze(const LandmarkDetector::CLNF& clnf, cv::Point3f& gaze, float fx, float fy, float cx, float cy, bool is_left)
{
    cv::Vec6f head = LandmarkDetector::GetPose(clnf, fx, fy, cx, cy);
    cv::Matx33f rot = Utilities::Euler2RotationMatrix(cv::Vec3f(head(3), head(4), head(5)));

    int part = -1;
    for (size_t i = 0; i < clnf.hierarchical_models.size(); i++) {
        if (is_left) {
            if (clnf.hierarchical_model_names[i] == "left_eye_28") {
                part = i;
            }
        } else {
            if (clnf.hierarchical_model_names[i] == "right_eye_28") {
                part = i;
            }
        }
    }
    if (part == -1) {
        gaze = cv::Point3f(0, 0, 0);
        return;
    }

    cv::Mat eye_landmarks = clnf.hierarchical_models[part].GetShape(fx, fy, cx, cy);
    cv::Point3f pupil = GazeAnalysis::GetPupilPosition(eye_landmarks);
    cv::Point3f ray_dir = pupil / norm(pupil);
    cv::Mat face_landmarks = clnf.GetShape(fx, fy, cx, cy).t();

    int eye_index = 1;
    if (is_left) {
        eye_index = 0;
    }

    cv::Mat offset_mat = (cv::Mat_<float>(3, 1) << 0.0, -3.5, 7.0);
    cv::Mat offset_mat_t = (cv::Mat(rot) * offset_mat ).t();
    cv::Point3f eye_offset = cv::Point3f(offset_mat_t);
    cv::Mat mat_l = face_landmarks.row(36 + eye_index * 6);
    cv::Mat mat_r = face_landmarks.row(39 + eye_index * 6);
    cv::Point3f lid_l = cv::Point3f(mat_l);
    cv::Point3f lid_r = cv::Point3f(mat_r);
    cv::Point3f eye_center = (lid_l + lid_r) / 2.0;
    cv::Point3f eyeball_center = eye_center + eye_offset;

    // 2Dに再投影
    float d = eye_center.z;
    float l2dx = lid_l.x * d / lid_l.z;
    float l2dy = lid_l.y * d / lid_l.z;
    float r2dx = lid_r.x * d / lid_r.z;
    float r2dy = lid_r.y * d / lid_r.z;
    float p2dx = pupil.x * d / pupil.z;
    float p2dy = pupil.y * d / pupil.z;
    float t = (p2dx - r2dx) / (l2dx - r2dx);
    if (t < 0.0) t = 0.0; else if (t > 1.0) t = 1.0;
    float newZ = lid_r.z + (lid_l.z - lid_r.z) * t;
    // 新しいzで、黒目の中心位置を再計算する。
    pupil.x = pupil.x * newZ / pupil.z;
    pupil.y = pupil.y * newZ / pupil.z;
    pupil.z = newZ;
    ray_dir = pupil / norm(pupil);

    cv::Point3f gaze_axis = pupil - eyeball_center;

    gaze = gaze_axis / norm(gaze_axis);
}


#include <starpu.h>
#include <starpu_mpi.h>
#include <vector>
#include <memory>
#include <iostream>
#ifdef USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <mpi.h>

void gemm(void *buffers[], void *cl_arg) {
    double *A0 = (double *)STARPU_MATRIX_GET_PTR(buffers[0]);
    double *A1 = (double *)STARPU_MATRIX_GET_PTR(buffers[1]);
    double *A2 = (double *)STARPU_MATRIX_GET_PTR(buffers[2]);
    int nx     = STARPU_MATRIX_GET_NY(buffers[0]);
    int ny     = STARPU_MATRIX_GET_NX(buffers[0]);
    assert(nx == ny);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, nx, nx, nx, 1.0, A0, nx, A1, nx, 1.0, A2, nx);
  }
struct starpu_codelet gemm_cl = {
    .where = STARPU_CPU,
    .cpu_funcs = { gemm, NULL },
    .nbuffers = 3,
    .modes = { STARPU_R, STARPU_R, STARPU_RW }
};

auto val = [&](int i, int j) { return  (double) (i % 37 + j * i % 49);};


// Boost.Geometry Index
//
// Quickbook Examples
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[rtree_polygons_vector

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <boost/geometry/index/rtree.hpp>

#include <cmath>
#include <vector>
#include <iostream>
#include <boost/foreach.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

int main()
{
    typedef bg::model::point<float, 2, bg::cs::cartesian> point;
    typedef bg::model::box<point> box;
    typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon
    typedef std::pair<box, unsigned> value;

    // polygons
    std::vector<polygon> polygons;

    // create some polygons
    for ( unsigned i = 0 ; i < 10 ; ++i )
    {
        // create a polygon
        polygon p;
        for ( float a = 0 ; a < 6.28316f ; a += 1.04720f )
        {
            float x = i + int(10*::cos(a))*0.1f;
            float y = i + int(10*::sin(a))*0.1f;
            p.outer().push_back(point(x, y));
        }

        // add polygon
        polygons.push_back(p);
    }

    // display polygons
    std::cout << "generated polygons:" << std::endl;
    BOOST_FOREACH(polygon const& p, polygons)
        std::cout << bg::wkt<polygon>(p) << std::endl;

    // create the rtree using default constructor
    bgi::rtree< value, bgi::rstar<16, 4> > rtree;

    // fill the spatial index
    for ( unsigned i = 0 ; i < polygons.size() ; ++i )
    {
        // calculate polygon bounding box
        box b = bg::return_envelope<box>(polygons[i]);
        // insert new value
        rtree.insert(std::make_pair(b, i));
    }

    // find values intersecting some area defined by a box
    box query_box(point(0, 0), point(5, 5));
    std::vector<value> result_s;
    rtree.query(bgi::intersects(query_box), std::back_inserter(result_s));

    // find 5 nearest values to a point
    std::vector<value> result_n;
    rtree.query(bgi::nearest(point(0, 0), 5), std::back_inserter(result_n));

    // note: in Boost.Geometry the WKT representation of a box is polygon

    // note: the values store the bounding boxes of polygons
    // the polygons aren't used for querying but are printed

    // display results
    std::cout << "spatial query box:" << std::endl;
    std::cout << bg::wkt<box>(query_box) << std::endl;
    std::cout << "spatial query result:" << std::endl;
    BOOST_FOREACH(value const& v, result_s)
        std::cout << bg::wkt<polygon>(polygons[v.second]) << std::endl;

    std::cout << "knn query point:" << std::endl;
    std::cout << bg::wkt<point>(point(0, 0)) << std::endl;
    std::cout << "knn query result:" << std::endl;
    BOOST_FOREACH(value const& v, result_n)
        std::cout << bg::wkt<polygon>(polygons[v.second]) << std::endl;

    return 0;
}

//]