#include <vector>
#include <iostream>
#include <nanoflann.hpp>
#include <eigen3/Eigen/Dense>
using Eigen::MatrixX3d;
using Eigen::Vector3d;
using nanoflann::KNNResultSet;
using nanoflann::SearchParams;
using kdt = nanoflann::KDTreeEigenMatrixAdaptor<MatrixX3d, 3, nanoflann::metric_L2>;
int main()
    // Create simple matrix
    MatrixX3d matrix(10, 3);
    for(unsigned int i = 0; i < 10; i++)
        double f_i = static_cast<double>(i);
        matrix.row(i) = Vector3d(f_i, 0, 0);
    // Create test points
    std::vector<Vector3d> test_vecs;
    for(unsigned int i = 0; i < 10; i++)
        double f_i = static_cast<double>(i);
        test_vecs.push_back(Vector3d(f_i, f_i, f_i));
    // Result buffer
    double distance;
    size_t index;
    KNNResultSet<double> result_set(1);
    result_set.init(&index, &distance);
    SearchParams sp;
    // KDTree
    kdt matrix_index(3, std::ref(matrix), 10);
    matrix_index.index->buildIndex();
    //Query points backwards
    for(int i = 9; i >= 0; i--)
        Vector3d curr_vec = test_vecs.at(i);
        matrix_index.index->findNeighbors(result_set, &curr_vec[0], sp);
        std::cout << i << std::endl;
        std::cout << index << " " << distance << std::endl << std::endl;
    // Query points forwards
    for(unsigned int i = 0; i < 10; i++)
        Vector3d curr_vec = test_vecs.at(i);