/**
 * vi:ts=4:shiftwidth=4:expandtab
 * vim600:fdm=marker
 *
 * maxentmodel.hpp  -  A Conditional Maximun Entropy Model
 *
 * Copyright (C) 2003 by Zhang Le <ejoy@users.sourceforge.net>
 * Begin       : 01-Jan-2003
 * Last Change : 08-Feb-2012.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

#ifndef MAXENTMODEL_H
#define MAXENTMODEL_H

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <vector>
#include <utility>
#include <boost/utility.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/shared_array.hpp>
#include <ostream>
#include <iostream>

#include "itemmap.hpp"
#include "meevent.hpp"

namespace boost {
    class timer;
}

/**
 * All classes and functions are placed in the namespace maxent.
 */
namespace maxent {
using namespace std;
using boost::shared_ptr;
using boost::shared_array;

extern int verbose;  // set this to 0 if you do not want verbose output

struct maxent_pickle_suite;
/**
 * This class implements a conditional Maximun Entropy Model.
 *
 * A conditional Maximun Entropy Model (also called log-linear model)has the
 * form:
 * \f$p(y|x)=\frac{1}{Z(x)} \exp \left[\sum_{i=1}^k\lambda_if_i(x,y) \right]\f$
 * Where x is a context and y is the outcome tag and p(y|x) is the conditional
 * probability.
 *
 * Normally the context x is composed of a set of contextual predicates.
 */
class MaxentModel /*: TODO: we need copyable? boost::noncopyable*/  {
    friend struct maxent_pickle_suite;

    // private:
    // virtual ~MaxentModel();

    public:
//    typedef std::string feature_type;
//    typedef std::string outcome_type;
    typedef me::feature_type feature_type;
    typedef me::feature_type outcome_type;
    typedef std::vector<pair<feature_type, float> > context_type;

    MaxentModel();

    void load(const string& model);

    void save(const string& model, bool binary = false) const;

    double eval(const context_type& context, const outcome_type& outcome) const;

    void eval_all(const context_type& context,
            std::vector<pair<outcome_type, double> >& outcomes,
            bool sort_result = true) const;

    outcome_type predict(const context_type& context) const;

    void begin_add_event();

    void add_event(const context_type& context,
            const outcome_type& outcome,
            size_t count = 1);

    void add_heldout_event(const context_type& context,
            const outcome_type& outcome,
            size_t count = 1);

    // wrapper functions for binary feature cases, provided for conviences
    void add_event(const vector<string>& context,
            const outcome_type& outcome,
            size_t count = 1);

    void add_heldout_event(const vector<string>& context,
            const outcome_type& outcome,
            size_t count = 1);

    double eval(const vector<string>& context,
            const outcome_type& outcome) const;

    void eval_all(const vector<string>& context,
            std::vector<pair<outcome_type, double> >& outcomes,
            bool sort_result = true) const;

    outcome_type predict(const vector<string>& context) const;

    /**
     * Add a set of events indicated by range [begin, end).
     * the value type of Iterator must be pair<context_type, outcome_type>
     */
    template <typename Iterator>
        void add_events(Iterator begin, Iterator end) {
            for (Iterator it = begin; it != end; ++it)
                this->add_event(it->first, it->second);
        }

    void end_add_event(size_t cutoff = 1);

    void train(size_t iter = 15, const std::string& method = "lbfgs",
            double sigma2 = 0.0, // non-zero enables Gaussian prior smoothing (global variance sigma^2)
            double tol = 1E-05);

     void dump_events(const string& model, bool binary = false) const;

    const char* __str__() const; // python __str__()

    // Python binding related functions {{{
#if defined(PYTHON_MODULE)

    // return the whole probabistic distribution [(outcome1, prob1),
    // (outcome2, prob2), ...] for a given context
    std::vector<pair<outcome_type, double> > py_eval(const context_type& context) const {
        static std::vector<pair<outcome_type, double> > outcomes;
        eval_all(context, outcomes);
        return outcomes;
    }
#endif
    // end py binding }}}

    private:
    double build_params(shared_ptr<me::ParamsType>& params,
            size_t& n_theta) const;
    double build_params2(shared_ptr<me::ParamsType>& params,
            size_t& n_theta) const;

            struct featid_hasher {
                size_t operator()(const pair<size_t, size_t>& p) const {
                    return p.first + p.second;
                }
            };

    struct cutoffed_event {
        cutoffed_event(size_t cutoff):m_cutoff(cutoff) {}
        bool operator()(const me::Event& ev) const {
            return ev.m_count < m_cutoff;
        }
        size_t m_cutoff;
    };

    struct cmp_outcome {
        bool operator()(const pair<outcome_type, double>& lhs,
                const pair<outcome_type, double>& rhs) const {
            return lhs.second > rhs.second;
        }
    };

    size_t m_n_theta;
    shared_ptr<me::MEEventSpace> m_es;
    shared_ptr<me::MEEventSpace> m_heldout_es;
    shared_ptr<me::PredMapType> m_pred_map;
    shared_ptr<me::OutcomeMapType> m_outcome_map;
    shared_ptr<me::ParamsType> m_params;
    shared_array<double> m_theta; // feature weights

    shared_ptr<boost::timer> m_timer;

    struct param_hasher {
        size_t operator()(const pair<size_t,size_t>& v) const {
            return size_t(~(v.first<< 1) + v.second);
        }
    };
};

#if defined(OLD_PYTHON_MODULE) //{{{ old python pickle support through Boost.Python
struct maxent_pickle_suite : boost::python::pickle_suite {
    static boost::python::tuple getstate(const MaxentModel& m)
    {
        if (!m.m_params)
            throw runtime_error("can not get state from empty model");
        using namespace boost::python;
        boost::python::list state;
        size_t i;

        shared_ptr<me::PredMapType> pred_map = m.m_pred_map;
        shared_ptr<me::OutcomeMapType> outcome_map = m.m_outcome_map;
        shared_ptr<me::ParamsType> params = m.m_params;
        size_t n_theta = m.m_n_theta;
        shared_array<double> theta = m.m_theta;

        // save pred_map
        state.append(pred_map->size());
        for (i = 0;i < pred_map->size(); ++i)
            state.append((*pred_map)[i]);

        // save outcome_map
        state.append(outcome_map->size());
        for (i = 0;i < outcome_map->size(); ++i)
            state.append((*outcome_map)[i]);

        // save params
        state.append(n_theta);
        assert(params->size() == pred_map->size());
        for (i = 0;i < params->size(); ++i) {
            boost::python::list oids;
            boost::python::list t;
            const std::vector<pair<size_t, size_t> >& a = (*params)[i];
            for (size_t j = 0; j < a.size(); ++j) {
                oids.append(a[j].first);
                t.append(a[j].second);
            }
            state.append(make_tuple(oids, t));
        }
        // save theta
        for (i = 0;i < n_theta; ++i)
            state.append(theta[i]);
        return boost::python::tuple(state);
    }

    static void setstate(MaxentModel& m, boost::python::tuple state)
    {
        using namespace boost::python;
        assert (!m.m_pred_map);
        assert (!m.m_outcome_map);
        assert (!m.m_params);
        assert (len(state) > 0);

        shared_ptr<me::PredMapType> pred_map(new me::PredMapType);
        shared_ptr<me::OutcomeMapType> outcome_map(new me::OutcomeMapType);
        shared_ptr<me::ParamsType> params(new me::ParamsType);
        size_t n_theta;
        shared_array<double> theta;

        size_t count;
        size_t i;
        size_t index = 0;

        // load pred_map
        count = extract<size_t>(state[index++]);
        for (i = 0; i < count; ++i)
            pred_map->add(extract<std::string>(state[index++]));

        // load outcome_map
        count = extract<size_t>(state[index++]);
        for (i = 0; i < count; ++i)
            outcome_map->add(extract<std::string>(state[index++]));

        // load params
        n_theta = extract<size_t>(state[index++]);
        for (i = 0; i < pred_map->size(); ++i) {
            tuple tmp(state[index++]);
            boost::python::list oids(tmp[0]);
            boost::python::list t(tmp[1]);
            std::vector<pair<size_t, size_t> > a;

            size_t k = extract<size_t>(oids.attr("__len__")());
            assert (k == len(t));

            for (size_t j = 0; j < k; ++j) {
                size_t oid = extract<size_t>(oids[j]);
                size_t fid = extract<size_t>(t[j]);
                a.push_back(std::make_pair(oid, fid));
            }
            params->push_back(a);
        }
        // extract theta
        theta.reset(new double[n_theta]);
        for (i = 0;i < n_theta; ++i)
            theta[i] = extract<double>(state[index++]);
        m.m_pred_map = pred_map;
        m.m_outcome_map = outcome_map;
        m.m_params = params;
        m.m_n_theta = n_theta;
        m.m_theta = theta;
    }
};
#endif // PYTHON_MODULE }}}

} // namespace maxent
#endif /* ifndef MAXENTMODEL_H */


// default libraries
#include "algorithm"
#include "iomanip"
#include "iostream"
#include "fstream"
#include "string"

// boost
#include "boost/array.hpp"
#include "boost/foreach.hpp"
#include "boost/lexical_cast.hpp"
#include "boost/mpi.hpp"
#include "boost/unordered_set.hpp"

// load balancing
#include "zoltan.h"

// adaptive grid (loaded with pragmas to avoid error spam)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-variable"
#pragma clang diagnostic ignored "-Wunused-parameter"
#pragma clang diagnostic ignored "-Wmissing-braces"
#include "dccrg.hpp"
#include "dccrg_cartesian_geometry.hpp"
#pragma clang diagnostic pop

// vector calculus
#include <Eigen/Dense>

// own header files
#include "cell.hpp" // Cell class for individual grid cells
#include "comm.hpp" // Domain decomposition communications
#include "inject.hpp" // Particle initializer & injector
#include "mesh.hpp" // Mesh and field related functions
#include "field.hpp" // actual field propagators
#include "parameters.hpp" // simulation variables
#include "common.h" // simulation variables
#include "particles.hpp" // particle spatial & momentum pushers
#include "io.hpp" // Simulation saver

// name spaces
using namespace std;
using namespace boost;
using namespace boost::mpi;
using namespace dccrg;
using namespace Eigen;

/// rank-specific switch for MPI data transfers
int Cell::transfer_mode = Cell::INIT;

// Vector and Matrix calculus type definitions
typedef Array<double, 4, 1> Vectord4;
typedef Array<double, 4, 4> Matrixd44;
typedef Array<double, 3, 2> Matrixd32; ///< old compatibility type

typedef Parameters P;

/// Underlying adaptive grid framework
static Dccrg<Cell, dccrg::Cartesian_Geometry> mpiGrid;

/** Get local cell IDs. This function creates a cached copy of the
 * cell ID lists to significantly improve performance. The cell ID
 * cache is recalculated every time the mesh partitioning changes.
 * @return Local cell IDs.*/
const std::vector<CellID>& getLocalCells() {
   return P::localCells;
}

/// Recalculate the cell listing
void recalculateLocalCellsCache() {
     {
        vector<CellID> dummy;
        dummy.swap(P::localCells);
     }
   P::localCells = mpiGrid.get_cells();
}

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

    cout << "Starting..." << endl;

    // Start up MPI
	if (MPI_Init(&argc, &argv) != MPI_SUCCESS) {
		cerr << "Couldn't initialize MPI." << endl;
		abort();
	}
	MPI_Comm communicator = MPI_COMM_WORLD;

	int rank = 0, comm_size = 0;
	MPI_Comm_rank(communicator, &rank);
	MPI_Comm_size(communicator, &comm_size);

    // create also communicator object
    Comm comm(rank, comm_size, communicator);
    cout << rank << ": Initialized MPI..." << endl;

    if(rank == 0) {
        cout << "--------------------------------------------------" << endl;
        cout << " Simulation parameters "<< endl;
        cout << "    c: " << c << endl;
        cout << "    e: " << e << endl;
        cout << "   dt: " << P::dt << endl;
        cout << "   me: " << me << endl;
        cout << "   mp: " << mp << endl;

        cout << " ----  " << endl;

        cout << "   Nx: " << P::Nx << endl;
        cout << "   Ny: " << P::Ny << endl;
        cout << "   Nz: " << P::Nz << endl;
        cout << " xmin: " << P::grid_xmin << endl;
        cout << " xmax: " << P::grid_xmax << endl;
        cout << " ymin: " << P::grid_ymin << endl;
        cout << " ymax: " << P::grid_ymax << endl;
        cout << " zmin: " << P::grid_zmin << endl;
        cout << " zmax: " << P::grid_zmax << endl;
        cout << "--------------------------------------------------" << endl;
    }

    // print rank, PID, and hostname
    char hostname[256];
    gethostname(hostname, sizeof(hostname));
    cout << rank << ": PID:" << ::getpid() << " host:" << hostname << endl;

	// initialize Zoltan
	float zoltan_version;
	if (Zoltan_Initialize(argc, argv, &zoltan_version) != ZOLTAN_OK) {
		cerr << "Zoltan_Initialize failed" << endl;
		abort();
	}
    cout << rank << ": Initialized Zoltan..." << endl;

    // initialize grid spatial scales
    dccrg::Cartesian_Geometry::Parameters geom_params;

	const std::array<uint64_t, 3> grid_length = {{P::Nx, P::Ny, P::Nz}};
	mpiGrid.initialize(grid_length,
                    communicator,
                    "RCB",
                    P::N_neighb,
                    P::max_ref_lvl,
                    P::Nx_wrap, P::Ny_wrap, P::Nz_wrap);

    geom_params.start[0] = P::grid_xmin;
    geom_params.start[1] = P::grid_ymin;
    geom_params.start[2] = P::grid_zmin;

    geom_params.level_0_cell_length[0] = abs(P::grid_xmax - P::grid_xmin)/double(P::Nx);
    geom_params.level_0_cell_length[1] = abs(P::grid_ymax - P::grid_ymin)/double(P::Ny);
    geom_params.level_0_cell_length[2] = abs(P::grid_zmax - P::grid_zmin)/double(P::Nz);

    if (!mpiGrid.set_geometry(geom_params)) {
		cerr << __FILE__ << ":" << __LINE__ << ": Couldn't set mpiGrid geometry" << endl;
		abort();
	}

    // Neighborhoods
    //--------------------------------------------------
    typedef dccrg::Types<3>::neighborhood_item_t neigh_t;

    // shift with +1/+1/+1 elements
    std::vector<neigh_t> neighborhood;
    neighborhood.clear();
    neighborhood.push_back({{1,0,0}});
    neighborhood.push_back({{0,1,0}});
    neighborhood.push_back({{0,0,1}});

    if (!mpiGrid.add_neighborhood(CP1_SHIFT, neighborhood)) {
        std::cerr << __FILE__ << ":" << __LINE__
            << " add_neighborhood failed"
            << std::endl;
        abort();
    }

    // shift with -1/-1/-1 elements
    neighborhood.clear();
    neighborhood.push_back({{-1,0,0}});
    neighborhood.push_back({{0,-1,0}});
    neighborhood.push_back({{0,0,-1}});

    if (!mpiGrid.add_neighborhood(CM1_SHIFT, neighborhood)) {
        std::cerr << __FILE__ << ":" << __LINE__
            << " add_neighborhood failed"
            << std::endl;
        abort();
    }

    // Full negative cube (in z-ordering for efficiency)
    // TODO: currently this is done via default -1/0/+1 neighborhood;
    //       use this instead; so we can skip +1 cells
    // std::vector<neigh_t> neighborhood2 = {{ {{1,0,0}},
    //                                        {{0,1,0}},
    //                                        {{0,0,1}}
    //                                      }};

    cout << rank << ": Initialized mpiGrid..." << endl;

    //--------------------------------------------------
    cout << rank << ": Injecting particles..." << endl;
    /* First we transfer everything to rank 0 for easy initialization.
       Then inject particles, then load balance back using Zoltan
    */
    Injector inject(P::grid_xmin, P::grid_xmax,
                    P::grid_ymin, P::grid_ymax,
                    P::grid_zmin, P::grid_zmax
                    );

    comm.move_all_to_master(mpiGrid);

    // inject into cylindrical shape
    // inject.cylinder(mpiGrid, P::Np, vb);

    // inject uniform background plasma first
    uint64_t Nbkg = (uint64_t)(P::Np*0.1);
    double vb_bkg = 1.0;
    inject.uniform(mpiGrid, Nbkg, vb_bkg);

    // inject thin current sheets
    uint64_t Nsheets = (uint64_t)(P::Np*0.9);
    double delta = 0.005; ///< Sheet thickness
    const double vb = 1.0; ///< Maxwellian temperature
    inject.two_sheets(mpiGrid, Nsheets, vb, delta);

    comm.load_balance(mpiGrid);
    cout << rank << ": load balanced..." << endl;

    // Initialize B field
    double B0 = 10000.0;
    double alpha = 0.0;
    inject.Harris_sheet(mpiGrid, B0, alpha, delta);

    // Initialize mesh and create fields
    //--------------------------------------------------
    Mesh mesh;
    mesh.rank = rank;

    // int debugWait = 0;
    // while (0 == debugWait)
    //     sleep(5);

    mesh.deposit_currents(mpiGrid);
    comm.update_ghost_zone_currents(mpiGrid);

    mesh.yee_currents(mpiGrid);
    comm.update_ghost_zone_yee_currents(mpiGrid);

    // initialize field solver
    Field_Solver field;

    field.push_half_B(mpiGrid);
    comm.update_ghost_zone_B(mpiGrid);

    field.push_E(mpiGrid);
    comm.update_ghost_zone_E(mpiGrid);

    field.push_half_B(mpiGrid);
    comm.update_ghost_zone_B(mpiGrid);

    // TODO
    // mesh.nodal_fields(mpiGrid);

    Particle_Mover particles;

    particles.update_velocities(mpiGrid);
    comm.update_ghost_zone_particles(mpiGrid);

    particles.propagate(mpiGrid);

    mesh.sort_particles_into_cells(mpiGrid);

    // Simulation save
    //--------------------------------------------------
    Save io;
    io.rank = rank;
    io.comm_size = comm_size;
    io.save_dir = "out/";
    io.filename = "pic";
    io.init();

    io.save_grid(mpiGrid);
    io.save_particles(mpiGrid);
    io.save_fields(mpiGrid);
    io.update_master_list();
    io.step++;

    cout << "Initialized save file" << endl;

    //--------------------------------------------------
    // Initialized everything; now starting main loop

	const unsigned int max_steps = 50;

    cout << "Starting particle propagation" << endl;
	for (unsigned int step = 1; step < max_steps; step++)
    {
        cout << " step: " << step << endl;

        field.push_half_B(mpiGrid);
        comm.update_ghost_zone_B(mpiGrid);

        // update particle velocities and locations
        particles.update_velocities(mpiGrid);
        particles.propagate(mpiGrid);
        comm.update_ghost_zone_particles(mpiGrid);

        mesh.sort_particles_into_cells(mpiGrid);

        field.push_half_B(mpiGrid);
        comm.update_ghost_zone_B(mpiGrid);

        field.push_E(mpiGrid);
        comm.update_ghost_zone_E(mpiGrid);

        mesh.deposit_currents(mpiGrid);
        comm.update_ghost_zone_currents(mpiGrid);

        mesh.yee_currents(mpiGrid);
        comm.update_ghost_zone_yee_currents(mpiGrid);

        // apply filters

        // save step
        io.save_grid(mpiGrid);
        io.save_particles(mpiGrid);
        io.save_fields(mpiGrid);
        io.update_master_list();
        io.step++;

    }

    io.finalize();

	MPI_Finalize();

    cout << "Finalized..." << endl;

	return EXIT_SUCCESS;
}


// TODO: This is currently double as float causes
// headaches with PROJ.4.
typedef double real;

#ifndef SWIG
/* The code is horrible. I'm deeply sorry. C++, and especially
 * Boost, is hell. */
#include <readosm.h>
#include <malloc.h>
#include <memory>
#include <stdexcept>
#include <string>
#include <vector>
#include <unordered_map>
#include <iostream>
#include <cmath>
#include <cstdlib>
#include <random>
#include <ctime>
#include <queue>
#include <set>
#include <list>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/random.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <proj_api.h>

using std::unique_ptr;
using std::shared_ptr;
using std::vector;
using std::string;
namespace bst = boost;
namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

#endif
typedef long long node_id_t;
#ifndef SWIG

typedef bst::adjacency_list<bst::vecS, bst::vecS, bst::bidirectionalS,
	bst::property<bst::vertex_name_t, node_id_t>,
	bst::property<bst::edge_weight_t, real>
	> Graph;
typedef bst::graph_traits < Graph >::vertex_descriptor Vertex;
typedef bst::graph_traits < Graph >::edge_descriptor Edge;

real rad_to_deg(real rad) {
	return rad / (M_PI/180.0);
}

real deg_to_rad(real deg) {
	return deg * (M_PI/180.0);
}

class OsmReaderError : public std::runtime_error {
	using std::runtime_error::runtime_error;
};

typedef bg::model::point<real, 2, bg::cs::cartesian> Point;
typedef bg::model::segment<Point> LineSegment;
typedef bg::model::box<Point> Bbox;

// OMFG! I have to implement this manually!?!
// Also giving up on generality, as, you know, Boost.
inline real vect_norm(Point p) {
	real c = p.get<0>();
	real result = c*c;
	c = p.get<1>();
	result += c*c;

	return std::sqrt(result);
}

struct LinesegProjectionResult {
	Point projected;
	real error;
	real t;
};
// There's probably something like this in the depths
// of Boost.Geometry, but couldn't find it, so here we go again.
LinesegProjectionResult lineseg_point_projection(LineSegment seg, Point p)
{
	auto a = get<0>(seg);
	auto b = get<1>(seg);

	// I guess Boost tries to make the APIs as difficut
	// as possible. This should be: auto segd = b - a;
	// but boost fucking wants this shit:
	auto segd = b; bg::subtract_point(segd, a);
	auto seglen = vect_norm(segd);
	auto normstart = p; bg::subtract_point(normstart, a);
	auto t = bg::dot_product(normstart, segd)/(seglen*seglen);

	real error;

	if(t > 1.0) {
		t = 1.0;
	}

	if(t < 0.0) {
		t = 0.0;
	}

	// Reusing stuff because the boost api is a fucking disaster.
	// With Eigen this was: auto proj = a + t*segd;
	bg::multiply_value(segd, t);
	auto proj = a;
	bg::add_point(proj, segd);

	bg::subtract_point(p, proj);
	error = vect_norm(p);
	LinesegProjectionResult result = {
		.projected=proj,
		.error=error,
		.t=t*seglen};
	return result;
}

template <class Key, class Value>
class default_map
{
	public:
	typedef std::pair<const Key, Value> value_type;
	typedef Key key_type;

	private:
	std::unordered_map<Key, Value, std::hash<Key>, std::equal_to<Key>> m_map;
	Value deflt;

	public:

	default_map(const Value def): deflt(def),
		m_map() {

	}

	Value& operator[](const Key& k) {
		auto it = m_map.find(k);
		if(it == m_map.end()) {
			return m_map[k] = deflt;
		}

		return it->second;
	}

	size_t count(const Key& k) {
		return m_map.count(k);
	}
};

template <class Callback>
void reverse_shortest_path(Vertex target, Graph& graph, Callback callback) {
	default_map<Vertex, real> distances(1.0/0.0);
	default_map<Vertex, real> cost(1.0/0.0);
	std::unordered_map<Vertex, Vertex> successor;
	auto weights = bst::get(bst::edge_weight, graph);

	typedef std::pair<real, Vertex> Node;
	std::priority_queue<Node, std::vector<Node>, std::greater<Node>> queue;
	queue.push(std::make_pair(0.0, target));

	while(!queue.empty()) {
		auto top = queue.top();
		auto current = top.second;
		auto current_dist = top.first;
		queue.pop();
		distances[current] = current_dist;
		if(!callback(current, current_dist, successor)) {
			break;
		}

		auto children = bst::in_edges(current, graph);
		for(auto edge=children.first; edge!=children.second; edge++) {
			auto alt = current_dist + weights[*edge];
			auto child = bst::source(*edge, graph);

			if(distances.count(child)) {
				// Sanity check that we don't get a new distance
				// for an already finished node
				assert(distances[child] <= alt);
				// We've already finished this one
				continue;
			}

			if(alt < cost[child]) {
				cost[child] = alt;
				queue.push(std::make_pair(alt, child));
				successor[child] = current;
			}
		}
	}
}

template <class SuccessorMap>
vector<Vertex> build_path_from_successors(Vertex start, Graph &g, SuccessorMap& suc) {
	vector<Vertex> result;

	auto end = suc.end();
	auto current = suc.find(start);
	result.push_back(start);
	while(current != end) {
		result.push_back(current->second);
		current = suc.find(current->second);
	}

	return result;
}

auto single_target_shortest_path = [](Vertex source, Vertex target, Graph& graph) {
	vector<Vertex> path;
	auto visitor = [&](Vertex current, real distance,
		std::unordered_map<Vertex, Vertex>& successors) {
		if(current != source) return true;
		path = build_path_from_successors(source, graph, successors);
		return false;
	};

	reverse_shortest_path(target, graph, visitor);

	return path;
};

#endif // SWIG

// A hack to return stuff to python as the
// Boost.Geometry Point is too bizarre
struct Point2d {
	real x; real y;
	Point2d(){}
	Point2d(Point& p) {
		x = p.get<0>();
		y = p.get<1>();
	}

	Point2d(real x, real y)
		:x(x), y(y){}
};

class CoordinateProjector {
	private:
	projPJ projector;
	projPJ wgs;

	public:
	CoordinateProjector(const char* proj_string)
			:projector(NULL), wgs(NULL) {
		projector = pj_init_plus(proj_string);
		if(!projector) {
			throw OsmReaderError("Couldn't initialize coordinate projection");
		}

		projPJ tmp_wgs = pj_init_plus("+init=epsg:4326");
		if(!tmp_wgs) {
			throw OsmReaderError("Couldn't initialize WGS coordinate system");
		}

		if(!(wgs = pj_latlong_from_proj(tmp_wgs))) {
			pj_free(tmp_wgs);
			throw OsmReaderError("Couldn't initialize WGS latlong coordinate system");
		}
		pj_free(tmp_wgs);
	}

	void operator()(double latitude, double longitude, real* x, real* y) {
		*y = deg_to_rad(latitude);
		*x = deg_to_rad(longitude);

		if(pj_transform(wgs, projector, 1, 1, x, y, NULL)) {
			throw OsmReaderError("Failed to project a coordinate to XY " + std::to_string(latitude) + " " + std::to_string(longitude));
		}
	}

	void inverse(real x, real y, double *latitude, double *longitude) {
		*latitude = y;
		*longitude = x;

		if(pj_transform(projector, wgs, 1, 1, longitude, latitude, NULL)) {
			throw OsmReaderError("Failed to project a coordinate to LatLng "  + std::to_string(x) + " " + std::to_string(y));
		}

		*latitude = rad_to_deg(*latitude);
		*longitude = rad_to_deg(*longitude);
	}

	~CoordinateProjector() {
		if(projector) pj_free(projector);
		if(wgs) pj_free(wgs);
	}
};

enum WayRole {
	WayRoleIgnore = 0,
	WayRoleOneWay,
	WayRoleTwoWay
};

WayRole busway_filter(const readosm_way *way) {
	const char *highway = NULL;
	const char *busway = NULL;
	const char *oneway = NULL;
	const char *junction = NULL;
	const char *ferry = NULL;

	for(size_t i=0; i < way->tag_count; ++i) {
		if(string("highway").compare(way->tags[i].key) == 0)
			highway = way->tags[i].value;
		if(string("busway").compare(way->tags[i].key) == 0)
			busway = way->tags[i].value;
		if(string("oneway").compare(way->tags[i].key) == 0)
			oneway = way->tags[i].value;
		if(string("junction").compare(way->tags[i].key) == 0)
			junction = way->tags[i].value;
		if(string("ferry").compare(way->tags[i].key) == 0)
			ferry = way->tags[i].value;
	}

	if(!(busway || highway || ferry)) return WayRoleIgnore;
	if(!highway) return WayRoleTwoWay;

	if(string("footway").compare(highway) == 0) return WayRoleIgnore;
	if(string("cycleway").compare(highway) == 0) return WayRoleIgnore;
	if(string("steps").compare(highway) == 0) return WayRoleIgnore;
	if(string("path").compare(highway) == 0) return WayRoleIgnore;
	if(string("construction").compare(highway) == 0) return WayRoleTwoWay;
	if(string("proposed").compare(highway) == 0) return WayRoleIgnore;
	if(string("bridleway").compare(highway) == 0) return WayRoleIgnore;

	if(string(highway).compare("motorway") == 0) return WayRoleOneWay;
	if(oneway && string(oneway).compare("yes") == 0) return WayRoleOneWay;
	if(junction && string(junction).compare("roundabout") == 0) return WayRoleOneWay;

	return WayRoleTwoWay;
}

WayRole train_filter(const readosm_way *way) {
	const char *railway = NULL;
	const char *oneway = NULL;
	for(size_t i=0; i < way->tag_count; ++i) {
		if(string("railway").compare(way->tags[i].key) == 0)
			railway = way->tags[i].value;
		if(string("oneway").compare(way->tags[i].key) == 0)
			oneway = way->tags[i].value;
	};

	if(!railway) return WayRoleIgnore;
	if(string("rail").compare(railway) != 0) return WayRoleIgnore;
	if(oneway && string(oneway).compare("yes") == 0) return WayRoleOneWay;
	return WayRoleTwoWay;
}

WayRole tram_filter(const readosm_way *way) {
	const char *railway = NULL;
	const char *oneway = NULL;
	for(size_t i=0; i < way->tag_count; ++i) {
		if(string("railway").compare(way->tags[i].key) == 0)
			railway = way->tags[i].value;
		if(string("oneway").compare(way->tags[i].key) == 0)
			oneway = way->tags[i].value;
	};

	if(!railway) return WayRoleIgnore;
	if(string("tram").compare(railway) != 0 && string("construction").compare(railway) != 0) return WayRoleIgnore;
	if(oneway && string(oneway).compare("yes") == 0) return WayRoleOneWay;
	return WayRoleTwoWay;
}

WayRole subway_filter(const readosm_way *way) {
	const char *railway = NULL;
	for(size_t i=0; i < way->tag_count; ++i) {
		if(string("railway").compare(way->tags[i].key) == 0)
			railway = way->tags[i].value;
	};

	if(!railway) return WayRoleIgnore;
	if(string("subway").compare(railway) != 0) return WayRoleIgnore;
	return WayRoleTwoWay;
}

class OsmGraph {
	private:
	CoordinateProjector& coord_proj;

	public:
	Graph graph;
	std::unordered_map<node_id_t, Point> node_coordinates;
	std::unordered_map<node_id_t, Vertex> id_to_vertex;
	bgi::rtree< std::pair<Bbox, std::pair<Edge, LineSegment> >, bgi::quadratic<16> > edge_index;

	private:
	static int handle_osm_node(const void *usr_data, const readosm_node *node) {
		auto self = (OsmGraph*)usr_data;

		real x, y;
		self->coord_proj(node->latitude, node->longitude, &x, &y);
		self->node_coordinates[node->id] = Point(x, y);

		return READOSM_OK;
	}

	void ensure_node(node_id_t node_id) {
		if(id_to_vertex.count(node_id)) return;
		if(node_coordinates.count(node_id) == 0) return;

		auto node_id_prop = bst::get(bst::vertex_name, graph);
		Vertex v = bst::add_vertex(graph);
		id_to_vertex[node_id] = v;
		node_id_prop[v] = node_id;
	}

	void add_new_edge(node_id_t src, node_id_t dst) {
		ensure_node(src); ensure_node(dst);
		if(!(id_to_vertex.count(src) && id_to_vertex.count(dst))) {
			return;
		}
		auto& s = node_coordinates[src];
		auto& e = node_coordinates[dst];
		auto seg = LineSegment(s, e);

		auto new_edge = bst::add_edge(
			id_to_vertex[src],
			id_to_vertex[dst],
			graph).first;

		auto length = bg::length(seg);
		auto edge_id = num_edges(graph);

		get(bst::edge_weight, graph)[new_edge] = length;
		Bbox bbox;
		bg::envelope(seg, bbox);
		auto entry = std::make_pair(bbox, std::make_pair(new_edge, seg));
		edge_index.insert(entry);

	}

	static int handle_osm_way(const void *usr_data, const readosm_way *way) {
		auto self = (OsmGraph*)usr_data;
		return self->do_handle_osm_way(way);
	}

	int do_handle_osm_way(const readosm_way *way) {
		// TODO: The graph search could be optimized quite a bit by
		//	just inserting one edge per way and calculating the
		//	edge length from the segments.
		//	Also duplicating both ways in the RTree causes almost
		//	double the storage/computation. The implementation is
		//	simpler this way though.
		auto& i2v = id_to_vertex;
		WayRole role = get_way_role(way);
		if(role == WayRoleIgnore) {
			return READOSM_OK;
		}

		for(size_t i=0; i < way->node_ref_count - 1; ++i) {
			add_new_edge(way->node_refs[i], way->node_refs[i+1]);
		}

		if(role == WayRoleOneWay) return READOSM_OK;

		for(size_t i=0; i < way->node_ref_count - 1; ++i) {
			add_new_edge(way->node_refs[i+1], way->node_refs[i]);
		}

		return READOSM_OK;
	}

	WayRole(&get_way_role)(const readosm_way *);

	public:
	OsmGraph(const char *filename, CoordinateProjector& proj, WayRole(*get_way_role)(const readosm_way *))
			:coord_proj(proj), get_way_role(*get_way_role) {
		const void *osm_handle;
		auto status = readosm_open(filename, &osm_handle);
		if(status != READOSM_OK) {
			throw new OsmReaderError("Failed to open input file");
		}
		// NOTE: Assumes currently that all nodes are read before
		//	the ways. Seems to be so, but not really assured
		//	anywhere.
		readosm_parse(osm_handle, this, handle_osm_node, handle_osm_way, NULL);
		readosm_close(osm_handle);
	}

	Point& get_vertex_point(Vertex vertex) {
		auto node_id = get(bst::vertex_name, graph)[vertex];
		return node_coordinates[node_id];
	}

	node_id_t get_vertex_id(Vertex vertex) {
		return get(bst::vertex_name, graph)[vertex];
	}

	std::vector< std::pair<Point2d, Point2d> > get_edge_coordinates() {
		vector< std::pair<Point2d, Point2d> > result;
		auto iters = bst::edges(graph);
		for(auto edge=iters.first; edge != iters.second; edge++) {
			auto src = Point2d(get_vertex_point(bst::source(*edge, graph)));
			auto dst = Point2d(get_vertex_point(bst::target(*edge, graph)));
			result.push_back(std::make_pair(src, dst));
		}

		return result;
	}

	~OsmGraph() {
	}
};

struct PositionHypothesis {
	std::shared_ptr<PositionHypothesis> parent;
	real timestamp;
	real total_likelihood;
	real measurement_likelihood;
	real transition_likelihood;
 	real measurement_error;
	Edge edge;
	real edge_offset;
	std::vector<Vertex> subpath;
	Point position;
	Point measurement;
};

real gaussian_logpdf(real x, real m, real s) {
	real normer = std::log(1.0/(s*std::sqrt(2*M_PI)));
	return normer - (x-m)*(x-m)/(2*s*s);
}

class StateLikelihoodModel {
	public:
	virtual real measurement(const Point& m, const Point& p) = 0;
	virtual real transition(const PositionHypothesis& parent, const PositionHypothesis& next, const vector<Vertex>& path, real path_length) = 0;
	virtual ~StateLikelihoodModel() {};

	virtual real best_transition_still_possible(real measured_dist, real distance) {
		return 0.0;
	}
};

inline real vector_angle(Point& a, Point &b) {
	auto cosangle = bg::dot_product(a, b)/(vect_norm(a)*vect_norm(b));
	// Avoid acos returning NaNs due to rounding errors
	if(cosangle < -1.0) {
		cosangle = -1.0;
	} else if(cosangle > 1.0) {
		cosangle = 1.0;
	}
	auto angle = std::acos(cosangle);
	return angle;
}

#if defined __FAST_MATH__
#error This will not currently work with fast math due to needed NaN checks
#endif
class DrawnGaussianStateModel : public StateLikelihoodModel {
	real measurement_std;
	real length_error_std;
	OsmGraph& graph;
	public:

	DrawnGaussianStateModel(real measurement_std, real length_error_std, OsmGraph& graph)
		:measurement_std(measurement_std), graph(graph), length_error_std(length_error_std) {}

	real measurement(const Point& m, const Point& p) {
		auto dx = m.get<0>() - p.get<0>();
		auto dy = m.get<1>() - p.get<1>();
		return gaussian_logpdf(dx, 0.0, measurement_std) +
			gaussian_logpdf(dy, 0.0, measurement_std);
	}

	real transition(const PositionHypothesis& parent, const PositionHypothesis& next, const vector<Vertex> &path, real path_length) {
		// The idea here is to calculate how big a share of the
		// path is in different direction than the measurement.
		// TODO: I have a hunch that this could be reduced to a very
		//	simple form of just some distance ratios.
		auto orig_direction = next.measurement;
		bg::subtract_point(orig_direction, parent.measurement);

		auto total_angle = 0.0;
		auto total_length = 0.0;
		int n_spans = 0;

		auto evaluate_span = [&](Point& prev_point, Point& next_point) {
			auto direction = next_point;
			bg::subtract_point(direction, prev_point);
			auto length = vect_norm(direction);
			prev_point = next_point;
			auto angle = vector_angle(orig_direction, direction);
			if(std::isnan(angle)) {
				return;
			}
			total_angle += angle/M_PI*length;
			total_length += length;
			n_spans++;

		};

		auto prev_point = parent.position;
		for(auto i=0; i < path.size(); i++) {
			auto next_point = graph.get_vertex_point(path[i]);
			evaluate_span(prev_point, next_point);
			prev_point = next_point;
		}

		if(path.size() > 0) {
			auto next_point = graph.get_vertex_point(bst::source(next.edge, graph.graph));
			evaluate_span(prev_point, next_point);
			prev_point = next_point;
		}

		auto next_point = next.position;
		evaluate_span(prev_point, next_point);

		if(total_length < 1e-6) {
			// Give some penalty for hanging around in the same
			// node. Otherwise truncates start and endpoints.
			// TODO: Could probably be handled more elegantly.
			total_angle = length_error_std;
		} else {
			total_angle /= total_length;
		}

		return gaussian_logpdf(total_angle, 0.0, length_error_std);

		//auto measured_length = bg::length(LineSegment(parent.measurement, next.measurement));
		//auto relative_dist = (path_length+length_error_std)/(measured_length+length_error_std);
		//return gaussian_logpdf(relative_dist-1.0, 0, 0.1);
		//return gaussian_logpdf(path_length, 0.0, length_error_std);
	}

	real best_transition_still_possible(real measured_length, real path_length) {
		return gaussian_logpdf(0.0, 0.0, length_error_std);
		//auto diff = path_length - measured_length;
		//if(diff < 0.0) diff = 0.0;
		//return gaussian_logpdf(path_length/(measured_length+1.0), 0, length_error_std);
		//return gaussian_logpdf(path_length, 0.0, length_error_std);
	}
};

class MapMatcher2d {
	// TODO: Probably doing a lot of large object copies. Let's hope
	//	for copy elision, and of course optimize later.
	// TODO: There's a huge problem in the approach! By examining just
	//	a single point on the edge, if the noise causes the measurement
	//	to "backtrack", the result is very bad! Should consider the whole
	//	edge instead.
	OsmGraph& graph;
	real search_radius;
	vector< shared_ptr<PositionHypothesis> >* hypotheses = NULL;
	Point previous_measurement;
	StateLikelihoodModel& state_model;

	public:
	int n_outliers = 0;

	MapMatcher2d(OsmGraph& g, StateLikelihoodModel& state_model, real search_radius=100.0)
		: graph(g), search_radius(search_radius), state_model(state_model)  {

	}

	~MapMatcher2d() {
		if(hypotheses) delete hypotheses;
	}

	void measurement(real ts, real x, real y) {
		Bbox search_area(
			Point(x - search_radius, y - search_radius),
			Point(x + search_radius, y + search_radius));

		auto query = bgi::intersects(search_area);
		std::vector< std::pair<Bbox, std::pair<Edge, LineSegment> > > results;
		graph.edge_index.query(query, std::back_inserter(results));

		auto point = Point(x, y);
		if(!hypotheses) {
			previous_measurement = point;
		}
		auto measured_dist = bg::length(LineSegment(point, previous_measurement));
		previous_measurement = point;

		auto new_hypotheses = new vector< shared_ptr<PositionHypothesis> >;
		auto best_total_likelihood = -1.0/0.0;

		#pragma omp parallel for
		for(auto i=results.begin(); i < results.end(); i++) {
			auto& result = *i;
			auto edge = result.second.first;
			auto seg = result.second.second;
			auto proj = lineseg_point_projection(seg, Point(x, y));
			auto error = proj.error;
			auto t = proj.t;
			if(error > search_radius) {
				continue;
			}

			auto hypo = shared_ptr<PositionHypothesis>(new PositionHypothesis {
				.parent=NULL,
				.timestamp=ts,
				.total_likelihood=0.0,
				.measurement_likelihood=state_model.measurement(point, proj.projected),
				.transition_likelihood=0.0,
				.measurement_error=error,
				.edge=edge,
				.edge_offset=t,
				.subpath=vector<Vertex>(),
				.position=proj.projected,
				.measurement=point
				});
			hypo->total_likelihood = hypo->measurement_likelihood;
			if(!hypotheses) {
				// First round, so don't search for
				// parents
				#pragma omp critical
				{
				new_hypotheses->push_back(hypo);
				}
				continue;
			}

			shared_ptr<PositionHypothesis> best_parent;
			auto best_likelihood = -1.0/0.0;
			auto best_transition = -1.0/0.0;
			vector<Vertex> best_path;

			/* No polymorphic lambdas in C++11 yet :( */
			auto consider_parent = [&] (shared_ptr<PositionHypothesis>& parent, real dist,
					vector<Vertex>& path ) {
				auto parent_likelihood = parent->total_likelihood;
				auto transition = state_model.transition(*parent, *hypo, path, dist);
				auto likelihood = parent_likelihood + transition;
				if(likelihood > best_likelihood) {
					best_parent = parent;
					best_likelihood = likelihood;
					best_transition = transition;
					best_path = path;
				}
			};

			std::unordered_map<Vertex, vector<shared_ptr<PositionHypothesis>> > targets;
			for(auto& prev: *hypotheses) {
				if(prev->edge == hypo->edge) {
					// The graph search doesn't work in the special case
					// where the parent and hypo are the same edge.
					auto real_dist = hypo->edge_offset - prev->edge_offset;
					if(real_dist < 0.0) {
						// Won't go into reverse direction. This is a prime example
						// of the problem that considering only single point in the
						// edge causes!
						continue;
					}
					// No path as it's the same edge
					vector<Vertex> path;
					consider_parent(prev, real_dist, path);
					continue;
				}
				targets[bst::target(prev->edge, graph.graph)].push_back(prev);
			}

			auto visit = [&] (Vertex current, real dist, std::unordered_map<Vertex, Vertex>& successors) {
				dist += hypo->edge_offset;

				// TODO: Make configurable!
				// TODO: With certain transition likelihood functions we could probably
				//	infer when no other hypothesis can win the current best,
				//	allowing for an "optimal" return and probably a lot earlier.
				if(dist > (10.0+measured_dist)*5.0) {
					//std::cout << "Hit the limit :(" << std::endl;
					return false;
				}

				auto it = targets.find(current);
				if(it == targets.end()) return true;
				auto hypo = it->second;
				auto path = build_path_from_successors(
						current, graph.graph, successors);

				for(auto& parent: it->second) {
					auto parent_left = get(bst::edge_weight, graph.graph)[parent->edge];
					parent_left -= parent->edge_offset;
					auto real_dist = dist + parent_left;
					consider_parent(parent, real_dist, path);
				}

				targets.erase(it);

				if(!targets.size()) {
					// All hypotheses found
					return false;
				}

				// Prune parents that can't win
				// TODO: This would prune a lot more if done
				//	for each found node and track the current
				//	worst.
				auto itr = targets.begin();
				while(itr != targets.end()) {
					for(auto& parent: itr->second) {
						auto parent_left = get(bst::edge_weight, graph.graph)[parent->edge];
						parent_left -= parent->edge_offset;
						auto real_dist = dist + parent_left;
						auto best_possible = state_model.best_transition_still_possible(
							measured_dist, real_dist);
						best_possible += parent->total_likelihood;
						if(best_possible >= best_likelihood) {
							goto dontremove;
						}
					}
					itr = targets.erase(itr);
					if(targets.size() == 0) {
						return false;
					}
					continue;

					dontremove:
					++itr;
				}

				return true;
			};

			reverse_shortest_path(
				bst::source(hypo->edge, graph.graph), graph.graph,
				visit
				);
			if(!best_parent) {
				// Not reachable from parents, skipidiskip
				// TODO: Getting here is very expensive. A nicer
				// way would be to simultaneously do multi-target
				// multi-source search (or just parallelise the different searches)
				// and give up when the search starts to look too bad.
				continue;
			}

			hypo->parent = best_parent;
			hypo->transition_likelihood = best_transition;
			hypo->total_likelihood += best_likelihood + best_transition;
			hypo->subpath = best_path;
			#pragma omp critical
			{
			new_hypotheses->push_back(hypo);
			if(hypo->total_likelihood > best_total_likelihood) {
				best_total_likelihood = 0.0;
			}
			}
		}

		if(new_hypotheses->size() == 0) {
			// Found no hypotheses! Let's hope it was an outlier
			// and ignore this round.
			delete new_hypotheses;
			n_outliers += 1;
			return;
		}

		delete hypotheses;
		hypotheses = new_hypotheses;
	}

	void measurement(real ts, Point p) {
		measurement(ts, p.get<0>(), p.get<1>());
	}

	void measurements(const std::vector<real>& ts, const std::vector<Point2d>& points) {
		auto s = ts.size();
		for(int i = 0; i < s; i++) {
			measurement(ts[i], points[i].x, points[i].y);
		}
	}

	std::list<Vertex> route_vertex_path(std::list<shared_ptr<PositionHypothesis> >& path) {
		std::list<Vertex> vertices;

		Vertex prev_inserted;
		auto insert_unique = [&](Vertex vertex) {
			if(vertices.size() and prev_inserted == vertex) return;
			vertices.push_back(vertex);
			prev_inserted = vertex;
		};

		for(auto current: path) {
			if(current->subpath.size() == 0) {
				// If the subpath length is zero, we are
				// on the same edge as the parent, so
				// don't add the source.
				continue;
			}
			for(auto vertex: current->subpath) {
				insert_unique(vertex);
			}
			insert_unique(bst::source(current->edge, graph.graph));
		}

		return vertices;
	}

	std::list<shared_ptr<PositionHypothesis> > get_hypothesis_path(shared_ptr<PositionHypothesis> current) {
		std::list< shared_ptr<PositionHypothesis> > states;
		while(current) {
			states.push_front(current);
			current = current->parent;
		}

		return states;
	}

	std::vector< Point2d > best_match_coordinates() {
		vector< Point2d > path;
		auto current = best_current_hypothesis();
		auto states = get_hypothesis_path(current);
		path.push_back(states.front()->position);
		states.pop_front();

		for(auto vertex: route_vertex_path(states)) {
			auto point = graph.get_vertex_point(vertex);
			path.push_back(point);
		}
		path.push_back(states.back()->position);

		return path;
	}

	std::vector< node_id_t > best_match_node_ids() {
		vector< node_id_t > path;
		auto current = best_current_hypothesis();
		auto states = get_hypothesis_path(current);
		// OSM id's have to be >= 1, so hacking
		// zero to be missing.
		path.push_back(0);
		states.pop_front();

		for(auto vertex: route_vertex_path(states)) {
			auto point = graph.get_vertex_id(vertex);
			path.push_back(point);
		}
		path.push_back(0);

		return path;
	}

	std::shared_ptr<PositionHypothesis> best_current_hypothesis() {
		shared_ptr<PositionHypothesis> current;
		auto max_likelihood = -1.0/0.0;
		// Find the best current state
		for(auto hypo: *hypotheses) {
			if(hypo->total_likelihood < max_likelihood) continue;
			max_likelihood = hypo->total_likelihood;
			current = hypo;
		}

		return current;
	}

	std::vector<std::shared_ptr<PositionHypothesis> > current_hypotheses() {
		return *hypotheses;
	}

};

template<class Rndgen>
std::vector<Point2d> get_random_path_custom(OsmGraph& graph, int n_waypoints, Rndgen& gen) {

	std::vector<Point2d> result;

	auto src = bst::random_vertex(graph.graph, gen);
	for(int i=0; i < n_waypoints; ++i) {
		auto dst = bst::random_vertex(graph.graph, gen);
		auto path = single_target_shortest_path(src, dst, graph.graph);
		if(!path.size()) {
			// Recurse until a path is found
			return get_random_path_custom(graph, n_waypoints, gen);
		}

		for(auto node: path) {
			result.push_back(graph.get_vertex_point(node));
		}

		src = dst;
	}

	return result;
}

std::vector<Point2d> get_random_path(OsmGraph& graph, int n_waypoints=1) {
	std::random_device rd;
	std::mt19937 gen(rd());
	return get_random_path_custom(graph, n_waypoints, gen);
}

std::vector<Point2d> get_shortest_node_path(OsmGraph& graph, node_id_t start, node_id_t end) {
	std::vector<Point2d> result;
	auto s = graph.id_to_vertex[start];
	auto e = graph.id_to_vertex[end];
	for(auto node: single_target_shortest_path(s, e, graph.graph)) {
		result.push_back(graph.get_vertex_point(node));
	}

	return result;
};


#include <Data/DATA.h>
#include <Driver/SIMULATION.h>
#include <Equation/MATRIX_BUNDLE.h>
#include <Equation/NONLINEAR_EQUATION.h>
#include <Evolution/EVOLUTION.h>
#include <Evolution/QUALITY.h>
#include <Force/FORCE.h>
#include <Force/FORCE_TYPE.h>
#include <Utilities/EIGEN_HELPERS.h>
#include <Utilities/LOG.h>
#include <iostream>
#include <vector>
#include <Eigen/IterativeSolvers>
using namespace Mechanics;
///////////////////////////////////////////////////////////////////////
template<class TV> Matrix<typename TV::Scalar,Dynamic,1> NONLINEAR_EQUATION<TV>::
Get_Unknowns(const DATA<TV>& data,const FORCE<TV>& force) const
{
    Matrix<T,Dynamic,1> velocities,forces,unknowns;
    data.Pack_Velocities(velocities);
    STORED_FORCE<T> stored_force;
    force.Pack_Forces(stored_force);
    forces=stored_force.Vector();
    unknowns.resize(velocities.size()+forces.size());
    for(int i=0;i<velocities.size();i++){
        unknowns(i)=velocities(i);}
    for(int i=0;i<forces.size();i++){
        unknowns(i+velocities.size())=forces(i);}
    return unknowns;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Increment_Unknowns(const Matrix<T,Dynamic,1>& unknowns,DATA<TV>& data,FORCE<TV>& force)
{
    int velocity_dof=Velocity_DOF();
    Matrix<T,Dynamic,1> solve_velocities=unknowns.block(0,0,velocity_dof,1);
    STORED_FORCE<T> stored_force;
    force.Pack_Forces(stored_force);
    stored_force.Set(unknowns.block(velocity_dof,0,unknowns.size()-velocity_dof,1));
    force.Increment_Forces(stored_force,1);

    Matrix<T,Dynamic,1> current_velocities;
    data.Pack_Velocities(current_velocities);
    Matrix<T,Dynamic,1> candidate_velocities=current_velocities+solve_velocities;
    Unpack_Velocities(data,candidate_velocities);
    data.Step();
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Identify_DOF(const DATA<TV>& data,const FORCE<TV>& force,int index)
{
    int current_index=0;
    for(int i=0;i<data.size();i++){
        int data_size=kinematic_projection_matrices[i].rows();//=data[i]->Velocity_DOF();
        if(index<current_index+data_size){
            data[i]->Identify_DOF(index-current_index);
            return;}
        current_index+=data_size;
    }
    for(int i=0;i<force.size();i++){
        int force_size=force[i]->DOF();
        if(index<current_index+force_size){
            force[i]->Identify_DOF(index-current_index);
            return;}
        current_index+=force_size;}
    LOG::cout<<"Unidentified DOF"<<std::endl;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Unpack_Velocities(DATA<TV>& data,const Matrix<T,Dynamic,1>& velocities)
{
    assert(data.size()==1);
    Matrix<Matrix<T,Dynamic,1>,Dynamic,1> full_velocities(data.size());
    int current_index=0;
    for(int i=0;i<data.size();i++){
        int compressed_size=kinematic_projection_matrices[i].rows();
        full_velocities[i]=kinematic_projection_matrices[i].transpose()*velocities.block(current_index,0,current_index+compressed_size,1);
        current_index+=compressed_size;
    }
    Matrix<T,Dynamic,1> full_velocity;
    Merge_Block_Vectors(full_velocities,full_velocity);
    data.Unpack_Velocities(full_velocity);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Store_Errors(DATA<TV>& data,const Matrix<T,Dynamic,1>& errors)
{
    assert(data.size()==1);
    Matrix<Matrix<T,Dynamic,1>,Dynamic,1> full_errors(data.size());
    int current_index=0;
    for(int i=0;i<data.size();i++){
        int compressed_size=kinematic_projection_matrices[i].rows();
        full_errors[i]=kinematic_projection_matrices[i].transpose()*errors.block(current_index,0,current_index+compressed_size,1);
        current_index+=compressed_size;
    }
    Matrix<T,Dynamic,1> full_error;
    Merge_Block_Vectors(full_errors,full_error);
    data.Store_Errors(full_error);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Initialize(DATA<TV>& data,FORCE<TV>& force)
{
    kinematic_projection_matrices.resize(data.size());
    for(int i=0;i<data.size();i++){
        data[i]->Kinematic_Projection(kinematic_projection_matrices[i]);}
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Linearize(DATA<TV>& data,FORCE<TV>& force,const T dt,const T time,const bool stochastic)
{
    system.Initialize(data,force);
    for(int i=0;i<data.size();i++){
        data[i]->Inertia(dt,system.jacobian_block_terms[i],system.inverse_inertia_matrices[i],system.error_blocks[i]);}
    for(int i=0;i<force.size();i++){
        force[i]->Identify_Interactions_And_Compute_Errors(data,force,dt,time,system,stochastic);}
    system.Scale_Errors(data,force,kinematic_projection_matrices);
    for(int i=0;i<force.size();i++){
        force[i]->Compute_Derivatives(data,force,system);}
    system.Scale_Derivatives(data,force,kinematic_projection_matrices);
    jacobian.resize(data.Velocity_DOF(),data.Velocity_DOF());
    Merge_Block_Matrices(system.jacobian_blocks,jacobian);
    Merge_Block_Vectors(system.error_blocks,error);

    // build the Hessian
    //SparseMatrix<T> hessian_addition;
    //Merge_Block_Matrices(system.hessian_blocks,hessian_addition);
    //accurate_hessian=jacobian.adjoint()*jacobian+hessian_addition;
    hessian=jacobian.adjoint()*jacobian;
}
///////////////////////////////////////////////////////////////////////
GENERIC_TYPE_DEFINITION(NONLINEAR_EQUATION)


// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// 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.

#include <boost/thread/locks.hpp>

#include "SurgSim/Devices/DeviceFilters/PoseTransform.h"

#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/Matrix.h"

using SurgSim::DataStructures::DataGroup;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Vector3d;

namespace SurgSim
{
namespace Devices
{

SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::PoseTransform, PoseTransform);

PoseTransform::PoseTransform(const std::string& name) :
	DeviceFilter(name),
	m_transform(RigidTransform3d::Identity()),
	m_transformInverse(RigidTransform3d::Identity()),
	m_translationScale(1.0)
{
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(PoseTransform, double, TranslationScale,
		getTranslationScale, setTranslationScale);
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(PoseTransform, RigidTransform3d, Transform, getTransform, setTransform);
}

void PoseTransform::filterInput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
{
	boost::lock_guard<boost::mutex> lock(m_mutex); // Prevent the transform or scaling from being set simultaneously.

	*result = dataToFilter;  // Pass on all the data entries.

	RigidTransform3d pose; // If there is a pose, scale the translation, then transform the result.
	if (dataToFilter.poses().get(DataStructures::Names::POSE, &pose))
	{
		pose.translation() *= m_translationScale;
		pose = m_transform * pose;
		result->poses().set(DataStructures::Names::POSE, pose);
	}

	// If there is a linear velocity, scale then rotate it.  The linear velocity is scaled because it is the change
	// in translation over time, and the translation is being scaled.
	Vector3d linearVelocity;
	if (dataToFilter.vectors().get(DataStructures::Names::LINEAR_VELOCITY, &linearVelocity))
	{
		linearVelocity *= m_translationScale;
		linearVelocity = m_transform.linear() * linearVelocity;
		result->vectors().set(DataStructures::Names::LINEAR_VELOCITY, linearVelocity);
	}

	Vector3d angularVelocity; // If there is an angular velocity, rotate it.
	if (dataToFilter.vectors().get(DataStructures::Names::ANGULAR_VELOCITY, &angularVelocity))
	{
		angularVelocity = m_transform.linear() * angularVelocity;
		result->vectors().set(DataStructures::Names::ANGULAR_VELOCITY, angularVelocity);
	}
}

void PoseTransform::filterOutput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
{
	boost::lock_guard<boost::mutex> lock(m_mutex); // Prevent the transform or scaling from being set simultaneously.

	*result = dataToFilter;  // Pass on all the data entries.

	// Since the haptic devices will compare the data in the output DataGroup to a raw input pose, the filter must
	// perform the reverse transform and scaling to data used by the haptic devices.

	// The force and torque must be transformed into device space.  In order to reliably display the desired
	// forces as calculated by the simulation, the nominal forces and torques are not scaled by the translation scaling.
	// The benefit is that increasing the translation scaling does not result in larger penetrations for the
	// same force, but the downside is that as the translation scaling increases it becomes more likely that the haptic
	// feedback loop at a surface will become "active" (smaller motions penetrating another object are sufficient to
	// create forces ejecting the device's collision representation).  Therefore, a device that is having its
	// translation scaled may required a force scaling filter to reduce the forces.
	Vector3d force;
	if (dataToFilter.vectors().get(DataStructures::Names::FORCE, &force))
	{
		force = m_transformInverse.linear() * force;
		result->vectors().set(DataStructures::Names::FORCE, force);
	}

	Vector3d torque;
	if (dataToFilter.vectors().get(DataStructures::Names::TORQUE, &torque))
	{
		torque = m_transformInverse.linear() * torque;
		result->vectors().set(DataStructures::Names::TORQUE, torque);
	}

	// The Jacobians must be transformed into device space.  The Jacobians are scaled based on the translation scaling,
	// so that the forces and torques displayed by the device will be correct for the scene-space motions, not for the
	// device-space motions.  Let R be the linear rotation portion of our transform, s be the translation scaling
	// factor, and J be the 3x3 upper-left corner of the spring Jacobian.  Then:
	// delta-translation-in-scene = s * R * delta-translation-in-device
	// delta-force-in-scene = J * delta-translation-in-scene
	// So to transform J into the device space, we left-multiply by R^-1, and right-multiply by R.
	// Then, because we want the forces to be scaled as in scene-space, we scale J by s.  The bottom-left 3x3 block
	// in springJacobian (or damperJacobian) also transforms from delta-translation and is treated the same, while the
	// two 3x3 blocks on the right half (of springJacobian or damperJacobian) will be multiplied by the delta-rotation
	// (not delta-translation) and so should not be scaled.
	DataGroup::DynamicMatrixType springJacobian;
	if (dataToFilter.matrices().get(DataStructures::Names::SPRING_JACOBIAN, &springJacobian))
	{
		springJacobian.block<3,3>(0, 0).applyOnTheLeft(m_transformInverse.linear());
		springJacobian.block<3,3>(0, 0).applyOnTheRight(m_transform.linear());
		springJacobian.block<3,3>(3, 0).applyOnTheLeft(m_transformInverse.linear());
		springJacobian.block<3,3>(3, 0).applyOnTheRight(m_transform.linear());
		springJacobian.block<3,3>(0, 3).applyOnTheLeft(m_transformInverse.linear());
		springJacobian.block<3,3>(0, 3).applyOnTheRight(m_transform.linear());
		springJacobian.block<3,3>(3, 3).applyOnTheLeft(m_transformInverse.linear());
		springJacobian.block<3,3>(3, 3).applyOnTheRight(m_transform.linear());
		springJacobian.block<6,3>(0, 0) *= m_translationScale;
		result->matrices().set(DataStructures::Names::SPRING_JACOBIAN, springJacobian);
	}

	RigidTransform3d inputPose;
	if (dataToFilter.poses().get(DataStructures::Names::INPUT_POSE, &inputPose))
	{
		inputPose = m_transformInverse * inputPose;
		inputPose.translation() /= m_translationScale;
		result->poses().set(DataStructures::Names::INPUT_POSE, inputPose);
	}

	DataGroup::DynamicMatrixType damperJacobian;
	if (dataToFilter.matrices().get(DataStructures::Names::DAMPER_JACOBIAN, &damperJacobian))
	{
		damperJacobian.block<3,3>(0, 0).applyOnTheLeft(m_transformInverse.linear());
		damperJacobian.block<3,3>(0, 0).applyOnTheRight(m_transform.linear());
		damperJacobian.block<3,3>(3, 0).applyOnTheLeft(m_transformInverse.linear());
		damperJacobian.block<3,3>(3, 0).applyOnTheRight(m_transform.linear());
		damperJacobian.block<3,3>(0, 3).applyOnTheLeft(m_transformInverse.linear());
		damperJacobian.block<3,3>(0, 3).applyOnTheRight(m_transform.linear());
		damperJacobian.block<3,3>(3, 3).applyOnTheLeft(m_transformInverse.linear());
		damperJacobian.block<3,3>(3, 3).applyOnTheRight(m_transform.linear());
		damperJacobian.block<6,3>(0, 0) *= m_translationScale;
		result->matrices().set(DataStructures::Names::DAMPER_JACOBIAN, damperJacobian);
	}

	Vector3d inputLinearVelocity;
	if (dataToFilter.vectors().get(DataStructures::Names::INPUT_LINEAR_VELOCITY, &inputLinearVelocity))
	{
		inputLinearVelocity = m_transformInverse.linear() * inputLinearVelocity;
		inputLinearVelocity /= m_translationScale;
		result->vectors().set(DataStructures::Names::INPUT_LINEAR_VELOCITY, inputLinearVelocity);
	}

	Vector3d inputAngularVelocity;
	if (dataToFilter.vectors().get(DataStructures::Names::INPUT_ANGULAR_VELOCITY, &inputAngularVelocity))
	{
		inputAngularVelocity = m_transformInverse.linear() * inputAngularVelocity;
		result->vectors().set(DataStructures::Names::INPUT_ANGULAR_VELOCITY, inputAngularVelocity);
	}
}

double PoseTransform::getTranslationScale() const
{
	return m_translationScale;
}

void PoseTransform::setTranslationScale(double translationScale)
{
	boost::lock_guard<boost::mutex> lock(m_mutex);
	m_translationScale = translationScale;
}

const RigidTransform3d& PoseTransform::getTransform() const
{
	return m_transform;
}

void PoseTransform::setTransform(const RigidTransform3d& transform)
{
	boost::lock_guard<boost::mutex> lock(m_mutex);
	m_transform = transform;
	m_transformInverse = m_transform.inverse();
}

};  // namespace Devices
};  // namespace SurgSim


#include <iostream>

#include <boost/program_options.hpp>

#include "functionality.hpp"
#include "functionality_eigen.hpp"

namespace po = boost::program_options;

int main() {

    std::cout << "Checkpoint 3" << std::endl;
    std::cout << "Hypergeometric probability: " << hypergeometricPmf(10'000, 4'270, 300, 128) << std::endl;
    std::cout << "Random matrix: " << std::endl;
    printRandomMatrix();

    // Use boost program options
    {
        po::options_description desc("Allowed options");
        desc.add_options()
                ("help", "produce help message")
                ("compression", po::value<int>(), "set compression level");

        po::variables_map vm;

        std::cout << desc << std::endl;
    }

    // int unused_variable = 0;

    return 0;
}


// External libraries
#include <boost/property_tree/ptree.hpp>

// Local libraries
#include <fmath/RungeKutta.h>
#include <fparameters/SpaceIterator.h>
#include <fparameters/Dimension.h>
#include <fparameters/parameters.h>

// Local headers
#include "write.h"
#include "State.h"
#include "modelParameters.h"
#include "globalVariables.h"
#include "adafFunctions.h"

//namespace {
inline double safeLog10( double x )	{ return x > 0.0 ? log10(x) : -100.0; }
//}
std::string dataName(std::string id) {
	return id + ".dat";
}

void generateViewScript(std::string path) {
	std::string filename = path.substr(path.find("\\") + 1);
	std::string folder = path.substr(0,path.find("\\"));
	std::ofstream file;
	file.open((folder+"/plots/plot-"+filename+".bat").c_str(), std::ios::out);
	file << "@../../plot-svg-and-view.bat " + filename;
	file.close();
}

void writeAllSpaceParam(const std::string& filename, const ParamSpaceValues& data)
{
	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);
	//const ParamSpace* a = &(data.ps);

	// a.iterate;  no me deja hacer esta operacion

	data.ps.iterate([&file, &data](const SpaceIterator& i){
		double logE = log10(i.val(DIM_E) / 1.6e-12);
		double logR = (i.val(DIM_R));
		double logT = (i.val(DIM_Rcd));
		double logQ = safeLog10(data.get(i)); //log10(salida.values(i));  // values(i));
//		salida.values(i);

		file << logE << '\t' << logR << '\t' << logT << '\t' <<
			logQ << std::endl;
			//logQ << std::endl;
	});

	file.close();
	generateViewScript(filename);
}

void writeEandRParamSpace(const std::string& filename, const ParamSpaceValues& data, int t,int vol)
{

	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);

	// version acotada
	//double time = log10(data.ps[1][t]);

	data.ps.iterate([&](const SpaceIterator& i) {

		double logE = log10(i.val(DIM_E) / EV_TO_ERG);
		double r = i.val(DIM_R);
		double voll = (vol == 1) ? volume(r) : 1.0;
		double logQ = safeLog10(data.get(i)*voll);
		file << logE << '\t' << i.coord[DIM_R] << '\t' << safeLog10(r/schwRadius) << '\t' << logQ << std::endl;

	}, { -1, -1, t });

	file.close();
	generateViewScript(filename);
}

void writeRParamSpace(const std::string& filename, const ParamSpaceValues& data, int t, int s)
{

	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);
	double Emin = data.ps[DIM_E].first();
	double Emax = data.ps[DIM_E].last();
	data.ps.iterate([&](const SpaceIterator& iR) {
		double tot = integSimpson(log(Emin),log(Emax),
						[&](double loge)
						{
							double e = exp(loge);
							double nPh = data.interpolate({{DIM_E,e}},&iR.coord);
							return e*nPh;
						},100);
		file << safeLog10(iR.val(DIM_R)/schwRadius) << '\t' << safeLog10(tot) << std::endl;
	},{t,-1,s});
	file.close();
	generateViewScript(filename);
}

void writeEandTParamSpace(const std::string& filename, const ParamSpaceValues& data, int r)
{

	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);

	// version acotada
	double logR = log10(data.ps[1][r]);

	file << "log(r)=" << logR << '\t' ;

	for (size_t t_ix = 0; t_ix < data.ps[2].size(); t_ix++) {
		double time = data.ps[2][t_ix];
		file << "t=" << log10(time) << '\t';
	}

	for (int E_ix = 0; E_ix < data.ps[0].size(); E_ix++) {

		file << std::endl;

		double logE = log10(data.ps[0][E_ix]/1.6e-12);

		file << logE << '\t';

		data.ps.iterate([&file, &data](const SpaceIterator& i){

			//double logR = log10(i.val(DIM_R));
			//double time = i.val(DIM_T);
			double logQ = safeLog10(data.get(i));

			file << logQ << '\t';
			;
		}, { E_ix, r, -1 });  //el -1 indica que las E se recorren, no quedan fijas
		//las otras dos dimensiones quedan fijas en las posiciones r y t (recordar que la primera es 0 )
	}
	file.close();
	generateViewScript(filename);
}

void writeRandTParamSpace(const std::string& filename, const ParamSpaceValues& data, int E)
{

	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);

	data.ps.iterate([&file, &data](const SpaceIterator& i){

		double r = i.val(DIM_R);
		//double theta = i.val(DIM_THETA);

		file << r << '\t' << '\t' << data.get(i) << std::endl;

	}, { E, -1, -1 });  //el -1 indica que las E se recorren, no quedan fijas
	//las otras dos dimensiones quedan fijas en las posiciones r y t (recordar que la primera es 0 )

	file.close();
	generateViewScript(filename);
}

void writeEnergyFunction(const std::string& filename, const ParamSpaceValues& data, int r, int t)
{

	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);

	// version acotada
	double logR = log10(data.ps[1][r]);
	// version larga
	double logT = log10(data.ps.dimensions[2]->values[t]);

	file << "log(r)=" << logR << '\t' << "log(t)=" << logT << std::endl;
	data.ps.iterate([&file, &data](const SpaceIterator& i){

		double logE = log10(i.val(DIM_E) / 1.6e-12);
		double logQ = safeLog10(data.get(i));

		file << logE << '\t' << logQ << std::endl;
		;
	}, { -1, r, t });  //el -1 indica que las E se recorren, no quedan fijas
	//las otras dos dimensiones quedan fijas en las posiciones r y t (recordar que la primera es 0 )

	file.close();
	generateViewScript(filename);
}

void writeMatrix(const std::string& filename, Particle& p, Matrix& a)
{
	std::ofstream file;
	file.open(dataName(filename).c_str(), std::ios::out);

	int nR = p.ps[DIM_R].size();  //ver el -1

	file << '\t';

	for (size_t z_j = 0; z_j < nR; z_j++) {
		const double r_j = p.ps[DIM_R][z_j];
		file << r_j << '\t' ;
	}

	file << std::endl;

	for (size_t z_i = 0; z_i < nR; z_i++) {

		const double r_i = p.ps[DIM_R][z_i];

		file << r_i << '\t';

		for (size_t z_j = 0; z_j < nR; z_j++) {
			const double r_j = p.ps[DIM_R][z_j];

			file << a[z_i][z_j] << '\t' ;
		}

		file << std::endl;
	}

	file.close();
	generateViewScript(filename);
}

void writeFields(State& st) {
	std::ofstream fields;
	fields.open("fields.dat",std::ios::out);
	fields		<< "r_in [Rs] = " << exp(logr.front()) << endl;
	fields		<< "r_out [Rs] = " << exp(logr.back()) << endl << endl;
	fields  << "rB1 [Rs]" 	<< "\t"
			<< "r [Rs]" 	<< "\t"
			<< "rB2 [Rs]" 	<< "\t"
			<< "MdotRIAF" 	<< "\t"
			<< "MdotCD" 	<< "\t"
			<< "Te"			<< "\t"
			<< "Ti"			<< "\t"
			<< "H/R"		<< "\t"
			<< "ne"			<< "\t"
			<< "v/c"		<< "\t"
			<< "B"			<< endl;

	st.photon.ps.iterate([&](const SpaceIterator& iR) {
		double r = iR.val(DIM_R);
		fields 	<< r/schwRadius/sqrt(paso_r) << "\t"
				<< r/schwRadius << "\t"
				<< r/schwRadius * sqrt(paso_r) << "\t"
				<< gAcc(r) << "\t"
				<< accRateColdDisk(r)/accRateOut << "\t"
				<< st.tempElectrons.get(iR) << "\t"
				<< st.tempIons.get(iR) << "\t"
				<< height_fun(r)/r << "\t"
				<< electronDensity(r) << "\t"
				<< abs(radialVel(r))/cLight <<"\t"
				<< st.magf.get(iR) << endl;
	},{0,-1,0});
	fields.close();

	std::ofstream SSD;
	SSD.open("SSD.dat",std::ios::out);
	SSD		<< "r_tr [Rs] = " << rTr/schwRadius << endl;
	SSD		<< "r_outCD [Rs] = " << rOutCD/schwRadius << endl << endl;
	SSD  	<< "rB1 [Rs]"			<< "\t"
			<< "r [Rs]" 			<< "\t"
			<< "rB2 [Rs]"			<< "\t"
			<< "MdotCD/MdotOut" 	<< endl;

	st.photon.ps.iterate([&](const SpaceIterator& iRcd) {
		double rCD = iRcd.val(DIM_Rcd);
		SSD 	<< rCD/sqrt(paso_rCD)/schwRadius << "\t"
				<< rCD/schwRadius << "\t"
				<< rCD*sqrt(paso_rCD)/schwRadius << "\t"
				<< accRateColdDisk(rCD)/accRateOut << endl;
	},{0,0,-1});
	SSD.close();
}

using namespace H5;

float *toFloatArray(const double *arr, size_t n) {
  float *ret = new float[n];
  for (int i = 0; i < n; i++) {
    ret[i] = float(arr[i]);
  }
  return ret;
}

void MultiplyVectorByScalar(Vector v, double s, Vector& w) {
	// Multiply vector v by scalar s and stores the data in w
    transform(v.begin(), v.end(), w.begin(), [s](double &x){ return x*s; });
}

void DivideVectorByVector(Vector v1, Vector v2, Vector& w) {
	// Multiply vector v by scalar s and stores the data in w
    transform(v1.begin(), v1.end(), v2.begin(), w.begin(), divides<double>());
}

void EvaluateFunctionInVector(fun1 f, Vector v, Vector& w) {
	// Evaluate f in vector v and stores the data in w
    transform(v.begin(), v.end(), w.begin(), [f](double &x){ return f(x); });
}

void myH5_write_single_float(const double x, H5File &h5f, H5std_string DATASET_NAME)
{
	float y[1];	*y = float(x);
	hsize_t dims[1] = { 1 };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5f.createDataSet(DATASET_NAME, PredType::NATIVE_FLOAT, dataspace);
	dataset.write(y, PredType::NATIVE_FLOAT);
}

void myH5_write_single_float(const double x, Group &h5g, H5std_string DATASET_NAME)
{
	float y[1];	*y = float(x);
	hsize_t dims[1] = { 1 };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5g.createDataSet(DATASET_NAME, PredType::NATIVE_FLOAT, dataspace);
	dataset.write(y, PredType::NATIVE_FLOAT);
}

void myH5_write_1d_float(const Vector v, H5File &h5f, H5std_string DATASET_NAME)
{
	size_t n = v.size();
	hsize_t dims[1] = { n };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5f.createDataSet(DATASET_NAME, PredType::NATIVE_FLOAT, dataspace);
	dataset.write(toFloatArray(v.data(), n), PredType::NATIVE_FLOAT);
}

void myH5_write_1d_float(const Vector v, Group &h5g, H5std_string DATASET_NAME)
{
	size_t n = v.size();
	hsize_t dims[1] = { n };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5g.createDataSet(DATASET_NAME, PredType::NATIVE_FLOAT, dataspace);
	dataset.write(toFloatArray(v.data(), n), PredType::NATIVE_FLOAT);
}

void myH5_write_single_double(const double x, H5File &h5f, H5std_string DATASET_NAME)
{
	float y[1];	*y = float(x);
	hsize_t dims[1] = { 1 };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5f.createDataSet(DATASET_NAME, PredType::NATIVE_DOUBLE, dataspace);
	dataset.write(y, PredType::NATIVE_DOUBLE);
}

void myH5_write_single_double(const double x, Group &h5g, H5std_string DATASET_NAME)
{
	double y[1];	*y = double(x);
	hsize_t dims[1] = { 1 };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5g.createDataSet(DATASET_NAME, PredType::NATIVE_DOUBLE, dataspace);
	dataset.write(y, PredType::NATIVE_DOUBLE);
}

void myH5_write_1d_double(const Vector v, H5File &h5f, H5std_string DATASET_NAME)
{
	size_t n = v.size();
	hsize_t dims[1] = { n };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5f.createDataSet(DATASET_NAME, PredType::NATIVE_DOUBLE, dataspace);
	dataset.write(v.data(), PredType::NATIVE_DOUBLE);
}

void myH5_write_1d_double(const Vector v, Group &h5g, H5std_string DATASET_NAME)
{
	size_t n = v.size();
	hsize_t dims[1] = { n };
	DataSpace dataspace(1, dims);
	DataSet dataset = h5g.createDataSet(DATASET_NAME, PredType::NATIVE_DOUBLE, dataspace);
	dataset.write(v.data(), PredType::NATIVE_DOUBLE);
}

int createH5file(const H5std_string FILE_NAME, const State& st)
{
	// Try block to detect exceptions raised by any of the calls inside it
    try {
        // Turn off the auto-printing when failure occurs so that we can
        // handle the errors appropriately
        Exception::dontPrint();

        // Create a new file using default property lists.
        H5File h5file(FILE_NAME, H5F_ACC_TRUNC);

		// HEADER ///////////////////////////////////////////////////////////////////////
		//Group *header_id = new Group(h5file->createGroup("Header"));
		Group header_id = h5file.createGroup("Header");
		myH5_write_single_float(blackHoleMass/solarMass, header_id, "M");
		myH5_write_single_float(accRateOut / (1.39e18 * blackHoleMass/solarMass), header_id, "Mdot_out");
		myH5_write_single_float(s, header_id, "sWind");
		myH5_write_single_float(delta, header_id, "delta_e");
		myH5_write_single_float(magFieldPar, header_id, "beta");
		myH5_write_single_float(alpha, header_id, "alpha");
		myH5_write_single_float(jAngMom, header_id, "jAngMom");

		//delete header_id;
		/////////////////////////////////////////////////////////////////////////////////

		// RIAF /////////////////////////////////////////////////////////////////////////

		Group RIAF_id = h5file.createGroup("RIAF");

		Vector rVec = st.photon.ps.dimensions[DIM_R]->values;
		Vector w(nR, 0.0);

		MultiplyVectorByScalar(rVec, 1.0/schwRadius, w);
		myH5_write_1d_float(w, RIAF_id, "r");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector([](double r) { return volume(r)/P3(schwRadius); }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "volume");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector([](double r) { return accRateADAF(r)/accRateOut; }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "Mdot");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector([](double r)
					{ return boltzmann/electronRestEnergy * electronTemp(r); }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "Te");

		EvaluateFunctionInVector([](double r)
					{ return boltzmann/protonRestEnergy * ionTemp(r); }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "Ti");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector([](double r) { return height_fun(r)/r; }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "H_R");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector(electronDensity, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "n_e");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector([](double r) {return abs(radialVel(r))/cLight; }, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "v_c");
		w.assign(w.size(), 0.0);

		EvaluateFunctionInVector(magneticField, rVec, w);
		myH5_write_1d_float(w, RIAF_id, "B");

		myH5_write_1d_float(redshift_to_inf, RIAF_id, "redshift_to_inf");

		RIAF_id.close();

		Group SSD_id = h5file.createGroup("SSD");
		SSD_id.close();

		Group Particles_id = h5file.createGroup("Particles");
		for (const auto &p : st.particles) {
   			Group p_id = Particles_id.createGroup(p->id);
			hsize_t dims[2] = { nR, nE };
			DataSpace dataspace(2, dims);
			p_id.createDataSet("Injection", PredType::NATIVE_FLOAT, dataspace);
			p_id.createDataSet("Distribution", PredType::NATIVE_FLOAT, dataspace);
		}
		Particles_id.close();
		h5file.close();

    } // end of try block
    // catch failure caused by the H5File operations
    catch (FileIException error) {
        error.printErrorStack();
        return -1;
    }
    // catch failure caused by the Group operations
    catch (GroupIException error) {
        error.printErrorStack();
        return -1;
    }
	catch (DataSetIException error) {
        error.printErrorStack();
        return -1;
    }
    // catch failure caused by the DataSpace operations
    catch (DataSpaceIException error) {
        error.printErrorStack();
        return -1;
    }
	return 0;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================

/*
  This file implements the function

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
            class P, class T, class R>
  bool
  johnson_all_pairs_shortest_paths
    (VertexAndEdgeListGraph& g,
     DistanceMatrix& D,
     const bgl_named_params<P, T, R>& params)
 */

#ifndef BOOST_GRAPH_JOHNSON_HPP
#define BOOST_GRAPH_JOHNSON_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

namespace boost {

  namespace detail {

    template <class VertexAndEdgeListGraph, class DistanceMatrix,
              class P, class T, class R, class VertexID, class Distance,
              class Weight, class Weight2, class DistanceZero>
    bool
    johnson_impl(VertexAndEdgeListGraph& g,
                 DistanceMatrix& D,
                 const bgl_named_params<P, T, R>& params,
                 VertexID id, Distance d, Weight w, Weight2 w_hat,
                 DistanceZero zero)
    {
      typedef graph_traits<VertexAndEdgeListGraph> Traits;
      typename Traits::vertex_iterator v, v_end, u, u_end;
      typename Traits::edge_iterator e, e_end;
      typename Traits::vertex_descriptor s = *vertices(g).first;
      typedef typename property_traits<Distance>::value_type DT;
      function_requires< BasicMatrixConcept<DistanceMatrix,
        typename Traits::vertex_descriptor, DT> >();

      std::vector<DT> h_vec(num_vertices(g));
      typedef typename std::vector<DT>::iterator iter_t;
      iterator_property_map<iter_t, VertexID, DT, DT&> h(h_vec.begin(), id);

      put(d, s, zero);
      if (bellman_ford_shortest_paths(g, num_vertices(g), params)) {
        for (tie(v, v_end) = vertices(g); v != v_end; ++v)
          put(h, *v, get(d, *v));
        for (tie(e, e_end) = edges(g); e != e_end; ++e)
          put(w_hat, *e,
              get(w, *e) + get(h, source(*e,g)) - get(h, target(*e,g)));
        for (tie(u, u_end) = vertices(g); u != u_end; ++u) {
          dijkstra_shortest_paths(g, *u, params.weight_map(w_hat));
          for (tie(v, v_end) = vertices(g); v != v_end; ++v)
            D[*u][*v] = get(d, *v) + get(h, *v) - get(h, *u);
        }
        return true;
      } else
        return false;
    }

    template <class VertexAndEdgeListGraph, class DistanceMatrix,
              class P, class T, class R, class Weight,
              class VertexID>
    bool
    johnson_dispatch(VertexAndEdgeListGraph& g,
                     DistanceMatrix& D,
                     const bgl_named_params<P, T, R>& params,
                     Weight w, VertexID id)
    {
      typedef typename property_traits<Weight>::value_type WT;
      typename std::vector<WT>::size_type
        n = is_default_param(get_param(params, vertex_distance))
        ? num_vertices(g) : 1;
      std::vector<WT> distance_map(n);

      return detail::johnson_impl
        (g, D, params, id,
         choose_param(get_param(params, vertex_distance),
                      make_iterator_property_map
                      (distance_map.begin(), id, distance_map[0])),
         w,
         choose_pmap(get_param(params, edge_weight2), g, edge_weight2),
         choose_param(get_param(params, distance_zero_t()),
                      WT()) );
    }

  } // namespace detail

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
            class P, class T, class R>
  bool
  johnson_all_pairs_shortest_paths
    (VertexAndEdgeListGraph& g,
     DistanceMatrix& D,
     const bgl_named_params<P, T, R>& params)
  {
    return detail::johnson_dispatch
      (g, D, params,
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
       choose_const_pmap(get_param(params, vertex_index), g, vertex_index)
       );
  }

  template <class VertexAndEdgeListGraph, class DistanceMatrix>
  bool
  johnson_all_pairs_shortest_paths
    (VertexAndEdgeListGraph& g, DistanceMatrix& D)
  {
    bgl_named_params<int,int> params(1);
    return detail::johnson_dispatch
      (g, D, params, get(edge_weight, g), get(vertex_index, g));
  }

} // namespace boost

#endif // BOOST_GRAPH_JOHNSON_HPP


/*
 *            Copyright 2009-2017 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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 "A_ol I_ol" BA_olI_ol,
 * WITHOUT WARRANTIE_ol OR CONDITION_ol OF ANY KIND, either express or implied.
 * _olee the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
// Overload of uBLAS prod function with MKL/GSL implementations
#include <votca/tools/linalg.h>

#include <boost/math/constants/constants.hpp>
#include "votca/xtp/radial_euler_maclaurin_rule.h"
#include "votca/xtp/aobasis.h"
#include <votca/xtp/aomatrix.h>

namespace votca { namespace xtp {

    std::vector<double> EulerMaclaurinGrid::getPruningIntervals(string element){

        std::vector<double> _r;

        // get Bragg-Slater Radius for this element
        double BSradius = _BraggSlaterRadii.at(element);

        // row type of element
        int RowType = _pruning_set.at(element);

        if ( RowType == 1 ){

            _r.push_back( 0.25 * BSradius );
            _r.push_back( 0.5 * BSradius );
            _r.push_back( 1.0 * BSradius );
            _r.push_back( 4.5 * BSradius );

        } else if ( RowType == 2 ){

            _r.push_back( 0.1667 * BSradius );
            _r.push_back( 0.5 * BSradius );
            _r.push_back( 0.9 * BSradius );
            _r.push_back( 3.5 * BSradius );

        } else if ( RowType == 3 ) {

            _r.push_back( 0.1 * BSradius );
            _r.push_back( 0.4 * BSradius );
            _r.push_back( 0.8 * BSradius );
            _r.push_back( 2.5 * BSradius );

        } else {

            cerr << "Pruning unsupported for RowType " << RowType << endl;
            exit(1);
        }

        return _r;
     }

void EulerMaclaurinGrid::getRadialCutoffs(std::vector<ctp::QMAtom* > _atoms, BasisSet* bs, string gridtype) {

            map<string, min_exp>::iterator it;
            std::vector< ctp::QMAtom* > ::iterator ait;
            std::vector< ctp::QMAtom* > ::iterator bit;

            double eps = Accuracy[gridtype];
            double _decaymin;
            int _lvalue;
            //  cout << endl << " Setting cutoffs for grid type " << gridtype << " eps = " << eps << endl;
            // 1) is only element based
            // loop over atoms
            for (ait = _atoms.begin(); ait < _atoms.end(); ++ait) {
                // get element type of the atom
                string name = (*ait)->type;
                // is this element already in map?
                it = _element_ranges.find(name);
                // only proceed, if element data does not exist yet
                if (it == _element_ranges.end()) {
                    // get first range estimate and add to map
                    min_exp this_atom;
                    double range_max = 0.0;
                    // get the basis set entry for this element
                    Element* _element = bs->getElement(name);
                    // and loop over all shells to figure out minimum decay constant and angular momentum of this function
                    for (Element::ShellIterator its = _element->firstShell(); its != _element->lastShell(); its++) {
                        Shell* shell = (*its);
                        _decaymin = 1e7;
                        _lvalue = 0;
                        int _lmax = shell->getLmax();
                        for (Shell::GaussianIterator itg = shell->firstGaussian(); itg != shell->lastGaussian(); itg++) {
                            GaussianPrimitive* gaussian = *itg;
                            double _decay = gaussian->decay;
                            if (_decay < _decaymin) {
                                _decaymin = _decay;
                                _lvalue = _lmax;
                            }
                        }
                        double range = DetermineCutoff(2 * _decaymin, 2 * _lvalue + 2, eps);
                        if (range > range_max) {
                            this_atom.alpha = _decaymin;
                            this_atom.l = _lvalue;
                            this_atom.range = range;
                            range_max = range;
                        }
                    } // shells

                    //   cout << "Element " << name << " alpha " << this_atom.alpha << " l " << this_atom.l << " Rcut " << this_atom.range << endl;
                    _element_ranges[name] = this_atom;
                } // new element
            } // atoms

            // calculate overlap matrix
            AOBasis aobasis;
            aobasis.AOBasisFill(bs, _atoms);
            AOOverlap _overlap;
            // Fill overlap
            _overlap.Fill(aobasis);

            // refining by going through all atom combinations
            // get collapsed index list
            int atidx = 0;
            std::vector<int> idxstart;
            std::vector<int> idxstop;
            int start = 0;
            int end = 0;
            for (AOBasis::AOShellIterator _row = aobasis.firstShell(); _row != aobasis.lastShell(); _row++) {

                const AOShell* _shell_row = aobasis.getShell(_row);

                if (_shell_row->getIndex() == atidx) {

                    end += _shell_row->getNumFunc();
                } else {

                    idxstart.push_back(start);
                    idxstop.push_back(end);
                    atidx++;
                    start = end;
                    end += _shell_row->getNumFunc();
                }
            }

            idxstart.push_back(start);
            idxstop.push_back(end);

            int aidx = 0;

            for (ait = _atoms.begin(); ait < _atoms.end(); ++ait) {

                int _a_start = idxstart[aidx];
                int _a_stop = idxstop[aidx];

                double range_max = 0.0;

                // get preset values for this atom type
                double exp_iat = _element_ranges.at((*ait)->type).alpha;
                int l_iat = _element_ranges.at((*ait)->type).l;

                vec pos_a = (*ait)->getPos() * tools::conv::bohr2ang;

                string type_diff;
                int bidx = 0;
                for (bit = _atoms.begin(); bit < _atoms.end(); ++bit) {

                    int _b_start = idxstart[bidx];
                    int _b_stop = idxstop[bidx];
                    vec pos_b = (*bit)->getPos() * tools::conv::bohr2ang;
                    // now do some overlap gymnastics
                    double s_max = 10.0;
                    if (aidx != bidx) {

                        // find overlap block of these two atoms
                        ub::matrix<double> _overlapblock = ub::project(_overlap.Matrix(), ub::range(_a_start, _a_stop), ub::range(_b_start, _b_stop));
                        // determine abs max of this block
                        s_max = 0.0;
                        for (unsigned i = 0; i < _overlapblock.size1(); i++) {
                            for (unsigned j = 0; j < _overlapblock.size2(); j++) {
                                s_max = std::max(s_max, std::abs(_overlapblock(i, j)));
                            }
                        }
                    }

                    if (s_max > 1e-5) {
                        // cout << " Atom " << aidx << " neighbor " << bidx << " s-max " << s_max << " exponents " << exp_iat << " and " << _element_ranges.at((*bit)->type).alpha << endl;
                        double range = DetermineCutoff(exp_iat + _element_ranges.at((*bit)->type).alpha, l_iat + _element_ranges.at((*bit)->type).l + 2, eps);
                        // now do some update trickery from Gaussian product formula
                        double dist = abs(pos_b - pos_a);
                        double shift_2g = dist * exp_iat / (exp_iat + _element_ranges.at((*bit)->type).alpha);
                        range += shift_2g;

                        if (aidx != bidx) range += dist;

                        if (range > range_max) {
                            //shiftm_2g = shift_2g;
                            range_max = range;
                            //iat_diff = bidx;
                            type_diff = (*bit)->type;
                        }

                    }

                    bidx++;
                }

                if (round(range_max) > _element_ranges.at((*ait)->type).range) {
                    _element_ranges.at((*ait)->type).range = round(range_max);
                }
                aidx++;
            }

          return;
        } // getRadialCutoffs

    void EulerMaclaurinGrid::getRadialGrid(BasisSet* bs , std::vector<ctp::QMAtom* > _atoms, string type, GridContainers& _grid) {

            map<string, min_exp>::iterator it;
            getRadialCutoffs(_atoms,bs,type);

            // go through all elements
            for ( it = _element_ranges.begin() ; it != _element_ranges.end() ; ++it){

                 // cout << "Element " << it->first << " alpha " << it->second.alpha << " l " << it->second.l << " Rcut " << it->second.range <<  " Rcut (Ang) " <<  it->second.range  * 0.529177249 << endl;

                std::vector<double> points;
                std::vector<double> weights;
                int numberofpoints = getGrid(it->first, type);
                //cout << " Setting grid for element " << it->first <<  " with " << numberofpoints << " points " <<  endl ;
                setGrid( numberofpoints, it->second.range, points, weights );

                //grid_element this_element;
                //this_element.gridpoint = points;
                //this_element.weight = weights;

                //_element_grids[it->first] = this_element;

                _grid._radial_grids[it->first].radius = points;
                _grid._radial_grids[it->first].weight = weights;

            }

    }

    void EulerMaclaurinGrid::setGrid(int np, double cutoff, std::vector<double>& point, std::vector<double>& weight ){

        double alpha = -cutoff/(log(1.0 - pow(  (1.0 + double(np))/(2.0 + double(np)),3)) );

        double factor = 3.0/(1.0+double(np));

        for ( int i = 0; i < np; i++){
            double q = double(i+1)/(double(np)+1.0);
            double r = -alpha*log(1.0-pow(q,3));
            double w = factor * alpha * r*r/( 1.0 - pow(q,3) ) * pow(q,2);

            point.push_back(r);
            weight.push_back(w);

        }

    }

    double EulerMaclaurinGrid::DetermineCutoff(double alpha, int l, double eps){

      // determine norm of function

     /* For a function f(r) = r^k*exp(-alpha*r^2) determine
        the radial distance r such that the fraction of the
        function norm that is neglected if the 3D volume
        integration is terminated at a distance r is less
        than or equal to eps. */

        double _cutoff    = 1.0; // initial value
        double _increment = 0.5; // increment

            while (_increment > 0.01) {
                double _neglected = getNeglected(alpha,l ,  _cutoff);
                // cout << "neglected is " << _neglected << endl;
                if (_neglected > eps) {
                    _cutoff += _increment;
                } else {
                    _cutoff -= _increment;
                    if (_cutoff < 0.0) _cutoff = 0.0;
                    _increment = 0.5 * _increment;
                    _cutoff += _increment;
                }
            }

        return _cutoff;

    }

    double EulerMaclaurinGrid::getNeglected(double alpha, int l, double cutoff){

        return RadialIntegral(alpha,l+2,cutoff) / RadialIntegral(alpha,l+2, 0.0);

    }

    double EulerMaclaurinGrid::RadialIntegral(double alpha, int l, double cutoff){

        const double pi = boost::math::constants::pi<double>();
        int ilo = l % 2;
        double value = 0.0;
        double valexp;
        if ( ilo == 0 ){
            double expo = sqrt(alpha)*cutoff;
            if ( expo > 40.0 ) {
                value = 0.0;
            } else {
                value = 0.5 * sqrt(  pi /alpha  ) * erfc(expo);
            }
        }

        double exponent = alpha*cutoff*cutoff;
        if ( exponent > 500.0 ) {
            valexp = 0.0;
            value = 0.0;
        } else {
            valexp = exp(-exponent);
            value = valexp/2.0/alpha;
        }

        for (  int i = ilo+2; i <= l; i+=2){
            value = ((i-1)*value + pow(cutoff,i-1)*valexp)/2.0/alpha;
        }

        return value;

    }

    int EulerMaclaurinGrid::getGrid(string element, string type){

        if ( type == "medium"){

            return MediumGrid.at(element);

        }
        else if ( type == "coarse"){

            return CoarseGrid.at(element);

        }
        else if ( type == "xcoarse"){

            return XcoarseGrid.at(element);

        }
        else if ( type == "fine"){

            return FineGrid.at(element);

        }
        else if ( type == "xfine"){

            return XfineGrid.at(element);

        }

        throw std::runtime_error("Grid type "+type+" is not implemented");
        return -1;

    }

}
}