/*
 * Author: Johannes M Dieterich
 */

#ifndef CARTESIANGRID_HPP
#define	CARTESIANGRID_HPP

#include <armadillo>
#include <cmath>
#include <memory>
#include <complex.h>
#include <tgmath.h>
#include "BasicGridComputer.hpp"
#include "FourierGrid.hpp"
#include "GVectorBuilder.hpp"
using namespace std;
using namespace arma;

class CartesianGrid: public FourierGrid {
public:

    virtual ~CartesianGrid(){
#ifdef LIBKEDF_DEBUG
        cout << "DEBUG: destructor for Cartesian grid called." << endl;
#endif
        _gNorms.reset();
    }

    void multiplyGNorms(){

#ifdef LIBKEDF_DEBUG
        cout << "DEBUG: multiplying gNorms in Cartesian grid." << endl;
#endif

        // do a Fourier operation
        cx_cube* rec = this->getReciprocalGrid();

        const cube* gNorms = this->getGNorms();

        const uword elems = rec->n_elem;

        #pragma omp parallel for default(none) shared(rec,gNorms)
        for(uword x = 0; x < elems; ++x){
            const double norm = gNorms->at(x);
            rec->at(x) *= -norm*norm;
        }
        this->completeReciprocal(rec);

#ifdef LIBKEDF_DEBUG
        cout << "DEBUG: Done multiplying gNorms in Cartesian grid." << endl;
#endif
    }

    void multiplyGVectorsX(){

        cx_cube* rec = getReciprocalGrid();
        const cube* gVecs = getGVectorsX();

        const uword elems = rec->n_elem;

        const complex<double> imag(0,1);

        #pragma omp parallel for default(none) shared(rec,gVecs)
        for(uword x = 0; x < elems; ++x){
            rec->at(x) *= imag*gVecs->at(x);;
        }
        completeReciprocal(rec);
    }

    void multiplyGVectorsY(){

        cx_cube* rec = getReciprocalGrid();
        const cube* gVecs = getGVectorsY();

        const uword elems = rec->n_elem;

        const complex<double> imag(0,1);

        #pragma omp parallel for default(none) shared(rec,gVecs)
        for(uword x = 0; x < elems; ++x){
            rec->at(x) *= imag*gVecs->at(x);
        }
        completeReciprocal(rec);
    }

    void multiplyGVectorsZ(){

        cx_cube* rec = getReciprocalGrid();
        const cube* gVecs = getGVectorsZ();

        const uword elems = rec->n_elem;

        const complex<double> imag(0,1);

        #pragma omp parallel for default(none) shared(rec,gVecs)
        for(uword x = 0; x < elems; ++x){
            rec->at(x) *= imag*gVecs->at(x);
        }
        completeReciprocal(rec);
    }

    double integrate() {

        const double sum = sumOver();

        return sum*_cellVolume/_noGridPoints;
    }

    double sumOver() {

        const cube* grid = this->readRealGrid();

#ifdef _OPENMP
        const size_t nSlices = grid->n_slices;
        const size_t nRows = grid->n_rows;
        const size_t nCols = grid->n_cols;
        double sum = 0.0;
        #pragma omp parallel for default(none) shared(grid,sum)
        for(size_t x = 0; x < nSlices; ++x){
            double tmpSum = 0.0;
            for(size_t col = 0; col < nCols; ++col){
                for(size_t row = 0; row < nRows; ++row){
                    tmpSum += grid->at(row,col,x);
                }
            }
            #pragma omp atomic
            sum += tmpSum;
        }
#else
        const double sum = accu(*grid);
#endif

        return sum;
    }

    void minMax(double& min, double& max){

        const cube* grid = this->readRealGrid();

        const size_t nSlices = grid->n_slices;
        const size_t nRows = grid->n_rows;
        const size_t nCols = grid->n_cols;

#ifdef _OPENMP
        // parallelize this

        double minVals[nSlices];
        double maxVals[nSlices];

        #pragma omp parallel for default(none) shared(grid,minVals,maxVals)
        for(size_t x = 0; x < nSlices; ++x){
            minVals[x] = 1e42;
            maxVals[x] = 0.0;
            for(size_t col = 0; col < nCols; ++col){
                for(size_t row = 0; row < nRows; ++row){
                    const double d = grid->at(row,col,x);
                    if(d < minVals[x]){
                        minVals[x] = d;
                    } else if(d > maxVals[x]){
                        maxVals[x] = d;
                    }
                }
            }
        }

        // do the aggregation
        double myMin = 1e42;
        double myMax = 0.0;
        for(size_t x = 0; x < nSlices; ++x){
            if(minVals[x] < myMin){
                myMin = minVals[x];
            }
            if(maxVals[x] > myMax){
                myMax = maxVals[x];
            }
        }
#else
        double myMin = 1e42;
        double myMax = 0.0;
        for(size_t x = 0; x < nSlices; ++x){
            for(size_t col = 0; col < nCols; ++col){
                for(size_t row = 0; row < nRows; ++row){
                    const double d = grid->at(row,col,x);
                    myMin = std::min(myMin,d);
                    myMax = std::max(myMax,d);
                }
            }
        }
#endif

        min = myMin;
        max = myMax;
    }

    const cube* getGNorms() {

        // lazy init
        if(_gNorms == NULL){
            setupGVectors(_cellVectors);
        }

        return _gNorms.get();
    }

    const cube* getGVectorsX(){

        // lazy init
        if(_gVectorsX == NULL){
            setupGVectors(_cellVectors);
        }

        return _gVectorsX.get();
    }

    const cube* getGVectorsY(){

        // lazy init
        if(_gVectorsY == NULL){
            setupGVectors(_cellVectors);
        }

        return _gVectorsY.get();
    }

    const cube* getGVectorsZ(){

        // lazy init
        if(_gVectorsZ == NULL){
            setupGVectors(_cellVectors);
        }

        return _gVectorsZ.get();
    }

    double stressNorm() const {
        return 3*_cellVolume;
    }

    size_t getGridPointsX() const {
        return _xDim;
    }

    size_t getGridPointsY() const {
        return _yDim;
    }

    size_t getGridPointsZ() const {
        return _zDim;
    }

    size_t getReciGridPointsX() const {
        return _xRecDim;
    }

    size_t getReciGridPointsY() const {
        return _yRecDim;
    }

    size_t getReciGridPointsZ() const {
        return _zRecDim;
    }

    unsigned long long getTotalGridPoints() const {

        const unsigned long long gridX = getGridPointsX();
        const unsigned long long gridY = getGridPointsY();
        const unsigned long long gridZ = getGridPointsZ();

        return gridX*gridY*gridZ;
    }

    double getCellVolume(){
        return _cellVolume;
    }

    double getCellX(){
        return _cellX;
    }

    double getCellY(){
        return _cellY;
    }

    double getCellZ(){
        return _cellZ;
    }

    void updateCellVectors(const double *vecX, const double *vecY, const double *vecZ){

        _cellVectors->at(0,0) = vecX[0];
        _cellVectors->at(0,1) = vecX[1];
        _cellVectors->at(0,2) = vecX[2];

        _cellVectors->at(1,0) = vecY[0];
        _cellVectors->at(1,1) = vecY[1];
        _cellVectors->at(1,2) = vecY[2];

        _cellVectors->at(2,0) = vecZ[0];
        _cellVectors->at(2,1) = vecZ[1];
        _cellVectors->at(2,2) = vecZ[2];

        // following http://mathworld.wolfram.com/Parallelepiped.html
        vec::fixed<3> vecA;
        vec::fixed<3> vecB;
        vec::fixed<3> vecC;

        vecA.at(0) = _cellVectors->at(0,0);
        vecA.at(1) = _cellVectors->at(0,1);
        vecA.at(2) = _cellVectors->at(0,2);

        vecB.at(0) = _cellVectors->at(1,0);
        vecB.at(1) = _cellVectors->at(1,1);
        vecB.at(2) = _cellVectors->at(1,2);

        vecC.at(0) = _cellVectors->at(2,0);
        vecC.at(1) = _cellVectors->at(2,1);
        vecC.at(2) = _cellVectors->at(2,2);

        const vec vecBC = cross(vecB, vecC);

        _cellVolume = abs(dot(vecA, vecBC));

        _cellX = sqrt(vecA.at(0)*vecA.at(0) + vecA.at(1)*vecA.at(1) + vecA.at(2)*vecA.at(2));
        _cellY = sqrt(vecB.at(0)*vecB.at(0) + vecB.at(1)*vecB.at(1) + vecB.at(2)*vecB.at(2));
        _cellZ = sqrt(vecC.at(0)*vecC.at(0) + vecC.at(1)*vecC.at(1) + vecC.at(2)*vecC.at(2));

        this->_gNorms.reset();
        this->_gVectorsX.reset();
        this->_gVectorsY.reset();
        this->_gVectorsZ.reset();

        this->_gNorms = NULL;
        this->_gVectorsX = NULL;
        this->_gVectorsY = NULL;
        this->_gVectorsZ = NULL;
    }

protected:

    CartesianGrid(const size_t xDim, const size_t yDim, const size_t zDim, const shared_ptr<mat> cellVectors)
        : _xDim(xDim), _yDim(yDim), _zDim(zDim), _xRecDim(floor(_xDim/2)+1), // this is due to the arma data format (which we use as basis: col/row/slice in contiguous -> least contiguous order)
        _yRecDim(yDim), _zRecDim(zDim), _noGridPoints(xDim*yDim*zDim), _norm((double) _xDim*_yDim*_zDim),
        _invNorm(1.0/_norm){

        _cellVectors = cellVectors;

        // following http://mathworld.wolfram.com/Parallelepiped.html
        vec::fixed<3> vecA;
        vec::fixed<3> vecB;
        vec::fixed<3> vecC;

        vecA.at(0) = _cellVectors->at(0,0);
        vecA.at(1) = _cellVectors->at(0,1);
        vecA.at(2) = _cellVectors->at(0,2);

        vecB.at(0) = _cellVectors->at(1,0);
        vecB.at(1) = _cellVectors->at(1,1);
        vecB.at(2) = _cellVectors->at(1,2);

        vecC.at(0) = _cellVectors->at(2,0);
        vecC.at(1) = _cellVectors->at(2,1);
        vecC.at(2) = _cellVectors->at(2,2);

        const vec vecBC = cross(vecB, vecC);

        _cellVolume = abs(dot(vecA, vecBC));

        _cellX = sqrt(vecA.at(0)*vecA.at(0) + vecA.at(1)*vecA.at(1) + vecA.at(2)*vecA.at(2));
        _cellY = sqrt(vecB.at(0)*vecB.at(0) + vecB.at(1)*vecB.at(1) + vecB.at(2)*vecB.at(2));
        _cellZ = sqrt(vecC.at(0)*vecC.at(0) + vecC.at(1)*vecC.at(1) + vecC.at(2)*vecC.at(2));

        // initialize to NULL for lazy init
        _gNorms = NULL;
        _gVectorsX = NULL;
        _gVectorsY = NULL;
        _gVectorsZ = NULL;
    }

    CartesianGrid(const CartesianGrid& orig) : _xDim(orig._xDim), _yDim(orig._yDim), _zDim(orig._zDim),
        _xRecDim(orig._xRecDim), _yRecDim(orig._yRecDim), _zRecDim(orig._zRecDim),
        _noGridPoints(orig._noGridPoints), _norm(orig._norm), _invNorm(orig._invNorm),
        _cellVolume(orig._cellVolume), _cellX(orig._cellX), _cellY(orig._cellY), _cellZ(orig._cellZ){

        _cellVectors = orig._cellVectors;

        _gNorms = orig._gNorms;
        _gVectorsX = orig._gVectorsX;
        _gVectorsY = orig._gVectorsY;
        _gVectorsZ = orig._gVectorsZ;
    }

    void setupGVectors(const shared_ptr<mat> cellVectors){
        setupSimpleGVectors(cellVectors);
    }

    void setupSimpleGVectors(const shared_ptr<mat> cellVectors){

        _gNorms = make_shared<cube>(_xRecDim,_yRecDim,_zRecDim);

        _gVectorsX = make_shared<cube>(_xRecDim,_yRecDim,_zRecDim);
        _gVectorsY = make_shared<cube>(_xRecDim,_yRecDim,_zRecDim);
        _gVectorsZ = make_shared<cube>(_xRecDim,_yRecDim,_zRecDim);

        // construct g norms and vectors
        GVectorBuilder::buildGVectors(cellVectors,_xRecDim,_yRecDim,_zRecDim,_gNorms,_gVectorsX,_gVectorsY,_gVectorsZ);
    }

    shared_ptr<mat> _cellVectors;
    shared_ptr<cube> _gNorms;
    shared_ptr<cube> _gVectorsX;
    shared_ptr<cube> _gVectorsY;
    shared_ptr<cube> _gVectorsZ;

    const size_t _xDim;
    const size_t _yDim;
    const size_t _zDim;
    const size_t _xRecDim;
    const size_t _yRecDim;
    const size_t _zRecDim;
    const size_t _noGridPoints;
    const double _norm;
    const double _invNorm;

    double _cellVolume;
    double _cellX;
    double _cellY;
    double _cellZ;
};

#endif	/* CARTESIANGRID_HPP */


#pragma once

#include "GNAObject.hh"
#include "GNAObjectBind1N.hh"
#include <Eigen/Dense>

class Integrator2Base : public GNAObjectBind1N<double>,
                        public TransformationBind<Integrator2Base>
{
public:
  TransformationDescriptor add_transformation(const std::string& name="");
  void set_edges(OutputDescriptor& hist2_output);

  void dump();

protected:
  Integrator2Base(size_t xbins, int  xorders, double* xedges, size_t ybins, int  yorders, double* yedges);
  Integrator2Base(size_t xbins, int* xorders, double* xedges, size_t ybins, int* yorders, double* yedges);

  Eigen::ArrayXi m_xorders;
  Eigen::ArrayXd m_xedges;
  Eigen::ArrayXd m_xweights;

  Eigen::ArrayXi m_yorders;
  Eigen::ArrayXd m_yedges;
  Eigen::ArrayXd m_yweights;

  Eigen::ArrayXXd m_weights;

  virtual void sample(FunctionArgs&)=0;

protected:
  void init_sampler();
  void check_sampler(TypesFunctionArgs& fargs);

private:
  void init_base(double* xedges, double* yedges);
  void check_base(TypesFunctionArgs&);
  void integrate(FunctionArgs&);
};


/*
 *
 * Copyright (c) 2002, 2003 Kresimir Fresl, Toon Knapen and Karl Meerbergen
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_HERMITIAN_H
#define BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_HERMITIAN_H

#include <boost/numeric/bindings/traits/traits.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#ifndef BOOST_UBLAS_HAVE_BINDINGS
#  include <boost/numeric/ublas/hermitian.hpp>
#endif
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/detail/ublas_uplo.hpp>

namespace boost { namespace numeric { namespace bindings { namespace traits {

  // ublas::hermitian_matrix<>
  template <typename T, typename F1, typename F2, typename A, typename M>
  struct matrix_detail_traits<boost::numeric::ublas::hermitian_matrix<T, F1, F2, A>, M>
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT( (boost::is_same<boost::numeric::ublas::hermitian_matrix<T, F1, F2, A>, typename boost::remove_const<M>::type>::value) );
#endif
#ifdef BOOST_BINDINGS_FORTRAN
    BOOST_STATIC_ASSERT((boost::is_same<
      typename F2::orientation_category,
      boost::numeric::ublas::column_major_tag
    >::value));
#endif

    typedef boost::numeric::ublas::hermitian_matrix<T, F1, F2, A> identifier_type;
    typedef M                                                     matrix_type;

    typedef hermitian_packed_t matrix_structure;
    typedef typename detail::ublas_ordering<
      typename F2::orientation_category
    >::type ordering_type;
    typedef typename detail::ublas_uplo< F1 >::type uplo_type;

    typedef T                                           value_type ;
    typedef typename detail::generate_const<M,T>::type* pointer ;

    static pointer storage (matrix_type& hm) {
      typedef typename detail::generate_const<M,A>::type array_type ;
      return vector_traits<array_type>::storage (hm.data());
    }
    static std::ptrdiff_t num_rows (matrix_type& hm) { return hm.size1(); }
    static std::ptrdiff_t num_columns (matrix_type& hm) { return hm.size2(); }
    static std::ptrdiff_t storage_size (matrix_type& hm) {
      return (num_rows (hm) + 1) * num_columns (hm) / 2;
    }
  };

  namespace detail {
     template <typename M>
     std::ptrdiff_t matrix_bandwidth( M const& m, upper_t ) {
        return matrix_traits<M const>::upper_bandwidth( m ) ;
     }

     template <typename M>
     std::ptrdiff_t matrix_bandwidth( M const& m, lower_t ) {
        // When the lower triangular band matrix is stored the
	// upper bandwidth must be zero
	assert( 0 == matrix_traits<M const>::upper_bandwidth( m ) ) ;
        return matrix_traits<M const>::lower_bandwidth( m ) ;
     }
  } // namespace detail

  // ublas::hermitian_adaptor<>
  template <typename M, typename F1, typename MA>
  struct matrix_detail_traits<boost::numeric::ublas::hermitian_adaptor<M, F1>, MA>
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT( (boost::is_same<boost::numeric::ublas::hermitian_adaptor<M, F1>, typename boost::remove_const<MA>::type>::value) );
#endif

    typedef boost::numeric::ublas::hermitian_adaptor<M, F1> identifier_type;
    typedef MA                                              matrix_type;
    typedef hermitian_t                                     matrix_structure;
    typedef typename matrix_traits<M>::ordering_type        ordering_type;
    typedef typename detail::ublas_uplo< F1 >::type         uplo_type;

    typedef typename M::value_type                                 value_type;
    typedef typename detail::generate_const<MA, value_type>::type* pointer;

  private:
    typedef typename detail::generate_const<MA, typename MA::matrix_closure_type>::type m_type;

  public:
    static pointer storage (matrix_type& hm) {
      return matrix_traits<m_type>::storage (hm.data());
    }
    static std::ptrdiff_t num_rows (matrix_type& hm) { return hm.size1(); }
    static std::ptrdiff_t num_columns (matrix_type& hm) { return hm.size2(); }
    static std::ptrdiff_t storage_size (matrix_type& hm) {
      return num_rows (hm) * num_columns (hm);
    }
    static std::ptrdiff_t leading_dimension (matrix_type& hm) {
      return matrix_traits<m_type>::leading_dimension (hm.data());
    }
    // For banded M
    static std::ptrdiff_t upper_bandwidth(matrix_type& hm) {
       return detail::matrix_bandwidth( hm.data(), uplo_type() );
    }
    static std::ptrdiff_t lower_bandwidth(matrix_type& hm) {
       return detail::matrix_bandwidth( hm.data(), uplo_type() );
    }
  };

}}}}

#endif // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#endif // BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_HERMITIAN_H


/*
 Copyright (c) 2014 Glen Joseph Fernandes
 glenfe at live dot com

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

#include <boost/config.hpp>
#include <cstddef>

namespace boost {
    namespace alignment {
        namespace detail {
            BOOST_CONSTEXPR inline bool is_alignment(std::size_t
                value) BOOST_NOEXCEPT
            {
                return (value > 0) && ((value & (value - 1)) == 0);
            }
        }
    }
}

#endif


/*!
 * Functions to compute the load
 * @author Sébastien Allgeyer
 * @date 5/3/21
 *
 */

#include "load_functions.h"

#include <boost/multi_array.hpp>
#include <iostream>
#include <fstream>
#include <iomanip>

#include "loading.h"
#include "tide.h"
#include "input_otl.h"
#include "boost_ma_type.h"
#include "utils.h"

/**
 * Compute the loading of a single point
 * @param tide_info [in] vector of classes containing the tide grids
 * @param input [in,out] class containing the coordinates information and also the loading vector [output]
 * @param load [in] class containing the Green's function
 * @param idx [in] index of the point in the list
 */
void load_1_point(tide *tide_info, otl_input *input, loading load,  int idx)
{

	MA2d greenZ;
	MA2d greenNS;
	MA2d greenEW;

	greenZ.resize(boost::extents[tide_info[0].get_nlat()][tide_info[0].get_nlon()]);
	greenNS.resize(boost::extents[tide_info[0].get_nlat()][tide_info[0].get_nlon()]);
	greenEW.resize(boost::extents[tide_info[0].get_nlat()][tide_info[0].get_nlon()]);

	auto greenZ_it = greenZ.origin();
	auto greenEW_it = greenEW.origin();
	auto greenNS_it = greenNS.origin();
	float lat0 = input->lat[idx];
	float lon0 = input->lon[idx];

	for (float *lat_ptr = tide_info[0].get_lat_ptr(); lat_ptr != tide_info[0].get_lat_ptr_end(); lat_ptr++) {
		for (float *lon_ptr = tide_info[0].get_lon_ptr(); lon_ptr < tide_info[0].get_lon_ptr_end(); lon_ptr++) {
			double dist, azimuth;
			calcDistanceBearing(&lat0, &lon0, lat_ptr, lon_ptr, &dist, &azimuth);

			*greenZ_it = load.interpolate_gz(dist);
			*greenNS_it = load.interpolate_gh(dist) * cos(azimuth);
			*greenEW_it = load.interpolate_gh(dist) * sin(azimuth);

			// *greenNS_it *= cos(azimuth);
			// *greenEW_it *= sin(azimuth);
			if (*greenZ_it != *greenZ_it)
			{
				std::cout << " nan detected for " << *lat_ptr << " " <<*lon_ptr << std::endl;
				std::cout << dist << "  " << azimuth << std::endl;
				exit(0);
			}
			greenZ_it++;
			greenNS_it++;
			greenEW_it++;
		}
	}

	// Computing load;
	for (int it = 0 ; it < input->tide_file.size() ; it++ ) {
		auto tideim_it = tide_info[it].get_out_ptr();
		auto gz_it = greenZ.origin();
		auto gNS_it = greenNS.origin();
		auto gEW_it = greenEW.origin();
		for (auto tidere_it = tide_info[it].get_in_ptr();
			 tidere_it != tide_info[it].get_in_ptr_end();
			 tidere_it++, tideim_it++, gz_it++, gNS_it++, gEW_it++) {

			input->dispEW_in[idx][it] += *gEW_it * *tidere_it;
			input->dispEW_out[idx][it]+= *gEW_it * *tideim_it;
			input->dispZ_in[idx][it]  += *gz_it  * *tidere_it;
			input->dispZ_out[idx][it] += *gz_it  * *tideim_it;
			input->dispNS_in[idx][it] += *gNS_it * *tidere_it;
			input->dispNS_out[idx][it]+= *gNS_it * *tideim_it;

		}
	}
}

void write_BLQ(otl_input *input, int mode)
{
	std::ofstream out ;
	out.open(input->output_blq_file);
	out << "$$ OUTPUT OF CODENAME\n";
	out << "$$ Processed " << input->tide_file.size() << " tides\n";
	for (unsigned int i=0; i< input->tide_file.size(); i++ )
		out << "$$	TF: " << input->tide_file[i] <<"\n";
	out << "$$ Wave Names:\n";
	for (unsigned int i=0; i< input->wave_names.size(); i++ )
		out << "$$	WN: " << input->wave_names[i] <<"\n";
	for (unsigned int i=0; i< input->code.size(); i++) {
		out <<"  " << input->code[i] <<"\n";
		out << "$$ " <<input->code[i] <<"                    RADI TANG  lon/lat: " <<input->lon[i] << " " << input->lat[i] << "\n";
		out.setf(std::ios::fixed);
		out << std::setprecision(5) ;
		// write Amplitudes
		for (int i_dir = 0; i_dir< 3; i_dir++)
		{
			for (int it = 0; it < input->wave_names.size() ; it++)  // Will need to write nwaves
				out << " " << std::setprecision(5)<< std::setw(8) <<  std::abs(input->out_disp[i][it][i_dir]) << " ";
			out << "\n";
		}
		// Now write the angles.
		for (int i_dir = 0; i_dir< 3; i_dir++)
		{
			for (int it = 0; it < input->wave_names.size() ; it++)  // Will need to write nwaves
				out << " " << std::setprecision(1) << std::setw(8)  << std::arg(input->out_disp[i][it][i_dir]) * 180 / M_PI << " ";
			out << "\n";
		}
	}
	out << "$$ END TABLE\n";
	out.close();
}

void write_BLQ(otl_input *input)
{
	FILE * fp = fopen(input->output_blq_file.c_str(),"w");
	fprintf(fp,"$$ OUTPUT OF make_otl_blq\n");
	fprintf(fp,"$$ Processed %li tides\n",input->tide_file.size());
	for (unsigned int i=0; i < input->tide_file.size(); i++)
		fprintf(fp,"$$    - %s\n",input->tide_file[i].c_str());
	fprintf(fp,"$$ Green function used is %s\n",input->green.c_str());

	for (unsigned int i=0; i< input->code.size(); i++) {
		fprintf(fp, "  %s\n", input->code[i].c_str());
		fprintf(fp, "$$ %s                    RADI TANG  lon/lat: %f %f\n", input->code[i].c_str(), input->lon[i],
				input->lat[i]);
		for (int it = 0; it < input->tide_file.size(); it++)
			fprintf(fp, " % 1.5f ", sqrt(input->dispZ_out[i][it] * input->dispZ_out[i][it] +
										 input->dispZ_in[i][it] * input->dispZ_in[i][it]));
		fprintf(fp, "\n");
		for (int it = 0; it < input->tide_file.size(); it++)
			fprintf(fp, " % 1.5f ", sqrt(input->dispEW_out[i][it] * input->dispEW_out[i][it] +
										 input->dispEW_in[i][it] * input->dispEW_in[i][it]));
		fprintf(fp, "\n");
		for (int it = 0; it < input->tide_file.size(); it++)
			fprintf(fp, " % 1.5f ", sqrt(input->dispNS_out[i][it] * input->dispNS_out[i][it] +
										 input->dispNS_in[i][it] * input->dispNS_in[i][it]));
		fprintf(fp, "\n");

		for (int it = 0; it < input->tide_file.size(); it++)
			fprintf(fp, " %8.1f ", rad_to_deg(atan2(input->dispZ_out[i][it], input->dispZ_in[i][it])));
		fprintf(fp, "\n");
		for (int it = 0; it < input->tide_file.size(); it++)
			fprintf(fp, " %8.1f ", rad_to_deg(atan2(input->dispEW_out[i][it], input->dispEW_in[i][it])));
		fprintf(fp, "\n");
		for (int it = 0; it < input->tide_file.size(); it++)
			fprintf(fp, " %8.1f ", rad_to_deg(atan2(input->dispNS_out[i][it], input->dispNS_in[i][it])));
		fprintf(fp, "\n");

	}
	fprintf(fp, "$$ END TABLE\n");
	fclose(fp);
}


#include <stdio.h>
#include <stdlib.h>
#include <iostream>
//#include <dlib/data_io.h>
#include <dlib/svm.h>
#include <dlib/random_forest.h>

using namespace std;
using namespace dlib;

typedef double raw_type;
typedef matrix<raw_type,28,28> random_forest_data_type;
typedef double label_type;
typedef std::vector<random_forest_data_type> vec_type;

namespace dlib
{
    class mnist_dense_feature_extractor
    {

    public:
        typedef uint32_t feature;
        typedef matrix<double, 28, 28> sample_type;

        mnist_dense_feature_extractor(
        ) = default;

        void setup (
            const std::vector<sample_type>& x,
            const std::vector<double>& y
        )
        {
            //DLIB_CASSERT(x.size() > 0);
            //DLIB_CASSERT(x.size() == y.size());
            for (auto& el : x)
                DLIB_CASSERT(el.size() == x[0].size(), "All the vectors in a training set have to have the same dimensionality.");

            DLIB_CASSERT(x[0].size() != 0, "The vectors can't be empty.");

            num_feats = x[0].size();
        }

        void get_random_features (
            dlib::rand& rnd,
            size_t num,
            std::vector<feature>& feats
        ) const
        {
            DLIB_ASSERT(max_num_feats() != 0);
            num = std::min(num, num_feats);

            feats.clear();
            for (size_t i = 0; i < num_feats; ++i)
                feats.push_back(i);

            // now pick num features at random
            for (size_t i = 0; i < num; ++i)
            {
                auto idx = rnd.get_integer_in_range(i,num_feats);
                std::swap(feats[i], feats[idx]);
            }
            feats.resize(num);
        }

        double extract_feature_value (
            const sample_type& item,
            const feature& f
        ) const
        {
            //DLIB_ASSERT(max_num_feats() != 0);

			int r = f%28;
			int c = f/28;

            return item(r,c);
        }

        size_t max_num_feats (
        ) const
        {
            return num_feats;
        }

        friend void serialize(const mnist_dense_feature_extractor& item, std::ostream& out)
        {
            serialize("mnist_dense_feature_extractor", out);
            serialize(item.num_feats, out);
        }

        friend void deserialize(mnist_dense_feature_extractor& item, std::istream& in)
        {
            check_serialized_version("mnist_dense_feature_extractor", in);
            deserialize(item.num_feats, in);
        }

    private:
        size_t num_feats = 0;
    };
}

/*
void random_forest_load_mnist_images(
	std::string path,
	std::vector<random_forest_data_type> &training_images,
	std::vector<label_type> &training_labels,
	std::vector<random_forest_data_type> &testing_images,
	std::vector<label_type> &testing_labels )
{
*/

void random_forest_load_mnist_images(
	std::string path,
	std::string image_file,
	std::string label_file,
	std::vector<random_forest_data_type> &images,
	std::vector<label_type> &labels, int count )
{

	unsigned char stash[28][28];

	FILE* fdata = fopen( ( path + "/" + image_file ).c_str(), "r" );
	FILE* flabels=fopen( ( path + "/" + label_file ).c_str(), "r");

	fread( stash, 1, 128/8, fdata );
	fread( stash, 1, 64/8, flabels );

	// Training data read
	cout << "Loading data...." << endl;
	for( int c=0; c < count; c++ )
	{
		random_forest_data_type img;

		fread( stash, 28, 28, fdata );

		for( int i=0; i < 28; i++ )
		{
			for( int j=0; j < 28; j++ )
			{
				//img( i+(j*28), 0 ) = (raw_type) ( (stash[i][j]) );
				img( i, j ) = (raw_type) ( (stash[i][j]) );
				//img.push_back(  random_forest_data_type( ( (stash[i][j])  ) ) );
			}
		}
		images.push_back( img );

		fread( &stash[0][0], 1, 1, flabels );
		labels.push_back( (stash[0][0]) );
	}
	fclose( fdata );
	fclose( flabels );

	return;
}

void print_image( random_forest_data_type &img, unsigned long label, unsigned long predicted );
void print_random_forest_image( random_forest_data_type &img );

void print_probs( matrix<double,1,10> &softmax_probs, int x );

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

	bool correct_out = false;
	bool wrong_out = false;

    // This example is going to run on the MNIST dataset.
    if (argc < 2)
    {
        cout << "This example needs the MNIST dataset to run!" << endl;
        cout << "You can get MNIST from http://yann.lecun.com/exdb/mnist/" << endl;
        cout << "Download the 4 files that comprise the dataset, decompress them, and" << endl;
        cout << "put them in a folder.  Then give that folder as input to this program." << endl;
        return 1;
    }

	if( argc > 3 )
	{
		int out_flag = atoi( argv[3] );

		if( out_flag == 1 )
			correct_out = true;

		if( out_flag == 2)
			wrong_out = true;

		if( out_flag == 3 )
			correct_out = wrong_out = true;
	}

    // MNIST is broken into two parts, a training set of 60000 images and a test set of
    // 10000 images.  Each image is labeled so that we know what hand written digit is
    // depicted.  These next statements load the dataset into memory.
    std::vector<random_forest_data_type> training_images;
    std::vector<label_type>         training_labels;
    std::vector<random_forest_data_type> testing_images;
    std::vector<label_type>         testing_labels;
    //load_mnist_dataset(argv[1], training_images, training_labels, testing_images, testing_labels);

    random_forest_load_mnist_images(argv[1],
			"/train-images.idx3-ubyte", "/train-labels.idx1-ubyte",
			training_images, training_labels, 60000 );

    random_forest_load_mnist_images(argv[1],
			"/t10k-images.idx3-ubyte", "/t10k-labels.idx1-ubyte",
			testing_images, testing_labels, 10000 );

//===============================================================
    // The random_forest functions use column vectors to contain a lot of the data on which they
    // operate. So the first thing we do here is declare a convenient typedef.

    // This typedef declares a matrix with 2 rows and 1 column.  It will be the object that
    // contains each of our 2 dimensional samples.   (Note that if you wanted more than 2
    // features in this vector you can simply change the 2 to something else.  Or if you
    // don't know how many features you want until runtime then you can put a 0 here and
    // use the matrix.set_size() member function)
//    typedef matrix<double, 2, 1> sample_type;
    typedef random_forest_data_type sample_type;

    // This is a typedef for the type of kernel we are going to use in this example.  In
    // this case I have selected the radial basis kernel that can operate on our 2D
    // sample_type objects
//    typedef radial_basis_kernel<sample_type> kernel_type;
  //  typedef radial_basis_kernel<random_forest_data_type> kernel_type;
	//typedef sparse_linear_kernel<random_forest_data_type> kernel_type;

//random_forest_multiclass_linear_trainer

    // Now we make objects to contain our samples and their respective labels.
//    std::vector<sample_type> samples;
//    std::vector<double> labels;

/*
    // Here we normalize all the samples by subtracting their mean and dividing by their
    // standard deviation.  This is generally a good idea since it often heads off
    // numerical stability problems and also prevents one large feature from smothering
    // others.  Doing this doesn't matter much in this example so I'm just doing this here
    // so you can see an easy way to accomplish this with the library.
    // let the normalizer learn the mean and standard deviation of the samples

    // now normalize each sample
    vector_normalizer<random_forest_data_type> normalizer;

	if( !(argc > 2) )
	{
		cout << "normalizing training set" << endl;

		normalizer.train(training_images);

		for (unsigned long i = 0; i < training_images.size(); ++i)
		    training_images[i] = normalizer(training_images[i]);

	    randomize_samples(training_images, training_labels);
	}

	{
		cout << "normalizing test set" << endl;

		normalizer.train(testing_images);

		for (unsigned long i = 0; i < testing_images.size(); ++i)
		    testing_images[i] = normalizer(testing_images[i]);

    	randomize_samples(testing_images, testing_labels);
	}

    // Now that we have some data we want to train on it.  However, there are two
    // parameters to the training.  These are the nu and gamma parameters.  Our choice for
    // these parameters will influence how good the resulting decision function is.  To
    // test how good a particular choice of these parameters is we can use the
    // cross_validate_trainer() function to perform n-fold cross validation on our training
    // data.  However, there is a problem with the way we have sampled our distribution
    // above.  The problem is that there is a definite ordering to the samples.  That is,
    // the first half of the samples look like they are from a different distribution than
    // the second half.  This would screw up the cross validation process but we can fix it
    // by randomizing the order of the samples with the following function call.
    //randomize_samples(testing_images, testing_labels);

    // The nu parameter has a maximum value that is dependent on the ratio of the +1 to -1
    // labels in the training data.  This function finds that value.
//    const double max_nu = maximum_nu(training_labels);

    // here we make an instance of the random_forest_nu_trainer object that uses our kernel type.
//    random_forest_nu_trainer<kernel_type> random_forest_trainer;

	typedef linear_kernel<random_forest_data_type> kernel_type;

    typedef random_forest_multiclass_linear_trainer<kernel_type, label_type> trainer_type;

*/

    typedef random_forest_regression_trainer<mnist_dense_feature_extractor> trainer_type;

	trainer_type random_forest_trainer;
	random_forest_trainer.set_num_trees(100);
	random_forest_trainer.set_seed("random forest");

	std::vector<double> oobs;
	//trainer_type::trained_function_type df = random_forest_trainer.train(training_images, training_labels, oobs);
	trainer_type::trained_function_type df; // = random_forest_trainer.train(training_images, training_labels, oobs);

//	typedef one_vs_one_trainer<any_trainer<sample_type> > ovo_trainer;
//	ovo_trainer random_forest_trainer;

    // Next, we will make two different binary classification trainer objects.  One
    // which uses kernel ridge regression and RBF kernels and another which uses a
    // support vector machine and polynomial kernels.  The particular details don't matter.
    // The point of this part of the example is that you can use any kind of trainer object
    // with the one_vs_one_trainer.
//    typedef polynomial_kernel<random_forest_data_type> poly_kernel;
//    typedef radial_basis_kernel<random_forest_data_type> rbf_kernel;

    // make the binary trainers and set some parameters

    //random_forest_trainer.set_trainer(rbf_trainer);

    // Now let's do 5-fold cross-validation using the one_vs_one_trainer we just setup.
    // As an aside, always shuffle the order of the samples before doing cross validation.
    // For a discussion of why this is a good idea see the random_forest_ex.cpp example.
//    randomize_samples(training_images, training_labels);
//    cout << "cross validation: \n" << cross_validate_multiclass_trainer(random_forest_trainer, training_images, training_labels, 5) << endl;

    // Next, if you wanted to obtain the decision rule learned by a one_vs_one_trainer you
    // would store it into a one_vs_one_decision_function.

    //trainer_type::trained_function_type df;

	if( argc > 2 )
	{
		deserialize( argv[2] ) >> df;
	}
	else
	{
		cout << "training the random_forest" << endl;
		random_forest_trainer.be_verbose();
		df = random_forest_trainer.train(training_images, training_labels);
		serialize( "random_forest_mnist.dat" ) << df;
	}

	int num_right=0, num_wrong=0;

	for( int c = 0; c < testing_images.size(); c++ )
	{
		if( df(testing_images[c]) == testing_labels[c] )
		{
			num_right++;
		}
		else
		{
			num_wrong++;
			print_random_forest_image( testing_images[c] );
			cout 	<< "predicted label: "<< (int)df(testing_images[c])
					<< ", true label: "<< (int)testing_labels[c]
					<< endl;
		}

	}

    // The output is:
    /*
        predicted label: 2, true label: 2
        predicted label: 1, true label: 1
    */

    cout << "testing num_right: " << num_right << endl;
    cout << "testing num_wrong: " << num_wrong << endl;
    cout << "testing accuracy:  " << num_right/(double)(num_right+num_wrong) << endl;

}
catch(std::exception& e)
{
    cout << e.what() << endl;
}

void print_probs2( matrix<float,1,10> &softmax_probs, int x )
{
	for( int i=0; i < 10; i++ )
	{
		cout << i << "=" << softmax_probs(0,i) << "  |  ";
	}
	cout << endl;
}

void print_probs( matrix<float,1,10> &softmax_probs, int x )
{
	for( int i=0; i < 10; i++ )
	{
		printf( "%d\t", i );
	}
	cout << endl;

	for( int i=0; i < 10; i++ )
	{
		printf( "%0.1f%%\t", (float)softmax_probs(0,i) * 100.0 );
	}
	cout << endl;
}

void print_random_forest_image( random_forest_data_type &img )
{

	for( int i=0; i<28; i++ )
	{
		for( int j=0; j<28; j++ )
		{
			cout << ( (img( i, j ) > 150) ? '#' :
						( (img( i, j ) > 75 ) ? '*' :
							( (img( i, j ) > 50) ? '-' : ' ' ) ) );
		}
		cout << endl;
	}
	//cout << "label=" << label << "  predicted=" << predicted << endl;
}


//==============================================================================
//         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 BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_SSE_SSE2_IS_EQUAL_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_SSE_SSE2_IS_EQUAL_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/operator/functions/is_equal.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/dispatch/meta/downgrade.hpp>
#include <boost/simd/swar/functions/details/shuffle.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                              ((simd_<double_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmpeq_pd(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                              ((simd_<single_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmpeq_ps(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                              ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmpeq_epi8(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                              ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmpeq_epi16(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                              ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmpeq_epi32(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<ints64_<A0>,boost::simd::tag::sse_>))
                              ((simd_<ints64_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::downgrade<A0, unsigned>::type  type;
      type tmp      = bitwise_cast<type>(a0 - a1);
      tmp           = bitwise_cast<type>(boost::simd::eq(tmp,Zero<type>()));
      type shuffled = details::shuffle<1,0,3,2>(tmp);
      return bitwise_cast<result_type>(tmp & shuffled);
    }
  };
} } }

#endif
#endif


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * Implement bayesFlt.hpp :
 *  constructor/destructors
 *  error handlers
 *  default virtual and member functions
 */
#include "bayesFlt.hpp"
#include <boost/limits.hpp>
#include <vector>		// Only for unique_samples

/* Filter namespace */
namespace Bayesian_filter
{

/* Minimum allowable reciprocal condition number for PD Matrix factorisations
 * Initialised default gives 5 decimal digits of headroom */
const Bayes_base::Float Numerical_rcond::limit_PD_init = std::numeric_limits<Bayes_base::Float>::epsilon() * Bayes_base::Float(1e5);

Bayes_base::~Bayes_base()
/* Default definition required for a pure virtual destructor
 */
{}

void Bayes_base::error (const Numeric_exception& e )
/* Filter error
 */
{
	throw e;
}

void Bayes_base::error (const Logic_exception& e )
/* Filter error
 */
{
	throw e;
}

Gaussian_predict_model::Gaussian_predict_model (std::size_t x_size, std::size_t q_size) :
		q(q_size), G(x_size, q_size)
/* Set the size of things we know about
 */
{}

Additive_predict_model::Additive_predict_model (std::size_t x_size, std::size_t q_size) :
		q(q_size), G(x_size, q_size)
/*
 * Set the size of things we know about
 */
{}

Linrz_predict_model::Linrz_predict_model (std::size_t x_size, std::size_t q_size) :
/* Set the size of things we know about
 */
		Additive_predict_model(x_size, q_size),
		Fx(x_size,x_size)
{}

Linear_predict_model::Linear_predict_model (std::size_t x_size, std::size_t q_size) :
/* Set the size of things we know about
 */
		Linrz_predict_model(x_size, q_size),
		xp(x_size)
{}

Linear_invertable_predict_model::Linear_invertable_predict_model (std::size_t x_size, std::size_t q_size) :
/*
 * Set the size of things we know about
 */
		Linear_predict_model(x_size, q_size),
		inv(x_size)
{}

Linear_invertable_predict_model::inverse_model::inverse_model (std::size_t x_size) :
		Fx(x_size,x_size)
{}

State_filter::State_filter (std::size_t x_size) :
	x(x_size)
/* Initialise filter and set the size of things we know about
 */
{
	if (x_size < 1)
		error (Logic_exception("Zero state filter constructed"));
}

Kalman_state_filter::Kalman_state_filter (std::size_t x_size) :
/* Initialise state size
 */
		State_filter(x_size), X(x_size,x_size)
{}

void Kalman_state_filter::init_kalman (const FM::Vec& x, const FM::SymMatrix& X)
/* Initialise from a state and state covariance
 *  Parameters that reference the instance's x and X members are valid
 */
{
	Kalman_state_filter::x = x;
	Kalman_state_filter::X = X;
	init ();
}

Bayes_base::Float
 Extended_kalman_filter::observe (Linrz_correlated_observe_model& h, const FM::Vec& z)
/* Extended linrz correlated observe, compute innovation for observe_innovation
 */
{
	update ();
	const FM::Vec& zp = h.h(x);		// Observation model, zp is predicted observation

	FM::Vec s = z;
	h.normalise(s, zp);
	FM::noalias(s) -= zp;
	return observe_innovation (h, s);
}

Bayes_base::Float
 Extended_kalman_filter::observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z)
/* Extended Kalman uncorrelated observe, compute innovation for observe_innovation
 */
{
	update ();
	const FM::Vec& zp = h.h(x);		// Observation model, zp is predicted observation

	FM::Vec s = z;
	h.normalise(s, zp);
	FM::noalias(s) -= zp;
	return observe_innovation (h, s);
}

Information_state_filter::Information_state_filter (std::size_t x_size) :
/* Initialise state size
 */
		y(x_size), Y(x_size,x_size)
{}

void Information_state_filter::init_information (const FM::Vec& y, const FM::SymMatrix& Y)
/* Initialise from a information state and information
 *  Parameters that reference the instance's y and Y members are valid
 */
{
	Information_state_filter::y = y;
	Information_state_filter::Y = Y;
	init_yY ();
}

Sample_state_filter::Sample_state_filter (std::size_t x_size, std::size_t s_size) :
		S(x_size,s_size)
/* Initialise state size
 * Postcond: s_size >= 1
 */
{
	if (s_size < 1)
		error (Logic_exception("Zero sample filter constructed"));
}

Sample_state_filter::~Sample_state_filter()
/* Default definition required for a pure virtual destructor
 * ISSUE Cannot be defined if Matrix has private destructor
 */
{
}

void Sample_state_filter::init_sample (const FM::ColMatrix& initS)
/* Initialise from a sampling
 */
{
	S = initS;
	init_S();
}

namespace {
	// Column proxy so S can be sorted indirectly
	struct ColProxy
	{
		const FM::ColMatrix* cm;
		std::size_t col;
		const ColProxy& operator=(const ColProxy& a)
		{
			col = a.col;
			return a;
		}
		// Provide a ordering on columns
		static bool less(const ColProxy& a, const ColProxy& b)
		{
			FM::ColMatrix::const_iterator1 sai = a.cm->find1(1, 0,a.col);
			FM::ColMatrix::const_iterator1 sai_end = a.cm->find1(1, a.cm->size1(),a.col);
			FM::ColMatrix::const_iterator1 sbi = b.cm->find1(1,0, b.col);
			while (sai != sai_end)
			{
				if (*sai < *sbi)
					return true;
				else if (*sai > *sbi)
					return false;

				++sai; ++sbi;
			} ;
			return false;		// Equal
		}
	};
}//namespace

std::size_t Sample_state_filter::unique_samples () const
/* Count number of unique (unequal value) samples in S
 * Implementation requires std::sort on sample column references
 */
{
						// Temporary container to Reference each element in S
	typedef std::vector<ColProxy> SRContainer;
	SRContainer sortR(S.size2());
	std::size_t col_index = 0;
	for (SRContainer::iterator si = sortR.begin(); si != sortR.end(); ++si) {
		(*si).cm = &S; (*si).col = col_index++;
	}
						// Sort the column proxies
	std::sort (sortR.begin(), sortR.end(), ColProxy::less);

						// Count element changes, precond: sortS not empty
	std::size_t u = 1;
	SRContainer::const_iterator ssi = sortR.begin();
	SRContainer::const_iterator ssp = ssi;
	++ssi;
	while (ssi < sortR.end())
	{
		if (ColProxy::less(*ssp, *ssi))
			++u;
		ssp = ssi;
		++ssi;
	}
	return u;
}

Sample_filter::Sample_filter (std::size_t x_size, std::size_t s_size) :
		Sample_state_filter(x_size,s_size)
/* Initialise filter and set the size of things we know about
 * Postcond: s_size >= 1
 */
{
}

void Sample_filter::predict (Functional_predict_model& f)
/* Predict samples forward
 *		Pre : S represent the prior distribution
 *		Post: S represent the predicted distribution
 */
{
						// Predict particles S using supplied predict model
	const std::size_t nSamples = S.size2();
	for (std::size_t i = 0; i != nSamples; ++i) {
		FM::ColMatrix::Column Si(S,i);
		FM::noalias(Si) = f.fx(Si);
	}
}

}//namespace


#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <quadrotor_msgs/Corrections.h>
#include <quadrotor_msgs/PositionCommand.h>
#include <quadrotor_msgs/SO3Command.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <so3_control/SO3Control.h>
#include <std_msgs/Bool.h>
#include <tf/transform_datatypes.h>

class SO3ControlNodelet : public nodelet::Nodelet
{
public:
  SO3ControlNodelet()
    : position_cmd_updated_(false)
    , position_cmd_init_(false)
    , des_yaw_(0)
    , des_yaw_dot_(0)
    , current_yaw_(0)
    , enable_motors_(true)
    , // FIXME
    use_external_yaw_(false)
  {
  }

  void onInit(void);

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  void publishSO3Command(void);
  void position_cmd_callback(
    const quadrotor_msgs::PositionCommand::ConstPtr& cmd);
  void odom_callback(const nav_msgs::Odometry::ConstPtr& odom);
  void enable_motors_callback(const std_msgs::Bool::ConstPtr& msg);
  void corrections_callback(const quadrotor_msgs::Corrections::ConstPtr& msg);
  void imu_callback(const sensor_msgs::Imu& imu);

  SO3Control      controller_;
  ros::Publisher  so3_command_pub_;
  ros::Subscriber odom_sub_;
  ros::Subscriber position_cmd_sub_;
  ros::Subscriber enable_motors_sub_;
  ros::Subscriber corrections_sub_;
  ros::Subscriber imu_sub_;

  bool        position_cmd_updated_, position_cmd_init_;
  std::string frame_id_;

  Eigen::Vector3d des_pos_, des_vel_, des_acc_, kx_, kv_;
  double          des_yaw_, des_yaw_dot_;
  double          current_yaw_;
  bool            enable_motors_;
  bool            use_external_yaw_;
  double          kR_[3], kOm_[3], corrections_[3];
};

void
SO3ControlNodelet::publishSO3Command(void)
{
  controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_,
                               des_yaw_dot_, kx_, kv_);

  const Eigen::Vector3d&    force       = controller_.getComputedForce();
  const Eigen::Quaterniond& orientation = controller_.getComputedOrientation();

  quadrotor_msgs::SO3Command::Ptr so3_command(
    new quadrotor_msgs::SO3Command); //! @note memory leak?
  so3_command->header.stamp    = ros::Time::now();
  so3_command->header.frame_id = frame_id_;
  so3_command->force.x         = force(0);
  so3_command->force.y         = force(1);
  so3_command->force.z         = force(2);
  so3_command->orientation.x   = orientation.x();
  so3_command->orientation.y   = orientation.y();
  so3_command->orientation.z   = orientation.z();
  so3_command->orientation.w   = orientation.w();
  for (int i = 0; i < 3; i++)
  {
    so3_command->kR[i]  = kR_[i];
    so3_command->kOm[i] = kOm_[i];
  }
  so3_command->aux.current_yaw          = current_yaw_;
  so3_command->aux.kf_correction        = corrections_[0];
  so3_command->aux.angle_corrections[0] = corrections_[1];
  so3_command->aux.angle_corrections[1] = corrections_[2];
  so3_command->aux.enable_motors        = enable_motors_;
  so3_command->aux.use_external_yaw     = use_external_yaw_;
  so3_command_pub_.publish(so3_command);
}

void
SO3ControlNodelet::position_cmd_callback(
  const quadrotor_msgs::PositionCommand::ConstPtr& cmd)
{
  des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
  des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
  des_acc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y,
                             cmd->acceleration.z);
  kx_ = Eigen::Vector3d(cmd->kx[0], cmd->kx[1], cmd->kx[2]);
  kv_ = Eigen::Vector3d(cmd->kv[0], cmd->kv[1], cmd->kv[2]);

  des_yaw_              = cmd->yaw;
  des_yaw_dot_          = cmd->yaw_dot;
  position_cmd_updated_ = true;
  position_cmd_init_    = true;

  publishSO3Command();
}

void
SO3ControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr& odom)
{
  const Eigen::Vector3d position(odom->pose.pose.position.x,
                                 odom->pose.pose.position.y,
                                 odom->pose.pose.position.z);
  const Eigen::Vector3d velocity(odom->twist.twist.linear.x,
                                 odom->twist.twist.linear.y,
                                 odom->twist.twist.linear.z);

  current_yaw_ = tf::getYaw(odom->pose.pose.orientation);

  controller_.setPosition(position);
  controller_.setVelocity(velocity);

  if (position_cmd_init_)
  {
    // We set position_cmd_updated_ = false and expect that the
    // position_cmd_callback would set it to true since typically a position_cmd
    // message would follow an odom message. If not, the position_cmd_callback
    // hasn't been called and we publish the so3 command ourselves
    // TODO: Fallback to hover if position_cmd hasn't been received for some
    // time
    if (!position_cmd_updated_)
      publishSO3Command();
    position_cmd_updated_ = false;
  }
}

void
SO3ControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr& msg)
{
  if (msg->data)
    ROS_INFO("Enabling motors");
  else
    ROS_INFO("Disabling motors");

  enable_motors_ = msg->data;
}

void
SO3ControlNodelet::corrections_callback(
  const quadrotor_msgs::Corrections::ConstPtr& msg)
{
  corrections_[0] = msg->kf_correction;
  corrections_[1] = msg->angle_corrections[0];
  corrections_[2] = msg->angle_corrections[1];
}

void
SO3ControlNodelet::imu_callback(const sensor_msgs::Imu& imu)
{
  const Eigen::Vector3d acc(imu.linear_acceleration.x,
                            imu.linear_acceleration.y,
                            imu.linear_acceleration.z);
  controller_.setAcc(acc);
}

void
SO3ControlNodelet::onInit(void)
{
  ros::NodeHandle n(getPrivateNodeHandle());

  std::string quadrotor_name;
  n.param("quadrotor_name", quadrotor_name, std::string("quadrotor"));
  frame_id_ = "/" + quadrotor_name;

  double mass;
  n.param("mass", mass, 0.5);
  controller_.setMass(mass);

  n.param("use_external_yaw", use_external_yaw_, true);

  n.param("gains/rot/x", kR_[0], 1.5);
  n.param("gains/rot/y", kR_[1], 1.5);
  n.param("gains/rot/z", kR_[2], 1.0);
  n.param("gains/ang/x", kOm_[0], 0.13);
  n.param("gains/ang/y", kOm_[1], 0.13);
  n.param("gains/ang/z", kOm_[2], 0.1);

  n.param("corrections/z", corrections_[0], 0.0);
  n.param("corrections/r", corrections_[1], 0.0);
  n.param("corrections/p", corrections_[2], 0.0);

  so3_command_pub_ = n.advertise<quadrotor_msgs::SO3Command>("so3_cmd", 10);

  odom_sub_ = n.subscribe("odom", 10, &SO3ControlNodelet::odom_callback, this,
                          ros::TransportHints().tcpNoDelay());
  position_cmd_sub_ =
    n.subscribe("position_cmd", 10, &SO3ControlNodelet::position_cmd_callback,
                this, ros::TransportHints().tcpNoDelay());

  enable_motors_sub_ =
    n.subscribe("motors", 2, &SO3ControlNodelet::enable_motors_callback, this,
                ros::TransportHints().tcpNoDelay());
  corrections_sub_ =
    n.subscribe("corrections", 10, &SO3ControlNodelet::corrections_callback,
                this, ros::TransportHints().tcpNoDelay());

  imu_sub_ = n.subscribe("imu", 10, &SO3ControlNodelet::imu_callback, this,
                         ros::TransportHints().tcpNoDelay());
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_DECLARE_CLASS(so3_control, SO3ControlNodelet, SO3ControlNodelet,
                        nodelet::Nodelet);


#include "drake/multibody/contact_solvers/contact_solver.h"

#include <iostream>
#include <memory>

#include <Eigen/SparseCore>
#include <gtest/gtest.h>

#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/multibody/contact_solvers/contact_solver_utils.h"
#include "drake/multibody/contact_solvers/test/multibody_sim_driver.h"
#include "drake/multibody/plant/contact_results.h"

namespace drake {
namespace multibody {

using test::MultibodySimDriver;

namespace contact_solvers {
namespace internal {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;
using Eigen::VectorXd;
using math::RigidTransformd;

constexpr double kEpsilon = std::numeric_limits<double>::epsilon();

// This class implements a ContactSolver that can only solve a very specific
// problem, that of a mass particle making contact with the ground.
// This allow us to exercise ContactSolver's APIs and test some internals such
// as the computation of the Delassus operator.
// In these tests we also verify the proper data flow from MultibodyPlant into
// the contact solver and out of MultibodyPlant as contact results.
template <typename T>
class ParticleSolver final : public ContactSolver<T> {
 public:
  // This solver knows about the problem it is trying to solve. Therefore its
  // constructor takes as input the mass of the particle in the problem.
  explicit ParticleSolver(double mass) : mass_(mass) {}

  virtual ~ParticleSolver() = default;

  // This implementation of SolveWithGuess() in addition makes a number of tests
  // on the data supplied by MultibodyPlant. We must perform these tests at this
  // scope to ensure data references are still "alive".
  ContactSolverStatus SolveWithGuess(const T& time_step,
                                   const SystemDynamicsData<T>& dynamics_data,
                                   const PointContactData<T>& contact_data,
                                   const VectorX<T>& v_guess,
                                   ContactSolverResults<T>* results) final {
    const int nv = dynamics_data.num_velocities();
    const int nc = contact_data.num_contacts();

    // Aliases to problem data.
    const LinearOperator<T>& Ainv = dynamics_data.get_Ainv();
    const VectorX<T>& v_star = dynamics_data.get_v_star();
    const LinearOperator<T>& Jc = contact_data.get_Jc();

    // Verify the expected Delassus operator is computed by
    // ContactSolver::FormDelassusOperatorMatrix().
    Eigen::SparseMatrix<T> Ws(3 * nc, 3 * nc);
    ContactSolver<T>::FormDelassusOperatorMatrix(Jc, Ainv, Jc, &Ws);
    // Only one out of nine coefficients is non-zero.
    EXPECT_EQ(Ws.nonZeros(), 3);
    const Matrix3<T> W = Matrix3<T>(Ws);
    const Matrix3d W_expected = Matrix3<T>::Identity() / mass_;
    EXPECT_TRUE(
        CompareMatrices(W, W_expected, kEpsilon, MatrixCompareType::absolute));

    // Generalized velocities when contact forces are zero.
    VectorX<T> vc_star(3 * nc);
    Jc.Multiply(v_star, &vc_star);  // vc_star = Jc⋅v_star
    const T vn_star = vc_star(2);         // Normal velocity.
    const T Wnn = W(2, 2);                // Normal equation.
    // We now need to solve the 1D  problem:
    //   0 ≤ Wnn π + vₙ* ⊥ π ≥ 0
    // Which, given Wnn > 0, has unique solution:
    //   π = -vₙ*/Wnn, if vₙ* < 0.
    //   π = 0       , if vₙ* > 0.
    const T pi = vn_star < 0 ? -vn_star / Wnn : 0.0;
    const T vn = vn_star < 0 ? 0.0 : vn_star;

    // With pi known, now we can solve for the tangential component, beta.
    EXPECT_EQ(contact_data.get_mu().size(), 1);  // since nc = 1.
    const T mu = contact_data.get_mu()(0);
    const Vector2<T> vt_star = vc_star.template head<2>();
    // First, assume stiction: vt=0 ==> beta = -vt_star/Wtt.
    const T Wtt = W(0, 0);  // For this case W(0, 0) = W(1, 1).
    Vector2<T> beta = -vt_star / Wtt;
    if (beta.norm() > mu * pi) {
      // The stiction assumption is violated, then we are in sliding.
      const Vector2<T> vt_star_hat = vt_star.normalized();
      beta = -mu * pi * vt_star_hat;
    }
    const Vector2<T> vt = Wtt * beta + vt_star;

    // Update the solver's state.
    gamma_ = Vector3<T>(beta(0), beta(1), pi);

    Jc.MultiplyByTranspose(gamma_, &jc_);
    Ainv.Multiply(jc_, &v_);  // v_ = M⁻¹⋅τc
    v_ += v_star;             // v_ = v* + M⁻¹⋅τc
    vc_ = Vector3<T>(vt(0), vt(1), vn);

    // Pack solution as ContactSolverResults.
    results->Resize(nv, nc);
    results->v_next = v_;
    ExtractNormal(vc_, &results->vn);
    ExtractTangent(vc_, &results->vt);
    ExtractNormal(gamma_, &results->fn);
    ExtractTangent(gamma_, &results->ft);
    // N.B. While contact solver works with impulses, results are reported as
    // forces.
    results->fn /= time_step;
    results->ft /= time_step;
    results->tau_contact = jc_ / time_step;

    return ContactSolverStatus::kSuccess;  // It always succeeds.
  }

 private:
  // The solver's state.
  VectorX<T> gamma_{3};  // Contact impulse.
  VectorX<T> v_{6};      // Generalized velocity, in this cases equals vn.
  // Cache, i.e. state dependent quantities.
  VectorX<T> jc_{6};  // Generalized contact impulse.
  VectorX<T> vc_{3};     // Contact velocity.

  // Problem parameters, for unit testing, an actual solver won't know
  // these.
  const T mass_{0.5};
};

class ParticleTest : public ::testing::Test {
 public:
  void SetUp() override {
    const std::string model_file =
        "drake/multibody/contact_solvers/test/"
        "particle_with_infinite_inertia.sdf";
    driver_.BuildModel(dt_, model_file);
    const auto& plant = driver_.plant();
    const auto& particle = plant.GetBodyByName("particle");
    // Add the ground, with the same friction as specified in the SDF file for
    // the particle.
    const double mu = driver_.GetDynamicFrictionCoefficients(particle)[0];
    driver_.AddGround(0.0 /*not used*/, 0.0 /*not used*/, mu);
    driver_.Initialize();
    const int nq = plant.num_positions();
    const int nv = plant.num_velocities();
    // Assert plant sizes.
    ASSERT_EQ(nq, 7);  // 4 dofs for a quaternion, 3 dofs for translation.
    ASSERT_EQ(nv, 6);  // 6 dofs for angular and translational velocity.
    solver_ = &driver_.mutable_plant().set_contact_solver(
        std::make_unique<ParticleSolver<double>>(kParticleMass_));

    // Verify that solvers/test/particle.sdf is in sync with this test.
    const double mass = particle.get_default_mass();
    ASSERT_NEAR(mass, kParticleMass_, kEpsilon);

    // MultibodyPlant state.
    SetInitialState();
  }

  // Set the particle to be in contact with the ground.
  void SetInitialState() {
    const auto& plant = driver_.plant();
    const auto& particle = plant.GetBodyByName("particle");
    auto& context = driver_.mutable_plant_context();
    const Vector3d p_WB(0, 0, -kPenetrationDistance_);
    plant.SetFreeBodyPose(&context, particle, math::RigidTransformd(p_WB));
    plant.SetFreeBodySpatialVelocity(&context, particle,
                                     SpatialVelocity<double>::Zero());
  }

  // Helper to retrieve contact results. Notice that this method will trigger
  // the computation of contact results through MultibodyPlant and therefore,
  // unless results were previously cached by MultibodyPlant, this method will
  // result in the invocation of ParticleSolver::SolveWithGuess().
  // This allow us to verify:
  //  - Proper invocation of the contact solver.
  //  - The proper data flow out from MultibodyPlant as contact results.
  const PointPairContactInfo<double>& EvalPointPairInfo() const {
    // Only one contact pair.
    const ContactResults<double>& contact_results = driver_.GetContactResults();
    DRAKE_DEMAND(contact_results.num_point_pair_contacts() == 1u);
    const PointPairContactInfo<double>& point_pair_contact_info =
        contact_results.point_pair_contact_info(0);
    return point_pair_contact_info;
  }

  // Helper to verify contact results.
  // @param f_Pc_W_expected expected contact force on the particle P at the
  // contact point C, expressed in the world frame W.
  // @param slip_expected The expected slip velocity.
  void VerifyContactInfo(const Vector3d& f_Pc_W_expected,
                         double slip_expected) const {
    const auto& particle = driver_.plant().GetBodyByName("particle");
    // We'll verify the expected results and therefore that MultibodyPlant was
    // able to properly load the contact results.
    const PointPairContactInfo<double>& info = EvalPointPairInfo();
    double direction = info.bodyB_index() == particle.index() ? 1.0 : -1.0;
    // Force on the particle P at contact point C.
    const Vector3d f_Pc_W = direction * info.contact_force();

    // Verify values.
    EXPECT_TRUE(CompareMatrices(f_Pc_W, f_Pc_W_expected, kEpsilon,
                                MatrixCompareType::absolute));
    EXPECT_NEAR(info.slip_speed(), slip_expected, kEpsilon);
    EXPECT_NEAR(info.separation_speed(), 0.0, kEpsilon);
  }

  // In addition to contact results, MultibodyPlant also reports contact
  // computations as generalized forces. This helper verifies those results.
  // @param f_Pc_W_expected expected contact force on the particle P at the
  // contact point C, expressed in world frame W.
  void VerifyGeneralizedContactForces(const Vector3d& f_Pc_W_expected) const {
    const auto& particle = driver_.plant().GetBodyByName("particle");
    const auto tau_c = driver_.plant()
                           .get_generalized_contact_forces_output_port(
                               particle.model_instance())
                           .Eval(driver_.plant_context());
    // The last three components should correspond to the expected force on P.
    EXPECT_TRUE(CompareMatrices(tau_c.tail<3>(), f_Pc_W_expected, kEpsilon,
                                MatrixCompareType::absolute));

    // The rotational component is non-zero given the non-zero offset location
    // of the contact point.
    const double phi0 = kPenetrationDistance_;
    // MultibodyPlant places C midway between the two interacting bodies.
    const Vector3d p_PoC_W(0, 0, phi0 / 2.0);
    const Vector3d t_P_W = p_PoC_W.cross(f_Pc_W_expected);
    EXPECT_TRUE(CompareMatrices(tau_c.head<3>(), t_P_W, kEpsilon,
                                MatrixCompareType::absolute));
  }

 protected:
  const double dt_{1.0e-3};
  const double kParticleMass_{0.5};
  const double kPenetrationDistance_{1.2e-2};
  MultibodySimDriver driver_;
  ParticleSolver<double>* solver_{nullptr};
};

// We test two conditions, stiction and sliding to verify the proper data flow.
TEST_F(ParticleTest, Stiction) {
  // Apply an in plane force:
  const Vector3d fapplied_P_W(2.0, 0.0, 0.0);
  const auto& particle = driver_.plant().GetBodyByName("particle");
  driver_.FixAppliedForce(particle, fapplied_P_W);

  // In stiction we expect the contact force to exactly balance the external
  // forces.
  const Vector3d weight_P_W(0.0, 0.0, -5.0);
  const Vector3d f_Pc_W_expected = -(weight_P_W + fapplied_P_W);

  VerifyContactInfo(f_Pc_W_expected, 0.0);
  VerifyGeneralizedContactForces(f_Pc_W_expected);
}

TEST_F(ParticleTest, Sliding) {
  const Vector3d fapplied_P_W(3.0, 0.0, 0.0);
  const auto& particle = driver_.plant().GetBodyByName("particle");
  driver_.FixAppliedForce(particle, fapplied_P_W);

  // The maximum friction force is mu * Weight = 2.5 N.
  const Vector3d weight_P_W(0.0, 0.0, -5.0);
  const Vector3d friction_P_W(-2.5, 0.0, 0.0);
  const Vector3d f_Pc_W_expected = -weight_P_W + friction_P_W;

  // Since the maximum friction force is mu * Weight = 2.5 N, the total force in
  // x will be 3N - 2.5N = 0.5N. Over a dt = 1e-3 sec, will cause velocity to
  // accelerate from zero to v = 1e-3 m/s (mass = 0.5 Kg) in the x direction.
  double slip_expected = 1.0e-3;

  VerifyContactInfo(f_Pc_W_expected, slip_expected);
  VerifyGeneralizedContactForces(f_Pc_W_expected);
}

}  // namespace
}  // namespace internal
}  // namespace contact_solvers
}  // namespace multibody
}  // namespace drake


#define BOOST_TEST_MODULE lue core time point
#include <boost/test/unit_test.hpp>
#include "lue/core/time.hpp"

BOOST_AUTO_TEST_CASE(seconds)
{
    using namespace lue::data_model::time;

    // Time point at 6 ticks in a clock with a tick period of 10 seconds,
    // so the point is located at 60 seconds
    std::size_t const nr_seconds_per_tick = 10;
    std::size_t const nr_ticks = 6;

    using TickPeriod = TickPeriod<Second>;
    using Clock = Clock<TickPeriod>;
    using Duration = Clock::Duration;
    using TimePoint = TimePoint<Clock>;

    TickPeriod tick_period{nr_seconds_per_tick};
    Clock clock{Epoch{}, tick_period};
    Duration duration{nr_ticks};
    TimePoint time_point{duration};

    BOOST_CHECK(time_point.duration() == duration);
}

BOOST_AUTO_TEST_CASE(days)
{
    using namespace lue::data_model::time;

    // Types for tracking days
    using TickPeriod = TickPeriod<Day>;
    using Clock = Clock<TickPeriod>;
    using Duration = Clock::Duration;
    using TimePoint = TimePoint<Clock>;

    // Clock with a resolution of 2 days
    TickPeriod tick_period{2};
    Clock clock(Epoch{}, tick_period);

    // Select day 6 (3 * 2 days)
    Duration duration{3};
    TimePoint day_6{duration};

    BOOST_CHECK_EQUAL(clock.nr_units(day_6), 6);
}


#include "dissimilarity.hpp"
#include "paths.hpp"
#include "graph/adjacency_array.hpp"
#include <Eigen/Sparse>
#include <vector>

namespace redwalk
{

std::vector<double> dissimilarity_sp(const adjacency_array<vid_t>& adj,
                                     const MatrixUS& sp)
{
  //Get number of vertices
  idx_t nv = adj.vcount();

  //Allocate D2 - what will become a "dist"
  //number of entries in contiguous symmetric matrix with no diagonal
  std::vector<double> D((nv * nv - nv) >> 1, 0.0);

  //Do avg shortest paths of neighbors
  //TODO: optimize
  idx_t idx = 0;
  redwalk::flt_t mspi, mspj;
  for(idx_t i = 0; i < (nv-1); i++)
  {
    NeighborhoodList<vid_t> nbrsi = adj[i];
    idx_t degi = nbrsi.size();
    for(idx_t j = (i+1); j < nv; j++)
    {
      mspi = 0; mspj = 0;
      NeighborhoodList<vid_t> nbrsj = adj[j];
      idx_t degj = nbrsj.size();
      for(idx_t n = 0; n < degi; n++)
        mspi += sp(nbrsi[n],j);
      mspi /= degi;
      for(idx_t n = 0; n < degj; n++)
        mspj += sp(nbrsj[n],i);
      mspj /= degj;
      D[idx] = (mspi < mspj) ? mspi : mspj;
      idx++;
    }
  }

  return D;
}

std::vector<double> dissimilarity_subsets_sp(
  const adjacency_array<vid_t>& adj,
  const vertex_list& s, const redwalk::MatrixUS& sp)
{
  const idx_t nv = adj.vcount();
  const idx_t ns = s.size();

  //to check if a vertex is a target
  std::vector<bool> is_targ(nv, false);
  for(idx_t i = 0; i < ns; i++)
    is_targ[s[i]] = true;

  //Set up triplet list for sparse matrix
  typedef Eigen::Triplet<redwalk::flt_t> Triplet;
  std::vector<Triplet> tripletList;
  tripletList.reserve(nv + 2 * adj.ecount()); //TODO: reserve exact or close to

  //Build the tripletlist
  for(idx_t i = 0; i < nv; i++)
  {
    tripletList.push_back(Triplet(i, i, redwalk::ONE_F));
    if(!is_targ[i])
    {
      redwalk::flt_t val = redwalk::NEG_ONE_F / (redwalk::flt_t)adj.degree(i);
      NeighborhoodList<vid_t> nbrs = adj[i];
      NeighborhoodList<vid_t>::iterator nitr;
      for(nitr = nbrs.begin(); nitr != nbrs.end(); nitr++)
        tripletList.push_back(Triplet(i,*nitr,val));
    }
  }

  //set up the random walk normalized laplacian
  Eigen::SparseMatrix<redwalk::flt_t> L(nv, nv);
  L.setFromTriplets(tripletList.begin(), tripletList.end());
  L.makeCompressed();

  //free the triplets
  std::vector<Triplet>().swap(tripletList);

  //factor the RWNL
  Eigen::SparseLU<Eigen::SparseMatrix<redwalk::flt_t, Eigen::ColMajor>,
                  Eigen::COLAMDOrdering<int> > solver;
  solver.analyzePattern(L);
  solver.factorize(L);

  //setup b
  redwalk::MatrixF b(nv, ns);
  b.fill(0);
  for(idx_t i = 0; i < ns; i++)
    b.row(s[i]) = sp.col(i).cast<redwalk::flt_t>();

  //Solve
  const redwalk::MatrixF x = solver.solve(b);

  //Free b
  b.resize(0,0);

  //Free L
  Eigen::SparseMatrix<redwalk::flt_t, Eigen::ColMajor>().swap(L);

  //Allocate D - what will become a "dist"
  //number of entries in contiguous symmetric matrix with no diagonal
  std::vector<double> D((ns * ns - ns) >> 1, 0.0);

  idx_t idx = 0;
  redwalk::flt_t mspi, mspj;
  for(idx_t i = 0; i < (ns-1); i++)
  {
    vid_t ti = s[i];
    idx_t degi = adj.degree(ti);
    NeighborhoodList<vid_t> nbrsi = adj[ti];
    for(idx_t j = i+1; j < ns; j++)
    {
      vid_t tj = s[j];
      idx_t degj = adj.degree(tj);
      NeighborhoodList<vid_t> nbrsj = adj[tj];
      mspi = 0; mspj = 0;
      for(idx_t n = 0; n < degi; n++)
        mspi += x(nbrsi[n],j);
      mspi /= degi;
      for(idx_t n = 0; n < degj; n++)
        mspj += x(nbrsj[n],i);
      mspj /= degj;
      D[idx] = (mspi < mspj) ? mspi : mspj;
      idx++;
    }
  }

  return D;
}

} //namespace redwalk


// Program that estimates the timing to classifier entries on a grid.
//
// Assumes that windows will be included based on a long tail
// distribution, so windows closer to previously selected windows are
// more likely to be chosen next.
//
// Output file is of the form:
//
// <nWindows>,<seconds>
//
// Usage: EstimateSpeciesGridTiming [options] <indexFile> <outputFile> <sampleImage1> ... <sampleImageN>

#include <gflags/gflags.h>
#include <fstream>
#include <boost/scoped_ptr.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <boost/random/mersenne_twister.hpp>
#include "SpeciesIDNode.h"

DEFINE_int32(samples, 100,
             "Number of samples to take along the number of possible entries");
DEFINE_int32(seed, 165494, "Random number generator seed");
DEFINE_int32(minW, 32, "Minimum width");
DEFINE_int32(minH, 32, "Minimum height");
DEFINE_int32(winStride, 16, "X and Y stride");
DEFINE_double(scale_stride, 1.20, "Width and height stride");

using namespace std;
using namespace boost;
using namespace boost::accumulators;
using namespace cv;
using namespace species_id;

// Create a mask of the valid spots in the grid
//
// Returns the number of valid entries
int CreateFullMask(const Size& imageSize, Mat_<bool>* mask) {
  vector<double> widths;
  for (double curW = FLAGS_minW; curW < imageSize.width;
       curW *= FLAGS_scale_stride) {
    widths.push_back(std::round(curW));
  }
  vector<double> heights;
  for (double curH = FLAGS_minH; curH < imageSize.height;
       curH *= FLAGS_scale_stride) {
    heights.push_back(std::round(curH));
  }

  vector<int> maskSize;
  maskSize.push_back(widths.size());
  maskSize.push_back(heights.size());
  maskSize.push_back(std::max<int>(1,
    (imageSize.width - FLAGS_minW) / FLAGS_winStride));
  maskSize.push_back(std::max<int>(1,
    (imageSize.height - FLAGS_minH) / FLAGS_winStride));
  *mask = Mat_<bool>::zeros(4, &maskSizes[0]);

  int nValid = 0;
  for (unsigned int widthI = 0u; widthI < widths.size(); ++widthI) {
    for (unsigned int heightI = 0u; heightI < heights.size(); ++heightI) {
      for (int i = 0, curX = 0; curX < imageSize.width-widths[widthI]; ++i) {
        for (int j = 0, curY = 0; curY < imageSize.height-heights[heightI];
             ++j) {
          (*mask)(widthI, heightI, i, j) = true;
          nValid++;
          curY += FLAGS_winStride;
        }
        curX += FLAGS_winStride;
      }
    }
  }

  return nValid;
}

Mat_<bool> createRandomMask(const Mat_<bool>& fullMask, int nWindows,
                            mt19937& randGen) {
  Mat_<bool> retval;

  // We select which windows to process using a gaussian process that
  // assumes regions close to previously selected regions are more
  // likely to be chosen.
  Mat_<double> pChoose(fullMask);

  if (nWindows == countNonZero(fullMask)) {
    return Mat_<bool>(fullMask);
  }

  for (int nChosen = 0; nChosen < nWindows; ++nChosen) {
    pChoose /= sum(pChoose);

  }
}

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

  google::ParseCommandLineFlags(&argc, &argv, true);

  string indexFile(argv[1]);
  string outputFile(argv[2]);

  // Grab the list of images
  vector<string> imageFiles;
  for (int i = 3; i < argc; ++i) {
    imageFiles.push_back(string(argv[i]));
  }

  // Create the species id node to use
  SpeciesIDNode node;
  node.LoadIndexFromFile(indexFile);

  // Create accumulators for each count of windows.
  vector<accumulator_set<double, stats<tag::variance> >
    timeAccumulators(FLAGS_samples + 1);

  // Intialize the random number generator
  mt19937 randGen(FLAGS_seed);

  for (vector<string>::const_iterator fileI = imageFiles.begin();
       fileI != imageFiles.end(); ++fileI) {

    Mat image = imread(*fileI);

    Mat_<bool> fullMask;
    int maxWindows = CreateFullMask(image.size(), &fullMask);

    for (int i = 0; i <= FLAGS_samples; ++i) {
      int nWindows = std::round<int>(((double)maxWindows) * i /
                                     FLAGS_samples);

      Mat_<bool> randomMask = CreateRandomMask(fullMask, nWindows, randGen);

      double processingTime = GetTimeForMaskedRegions(image, randomMask);
      timeAccumulators[i](processingTime);
    }
  }

}


//==================================================================================================
/*!
  @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_LOGICAL_NOT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOGICAL_NOT_HPP_INCLUDED

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

 /*!

    @ingroup group-boolean
    Function object implementing logical_not capabilities

    return the logical negation of the parameter,
    i.e. True or False of the logical type associated entry type according that
    the input is @ref Zero or non @ref Zero (False or True).

    Infix notation can be used with operator '!'

    @par Semantic:

    For every parameter of type @c T

    @code
    auto r = logical_not(x);
    @endcode

    is similar to:

    @code
    auto r =!x;
    @endcode

    @see  logical_or, logical_xor, logical_notand,
    logical_andnot, logical_notor, logical_ornot

  **/
  as_logical_t<Value0> logical_not(Value0 const& x, Value1 const& y);
} }
#endif

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

#endif


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#ifndef BOOST_INTEGRATOR_RESTRICTED_TWO_BODY_PROBLEM_HPP
#define BOOST_INTEGRATOR_RESTRICTED_TWO_BODY_PROBLEM_HPP

#include <cstdlib>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <cmath>
#include <limits>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include "NAOS/constants.hpp"
#include "NAOS/basicMath.hpp"
#include "NAOS/basicAstro.hpp"
#include "NAOS/misc.hpp"

namespace naos
{

//! Restricted two body problem integration
/*!
 * integrate the equations of motion for a particle around a point mass gravitational centre.
 */
void boostIntegratorRestrictedTwoBodyProblem( const double gravParameter,
                                              std::vector< double > &initialOrbitalElements,
                                              const double initialStepSize,
                                              const double startTime,
                                              const double endTime,
                                              std::ostringstream &outputFilePath,
                                              const int dataSaveIntervals );

} // namespace naos

#endif // BOOST_INTEGRATOR_RESTRICTED_TWO_BODY_PROBLEM_HPP


#ifndef POAC_CORE_RESOLVER_SAT_HPP
#define POAC_CORE_RESOLVER_SAT_HPP

// std
#include <iostream>
#include <vector>
#include <utility>
#include <string>
#include <numeric>
#include <algorithm>
#include <map>
#include <cmath>

// external
#include <boost/range/adaptor/indexed.hpp>
#include <mitama/result/result.hpp>

namespace poac::core::resolver::sat {
    enum class Status {
        satisfied, // found a satisfying assignment
        unsatisfied, // found no satisfying assignment
        normal, // Successful completion OR unsolved
    };

    std::vector<int>
    to_assignments(const std::vector<int>& literals) {
        std::vector<int> assignments;
        for (auto&& l : literals | boost::adaptors::indexed()) {
            const int literal = l.index() + 1;
            if (l.value() != -1) {
                assignments.emplace_back((l.value() % 2 == 0 ? 1 : -1) * literal);
            } else { // for literals that can take either value, arbitrarily assign them to be true
                assignments.emplace_back(literal);
            }
        }
        return assignments;
    }

    // the difference in number of occurrences
    template <
        template <class T, class=std::allocator<T>> typename TwoDim,
        template <class T, class=std::allocator<T>> typename OneDim,
        typename T, typename U
    >
    T calc_literal_polarity(const TwoDim<OneDim<T>>& rng, const U& i) {
        T acc = 0;
        for (const auto& rn : rng) {
            for (const auto& r : rn) {
                if (std::abs(r) == i) {
                    r > 0 ? ++acc : --acc;
                }
            }
        }
        return acc;
    }

    int literal_to_index(int l) {
        return std::abs(l) - 1;
    }

    // 1もしくは-1の数を clauses 全体から探索
    // 既に割り当て済みの変数は，delete_applyed_literal関数でclausesから削除ずみなので，現状のclausesに内在する変数から一番数が多い変数のindexを返す
    int maximum_literal_number_index(const std::vector<std::vector<int>>& clauses) {
        std::map<int, int> frequency;
        for (const auto& clause : clauses) {
            for (const auto& literal : clause) {
                auto result = frequency.insert(
                    { literal_to_index(literal), 1 }
                );
                if (!result.second) {
                    result.first->second++;
                }
            }
        }
        auto x = std::max_element(
            frequency.begin(), frequency.end(),
            [](const auto& p1, const auto& p2){
                return p1.second > p2.second;
            });
        return x->first;
    }

    // 変数割り当てが決定された変数をcluasesから削除する
    Status
    delete_set_literal(std::vector<std::vector<int>>& clauses, const int& index, const int& set_val) {
        for (auto itr1 = clauses.begin(); itr1 != clauses.end(); ++itr1) {
            for (auto itr2 = itr1->begin(); itr2 != itr1->end(); ++itr2) {
                // set_val -> unassigned(-1) -> always false
                // set_val -> true(0) -> value == index + 1
                // set_val -> false(1) -> value == -(index + 1)
                if (set_val >= 0 && (set_val == 0 ? index + 1 : -1 * (index + 1)) == *itr2) {
                    clauses.erase(itr1);
                    --itr1; // reset iterator
                    if (clauses.empty()) {
                        return Status::satisfied;
                    }
                    break; // to the next clause
                } else if (index == literal_to_index(*itr2)) { // the literal with opposite polarity
                    itr1->erase(itr2); // remove the literal from the clause
                    --itr2; // reset iterator
                    if (itr1->empty()) {
                        // unsatisfiable currently
                        return Status::unsatisfied;
                    }
                    break; // to the next clause
                }
            }
        }
        return Status::normal;
    }

    // unit resolution
    Status
    unit_propagate(std::vector<std::vector<int>>& clauses, std::vector<int>& literals) {
        bool unit_clause_found = true;
        while (unit_clause_found) {
            unit_clause_found = false;

            for (auto itr = clauses.begin(); itr != clauses.end(); ++itr) {
                if (itr->size() == 1) { // unit clause ({3}, {5}, ...)
                    unit_clause_found = true;

                    // 0 - if true, 1 - if false, set the literal
                    literals[literal_to_index(*itr->begin())] = *itr->begin() < 0;

                    const int index = literal_to_index(*itr->begin());
                    Status result = delete_set_literal(clauses, index, literals[index]);
                    if (result == Status::satisfied || result == Status::unsatisfied) {
                        return result;
                    }
                    // 今回のdeleteで，別のunit caluseができているかもしれないので，もう一度先頭からループし直す
                    break;
                } else if (itr->empty()) {
                    // the formula is unsatisfiable in this branch
                    return Status::unsatisfied;
                }
            }
        }
        return Status::normal;
    }

    // recursive DPLL algorithm
    [[nodiscard]] mitama::result<std::vector<int>, std::string>
    dpll(std::vector<std::vector<int>>& clauses, std::vector<int>& literals) {
        if (clauses.empty()) {
            return mitama::success(to_assignments(literals));
        } else if (
            Status result = unit_propagate(clauses, literals);
            result == Status::satisfied
        ) {
            return mitama::success(to_assignments(literals));
        } else if (result == Status::unsatisfied) {
            return mitama::failure(
                "could not solve dependencies.\n"
                "detail: given SAT problem was unsatisfied."
            );
        }

        // 最大の頻度を持つ変数が、次に割り当てられる値になります。
        const int i = maximum_literal_number_index(clauses);
        // need to apply twice, once true, the other false
        for (int j = 0; j < 2; ++j) {
            // copy the formula before recursive circulation
            std::vector<int> new_literals = literals;

            // if the number of literals with positive polarity are greater
            // cond ? positive : negative
            new_literals[i] = calc_literal_polarity(clauses, i + 1) > 0 ? j : (j + 1) % 2;

            // apply the change to all the clauses
            if (Status result = delete_set_literal(clauses, i, new_literals[i]);
                result == Status::satisfied
            ) {
                return mitama::success(to_assignments(new_literals));
            } else if (result == Status::unsatisfied) {
                // in this branch, return normally
                continue;
            }

            const auto result = dpll(clauses, new_literals);
            if (result.is_ok()) {
                return result;
            }
        }
        return mitama::failure(
            "could not solve dependencies.\n"
            "detail: given SAT problem was unsatisfied."
        );
    }

    [[nodiscard]] mitama::result<std::vector<int>, std::string>
    solve(std::vector<std::vector<int>> clauses, const unsigned long& variables) {
        // indexに対応するリテラル値の割り当て状態を表現する
        // a vector that stores the value assigned to each variable, where
        // -1 - unassigned, 0 - true, 1 - false
        std::vector<int> literals(variables, -1);
        return dpll(clauses, literals);
    }
} // end namespace
#endif // !POAC_CORE_RESOLVER_SAT_HPP


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// 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.

/**
 * \file geometry_tools.hpp
 * \brief
 * \details
 *
 * \author Autonomous Space Robotics Lab (ASRL)
 */
#pragma once

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include <vtr_vision/types.hpp>

// PCL
#if 0
#include <pcl_ros/point_cloud.hpp>  //this file had not been ported to ROS2 yet
#include <pcl_ros/segmentation/sac_segmentation.hpp>
#endif

namespace vtr {
namespace vision {

/////////////////////////////////////////////////////////////////////////////////
/// @brief Triangulates a point from a rig and keypoints
/////////////////////////////////////////////////////////////////////////////////
Eigen::Vector3d triangulateFromRig(
    const RigCalibration &rig_calibration,
    const std::vector<cv::Point2f> &keypoints,
    const FeatureInfos &kp_infos = FeatureInfos(),
    double *covariance = nullptr);

/////////////////////////////////////////////////////////////////////////////////
/// @brief Triangulates a point from a set of cameras and keypoints
/////////////////////////////////////////////////////////////////////////////////
Eigen::Vector3d triangulateFromCameras(
    const CameraIntrinsics &intrinsics, const Transforms &extrinsics,
    const std::vector<cv::Point2f> &keypoints,
    const FeatureInfos &kp_infos = FeatureInfos(),
    double *covariance = nullptr);

#if 0
/////////////////////////////////////////////////////////////////////////////////
/// @brief Estimates a plane from a PCL point cloud
/////////////////////////////////////////////////////////////////////////////////
bool estimatePlane(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                              const double distance_thresh,
                              pcl::ModelCoefficients &coefficients,
                              pcl::PointIndices &inliers);
#endif

/////////////////////////////////////////////////////////////////////////////////
/// @brief Estimat the distance from a plane
/////////////////////////////////////////////////////////////////////////////////
double estimatePlaneDepth(const Eigen::Vector3d &point,
                          const Eigen::Vector4f &coefficients);

}  // namespace vision
}  // namespace vtr


/*
//# /Users/arnold/Documents/prg/clang/bin/clang++ -U__STRICT_ANSI__ -std=c++14 -I/Users/arnold/Documents/prg/libraries/boost_1_56_0/ -I/Developer/SDKs/MacOSX10.6.sdk/usr/include -Wall -O2 -L/Users/arnold/Documents/prg/libraries/boost_1_56_0/stage/lib  -o rtree rtree.cpp -lboost_system -lboost_filesystem -lboost_iostreams
# -lpng16 -I/Users/arnold/Documents/prg/libraries/libpng-1.6.21
# g++ -std=c++14 rtree.cpp -o rtree -O2 -s -Wall

/Users/arnold/Documents/prg/clang/bin/clang++ -U__STRICT_ANSI__ -std=c++14 -I/Users/arnold/Documents/prg/libraries/boost_1_56_0/ -I/Developer/SDKs/MacOSX10.6.sdk/usr/include -Wall -O2 -L/Users/arnold/Documents/prg/libraries/boost_1_56_0/stage/lib  -o rtree rtree.cpp
 */

#include <boost/format.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <exception>
#include <cstdint>
#include <memory>
#include <utility>

using namespace std;
using boost::format;

namespace bg  = boost::geometry;
using point   = bg::model::point<int, 2, bg::cs::cartesian>;
using box     = bg::model::box<point>;
using rtval   = pair<box, string>;
using rtree_t = bg::index::rtree<rtval, bg::index::rstar<16>>;

struct ResultInfo {
	ResultInfo(string&& s, unsigned nr)
		: resultinfo_(std::forward<std::string>(s)), nr_{nr}
	{}
	string resultinfo_;
	unsigned nr_;
};

using shri  = shared_ptr<ResultInfo>;

struct KtoRange {
	unsigned i;
};

int main (int argc, char const *argv[])
{
	int ret = 0;
	try {
		rtree_t rtree;
		ifstream f("rtree_defs.txt");
		if (!(f.is_open() && f.good())) {
			throw "Konnte Datei nicht öffnen";
		}
		unsigned line_no {0};
		int xmin, ymin, xmax, ymax;
		string node_name;
		while(f >> xmin >> xmax >> ymin >> ymax >> node_name) {
			box b(point(xmin, ymin), point(xmax, ymax));
			rtree.insert(make_pair(b, node_name));
			++line_no;
			cout << format{"%3d: %-10s ((%d, %d), (%d, %d))"}
						% line_no % node_name % xmin % ymin % xmax % ymax
				 << endl;
		}
		if (!f.eof()) {
			throw "Fehler beim Lesen";
		}
		cout << "Finished reading." << endl;
		vector<rtval> result;
		while(true) {
			int x, y;
			cout << "Enter x y: ";
			if (!(cin >> x >> y)) {
				cerr << "Exiting" << endl;
				break;
			}
			point p(x, y);
			result.clear();
			// rtree.query(bg::index::intersects(box(p, p)), std::back_inserter(result));
			rtree.query(bg::index::intersects(p), std::back_inserter(result));
			unsigned i = 0;
			cout << result.size() << " results found" << endl;
			for(const auto& r : result) {
				++i;
				cout << i << ": " << r.second << endl;
			}
		}
	} catch(const std::exception& e) {
		cerr << "std::exception: " << e.what() << endl;
	} catch(const char* e) {
		cerr << "const char*: " << e << endl;
	} catch(...) {
		cerr << "Unknown exception" << endl;
	}
	return ret;
}