#pragma once

#include "wmtk/TetMesh.h"
#include "wmtk/utils/GeoUtils.h"

// clang-format off
#include <wmtk/utils/DisableWarnings.hpp>
#include <tbb/concurrent_map.h>
#include <wmtk/utils/EnableWarnings.hpp>
// clang-format on

#include <Eigen/Core>
#include <array>
#include <vector>

namespace wmtk {
/**
 * @brief Before triangle insertion, find which ones are already present in the mesh.
 * Note that the vertices are already the same, so just do a dictionary-find for the face
 * indices.
 * @param faces
 * @param output is_matched
 */
void match_tet_faces_to_triangles(
    const wmtk::TetMesh& m,
    const std::vector<std::array<size_t, 3>>& faces,
    tbb::concurrent_vector<bool>& is_matched,
    tbb::concurrent_map<std::array<size_t, 3>, std::vector<int>>& tet_face_tags);

bool remove_duplicates(
    std::vector<Eigen::Vector3d>& vertices,
    std::vector<std::array<size_t, 3>>& faces,
    double);
} // namespace wmtk

namespace wmtk {

template <typename rational>
auto triangle_insert_prepare_info(
    const wmtk::TetMesh& m,
    const std::array<size_t, 3>& face_v,
    std::vector<std::array<size_t, 3>>& marking_tet_faces,
    const std::function<bool(const std::array<size_t, 3>&)>& try_acquire_triangle,
    const std::function<bool(const std::vector<wmtk::TetMesh::Tuple>&)>& try_acquire_tetra,
    const std::function<Eigen::Matrix<rational, 3, 1>(size_t)>& vertex_pos_r)
{
    using Vector3r = Eigen::Matrix<rational, 3, 1>;
    using Vector2r = Eigen::Matrix<rational, 2, 1>;
    using Tuple = wmtk::TetMesh::Tuple;
    constexpr int EMPTY_INTERSECTION = 0;
    constexpr int TRI_INTERSECTION = 1;
    constexpr int PLN_INTERSECTION = 2;

    static constexpr std::array<std::array<int, 2>, 6> map_leid2lfids = {
        {{{0, 2}}, {{0, 3}}, {{0, 1}}, {{1, 2}}, {{2, 3}}, {{1, 3}}}};
    static constexpr std::array<std::array<int, 3>, 4> local_faces = {
        {{{0, 1, 2}}, {{0, 2, 3}}, {{0, 1, 3}}, {{1, 2, 3}}}};
    static constexpr std::array<std::array<int, 2>, 6> local_edges = {
        {{{0, 1}}, {{1, 2}}, {{0, 2}}, {{0, 3}}, {{1, 3}}, {{2, 3}}}};

    bool success_flag = false;
    std::set<size_t> visited;

    std::array<Vector3r, 3> tri = {
        vertex_pos_r(face_v[0]),
        vertex_pos_r(face_v[1]),
        vertex_pos_r(face_v[2])};
    //
    Vector3r tri_normal = (tri[1] - tri[0]).cross(tri[2] - tri[0]);
    //
    std::array<Vector2r, 3> tri2;
    int squeeze_to_2d_dir = wmtk::project_triangle_to_2d(tri, tri2);

    std::vector<Tuple> intersected_tets;
    std::map<std::array<size_t, 2>, std::tuple<int, Vector3r, size_t, int>> map_edge2point;
    std::map<std::array<size_t, 3>, bool> map_face2intersected;
    // e = (e0, e1) ==> (intersection_status, intersection_point, edge's_tid,
    // local_eid_in_tet)
    std::set<std::array<size_t, 2>> intersected_tet_edges;
    //
    std::queue<Tuple> tet_queue;

    if (!try_acquire_triangle(face_v)) {
        return std::tuple(
            success_flag,
            std::vector<Tuple>(),
            std::vector<Tuple>(),
            std::vector<Vector3r>());
    }

    for (int j = 0; j < 3; j++) {
        auto loc = m.tuple_from_vertex(face_v[j]);
        auto conn_tets = m.get_one_ring_tets_for_vertex(loc);
        for (const auto& t : conn_tets) {
            if (visited.find(t.tid(m)) != visited.end()) continue;
            tet_queue.push(t);
            visited.insert(t.tid(m));
        }
    }
    //
    constexpr auto is_seg_cut_tri_2 = [](const std::array<Vector2r, 2>& seg2d,
                                         const std::array<Vector2r, 3>& tri2d) {
        // overlap == seg has one endpoint inside tri OR seg intersect with tri edges
        bool is_inside = wmtk::is_point_inside_triangle(seg2d[0], tri2d) ||
                         wmtk::is_point_inside_triangle(seg2d[1], tri2d);
        if (is_inside) {
            return true;
        } else {
            for (int j = 0; j < 3; j++) {
                std::array<Vector2r, 2> tri_seg2 = {{tri2d[j], tri2d[(j + 1) % 3]}};
                rational _;
                bool is_intersected =
                    wmtk::open_segment_open_segment_intersection_2d(seg2d, tri_seg2, _);
                if (is_intersected) {
                    return true;
                }
            }
        }
        return false;
    };
    //
    // BFS
    while (!tet_queue.empty()) {
        auto tet = tet_queue.front();
        tet_queue.pop();

        std::array<bool, 4> is_tet_face_intersected = {{false, false, false, false}};

        /// check vertices position
        int cnt_pos = 0;
        int cnt_neg = 0;
        std::map<size_t, int> vertex_sides;
        std::vector<int> coplanar_f_lvids;
        //
        auto vs = m.oriented_tet_vertices(tet);

        auto retry_flag = !try_acquire_tetra(std::vector<Tuple>{{tet}});
        if (retry_flag) {
            return std::tuple(
                success_flag,
                std::vector<Tuple>(),
                std::vector<Tuple>(),
                std::vector<Vector3r>());
        }

        std::array<size_t, 4> vertex_vids;
        for (int j = 0; j < 4; j++) {
            vertex_vids[j] = vs[j].vid(m);
            Vector3r dir = vertex_pos_r(vertex_vids[j]) - tri[0];
            auto side = dir.dot(tri_normal);
            if (side > 0) {
                cnt_pos++;
                vertex_sides[vertex_vids[j]] = 1;
            } else if (side < 0) {
                cnt_neg++;
                vertex_sides[vertex_vids[j]] = -1;
            } else {
                coplanar_f_lvids.push_back(j);
                vertex_sides[vertex_vids[j]] = 0;
            }
        }
        //
        if (coplanar_f_lvids.size() == 1) {
            int lvid = coplanar_f_lvids[0];
            int vid = vertex_vids[lvid];
            auto p = wmtk::project_point_to_2d(vertex_pos_r(vid), squeeze_to_2d_dir);
            bool is_inside = wmtk::is_point_inside_triangle(p, tri2);
            //
            if (is_inside) {
                auto conn_tets = m.get_one_ring_tets_for_vertex(vs[lvid]);
                for (auto& t : conn_tets) {
                    if (visited.find(t.tid(m)) != visited.end()) continue;
                    visited.insert(t.tid(m));
                    tet_queue.push(t);
                }
            }
        } else if (coplanar_f_lvids.size() == 2) {
            std::array<Vector2r, 2> seg2;
            seg2[0] = wmtk::project_point_to_2d(
                vertex_pos_r(vertex_vids[coplanar_f_lvids[0]]),
                squeeze_to_2d_dir);
            seg2[1] = wmtk::project_point_to_2d(
                vertex_pos_r(vertex_vids[coplanar_f_lvids[1]]),
                squeeze_to_2d_dir);
            if (is_seg_cut_tri_2(seg2, tri2)) {
                std::array<int, 2> le = {{coplanar_f_lvids[0], coplanar_f_lvids[1]}};
                if (le[0] > le[1]) std::swap(le[0], le[1]);
                int leid =
                    std::find(local_edges.begin(), local_edges.end(), le) - local_edges.begin();
                is_tet_face_intersected[map_leid2lfids[leid][0]] = true;
                is_tet_face_intersected[map_leid2lfids[leid][1]] = true;
                //
                for (int j = 0; j < 2; j++) {
                    auto conn_tets = m.get_one_ring_tets_for_vertex(vs[coplanar_f_lvids[j]]);
                    for (auto& t : conn_tets) {
                        if (visited.find(t.tid(m)) != visited.end()) continue;
                        visited.insert(t.tid(m));
                        // add lock
                        tet_queue.push(t);
                    }
                }
            }
        } else if (coplanar_f_lvids.size() == 3) {
            bool is_cut = false;
            for (int i = 0; i < 3; i++) {
                std::array<Vector2r, 2> seg2;
                seg2[0] = wmtk::project_point_to_2d(
                    vertex_pos_r(vertex_vids[coplanar_f_lvids[i]]),
                    squeeze_to_2d_dir);
                seg2[1] = wmtk::project_point_to_2d(
                    vertex_pos_r(vertex_vids[coplanar_f_lvids[(i + 1) % 3]]),
                    squeeze_to_2d_dir);
                if (is_seg_cut_tri_2(seg2, tri2)) {
                    is_cut = true;
                    break;
                }
            }
            if (is_cut) {
                std::array<size_t, 3> f = {
                    {vertex_vids[coplanar_f_lvids[0]],
                     vertex_vids[coplanar_f_lvids[1]],
                     vertex_vids[coplanar_f_lvids[2]]}};
                std::sort(f.begin(), f.end());
                marking_tet_faces.push_back(f);
                // tet_face_tags[f].push_back(face_id);
                //
                for (int j = 0; j < 3; j++) {
                    auto conn_tets = m.get_one_ring_tets_for_vertex(vs[coplanar_f_lvids[j]]);
                    for (auto& t : conn_tets) {
                        if (visited.find(t.tid(m)) != visited.end()) continue;
                        visited.insert(t.tid(m));
                        // add lock
                        tet_queue.push(t);
                    }
                }
            }
        }
        //
        if (cnt_pos == 0 || cnt_neg == 0) {
            continue;
        }

        /// check edges
        std::array<Tuple, 6> edges = m.tet_edges(tet);
        //
        std::vector<std::array<size_t, 2>> edge_vids;
        for (auto& loc : edges) {
            size_t v1_id = loc.vid(m);
            auto tmp = m.switch_vertex(loc);
            size_t v2_id = tmp.vid(m);
            std::array<size_t, 2> e = {{v1_id, v2_id}};
            if (e[0] > e[1]) std::swap(e[0], e[1]);
            edge_vids.push_back(e);
        }

        /// check if the tet edges intersects with the triangle
        bool need_subdivision = false;
        for (int l_eid = 0; l_eid < edges.size(); l_eid++) {
            const std::array<size_t, 2>& e = edge_vids[l_eid];
            if (vertex_sides[e[0]] * vertex_sides[e[1]] >= 0) continue;

            if (map_edge2point.find(e) != map_edge2point.end()) {
                if (std::get<0>(map_edge2point[e]) == TRI_INTERSECTION) {
                    for (int k = 0; k < 2; k++)
                        is_tet_face_intersected[map_leid2lfids[l_eid][k]] = true;
                    need_subdivision = true;
                }
                continue;
            }

            std::array<Vector3r, 2> seg = {{vertex_pos_r(e[0]), vertex_pos_r(e[1])}};
            Vector3r p(0, 0, 0);
            int intersection_status = EMPTY_INTERSECTION;
            bool is_inside_tri = false;
            bool is_intersected_plane =
                wmtk::open_segment_plane_intersection_3d(seg, tri, p, is_inside_tri);
            if (is_intersected_plane && is_inside_tri) {
                intersection_status = TRI_INTERSECTION;
            } else if (is_intersected_plane) {
                intersection_status = PLN_INTERSECTION;
            }

            map_edge2point[e] = std::make_tuple(intersection_status, p, tet.tid(m), l_eid);
            if (intersection_status == EMPTY_INTERSECTION) {
                continue;
            } else if (intersection_status == TRI_INTERSECTION) {
                for (int k = 0; k < 2; k++)
                    is_tet_face_intersected[map_leid2lfids[l_eid][k]] = true;
                need_subdivision = true;
            }

            // add new tets
            if (need_subdivision) {
                auto incident_tets = m.get_incident_tets_for_edge(edges[l_eid]);
                for (auto& t : incident_tets) {
                    int tid = t.tid(m);
                    if (visited.find(tid) != visited.end()) continue;

                    tet_queue.push(t);
                    visited.insert(tid);
                }
            }
        }

        /// check if the tet (open) face intersects with the triangle
        for (int j = 0; j < 4; j++) { // for each tet face
            if (is_tet_face_intersected[j]) continue;
            std::array<size_t, 3> f = {
                {vs[local_faces[j][0]].vid(m),
                 vs[local_faces[j][1]].vid(m),
                 vs[local_faces[j][2]].vid(m)}};
            std::sort(f.begin(), f.end());
            auto it = map_face2intersected.find(f);
            if (it != map_face2intersected.end()) {
                if (it->second) need_subdivision = true;
                continue;
            }

            {
                int cnt_pos1 = 0;
                int cnt_neg1 = 0;
                for (int k = 0; k < 3; k++) {
                    if (vertex_sides[f[k]] > 0)
                        cnt_pos1++;
                    else if (vertex_sides[f[k]] < 0)
                        cnt_neg1++;
                }
                if (cnt_pos1 == 0 || cnt_neg1 == 0) continue;
            }

            std::array<Vector3r, 3> tet_tri = {
                {vertex_pos_r(f[0]), vertex_pos_r(f[1]), vertex_pos_r(f[2])}};
            //
            std::array<int, 3> tet_tri_v_sides;
            Vector3r tet_tri_normal = (tet_tri[1] - tet_tri[0]).cross(tet_tri[2] - tet_tri[0]);
            for (int k = 0; k < 3; k++) {
                Vector3r dir = tri[k] - tet_tri[0];
                auto side = dir.dot(tet_tri_normal);
                if (side == 0)
                    tet_tri_v_sides[k] = 0;
                else if (side > 0)
                    tet_tri_v_sides[k] = 1;
                else
                    tet_tri_v_sides[k] = -1;
            }

            bool is_intersected = false;
            for (int k = 0; k < 3; k++) { // check intersection
                if ((tet_tri_v_sides[k] >= 0 && tet_tri_v_sides[(k + 1) % 3] >= 0) ||
                    (tet_tri_v_sides[k] <= 0 && tet_tri_v_sides[(k + 1) % 3] <= 0))
                    continue;
                Vector3r _p;
                is_intersected = wmtk::open_segment_triangle_intersection_3d(
                    {{tri[k], tri[(k + 1) % 3]}},
                    tet_tri,
                    _p);
                if (is_intersected) {
                    need_subdivision = true; // is recorded
                    break;
                }
            }
            map_face2intersected[f] = is_intersected;

            if (is_intersected) {
                auto res = m.switch_tetrahedron(m.tuple_from_face(tet.tid(m), j));
                if (res.has_value()) {
                    auto n_tet = res.value();
                    int tid = n_tet.tid(m);
                    if (visited.find(tid) != visited.end()) continue;
                    // add lock
                    tet_queue.push(n_tet);
                    visited.insert(tid);
                }
            }
        }

        /// record the tets
        if (need_subdivision) {
            intersected_tets.push_back(tet);
            //
            for (auto& e : edge_vids) {
                intersected_tet_edges.insert(e);
            }
        }
    } // End BFS while (!tet_queue.empty())

    // erase edge without intersections OR edge with intersection but not belong to
    // intersected tets
    for (auto it = map_edge2point.begin(), ite = map_edge2point.end(); it != ite;) {
        if (std::get<0>(it->second) == EMPTY_INTERSECTION ||
            intersected_tet_edges.find(it->first) == intersected_tet_edges.end())
            it = map_edge2point.erase(it);
        else
            ++it;
    }
    success_flag = true;
    std::vector<TetMesh::Tuple> intersected_edges;
    std::vector<Vector3r> intersected_pos;
    for (auto& info : map_edge2point) {
        auto& [_, p, tid, l_eid] = info.second;
        intersected_edges.push_back(m.tuple_from_edge(tid, l_eid));
        intersected_pos.push_back(p);
    }
    return std::tuple(success_flag, intersected_tets, intersected_edges, intersected_pos);
}
} // namespace wmtk


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


// Copyright (c) 2017-2018 The Particl Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <key/stealth.h>

#include <key.h>
#include <pubkey.h>
#include <key_io.h>
#include <script/signingprovider.h>
#include <test/util/setup_common.h>
#include <util/strencodings.h>

#include <serialize.h>
#include <streams.h>
#include <string>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(stealth_tests, ParticlBasicTestingSetup)

static const std::string strSecret1C("GzFRfngjf5aHMuAzWDZWzJ8eYqMzp29MmkCp6NgzkXFibrh45tTc");
static const std::string strSecret2C("H5hDgLvFjLcZG9jyxkUTJ28P6N5T7iMBQ79boMuaPafxXuy8hb9n");
static const std::string strSecret3C("Gyi9eQQC7qYRqwgAs1K1CdTEr9U7XR1RvDQdYjpK2dDU92N2cutY");

BOOST_AUTO_TEST_CASE(stealth_key_1)
{
    const char *testAddr = "SPGx5uFievEkR6gCz9FAoWYWADGPZAddbsQeXLtMggpDGkvPAWC5qiSNkYbG6H8obrDJkxvp8FzCRZkimapA22Nrjpw7EofyoNSe4x";

    CStealthAddress sxAddr;

    BOOST_CHECK(true == sxAddr.SetEncoded(testAddr));

    BOOST_CHECK(HexStr(sxAddr.scan_pubkey) == "029b62c32a5561946b1b43cce1235a3b47d82abde25807cb9df2a65a1941558a8d");
    BOOST_CHECK(HexStr(sxAddr.spend_pubkey) == "02f0e2f682c8a07fdba7a3a97f823261008c7f53156c311d20216af0b6cc8148c3");

    BOOST_CHECK(sxAddr.Encoded() == testAddr);
};

BOOST_AUTO_TEST_CASE(stealth_key_serialise)
{
    CStealthAddress sxAddr;

    CBitcoinSecret bsecret1, bsecret2;
    BOOST_CHECK(bsecret1.SetString(strSecret1C));
    BOOST_CHECK(bsecret2.SetString(strSecret2C));

    sxAddr.scan_secret = bsecret1.GetKey();
    CKey spend_secret = bsecret2.GetKey();

    sxAddr.spend_secret_id = spend_secret.GetPubKey().GetID();

    CPubKey pkTemp = sxAddr.scan_secret.GetPubKey();
    BOOST_CHECK(pkTemp.size() == EC_COMPRESSED_SIZE);

    sxAddr.scan_pubkey.resize(EC_COMPRESSED_SIZE);
    memcpy(&sxAddr.scan_pubkey[0], pkTemp.begin(), EC_COMPRESSED_SIZE);

    pkTemp = spend_secret.GetPubKey();
    BOOST_CHECK(pkTemp.size() == EC_COMPRESSED_SIZE);
    sxAddr.spend_pubkey.resize(EC_COMPRESSED_SIZE);
    memcpy(&sxAddr.spend_pubkey[0], pkTemp.begin(), EC_COMPRESSED_SIZE);

    const char *testAddr = "SPGyji8uZFip6H15GUfj6bsutRVLsCyBFL3P7k7T7MUDRaYU8GfwUHpfxonLFAvAwr2RkigyGfTgWMfzLAAP8KMRHq7RE8cwpEEekH";
    BOOST_CHECK(sxAddr.Encoded() == testAddr);

    sxAddr.prefix.number_bits = 8;
    sxAddr.prefix.bitfield = 0xaaaaaaaa;

    const char *testAddr2 = "2w3KXRWZtyMyYrHdKqK7voiwiRy66gbpxEF3WcmHUF3M7qV3tj6AsRTFQyKV5wYmgu5PJFy1RN8kmNaF63ZSQfJ18DHMKvxdj1DpCmmQ";
    BOOST_CHECK(sxAddr.Encoded() == testAddr2);

    CDataStream ss(SER_DISK, 0);

    ss << sxAddr;
    CStealthAddress sxAddr2;
    ss >> sxAddr2;

    BOOST_CHECK(sxAddr2.Encoded() == testAddr2);
    BOOST_CHECK(sxAddr2.prefix.number_bits = 8);
    BOOST_CHECK(sxAddr2.prefix.bitfield == 0xaaaaaaaa);

    BOOST_CHECK(sxAddr2.scan_secret.IsValid());
    BOOST_CHECK(sxAddr.scan_secret == sxAddr2.scan_secret);

    BOOST_CHECK(sxAddr.spend_secret_id == sxAddr2.spend_secret_id);

    ss.clear();
    BOOST_CHECK(ss.empty());

    // Add 2nd spend pubkey
    sxAddr.number_signatures = 1;

    sxAddr.spend_pubkey.resize(EC_COMPRESSED_SIZE * 2);

    CBitcoinSecret bsecret3;
    BOOST_CHECK(bsecret3.SetString(strSecret3C));
    CKey kSpend2 = bsecret3.GetKey();

    ec_point pkSpend2;
    BOOST_CHECK(SecretToPublicKey(kSpend2, pkSpend2) == 0);

    memcpy(&sxAddr.spend_pubkey[EC_COMPRESSED_SIZE], &pkSpend2[0], EC_COMPRESSED_SIZE);

    const char *testAddr3 = "3XuQSjM7p8o7qg7jrk7UmXXpHVv6tdPodYvEZKeSx1ENYTE51dL5Jj4GwMFdare6uL3znPj7jVN6Bo9jqnvRwyywuur8GcijEcjYcbaLteR2eG47eqRNACfRtRzYJRbTmZCPCxqELsrcxznaossEi";
    BOOST_CHECK(sxAddr.Encoded() == testAddr3);

    ss << sxAddr;
    ss >> sxAddr2;
    BOOST_CHECK(sxAddr2.number_signatures == 1);
    BOOST_CHECK(sxAddr2.spend_pubkey.size() == EC_COMPRESSED_SIZE * 2);

    // verify pkSpend 2
    BOOST_CHECK(memcmp(&pkSpend2[0], &sxAddr.spend_pubkey[EC_COMPRESSED_SIZE], EC_COMPRESSED_SIZE) == 0);

    // verify pkSpend 1
    BOOST_CHECK(sxAddr.spend_pubkey == sxAddr2.spend_pubkey);
}

void makeNewStealthKey(CStealthAddress &sxAddr, FillableSigningProvider &keystore)
{
    InsecureNewKey(sxAddr.scan_secret, true);

    CKey spend_secret;
    InsecureNewKey(spend_secret, true);
    //sxAddr.spend_secret_id = spend_secret.GetPubKey().GetID();

    SecretToPublicKey(sxAddr.scan_secret, sxAddr.scan_pubkey);
    SecretToPublicKey(spend_secret, sxAddr.spend_pubkey);

    // verify
    CPubKey pkTemp = sxAddr.scan_secret.GetPubKey();
    BOOST_CHECK(pkTemp.size() == EC_COMPRESSED_SIZE);
    BOOST_CHECK(memcmp(&sxAddr.scan_pubkey[0], pkTemp.begin(), EC_COMPRESSED_SIZE) == 0);

    pkTemp = spend_secret.GetPubKey();
    BOOST_CHECK(pkTemp.size() == EC_COMPRESSED_SIZE);
    BOOST_CHECK(pkTemp.size() == EC_COMPRESSED_SIZE);
    BOOST_CHECK(memcmp(&sxAddr.spend_pubkey[0], pkTemp.begin(), EC_COMPRESSED_SIZE) == 0);

    keystore.AddKeyPubKey(spend_secret, pkTemp);
}

BOOST_AUTO_TEST_CASE(stealth_key_address)
{
    SeedInsecureRand();
    FillableSigningProvider keystore;
    ECC_Start_Stealth();

    for (size_t k = 0; k < 32; ++k) {
        CStealthAddress sxAddr;
        makeNewStealthKey(sxAddr, keystore);
        sxAddr.prefix.number_bits = k;
        sxAddr.prefix.bitfield = 0xaaaaaaaa;

        CBitcoinAddress addrC(sxAddr.Encoded());
        BOOST_CHECK(addrC.IsValid() == true);
        BOOST_CHECK(addrC.ToString() == sxAddr.Encoded());

        CTxDestination dest = addrC.Get();

        BOOST_CHECK(dest.type() == typeid(CStealthAddress));
        CStealthAddress sxAddrOut = boost::get<CStealthAddress>(dest);

        BOOST_CHECK(sxAddrOut == sxAddr);
        BOOST_CHECK(sxAddrOut.Encoded() == sxAddr.Encoded());

        CBitcoinAddress addrC2(dest);
        BOOST_CHECK(addrC.ToString() == addrC2.ToString());
    }

    ECC_Stop_Stealth();
}

BOOST_AUTO_TEST_CASE(stealth_key)
{
    SeedInsecureRand();
    FillableSigningProvider keystore;

    ECC_Start_Stealth();

    for (size_t i = 0; i < 16; ++i) {
        CStealthAddress sxAddr;

        makeNewStealthKey(sxAddr, keystore);

        CKey sEphem;
        CKey secretShared;
        ec_point pkSendTo;

        // Send, secret = ephem_secret, pubkey = scan_pubkey
        // NOTE: StealthSecret can fail if hash is out of range, retry with new ephemeral key
        int k, nTries = 24;
        for (k = 0; k < nTries; ++k) {
            InsecureNewKey(sEphem, true);
            if (StealthSecret(sEphem, sxAddr.scan_pubkey, sxAddr.spend_pubkey, secretShared, pkSendTo) == 0)
                break;
        }
        BOOST_CHECK_MESSAGE(k < nTries, "StealthSecret failed.");
        BOOST_CHECK(pkSendTo.size() == EC_COMPRESSED_SIZE);

        ec_point ephem_pubkey;
        CPubKey pkTemp = sEphem.GetPubKey();
        BOOST_CHECK(pkTemp.size() == EC_COMPRESSED_SIZE);
        ephem_pubkey.resize(EC_COMPRESSED_SIZE);
        memcpy(&ephem_pubkey[0], pkTemp.begin(), EC_COMPRESSED_SIZE);

        CKey secretShared_verify;
        ec_point pkSendTo_verify;

        // Receive, secret = scan_secret, pubkey = ephem_pubkey
        BOOST_CHECK(StealthSecret(sxAddr.scan_secret, ephem_pubkey, sxAddr.spend_pubkey, secretShared_verify, pkSendTo_verify) == 0);

        BOOST_CHECK(pkSendTo == pkSendTo_verify);
        BOOST_CHECK(secretShared == secretShared_verify);

        CKeyID iSpend = sxAddr.GetSpendKeyID();
        CKey kSpend;
        BOOST_CHECK(keystore.GetKey(iSpend, kSpend));
        CKey kSpendOut;
        BOOST_CHECK(StealthSharedToSecretSpend(secretShared_verify, kSpend, kSpendOut) == 0);
        pkTemp = kSpendOut.GetPubKey();
        BOOST_CHECK(CPubKey(pkSendTo) == pkTemp);

        CKey kSpendOut_test2;
        BOOST_CHECK(StealthSecretSpend(sxAddr.scan_secret, ephem_pubkey, kSpend, kSpendOut_test2) == 0);
        pkTemp = kSpendOut_test2.GetPubKey();
        BOOST_CHECK(CPubKey(pkSendTo) == pkTemp);
    }

    ECC_Stop_Stealth();
}

BOOST_AUTO_TEST_SUITE_END()


#include <filter/FilterManager.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <std_msgs/Float64.h>
#include <utils/ParamsManager.h>

#include <Eigen/Eigen>
#include <exception>

#include "utils/colors.h"

// Main function
int main(int argc, char** argv) {
  try {
    // Launch our ros node
    ros::init(argc, argv, "kfa_node");
    ros::NodeHandle nh("~");

    auto* params = new ParamsManager(nh);
    auto* filter = new FilterManager(nh, params);

    // ---------------------------------------------------------------------------
    // WAIT MODE
    // Wait for a specified amount of time, this is helpfull
    // for debugging with CLion. (attach to the process).
    double wait_mode;
    nh.param<double>("wait_mode", wait_mode, 0);
    if (wait_mode > 0) {
      ros::Duration wait_dur = ros::Duration(wait_mode);
      ros::Time begin = ros::Time::now();
      ros::Time time = begin;
      ros::Rate rate(5);
      ROS_INFO("Waiting for %f sec", wait_mode);
      while (ros::ok() && time < (begin + wait_dur)) {
        rate.sleep();
        ros::spinOnce();
        time = ros::Time::now();
      }
      ROS_INFO("End waiting");
    }

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

    // Location of the ROS bag we want to read in
    std::string path_to_bag;
    nh.param<std::string>("path_bag", path_to_bag, "");
    if (path_to_bag.empty()) {
      ROS_ERROR("No path to ros bag specified");
      return 0;
    }
    ROS_INFO("ros bag path is: %s", path_to_bag.c_str());

    // Get our start location and how much of the bag we want to play
    // Make the bag duration < 0 to just process to the end of the bag
    double bag_start;
    double bag_durr;
    nh.param<double>("bag_start", bag_start, 0);
    nh.param<double>("bag_durr", bag_durr, -1);
    ROS_INFO("bag start: %.1f", bag_start);
    ROS_INFO("bag duration: %.1f", bag_durr);

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

    // Load rosbag here, and find messages we can play
    rosbag::Bag bag;
    bag.open(path_to_bag, rosbag::bagmode::Read);

    // We should load the bag as a view
    // Here we go from beginning of the bag to the end of the bag
    rosbag::View view_full;
    rosbag::View view;

    // Start a few seconds in from the full view time
    // If we have a negative duration then use the full bag length
    view_full.addQuery(bag);
    ros::Time time_init = view_full.getBeginTime();
    time_init += ros::Duration(bag_start);
    ros::Time time_finish = (bag_durr < 0)
                                ? view_full.getEndTime()
                                : time_init + ros::Duration(bag_durr);
    ROS_INFO("time start = %.6f", time_init.toSec());
    ROS_INFO("time end   = %.6f", time_finish.toSec());
    view.addQuery(bag, time_init, time_finish);

    // Check to make sure we have data to play
    if (view.size() == 0) {
      ROS_ERROR("No messages to play on specified topics. Exiting.");
      ros::shutdown();
      return EXIT_FAILURE;
    }

    // Step through the rosbag
    for (const rosbag::MessageInstance& m : view) {
      // If ros wants us to stop, break out
      if (!ros::ok()) break;

      // Handle Pose (GT, GPS) measurement
      geometry_msgs::PoseStamped::ConstPtr pose_m =
          m.instantiate<geometry_msgs::PoseStamped>();

      // Handle vector Radar measurement
      geometry_msgs::Vector3Stamped::ConstPtr vect_m =
          m.instantiate<geometry_msgs::Vector3Stamped>();

      // GT measurement
      if (pose_m != nullptr && m.getTopic() == params->topic_gt) {
        // convert into correct format
        // int seq = (*pose_noise).header.seq; // unused
        double timem = (*pose_m).header.stamp.toSec();
        Eigen::Vector3d pose;
        pose(0, 0) = (*pose_m).pose.position.x;
        pose(1, 0) = (*pose_m).pose.position.y;
        pose(2, 0) = (*pose_m).pose.position.z;

        filter->feed_m_gt(timem, pose);
      }

      // GPS measurement
      if (pose_m != nullptr && m.getTopic() == params->topic_gps) {
        // convert into correct format
        // int seq = (*pose_noise).header.seq; // unused
        double timem = (*pose_m).header.stamp.toSec();
        Eigen::Vector3d pose;
        pose(0, 0) = (*pose_m).pose.position.x;
        pose(1, 0) = (*pose_m).pose.position.y;
        pose(2, 0) = (*pose_m).pose.position.z;

        filter->state_estimation();
        filter->feed_m_gps(timem, pose);
      }

      // Radar measurement
      if (vect_m != nullptr && m.getTopic() == params->topic_radar) {
        // convert into correct format
        // int seq = (*pose_noise).header.seq; // unused
        double timem = (*vect_m).header.stamp.toSec();
        Eigen::Vector3d beam;
        beam(0, 0) = (*vect_m).vector.x;
        beam(1, 0) = (*vect_m).vector.y;
        beam(2, 0) = (*vect_m).vector.z;

        filter->feed_m_radar(timem, beam);
      }

      filter->display();
    }
  } catch (std::exception& e) {
    std::cout << "An exception occurred: \n" << e.what() << std::endl;
  }

  return 0;
}


//   GAMBIT: Global and Modular BSM Inference Tool
//   *********************************************
///  \file
///
///  Type definition header for module FlavBit.
///
///  Compile-time registration of type definitions
///  required for the rest of the code to
///  communicate with FlavBit.
///
///  Add to this if you want to define a new type
///  for the functions in FlavBit to return, but
///  you don't expect that type to be needed by
///  any other modules.
///
///  *********************************************
///
///  Authors (add name and date if you modify):
///
///  \author Marcin Chrzaszcz
///          (mchrzasz@cern.ch)
///  \date 2015 July
///
///  \author Tomas Gonzalo
///          (t.e.gonzalo@fys.uio.no)
///  \date 2017 July
///
///  *********************************************

#ifndef __FlavBit_types_hpp__
#define __FlavBit_types_hpp__

#include <string>
#include <vector>

#include <boost/numeric/ublas/matrix.hpp>

namespace Gambit
{

  namespace FlavBit
  {

    namespace ublas = boost::numeric::ublas;

    /// Simple structure for holding a correlation value and name of the correlated observable
    struct Correlation
    {
      double corr_val;
      std::string corr_name;
    };

    /// Representation of a single entry in the FlavBit YAML database
    struct Measurement
    {
      bool is_limit;
      bool exp_one_sided;
      double exp_value;
      double exp_stat_error;
      double exp_sys_error;
      double exp_error;
      double exp_90_CL;
      double exp_95_CL;
      double th_error;
      std::string name;
      std::string exp_source;
      std::string th_error_source;
      std::string th_error_type;
      std::vector<Correlation> corr;
    };

    /// Structure for holding predicted and observed values of multiple observables,
    /// and experimental and theoretical covariance matrices for their uncertainties.
    struct predictions_measurements_covariances
    {
      std::string LL_name;
      boost::numeric::ublas::matrix<double> value_exp;
      boost::numeric::ublas::matrix<double> cov_exp;
      boost::numeric::ublas::matrix<double> value_th;
      boost::numeric::ublas::matrix<double> cov_th;
      std::vector<double> diff;
      int dim;
    };

  }
}

#endif


#include <Eigen/Core>
#include <Eigen/Sparse>
#include <igl/AABB.h>
#include <igl/in_element.h>

void in_element_aabb(Eigen::MatrixXd & queries, Eigen::MatrixXd & V, Eigen::MatrixXi & F, Eigen::VectorXi & I)
{
    std::vector<Eigen::Triplet<double> > ijv;
    Eigen::MatrixXd bb_mins;
    Eigen::MatrixXd bb_maxs;
    Eigen::VectorXi elements;
    int dim = V.cols();
    bb_mins.resize(0,dim);
    bb_maxs.resize(0,dim);
    elements.resize(0,1);
    switch(V.cols())
  {
    case 3:
    {
      igl::AABB<Eigen::MatrixXd,3> aabb;
      aabb.init(V,F,bb_mins,bb_maxs,elements);
      igl::in_element(V,F,queries,aabb,I);
      break;
    }
    case 2:
    {
      igl::AABB<Eigen::MatrixXd,2> aabb;
      aabb.init(V,F,bb_mins,bb_maxs,elements);
      igl::in_element(V,F,queries,aabb,I);
      break;
    }
  }
}


// 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 <Eigen/Core>
#include <catch2/catch.hpp>
#include <map>
#include <string>
#include <vector>
#include "ear/bs2051.hpp"
#include "ear/common/geom.hpp"
#include "ear/common/helpers/make_unique.hpp"
#include "ear/ear.hpp"
#include "ear/metadata.hpp"
#include "helper/vector_approx.hpp"

using namespace ear;

using Gains = std::vector<float>;

DirectSpeakersTypeMetadata tmWithLabels(std::vector<std::string> labels) {
  DirectSpeakersTypeMetadata tm;
  tm.speakerLabels = labels;
  return tm;
}

Gains directPv(const Layout& layout, const std::string& channel) {
  Eigen::VectorXd pv = Eigen::VectorXd::Zero(layout.channels().size());
  auto channels = layout.channelNames();
  auto it = std::find(channels.begin(), channels.end(), channel);
  pv(std::distance(channels.begin(), it)) = 1.0;
  return Gains(pv.data(), pv.data() + pv.rows() * pv.cols());
}

TEST_CASE("test_speaker_label") {
  Layout layout = getLayout("4+5+0");
  GainCalculatorDirectSpeakers p(layout);

  // DirectSpeakersTypeMetadata metadata;
  Gains actual(layout.channels().size());
  for (const std::string& prefix :
       {"", "urn:itu:bs:2051:0:speaker:", "urn:itu:bs:2051:1:speaker:"}) {
    // normal case
    p.calculate(tmWithLabels(std::vector<std::string>{prefix + "M+000"}),
                actual);
    REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+000"));
    p.calculate(tmWithLabels(std::vector<std::string>{prefix + "M+030"}),
                actual);
    REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+030"));

    // missing channels are ignored
    p.calculate(tmWithLabels(std::vector<std::string>{prefix + "M+030",
                                                      prefix + "B+000"}),
                actual);
    REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+030"));
    p.calculate(tmWithLabels(std::vector<std::string>{prefix + "B+000",
                                                      prefix + "M+030"}),
                actual);
    REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+030"));

    // matching more than one channel should pick the first
    p.calculate(tmWithLabels(std::vector<std::string>{prefix + "M+000",
                                                      prefix + "M+030"}),
                actual);
    REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+000"));
    p.calculate(tmWithLabels(std::vector<std::string>{prefix + "M+030",
                                                      prefix + "M+000"}),
                actual);
    REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+030"));
  }
}

TEST_CASE("test_speaker_label_additional_substitutions") {
  Layout layout = getLayout("4+5+0");
  GainCalculatorDirectSpeakers p(layout, {{"foo", "M+030"}});

  Gains actual(layout.channels().size());
  p.calculate(tmWithLabels(std::vector<std::string>{"foo"}), actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+030"));
}

TEST_CASE("test_one_lfe_out") {
  // When there is only one LFE output, both LFE1 and LFE2 should be sent to it.
  Layout layout = getLayout("4+5+0");
  GainCalculatorDirectSpeakers p(layout);

  Gains actual(layout.channels().size());
  p.calculate(tmWithLabels(std::vector<std::string>{"LFE1"}), actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "LFE1"));
  p.calculate(tmWithLabels(std::vector<std::string>{"LFE2"}), actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "LFE1"));
}

TEST_CASE("test_no_lfe_out") {
  // When there is no LFE output, both LFE1 and LFE2 should be discarded.
  Layout layout = getLayout("0+2+0");
  GainCalculatorDirectSpeakers p(layout);

  Gains actual(layout.channels().size());
  p.calculate(tmWithLabels(std::vector<std::string>{"LFE1"}), actual);
  REQUIRE_VECTOR_APPROX(actual, Gains(layout.channels().size(), 0.0));
  p.calculate(tmWithLabels(std::vector<std::string>{"LFE2"}), actual);
  REQUIRE_VECTOR_APPROX(actual, Gains(layout.channels().size(), 0.0));
}

TEST_CASE("test_lfe_just_frequency") {
  // When there is only one LFE output, both LFE1 and LFE2 should be sent to it.
  Layout layout = getLayout("4+5+0");
  GainCalculatorDirectSpeakers p(layout);

  DirectSpeakersTypeMetadata tm;

  SECTION("just frequency") {
    tm.channelFrequency.lowPass = 100.0;
    Gains actual(layout.channels().size());
    p.calculate(tm, actual);
    REQUIRE_VECTOR_APPROX(actual, directPv(layout, "LFE1"));
  }

  SECTION("frequency and speakerLabel") {
    tm.channelFrequency.lowPass = 100.0;
    tm.speakerLabels = {"LFE1"};
    Gains actual(layout.channels().size());
    p.calculate(tm, actual);
    REQUIRE_VECTOR_APPROX(actual, directPv(layout, "LFE1"));
  }
}

TEST_CASE("test_dist_bounds_polar") {
  Layout layout = getLayout("9+10+3");
  GainCalculatorDirectSpeakers p(layout);

  DirectSpeakersTypeMetadata tm;
  PolarSpeakerPosition pos;

  Gains actual(layout.channels().size(), 0.0);
  Gains expected(layout.channels().size(), 0.0);

  // test horizontal bounds
  std::fill(expected.begin(), expected.end(), 0.0);
  expected[layout.indexForName("M+000").get()] = std::sqrt(0.5);
  expected[layout.indexForName("M+030").get()] = std::sqrt(0.5);

  // no bounds, and position not on speaker -> use psp
  pos = PolarSpeakerPosition(15.0, 0.0, 1.0);
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, expected);

  // lower bound expanded to speaker -> direct on that speaker
  pos = PolarSpeakerPosition(15.0, 0.0, 1.0);
  pos.azimuthMin = 0.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+000"));

  // upper bound expanded to speaker -> direct on that speaker
  pos = PolarSpeakerPosition(15.0, 0.0, 1.0);
  pos.azimuthMax = 30.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+030"));

  // upper and lower bounds expanded to speakers -> use psp again
  pos = PolarSpeakerPosition(15.0, 0.0, 1.0);
  pos.azimuthMin = 0.0;
  pos.azimuthMax = 30.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, expected);

  // closer to one than the other -> direct to that speaker
  pos = PolarSpeakerPosition(14.0, 0.0, 1.0);
  pos.azimuthMin = 0.0;
  pos.azimuthMax = 30.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+000"));

  // test vertical bounds
  std::fill(expected.begin(), expected.end(), 0.0);
  expected[layout.indexForName("M+000").get()] = std::sqrt(0.5);
  expected[layout.indexForName("U+000").get()] = std::sqrt(0.5);

  // // no bounds, and position not on speaker -> use psp
  pos = PolarSpeakerPosition(0.0, 15.0, 1.0);
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, expected);

  // lower bound expanded to speaker -> direct on that speaker
  pos = PolarSpeakerPosition(0.0, 15.0, 1.0);
  pos.elevationMin = 0.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+000"));

  // upper bound expanded to speaker -> direct on that speaker
  pos = PolarSpeakerPosition(0.0, 15.0, 1.0);
  pos.elevationMax = 30.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "U+000"));

  // upper and lower bounds expanded to speakers -> use psp again
  pos = PolarSpeakerPosition(0.0, 15.0, 1.0);
  pos.elevationMin = 0.0;
  pos.elevationMax = 30.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, expected);

  // closer to one than the other -> direct to that speaker
  pos = PolarSpeakerPosition(0.0, 14.0, 1.0);
  pos.elevationMin = 0.0;
  pos.elevationMax = 30.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+000"));

  // check that pole speakers are used even if 0 is excluded by azimuth range
  pos = PolarSpeakerPosition(15.0, 90.0, 1.0);
  pos.azimuthMin = 10.0;
  pos.azimuthMax = 20.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "T+000"));
}

TEST_CASE("test_dist_bounds_cart", "[!shouldfail]") {
  Layout layout = getLayout("9+10+3");
  GainCalculatorDirectSpeakers p(layout);

  DirectSpeakersTypeMetadata tm;
  CartesianSpeakerPosition pos;

  Gains actual(layout.channels().size(), 0.0);
  Gains expected(layout.channels().size(), 0.0);

  // on speaker -> direct
  pos = CartesianSpeakerPosition(1.0, 0.0, 0.0);
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M-090"));

  // lower bound on speaker -> direct
  pos = CartesianSpeakerPosition(1.0, 0.1, 0.0);
  pos.YMin = 0.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M-090"));

  // upper bound on speaker -> direct
  pos = CartesianSpeakerPosition(1.0, -0.1, 0.0);
  pos.YMax = 0.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M-090"));

  // pick closest within bound
  Eigen::Vector3d pos30 = cart(30.0, 0.0, 1.0);
  Eigen::Vector3d pos5 = cart(5.0, 0.0, 1.0);
  Eigen::Vector3d pos25 = cart(25.0, 0.0, 1.0);

  pos = CartesianSpeakerPosition(pos5(0), pos5(1), 0.0);
  pos.XMin = pos30(0);
  pos.XMax = -pos30(0);
  pos.YMin = pos30(1);
  pos.YMax = 1.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+000"));

  pos = CartesianSpeakerPosition(pos25(0), pos25(1), 0.0);
  pos.XMin = pos30(0);
  pos.XMax = -pos30(0);
  pos.YMin = pos30(1);
  pos.YMax = 1.0;
  tm.position = pos;
  p.calculate(tm, actual);
  REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+030"));
}

TEST_CASE("mapping") {
  auto layout = getLayout("4+5+0").withoutLfe();
  GainCalculatorDirectSpeakers p(layout);
  Gains actual(layout.channels().size(), 0.0);

  DirectSpeakersTypeMetadata tm;
  tm.audioPackFormatID = "AP_0001000f";

  SECTION("AC_0001001c") {
    tm.position = PolarSpeakerPosition(135.0, 0.0, 1.0);
    tm.speakerLabels = {"urn:itu:bs:2051:0:speaker:M+135"};

    p.calculate(tm, actual);
    REQUIRE_VECTOR_APPROX(actual, directPv(layout, "M+110"));
  }

  SECTION("AC_00010011") {
    tm.position = PolarSpeakerPosition(180.0, 30.0, 1.0);
    tm.speakerLabels = {"urn:itu:bs:2051:0:speaker:U+180"};
    p.calculate(tm, actual);

    Gains expected(layout.channels().size(), 0.0);
    expected[layout.indexForName("U+110").get()] = std::sqrt(0.5);
    expected[layout.indexForName("U-110").get()] = std::sqrt(0.5);

    REQUIRE_VECTOR_APPROX(actual, expected);
  }
}

TEST_CASE("mapping_per_input") {
  auto layout = getLayout("4+5+0").withoutLfe();
  GainCalculatorDirectSpeakers p(layout);
  Gains actual(layout.channels().size(), 0.0);

  DirectSpeakersTypeMetadata tm;
  tm.speakerLabels = {"urn:itu:bs:2051:0:speaker:M+090"};
  tm.position = PolarSpeakerPosition(90.0, 0.0, 1.0);

  SECTION("AP_00010009") {
    tm.audioPackFormatID = "AP_00010009";
    p.calculate(tm, actual);

    Gains expected(layout.channels().size(), 0.0);
    expected[layout.indexForName("M+030").get()] = std::sqrt(1.0 / 3.0);
    expected[layout.indexForName("M+110").get()] = std::sqrt(2.0 / 3.0);

    REQUIRE_VECTOR_APPROX(actual, expected);
  }

  SECTION("AP_00010017") {
    tm.audioPackFormatID = "AP_00010017";
    p.calculate(tm, actual);

    Gains expected(layout.channels().size(), 0.0);
    expected[layout.indexForName("M+030").get()] = std::sqrt(0.5);
    expected[layout.indexForName("M+110").get()] = std::sqrt(0.5);

    REQUIRE_VECTOR_APPROX(actual, expected);
  }
}

TEST_CASE("not implemented") {
  auto layout = getLayout("4+7+0").withoutLfe();
  GainCalculatorDirectSpeakers p(layout);
  Gains gains(layout.channels().size(), 0.0);

  DirectSpeakersTypeMetadata tm;

  SECTION("screenEdgeLock horizontal") {
    PolarSpeakerPosition pos;
    pos.screenEdgeLock.horizontal = "left";
    tm.position = pos;
    REQUIRE_THROWS_AS(p.calculate(tm, gains), not_implemented);
  }

  SECTION("screenEdgeLock vertical") {
    PolarSpeakerPosition pos;
    pos.screenEdgeLock.vertical = "top";
    tm.position = pos;
    REQUIRE_THROWS_AS(p.calculate(tm, gains), not_implemented);
  }

  SECTION("cartesian positions") {
    tm.position = CartesianSpeakerPosition();
    REQUIRE_THROWS_AS(p.calculate(tm, gains), not_implemented);
  }
}

TEST_CASE("adm errors") {
  auto layout = getLayout("4+7+0").withoutLfe();
  GainCalculatorDirectSpeakers p(layout);
  Gains gains(layout.channels().size(), 0.0);

  DirectSpeakersTypeMetadata tm;

  SECTION("audioPackFormatID without speakerLabels") {
    tm.audioPackFormatID = "AP_00010002";
    REQUIRE_THROWS_AS(p.calculate(tm, gains), adm_error);
  }
}

TEST_CASE("warnings") {
  auto layout = getLayout("4+7+0").withoutLfe();
  GainCalculatorDirectSpeakers p(layout);
  Gains gains(layout.channels().size(), 0.0);

  DirectSpeakersTypeMetadata tm;

  SECTION("non-LFE frequency") {
    tm.channelFrequency.lowPass = 300.0;
    std::vector<Warning> warnings;
    p.calculate(tm, gains, [&](const Warning& w) { warnings.push_back(w); });
    REQUIRE(warnings.size() == 1);
    REQUIRE(warnings[0].code == Warning::Code::FREQ_NOT_LFE);
  }

  SECTION("LFE mismatch") {
    tm.channelFrequency.lowPass = 100.0;
    tm.speakerLabels = {"M+000"};
    std::vector<Warning> warnings;
    p.calculate(tm, gains, [&](const Warning& w) { warnings.push_back(w); });
    REQUIRE(warnings.size() == 1);
    REQUIRE(warnings[0].code == Warning::Code::FREQ_SPEAKERLABEL_LFE_MISMATCH);
  }
}

// TODO: implement screenEdgeLock tests


/* \author Bastian Steder */

#include <iostream>

#include <boost/thread/thread.hpp>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/range_image/range_image.h>
#include <pcl/features/narf.h>
#include <pcl/console/parse.h>

float angular_resolution = 0.5f;
int rotation_invariant = 0;
float support_size = 0.3f;
int descriptor_size = 36;
pcl::RangeImage::CoordinateFrame coordinate_frame =
    pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange = false;

typedef pcl::PointXYZ PointType;

void printUsage(const char *progName) {
    std::cout
        << "\n\nUsage: " << progName << " [options] <scene.pcd>\n\n"
        << "Options:\n"
        << "-------------------------------------------\n"
        << "-r <float>   angular resolution in degrees (default "
        << angular_resolution << ")\n"
        << "-s <float>   support size for the interest points (diameter of the "
           "used sphere - "
        << "default " << support_size << ")\n"
        << "-d <int>     descriptor size (default " << descriptor_size << ")\n"
        << "-c <int>     coordinate frame of the input point cloud (default "
        << (int)coordinate_frame << ")\n"
        << "-o <0/1>     switch rotational invariant version of the feature "
           "on/off"
        << " (default " << (int)rotation_invariant << ")\n"
        << "-m           set unseen pixels to max range\n"
        << "-h           this help\n"
        << "\n\n";
}

int main(int argc, char **argv) {
    // --------------------------------------
    // -----Parse Command Line Arguments-----
    // --------------------------------------
    if (pcl::console::find_argument(argc, argv, "-h") >= 0) {
        printUsage(argv[0]);
        return 0;
    }
    if (pcl::console::find_argument(argc, argv, "-m") >= 0) {
        setUnseenToMaxRange = true;
        std::cout << "Setting unseen values in range image to maximum range "
                     "readings.\n";
    }
    if (pcl::console::parse(argc, argv, "-o", rotation_invariant) >= 0)
        std::cout << "Switching rotation invariant feature version "
                  << (rotation_invariant ? "on" : "off") << ".\n";
    int tmp_coordinate_frame;
    if (pcl::console::parse(argc, argv, "-c", tmp_coordinate_frame) >= 0) {
        coordinate_frame =
            pcl::RangeImage::CoordinateFrame(tmp_coordinate_frame);
        std::cout << "Using coordinate frame " << (int)coordinate_frame
                  << ".\n";
    }
    if (pcl::console::parse(argc, argv, "-s", support_size) >= 0)
        std::cout << "Setting support size to " << support_size << ".\n";
    if (pcl::console::parse(argc, argv, "-d", descriptor_size) >= 0)
        std::cout << "Setting descriptor size to " << descriptor_size << ".\n";
    if (pcl::console::parse(argc, argv, "-r", angular_resolution) >= 0)
        std::cout << "Setting angular resolution to " << angular_resolution
                  << "deg.\n";
    angular_resolution = pcl::deg2rad(angular_resolution);

    // -----------------------
    // -----Read pcd file-----
    // -----------------------
    pcl::PointCloud<PointType>::Ptr point_cloud_ptr(
        new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType> &point_cloud = *point_cloud_ptr;
    pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
    Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
    std::vector<int> pcd_filename_indices =
        pcl::console::parse_file_extension_argument(argc, argv, "pcd");
    if (!pcd_filename_indices.empty()) {
        std::string filename = argv[pcd_filename_indices[0]];
        if (pcl::io::loadPCDFile(filename, point_cloud) == -1) {
            std::cout << "Was not able to open file \"" << filename << "\".\n";
            printUsage(argv[0]);
            return 0;
        }
        scene_sensor_pose =
            Eigen::Affine3f(Eigen::Translation3f(
                point_cloud.sensor_origin_[0], point_cloud.sensor_origin_[1],
                point_cloud.sensor_origin_[2])) *
            Eigen::Affine3f(point_cloud.sensor_orientation_);
        std::string far_ranges_filename =
            pcl::getFilenameWithoutExtension(filename) + "_far_ranges.pcd";
        if (pcl::io::loadPCDFile(far_ranges_filename.c_str(), far_ranges) == -1)
            std::cout << "Far ranges file \"" << far_ranges_filename
                      << "\" does not exists.\n";
    } else {
        std::cout << "\nNo *.pcd file for scene given.\n\n";
        printUsage(argv[0]);
        return 1;
    }

    // -----------------------------------------------
    // -----Create RangeImage from the PointCloud-----
    // -----------------------------------------------
    float noise_level = 0.0;
    float min_range = 0.0f;
    int border_size = 1;
    boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
    pcl::RangeImage &range_image = *range_image_ptr;
    range_image.createFromPointCloud(point_cloud, angular_resolution,
                                     pcl::deg2rad(360.0f), pcl::deg2rad(180.0f),
                                     scene_sensor_pose, coordinate_frame,
                                     noise_level, min_range, border_size);
    range_image.integrateFarRanges(far_ranges);
    if (setUnseenToMaxRange)
        range_image.setUnseenToMaxRange();

    // Extract NARF features:
    std::cout << "Now extracting NARFs in every image point.\n";
    std::vector<std::vector<pcl::Narf *>> narfs;
    narfs.resize(range_image.points.size());
    int last_percentage = -1;
    for (unsigned int y = 0; y < range_image.height; ++y) {
        for (unsigned int x = 0; x < range_image.width; ++x) {
            int index = y * range_image.width + x;
            int percentage = (int)((100 * index) / range_image.points.size());
            if (percentage > last_percentage) {
                std::cout << percentage << "% " << std::flush;
                last_percentage = percentage;
            }
            pcl::Narf::extractFromRangeImageAndAddToList(
                range_image, x, y, descriptor_size, support_size,
                rotation_invariant != 0, narfs[index]);
            // std::cout << "Extracted "<<narfs[index].size ()<<" features for
            // pixel "<<x<<","<<y<<".\n";
        }
    }
    std::cout << "100%\n";
    std::cout << "Done.\n\n Now you can click on points in the image to "
                 "visualize how the descriptor is "
              << "extracted and see the descriptor distances to every other "
                 "point..\n";

    //---------------------
    // -----Show image-----
    // --------------------
    pcl::visualization::RangeImageVisualizer range_image_widget(
        "Scene range image"),
        surface_patch_widget("Descriptor's surface patch"),
        descriptor_widget("Descriptor"),
        descriptor_distances_widget("descriptor distances");
    range_image_widget.showRangeImage(range_image);
    // range_image_widget.visualize_selected_point = true;

    //--------------------
    // -----Main loop-----
    //--------------------
    while (true) {
        range_image_widget.spinOnce();   // process GUI events
        surface_patch_widget.spinOnce(); // process GUI events
        descriptor_widget.spinOnce();    // process GUI events
        pcl_sleep(0.01);

        // if (!range_image_widget.mouse_click_happened)
        continue;
        // range_image_widget.mouse_click_happened = false;
        // float clicked_pixel_x_f = range_image_widget.last_clicked_point_x,
        // clicked_pixel_y_f = range_image_widget.last_clicked_point_y;
        int clicked_pixel_x, clicked_pixel_y;
        // range_image.real2DToInt2D (clicked_pixel_x_f, clicked_pixel_y_f,
        // clicked_pixel_x, clicked_pixel_y);
        if (!range_image.isValid(clicked_pixel_x, clicked_pixel_y))
            continue;
        // Vector3f clicked_3d_point;
        // range_image.getPoint (clicked_pixel_x, clicked_pixel_y,
        // clicked_3d_point);

        // surface_patch_widget.show (false);
        // descriptor_widget.show (false);"

        int selected_index =
            clicked_pixel_y * range_image.width + clicked_pixel_x;
        pcl::Narf narf;
        if (!narf.extractFromRangeImage(range_image, clicked_pixel_x,
                                        clicked_pixel_y, descriptor_size,
                                        support_size)) {
            continue;
        }

        int surface_patch_pixel_size = narf.getSurfacePatchPixelSize();
        float surface_patch_world_size = narf.getSurfacePatchWorldSize();
        surface_patch_widget.showFloatImage(
            narf.getSurfacePatch(), surface_patch_pixel_size,
            surface_patch_pixel_size, -0.5f * surface_patch_world_size,
            0.5f * surface_patch_world_size, true);
        float surface_patch_rotation = narf.getSurfacePatchRotation();
        float patch_middle = 0.5f * (float(surface_patch_pixel_size - 1));
        float angle_step_size = pcl::deg2rad(360.0f) / narf.getDescriptorSize();
        float cell_size =
                  surface_patch_world_size / float(surface_patch_pixel_size),
              cell_factor = 1.0f / cell_size,
              max_dist = 0.5f * surface_patch_world_size,
              line_length = cell_factor * (max_dist - 0.5f * cell_size);
        for (int descriptor_value_idx = 0;
             descriptor_value_idx < narf.getDescriptorSize();
             ++descriptor_value_idx) {
            float angle =
                descriptor_value_idx * angle_step_size + surface_patch_rotation;
            // surface_patch_widget.markLine (patch_middle, patch_middle,
            // patch_middle+line_length*sinf (angle),
            // patch_middle+line_length*-cosf (angle),
            // pcl::visualization::Vector3ub (0,255,0));
        }
        std::vector<float> rotations, strengths;
        narf.getRotations(rotations, strengths);
        float radius = 0.5f * surface_patch_pixel_size;
        for (unsigned int i = 0; i < rotations.size(); ++i) {
            // surface_patch_widget.markLine (radius-0.5, radius-0.5,
            // radius-0.5f + 2.0f*radius*sinf (rotations[i]), radius-0.5f
            // - 2.0f*radius*cosf (rotations[i]), pcl::visualization::Vector3ub
            // (255,0,0));
        }

        descriptor_widget.showFloatImage(narf.getDescriptor(),
                                         narf.getDescriptorSize(), 1, -0.1f,
                                         0.3f, true);

        //===================================
        //=====Compare with all features=====
        //===================================
        const std::vector<pcl::Narf *> &narfs_of_selected_point =
            narfs[selected_index];
        if (narfs_of_selected_point.empty())
            continue;

        // descriptor_distances_widget.show (false);
        float *descriptor_distance_image = new float[range_image.points.size()];
        for (unsigned int point_index = 0;
             point_index < range_image.points.size(); ++point_index) {
            float &descriptor_distance = descriptor_distance_image[point_index];
            descriptor_distance = std::numeric_limits<float>::infinity();
            std::vector<pcl::Narf *> &narfs_of_current_point =
                narfs[point_index];
            if (narfs_of_current_point.empty())
                continue;
            for (unsigned int i = 0; i < narfs_of_selected_point.size(); ++i) {
                for (unsigned int j = 0; j < narfs_of_current_point.size();
                     ++j) {
                    descriptor_distance = (std::min)(
                        descriptor_distance,
                        narfs_of_selected_point[i]->getDescriptorDistance(
                            *narfs_of_current_point[j]));
                }
            }
        }
        descriptor_distances_widget.showFloatImage(
            descriptor_distance_image, range_image.width, range_image.height,
            -std::numeric_limits<float>::infinity(),
            std::numeric_limits<float>::infinity(), true);
        delete[] descriptor_distance_image;
    }
}


//
// Created by peri
//

#ifndef FACEDETECTION_HOG_HPP
#define FACEDETECTION_HOG_HPP

#include <boost/numeric/ublas/matrix.hpp>
#include <jpeglib.h>
//#include <boost/python/numpy/ndarray.hpp>
#include <opencv2/opencv.hpp>

using namespace boost::numeric::ublas;

class HoG{

public:
    std::vector<double> final_feature_vector;

    void gradients(cv::Mat& img, cv::Mat& gx, cv::Mat& gy);

    void magnitude_orientation(cv::Mat& gx, cv::Mat& gy, cv::Mat& mag, cv::Mat& angle);

public:
    void feature(cv::Mat& img, size_t patch, size_t block_size);

    void create_histogram(cv::Mat& img, cv::Mat& mag, cv::Mat& angle, size_t block_size);

public:
    void GammaCorrection(cv::Mat& src, cv::Mat& dst, float fGamma);

};

#endif //FACEDETECTION_HOG_HPP


#pragma once

#include <Eigen/Dense>
#include <string>
#include <vector>

namespace polyfem
{
	class MshReader
	{
	public:
		static bool load(const std::string &path, Eigen::MatrixXd &vertices, Eigen::MatrixXi &cells, std::vector<std::vector<int>> &elements, std::vector<std::vector<double>> &weights);
	};
}