#include "RBGL.hpp"

#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/is_kuratowski_subgraph.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/max_cardinality_matching.hpp>

// NOTE:
//    this "undef" is to avoid following conflict:
// in boost: boost/tuple/tuple.hpp:get(tuples::cons<HT, TT>& c)
// in R:     Rinternals.h: #define cons                  Rf_cons
#undef cons
#include <boost/graph/is_straight_line_drawing.hpp>

using namespace boost;

// Note: if these are inside planarFaceTraversal, it causes major
//      hiccups in g++, this could be reproduced in the example codes
/////////////////////////////////////////////////////////////////////
template <typename Vertex, typename Edge>
struct my_output_visitor : public planar_face_traversal_visitor
{
  my_output_visitor(): v_vis(0), f_vis(0) { }
  ~my_output_visitor() 		{ }
  void begin_face() 		{ v_vis.clear(); }
  void end_face() 		{ f_vis.push_back(v_vis); }
  void next_vertex(Vertex v) 	{ v_vis.push_back(v); }

  typedef std::vector< Vertex > v_vis_t;
  v_vis_t v_vis;
  std::vector< v_vis_t > f_vis;
};

template<typename Graph, typename Vertex>
struct my_add_edge_visitor : public default_add_edge_visitor
{
   void visit_vertex_pair(Vertex u, Vertex v, Graph& g)
   {
        add_edge(u, v, g);
        std::cout << " add edge: " << u << " " << v << std::endl;
        e_vis.push_back(std::make_pair(u, v));
   }

   typedef std::vector< std::pair< Vertex, Vertex> > e_vis_t;
   e_vis_t e_vis;
};

//////////////////////////////////////////////////////////////////////
//a class to hold the coordinates of the straight line embedding
struct coord_t
{
  std::size_t x;
  std::size_t y;
};

//////////////////////////////////////////////////////////////////////

extern "C"
{
    typedef adjacency_list
    	       < vecS, vecS, undirectedS,
      	         property<vertex_index_t, int>,
      	         property<edge_index_t, int>
    	       > planarGraph;

    typedef graph_traits<planarGraph>::vertex_descriptor Vertex;
    typedef graph_traits<planarGraph>::edge_descriptor Edge;

    typedef property_map<planarGraph, vertex_index_t>::type Vertex_Index_t;
    typedef property_map<planarGraph, edge_index_t>::type Edge_Index_t;

    typedef std::vector<Vertex> Vertex_Vec_t;

    typedef std::vector< Edge > Edge_Vec_t;
    typedef std::vector< Edge_Vec_t > embedding_storage_t;
    typedef boost::iterator_property_map
    	    < embedding_storage_t::iterator, Vertex_Index_t > embedding_t;

    typedef std::vector< coord_t > Coord_Vec_t;
    typedef boost::iterator_property_map
   	    < Coord_Vec_t::iterator, Vertex_Index_t > straight_line_drawing_t;

    Edge_Index_t 	e_index;
    Coord_Vec_t 	straight_line_drawing_storage(0);
    embedding_storage_t embedding_storage(0);

    graph_traits<planarGraph>::edges_size_type 	edge_count;
    graph_traits<planarGraph>::edge_iterator 	ei, ei_end;
    graph_traits<planarGraph>::vertex_iterator 	vi, vi_end;

     void initPlanarGraph(planarGraph* g,
			    SEXP num_verts_in,
                            SEXP num_edges_in,
                            SEXP R_edges_in)
     {
        if ( !isInteger(R_edges_in) ) error("R_edges_in should be integer");

        int NE = asInteger(num_edges_in);
        int* edges_in = INTEGER(R_edges_in);

        for (int i = 0; i < NE ; i++, edges_in += 2)
	{
            boost::add_edge(*edges_in, *(edges_in+1), 1, *g);
        }
     }

     SEXP boyerMyrvoldPlanarityTest(SEXP num_verts_in,
			SEXP num_edges_in, SEXP R_edges_in )
     {
	planarGraph g;
	initPlanarGraph(&g, num_verts_in, num_edges_in, R_edges_in);

	SEXP ans;
	PROTECT(ans = NEW_INTEGER(1));

	INTEGER(ans)[0] = boyer_myrvold_planarity_test(g);

	UNPROTECT(1);
	return ans;
     }

     SEXP planarFaceTraversal(SEXP num_verts_in,
                        SEXP num_edges_in, SEXP R_edges_in )
     {
	planarGraph g;
	initPlanarGraph(&g, num_verts_in, num_edges_in, R_edges_in);

	// Initialize the interior edge index
	e_index = get(edge_index, g);
	edge_count = 0;
	for ( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
	    put(e_index, *ei, edge_count++);

  	embedding_storage.clear();
	embedding_storage.resize(num_vertices(g));

  	if ( boyer_myrvold_planarity_test(
		   boyer_myrvold_params::graph = g,
                   boyer_myrvold_params::embedding = &embedding_storage[0]) )
  	{
  	   my_output_visitor<Vertex, Edge> v_vis;
  	   planar_face_traversal(g, &embedding_storage[0], v_vis);

//#if RBGL_DEBUG
  	   std::cout << "we get the following: " << std::endl;
  	   for ( int i = 0; i < v_vis.f_vis.size(); i++ )
  	   {
    	      for ( int j = 0; j < v_vis.f_vis[i].size(); j++ )
      	         std::cout << v_vis.f_vis[i][j] << " ";
    	      std::cout << std::endl;
  	   }
//#endif
           SEXP ans, ansList;
           PROTECT(ansList = allocVector(VECSXP,v_vis.f_vis.size()));

           for ( int i = 0; i < v_vis.f_vis.size(); i++ )
           {
                PROTECT(ans = allocVector(INTSXP, v_vis.f_vis[i].size()));

                for ( int j = 0; j < v_vis.f_vis[i].size(); j++ )
                    INTEGER(ans)[j] = v_vis.f_vis[i][j];

                SET_VECTOR_ELT(ansList,i, ans);
           }

           UNPROTECT(1+v_vis.f_vis.size());
           return ansList;
  	}
  	else
	{
       	   std::cout << "Input graph is not planar" << std::endl;

           SEXP ans;
           PROTECT(ans = NEW_INTEGER(1));

           INTEGER(ans)[0] = 0;

           UNPROTECT(1);
           return ans;
	}
     }

     SEXP planarCanonicalOrdering(SEXP num_verts_in,
                        SEXP num_edges_in, SEXP R_edges_in )
     {
	planarGraph g;
	initPlanarGraph(&g, num_verts_in, num_edges_in, R_edges_in);

        // Initialize the interior edge index
	e_index = get(edge_index, g);
        edge_count = 0;
        for ( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
            put(e_index, *ei, edge_count++);

  	embedding_storage.clear();
	embedding_storage.resize(num_vertices(g));

  	embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));

        if ( boyer_myrvold_planarity_test(
		   boyer_myrvold_params::graph = g,
                   boyer_myrvold_params::embedding = embedding) )
        {
           my_add_edge_visitor<planarGraph, Vertex> e1_vis;

           make_connected(g, get(vertex_index, g), e1_vis);

           make_biconnected_planar(g, &embedding_storage[0],
                        get(edge_index, g), e1_vis);

           my_add_edge_visitor<planarGraph, Vertex> e_vis;
           make_maximal_planar(g, &embedding_storage[0],
                        get(vertex_index, g), get(edge_index, g), e_vis);

	   // input has to be maximal planar graph with 2+ nodes
  	   Vertex_Vec_t ordering;
  	   planar_canonical_ordering(g, embedding, std::back_inserter(ordering));

//#if RBGL_DEBUG
  	   Vertex_Vec_t::iterator oi, oi_end = ordering.end();
  	   std::cout << "The planar canonical ordering is: ";

  	   for ( oi = ordering.begin(); oi != oi_end; ++oi)
    	       std::cout << *oi << " ";
  	   std::cout << std::endl;

  	   straight_line_drawing_storage.clear(); straight_line_drawing_storage.resize(num_vertices(g));
  	   straight_line_drawing_t straight_line_drawing
    		   (straight_line_drawing_storage.begin(), get(vertex_index,g));

  	   // Compute the straight line drawing
  	   chrobak_payne_straight_line_drawing(g,
                                      embedding,
                                      ordering.begin(),
                                      ordering.end(),
                                      straight_line_drawing);

  	   std::cout << "The straight line drawing is: " << std::endl;
  	   for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
    	   {
      	      std::cout << *vi << " -> ("
		   << straight_line_drawing[*vi].x << ", "
		   << straight_line_drawing[*vi].y << ")"
		   << std::endl;
    	   }
//#endif

	   SEXP ans;
	   PROTECT(ans = allocVector(INTSXP, ordering.size()));

	   for ( int i = 0; i < ordering.size(); i++ )
	      INTEGER(ans)[i] = ordering[i];

	   UNPROTECT(1);
	   return ans;
	}
	else
	{
	   std::cout << "Input graph is not planar" << std::endl;

	   SEXP ans;
	   PROTECT(ans = NEW_INTEGER(1));

	   INTEGER(ans)[0] = 0;

	   UNPROTECT(1);
	   return ans;
	}
     }

     SEXP chrobakPayneStraightLineDrawing(SEXP num_verts_in,
                        SEXP num_edges_in, SEXP R_edges_in )
     {
	planarGraph g;
	initPlanarGraph(&g, num_verts_in, num_edges_in, R_edges_in);

  	embedding_storage.clear(); embedding_storage.resize(num_vertices(g));
  	embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));

	// input must be maximal planar graph with 3+ vertices
  	if ( boyer_myrvold_planarity_test(
		   boyer_myrvold_params::graph = g,
                   boyer_myrvold_params::embedding = embedding) )
	{
           my_add_edge_visitor<planarGraph, Vertex> e1_vis;

           make_connected(g, get(vertex_index, g), e1_vis);

           make_biconnected_planar(g, &embedding_storage[0],
                        get(edge_index, g), e1_vis);

           my_add_edge_visitor<planarGraph, Vertex> e_vis;
           make_maximal_planar(g, &embedding_storage[0],
                        get(vertex_index, g), get(edge_index, g), e_vis);

  	   std::vector< Vertex > ordering;
  	   planar_canonical_ordering(g, embedding, std::back_inserter(ordering));

  	   // property map to hold the mapping from vertices to coord_t's
  	   straight_line_drawing_storage.clear();
	   straight_line_drawing_storage.resize(num_vertices(g));
  	   straight_line_drawing_t straight_line_drawing
        	   (straight_line_drawing_storage.begin(), get(vertex_index,g));

  	   // Compute the straight line drawing
  	   chrobak_payne_straight_line_drawing(g,
                                      embedding,
                                      ordering.begin(),
                                      ordering.end(),
                                      straight_line_drawing);

//#if RBGL_DEBUG
  	   std::cout << "The straight line drawing is: " << std::endl;
  	   for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
    	   {
      	      std::cout << *vi << " -> ("
		<< straight_line_drawing[*vi].x << ", "
		<< straight_line_drawing[*vi].y << ")"
                << std::endl;
    	   }
//#endif

	   SEXP ans;
	   PROTECT(ans = allocMatrix(INTSXP, 2, num_vertices(g)));

	   int j = 0;
  	   for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
	   {
	      INTEGER(ans)[j++] = straight_line_drawing[*vi].x;
	      INTEGER(ans)[j++] = straight_line_drawing[*vi].y;
	   }

	   UNPROTECT(1);
	   return ans;
	}
	else
	{
	   std::cout << "Input graph is not planar" << std::endl;

	   SEXP ans;
	   PROTECT(ans = NEW_INTEGER(1));

	   INTEGER(ans)[0] = 0;

	   UNPROTECT(1);
	   return ans;
	}
     }

     SEXP isStraightLineDrawing(SEXP num_verts_in,
                        SEXP num_edges_in, SEXP R_edges_in,
			SEXP drawing_in )
     {
	planarGraph g;
	initPlanarGraph(&g, num_verts_in, num_edges_in, R_edges_in);

	bool rstl;

  	straight_line_drawing_storage.clear();
	straight_line_drawing_storage.resize(num_vertices(g));
  	straight_line_drawing_t straight_line_drawing
		(straight_line_drawing_storage.begin(), get(vertex_index,g) );

	for ( int i = 0, j = 0; i < num_vertices(g); i++ )
	{
	   straight_line_drawing[i].x = INTEGER(drawing_in)[j++];
	   straight_line_drawing[i].y = INTEGER(drawing_in)[j++];
      	    std::cout << i << " -> ("
		<< straight_line_drawing[i].x << ", "
		<< straight_line_drawing[i].y << ")"
                << std::endl;
	}

  	// Verify that the drawing is actually a plane drawing
  	if (is_straight_line_drawing(g, straight_line_drawing))
	{
    		std::cout << "Is a plane drawing." << std::endl;
		rstl = 1;
	}
  	else
	{
    		std::cout << "Is not a plane drawing." << std::endl;
		rstl = 0;
	}

	SEXP ans;
	PROTECT(ans = NEW_INTEGER(1));

	INTEGER(ans)[0] = rstl;
	UNPROTECT(1);
	return ans;
     }

     SEXP isKuratowskiSubgraph(SEXP num_verts_in,
                        SEXP num_edges_in, SEXP R_edges_in )
     {
	planarGraph g;
	initPlanarGraph(&g, num_verts_in, num_edges_in, R_edges_in);

  	// Initialize the interior edge index
	e_index = get(edge_index, g);
  	edge_count = 0;
  	for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    		put(e_index, *ei, edge_count++);

	bool is_planar = 0, is_kuratowski = 0;

  	Edge_Vec_t kuratowski_edges;
  	if ( boyer_myrvold_planarity_test(
		   boyer_myrvold_params::graph = g,
                   boyer_myrvold_params::kuratowski_subgraph =
                                       std::back_inserter(kuratowski_edges)) )
	{
	   is_planar = 1;
    	   std::cout << "Input graph is planar" << std::endl;
	}
  	else
    	{
	   is_planar = 0;
      	   std::cout << "Input graph is not planar" << std::endl;

      	   std::cout << "Edges in the Kuratowski subgraph: ";

      	   Edge_Vec_t::iterator ki, ki_end = kuratowski_edges.end();
      	   for ( ki = kuratowski_edges.begin(); ki != ki_end; ++ki )
              std::cout << *ki << " ";
      	   std::cout << std::endl;

      	   std::cout << "Is a kuratowski subgraph? ";
      	   if (is_kuratowski_subgraph(g, kuratowski_edges.begin(), kuratowski_edges.end()))
	   {
              std::cout << "Yes." << std::endl;
	      is_kuratowski = 1;
	   }
      	   else
	   {
              std::cout << "No." << std::endl;
	      is_kuratowski = 0;
	   }
    	}

	SEXP ans1, ans2, k_vec, ansList;

	PROTECT(ansList = allocVector(VECSXP,3));

	PROTECT(ans1 = NEW_INTEGER(1));
	INTEGER(ans1)[0] = is_planar;
	SET_VECTOR_ELT(ansList,0,ans1);

	PROTECT(ans2 = NEW_INTEGER(1));
	INTEGER(ans2)[0] = is_kuratowski;
	SET_VECTOR_ELT(ansList,1,ans2);

	PROTECT(k_vec = allocMatrix(INTSXP, 2, kuratowski_edges.size()));

	int i, j = 0;
	Edge_Vec_t::iterator ki, ki_end = kuratowski_edges.end();
  	for ( ki = kuratowski_edges.begin(); ki != ki_end; ++ki )
	{
	      INTEGER(k_vec)[j++] = source(*ki, g);
	      INTEGER(k_vec)[j++] = target(*ki, g);
	}

	SET_VECTOR_ELT(ansList,2, k_vec);

	UNPROTECT(4);
	return ansList;
     }

     SEXP makeConnected(SEXP num_verts_in,
                        SEXP num_edges_in, SEXP R_edges_in )
     {
	planarGraph g;
	initPlanarGraph(&g, num_verts_in, num_edges_in, R_edges_in);

	my_add_edge_visitor<planarGraph, Vertex> e_vis;

	make_connected(g, get(vertex_index, g), e_vis);

  	for ( int i = 0; i < e_vis.e_vis.size(); i++ )
  	{
            std::cout << e_vis.e_vis[i].first << " "
                  << e_vis.e_vis[i].second
                  << std::endl;
  	}

	// output is a graph
	SEXP ans;
	PROTECT(ans = allocMatrix(INTSXP, 2, num_edges(g)));

	int i;
  	for( i = 0, tie(ei, ei_end) = edges(g); ei != ei_end; ++ei )
	{
	   INTEGER(ans)[i++] = source(*ei, g);
	   INTEGER(ans)[i++] = target(*ei, g);
	}

	UNPROTECT(1);
	return ans;
     }

     SEXP makeBiconnectedPlanar(SEXP num_verts_in,
                        SEXP num_edges_in, SEXP R_edges_in )
     {
	planarGraph g;
	initPlanarGraph(&g, num_verts_in, num_edges_in, R_edges_in);

  	//Initialize the interior edge index
	e_index = get(edge_index, g);
  	edge_count = 0;
  	for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    		put(e_index, *ei, edge_count++);

  	embedding_storage.clear();
	embedding_storage.resize(num_vertices(g));

	bool is_planar = 0;

  	if ( boyer_myrvold_planarity_test(
		   boyer_myrvold_params::graph = g,
                   boyer_myrvold_params::embedding = &embedding_storage[0]) )
	{
    	   std::cout << "Input graph is planar" << std::endl;

	   is_planar = 1;

	   // input must be connected planar graph
	   my_add_edge_visitor<planarGraph, Vertex> e_vis;

	   make_connected(g, get(vertex_index, g), e_vis);

  	   make_biconnected_planar(g, &embedding_storage[0], get(edge_index, g), e_vis);

  	   for ( int i = 0; i < e_vis.e_vis.size(); i++ )
  	   {
              std::cout << e_vis.e_vis[i].first << " "
                  	<< e_vis.e_vis[i].second
                  	<< std::endl;
  	   }

  	   if (boyer_myrvold_planarity_test(g))
    	      std::cout << "Also, the graph is still planar." << std::endl;
  	   else
    	      std::cout << "But the graph is not still planar." << std::endl;
	}
  	else
	{
    	   std::cout << "Input graph is not planar" << std::endl;

	   is_planar = 0;
	}

	// output is a graph
        SEXP ans, b_vec, ansList;

        PROTECT(ansList = allocVector(VECSXP,2));

        PROTECT(ans = NEW_INTEGER(1));
        INTEGER(ans)[0] = is_planar;
        SET_VECTOR_ELT(ansList,0,ans);

	PROTECT(b_vec = allocMatrix(INTSXP, 2, num_edges(g)));

	int i;
  	for( i = 0, tie(ei, ei_end) = edges(g); ei != ei_end; ++ei )
	{
	   INTEGER(b_vec)[i++] = source(*ei, g);
	   INTEGER(b_vec)[i++] = target(*ei, g);
	}

        SET_VECTOR_ELT(ansList,1,b_vec);

	UNPROTECT(3);
	return ansList;
     }

     SEXP makeMaximalPlanar(SEXP num_verts_in,
                        SEXP num_edges_in, SEXP R_edges_in )
     {
	planarGraph g;
	initPlanarGraph(&g, num_verts_in, num_edges_in, R_edges_in);

  	//Initialize the interior edge index
	e_index = get(edge_index, g);
  	edge_count = 0;
  	for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    		put(e_index, *ei, edge_count++);

	bool is_planar = 0;

  	embedding_storage.clear();
	embedding_storage.resize(num_vertices(g));
  	if ( boyer_myrvold_planarity_test(
		   boyer_myrvold_params::graph = g,
                   boyer_myrvold_params::embedding = &embedding_storage[0]) )
	{
    	   std::cout << "Input graph is planar" << std::endl;
	   is_planar = 1;

  	   // input to make_maximal_planar must be biconnected
	   my_add_edge_visitor<planarGraph, Vertex> e1_vis;

	   make_connected(g, get(vertex_index, g), e1_vis);

  	   make_biconnected_planar(g, &embedding_storage[0],
			get(edge_index, g), e1_vis);

  	   // Re-initialize the edge index, since we just added a few edges
  	   edge_count = 0;
  	   for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    	   	put(e_index, *ei, edge_count++);

  	   embedding_storage.clear();
	   embedding_storage.resize(num_vertices(g));
  	   if ( boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                           boyer_myrvold_params::embedding = &embedding_storage[0]))
	   {
    	      std::cout << "After calling make_biconnected, the graph is still planar"
              		<< std::endl;
	   }
  	   else
	   {
	      // should NOT be here
    	      std::cout << "After calling make_biconnected, the graph is not planar"
              		<< std::endl;
	   }

  	   // input must be biconnected w/ 3+ nodes
	   my_add_edge_visitor<planarGraph, Vertex> e_vis;
  	   make_maximal_planar(g, &embedding_storage[0],
			get(vertex_index, g), get(edge_index, g), e_vis);

	}
  	else
	{
    	   std::cout << "Input graph is not planar" << std::endl;
	   is_planar = 0;
	}

	// output is a graph
        SEXP ans, b_vec, ansList;

        PROTECT(ansList = allocVector(VECSXP,2));

        PROTECT(ans = NEW_INTEGER(1));
        INTEGER(ans)[0] = is_planar;
        SET_VECTOR_ELT(ansList,0,ans);

        PROTECT(b_vec = allocMatrix(INTSXP, 2, num_edges(g)));

        int i;
        for( i = 0, tie(ei, ei_end) = edges(g); ei != ei_end; ++ei )
        {
           INTEGER(b_vec)[i++] = source(*ei, g);
           INTEGER(b_vec)[i++] = target(*ei, g);
        }

        SET_VECTOR_ELT(ansList,1,b_vec);

        UNPROTECT(3);
        return ansList;

     }

    SEXP edmondsMaxCardinalityMatching(SEXP num_verts_in,
				SEXP num_edges_in,SEXP R_edges_in)
    {
	typedef graph_traits<Graph_ui>::vertex_descriptor Vertex;

	Graph_ui g(num_verts_in, num_edges_in, R_edges_in);

	graph_traits<Graph_ui>::vertex_iterator vi, vi_end;

	std::vector<Vertex> mate(num_vertices(g));

	bool is_max = checked_edmonds_maximum_cardinality_matching(g, &mate[0]);

	SEXP ans, b_vec, ansList;

	PROTECT(ansList = allocVector(VECSXP,2));

        PROTECT(ans = NEW_INTEGER(1));
        INTEGER(ans)[0] = is_max;
        SET_VECTOR_ELT(ansList,0,ans);

        int m_cnt;
        for( m_cnt = 0, tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi )
        {
	   if ( mate[*vi] != graph_traits< Graph_ui >::null_vertex() &&
		*vi < mate[*vi] )
	   m_cnt++;
	}

        PROTECT(b_vec = allocMatrix(INTSXP, 2, m_cnt));

	int i;
        for( i = 0, tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi )
        {
	   if ( mate[*vi] != graph_traits< Graph_ui >::null_vertex() &&
		*vi < mate[*vi] )
	   {
           	INTEGER(b_vec)[i++] = *vi;
           	INTEGER(b_vec)[i++] = mate[*vi];
	   }
        }

        SET_VECTOR_ELT(ansList,1,b_vec);

	UNPROTECT(3);
	return ansList;

    }
}


#include <random>
#include "../src/codons/swap_codons.cpp"
#define BOOST_TEST_MODULE "Codons module test"
#include <boost/test/unit_test.hpp>

using namespace std;

BOOST_AUTO_TEST_CASE(SwapSynonymousCodonsTest)
{
    string sequence = "TTGTGCATG";

    // Basic test
    string swapped = SwapSynonymousCodons(sequence, mt19937(444));
    BOOST_CHECK_EQUAL(swapped, "CTTTGCATG");

    // Different seed = different outcome
    string swapped2 = SwapSynonymousCodons(sequence, mt19937(123));
    BOOST_CHECK_EQUAL(swapped2, "CTATGCATG");

    // Test with short string
    string swapped4 = SwapSynonymousCodons("AA", mt19937(444));
    BOOST_CHECK_EQUAL(swapped4, "AA");

    // Test when string length is not a multiple of 3
    string swapped5 = SwapSynonymousCodons("TTGTGCATGC", mt19937(444));
    BOOST_CHECK_EQUAL(swapped5, "CTTTGCATGC");

    // Test with stop codon
    string swapped6 = SwapSynonymousCodons("CTTTGCTAG", mt19937(666));
    BOOST_CHECK_EQUAL(swapped6, "CTGTGCTAA");

    // Test with unknown codon
    string swapped7 = SwapSynonymousCodons("CTTXTCTAG", mt19937(666));
    BOOST_CHECK_EQUAL(swapped7, "CTGXTCTAA");
}

BOOST_AUTO_TEST_CASE(ShuffleCodonsTest)
{
    string sequence = "TTGTGCATGCCC";

    // Basic test
    string shuffled = ShuffleCodons(sequence, mt19937(444));
    BOOST_CHECK_EQUAL(shuffled, "CCCTGCTTGATG");

    // Different seed = different outcome
    string shuffled2 = ShuffleCodons(sequence, mt19937(123));
    BOOST_CHECK_EQUAL(shuffled2, "ATGTTGTGCCCC");

    // Test with short string
    string shuffled4 = ShuffleCodons("AA", mt19937(444));
    BOOST_CHECK_EQUAL(shuffled4, "AA");

    // Test when string length is not a multiple of 3
    BOOST_CHECK_EQUAL(ShuffleCodons("TTGTGCATGCCCA", mt19937(444)), "CCCTGCTTGATGA");
    BOOST_CHECK_EQUAL(ShuffleCodons("TTGTGCATGCCCAT", mt19937(444)), "CCCTGCTTGATGAT");
}


// Copyright 2018-2020 Drexel University
// Author: Geoffrey Mainland <mainland@drexel.edu>

#ifndef CHANNEL_H_
#define CHANNEL_H_

#include <complex>
#include <vector>

#include <boost/functional/hash.hpp>

struct Channel {
    Channel() : fc(0.0), bw(0.0) {};
    Channel(double fc_, double bw_) : fc(fc_), bw(bw_) {};

    bool operator ==(const Channel &other) const
    {
        return fc == other.fc && bw == other.bw;
    }

    bool operator !=(const Channel &other) const
    {
        return !(*this == other);
    }

    bool operator <(const Channel &other) const
    {
        return fc < other.fc;
    }

    bool operator >(const Channel &other) const
    {
        return fc > other.fc;
    }

    bool intersects(const Channel &other) const
    {
        double start = fc - bw/2.;
        double end = fc + bw/2.;
        double other_start = other.fc - other.bw/2.;
        double other_end = other.fc + other.bw/2.;

        return (start < other_end) && (end > other_start);
    }

    /** @brief Frequency shift from center */
    double fc;

    /** @brief Bandwidth */
    double bw;
};

#if !defined(DOXYGEN)
// Doxygen complains:
// warning: Internal inconsistency: scope for class std::hash< Channel > not found!
template<>
struct std::hash<Channel> {
    size_t operator()(const Channel &chan)
    {
        std::size_t h = std::hash<double>{}(chan.fc);

        boost::hash_combine(h, std::hash<double>{}(chan.bw));
        return h;
    }
};
#endif /* !defined(DOXYGEN) */

using C = std::complex<float>;

/** @brief FIR taps */
using Taps = std::vector<C>;

/** @brief A vector of pairs of channels and taps */
using Channels = std::vector<std::pair<Channel, Taps>>;

#endif /* CHANNEL_H_ */


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

  Copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-boolean
    This function object conditionally returns @c t (respectively @c f) if @c c
    is @ref True (respectively  @ref False)

    @pre If both parameters are pack, they must have same cardinal.

    @par Header <boost/simd/function/if_else.hpp>

    @par Example:

      @snippet if_else.cpp if_else

    @par Possible output:

      @snippet if_else.txt if_else

  **/
  Value1 if_else(Value0 const& c, Value1 const& t, Value1 const& f);
} }
#endif

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

#endif


/*************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                           *
 * All rights reserved.                                                  *
 *                                                                       *
 * This file is part of the mfmg libary. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the *
 * top-level directory                                                   *
 *                                                                       *
 * SPDX-License-Identifier: BSD-3-Clause                                 *
 *************************************************************************/

#ifndef MFMG_ANASAZI_TRAITS_HPP
#define MFMG_ANASAZI_TRAITS_HPP

#include <mfmg/common/exceptions.hpp>
#include <mfmg/dealii/multivector.hpp>

#include <deal.II/lac/sparse_matrix.h>

#include <AnasaziMultiVecTraits.hpp>
#include <AnasaziOperatorTraits.hpp>

#include <algorithm>
#include <random>
#include <vector>

namespace Anasazi
{
using mfmg::ASSERT;
using mfmg::ASSERT_THROW_NOT_IMPLEMENTED;

template <typename VectorType>
class MultiVecTraits<double, mfmg::MultiVector<VectorType>>
{
  using MultiVectorType = mfmg::MultiVector<VectorType>;

public:
  static Teuchos::RCP<MultiVectorType> Clone(const MultiVectorType &mv,
                                             const int numvecs)
  {
    return Teuchos::rcp(new MultiVectorType(numvecs, mv.size()));
  }

  static Teuchos::RCP<MultiVectorType> CloneCopy(const MultiVectorType &mv)
  {
    auto n_vectors = mv.n_vectors();

    auto new_mv = Teuchos::rcp(new MultiVectorType(n_vectors));
    for (int i = 0; i < n_vectors; i++)
      *(*new_mv)[i] = *mv[i];

    return new_mv;
  }

  static Teuchos::RCP<MultiVectorType> CloneCopy(const MultiVectorType &mv,
                                                 const std::vector<int> &index)
  {
    int n_vectors = index.size();

    auto new_mv = Teuchos::rcp(new MultiVectorType(n_vectors));
    for (int i = 0; i < n_vectors; i++)
      *(*new_mv)[i] = *mv[index[i]];

    return new_mv;
  }

  static Teuchos::RCP<MultiVectorType> CloneCopy(const MultiVectorType &mv,
                                                 const Teuchos::Range1D &index)
  {
    auto n_vectors = index.size();

    auto new_mv = Teuchos::rcp(new MultiVectorType(n_vectors));
    for (int i = 0; i < n_vectors; i++)
      *(*new_mv)[i] = *mv[index.lbound() + i];

    return new_mv;
  }

  static Teuchos::RCP<MultiVectorType>
  CloneViewNonConst(MultiVectorType &mv, const std::vector<int> &index)
  {
    int n_vectors = index.size();

    auto new_mv = Teuchos::rcp(new MultiVectorType(n_vectors));
    for (int i = 0; i < n_vectors; i++)
      (*new_mv)[i] = mv[index[i]];

    return new_mv;
  }

  static Teuchos::RCP<MultiVectorType>
  CloneViewNonConst(MultiVectorType &mv, const Teuchos::Range1D &index)
  {
    auto n_vectors = index.size();

    auto new_mv = Teuchos::rcp(new MultiVectorType(n_vectors));
    for (int i = 0; i < n_vectors; i++)
      (*new_mv)[i] = mv[index.lbound() + i];

    return new_mv;
  }

  static Teuchos::RCP<const MultiVectorType>
  CloneView(const MultiVectorType &mv, const std::vector<int> &index)
  {
    int n_vectors = index.size();

    auto new_mv = Teuchos::rcp(new MultiVectorType(n_vectors));
    for (int i = 0; i < n_vectors; i++)
      (*new_mv)[i] = mv[index[i]];

    return new_mv;
  }

  static Teuchos::RCP<const MultiVectorType>
  CloneView(MultiVectorType &mv, const Teuchos::Range1D &index)
  {
    auto n_vectors = index.size();

    auto new_mv = Teuchos::rcp(new MultiVectorType(n_vectors));
    for (int i = 0; i < n_vectors; i++)
      (*new_mv)[i] = mv[index.lbound() + i];

    return new_mv;
  }

  static ptrdiff_t GetGlobalLength(const MultiVectorType &mv)
  {
    return mv.size();
  }

  static int GetNumberVecs(const MultiVectorType &mv) { return mv.n_vectors(); }

  static void MvTimesMatAddMv(const double alpha, const MultiVectorType &A,
                              const Teuchos::SerialDenseMatrix<int, double> &B,
                              const double beta, MultiVectorType &mv)
  {
    auto n_vectors = A.n_vectors();

    ASSERT(B.numRows() == n_vectors, "");
    ASSERT(B.numCols() == mv.n_vectors(), "");
    ASSERT(mv.size() == A.size(), "");

    for (int j = 0; j < B.numCols(); j++)
    {
      *mv[j] *= beta;
      for (int k = 0; k < n_vectors; k++)
        (*mv[j]).add(alpha * B(k, j), *A[k]);
    }
  }

  static void MvAddMv(const double alpha, const MultiVectorType &A,
                      const double beta, const MultiVectorType &B,
                      MultiVectorType &mv)
  {
    auto n_vectors = A.n_vectors();
    ASSERT(A.size() == B.size(), "");
    ASSERT(B.n_vectors() == n_vectors, "");
    ASSERT(mv.n_vectors() == B.n_vectors(), "");
    ASSERT(mv.size() == B.size(), "");

    for (int i = 0; i < n_vectors; i++)
    {
      *mv[i] = *A[i];
      (*mv[i]).sadd(alpha, beta, *B[i]);
    }
  }

  static void MvScale(MultiVectorType &mv, const double alpha)
  {
    for (int i = 0; i < mv.n_vectors(); i++)
      *mv[i] *= alpha;
  }

  static void MvScale(MultiVectorType &mv, const std::vector<double> &alpha)
  {
    auto n_vectors = mv.n_vectors();

    ASSERT(int(alpha.size()) == n_vectors, "");
    for (int i = 0; i < n_vectors; i++)
      *mv[i] *= alpha[i];
  }

  static void MvTransMv(const double alpha, const MultiVectorType &A,
                        const MultiVectorType &B,
                        Teuchos::SerialDenseMatrix<int, double> &C)
  {
    ASSERT(A.size() == B.size(), "");
    ASSERT(C.numRows() == A.n_vectors(), "");
    ASSERT(C.numCols() == B.n_vectors(), "");

    for (int i = 0; i < A.n_vectors(); i++)
      for (int j = 0; j < B.n_vectors(); j++)
        C(i, j) = alpha * (*A[i] * *B[j]);
  }

  static void MvDot(const MultiVectorType &mv, const MultiVectorType &A,
                    std::vector<double> &b)
  {
    auto n_vectors = mv.n_vectors();
    ASSERT(A.size() == mv.size(), "");
    ASSERT(A.n_vectors() == n_vectors, "");

    b.resize(n_vectors);
    for (int i = 0; i < n_vectors; i++)
    {
      b[i] = *mv[i] * *A[i];
    }
  }

  static void
  MvNorm(const MultiVectorType &mv,
         std::vector<typename Teuchos::ScalarTraits<double>::magnitudeType>
             &normvec)
  {
    auto n_vectors = mv.n_vectors();

    normvec.resize(n_vectors);
    for (int i = 0; i < n_vectors; i++)
      normvec[i] = (*mv[i]).l2_norm();
  }

  static void SetBlock(const MultiVectorType &A, const std::vector<int> &index,
                       MultiVectorType &mv)
  {
    int n_vectors = index.size();

    ASSERT(A.n_vectors() == n_vectors, "");
    ASSERT(mv.n_vectors() >= n_vectors, "");
    ASSERT(A.size() == mv.size(), "");

    for (int i = 0; i < n_vectors; i++)
      *mv[index[i]] = *A[i];
  }

  static void SetBlock(const MultiVectorType &A, const Teuchos::Range1D &index,
                       MultiVectorType &mv)
  {
    auto n_vectors = index.size();

    ASSERT(A.n_vectors() == index.size(), "");
    ASSERT(mv.n_vectors() >= index.size(), "");
    ASSERT(A.size() == mv.size(), "");

    for (int i = 0; i < n_vectors; i++)
      *mv[index.lbound() + i] = *A[i];
  }

  static void Assign(const MultiVectorType &A, MultiVectorType &mv)
  {
    std::ignore = A;
    std::ignore = mv;
    ASSERT_THROW_NOT_IMPLEMENTED();
  }

  static void MvRandom(MultiVectorType &mv)
  {
    auto n_vectors = mv.n_vectors();

    std::mt19937 gen(1337);
    std::uniform_real_distribution<double> dist(0, 1);
    for (int i = 0; i < n_vectors; i++)
    {
      auto &v = *mv[i];
      std::transform(v.begin(), v.end(), v.begin(),
                     [&](auto &) { return dist(gen); });
    }
  }

  static void MvInit(MultiVectorType &mv, const double alpha = 0.)
  {
    int n_vectors = mv.n_vectors();
    for (int i = 0; i < n_vectors; i++)
      *mv[i] = alpha;
  }

  static void MvPrint(const MultiVectorType &mv, std::ostream &os)
  {
    std::ignore = mv;
    std::ignore = os;
    ASSERT_THROW_NOT_IMPLEMENTED();
  }
};

template <typename VectorType, typename ValueType>
class OperatorTraits<double, mfmg::MultiVector<VectorType>,
                     dealii::SparseMatrix<ValueType>>
{
  using MultiVectorType = mfmg::MultiVector<VectorType>;
  using OperatorType = dealii::SparseMatrix<ValueType>;

public:
  static void Apply(const OperatorType &op, const MultiVectorType &x,
                    MultiVectorType &y)
  {
    auto n_vectors = x.n_vectors();

    ASSERT(x.size() == y.size(), "");
    ASSERT(y.n_vectors() == n_vectors, "");

    for (int i = 0; i < n_vectors; i++)
      op.vmult(*y[i], *x[i]);
  };
};

} // namespace Anasazi

#endif


#include <iostream>
#include <string>
#include <vector>
#include "cnpy.h"
#include "float_conversion.h"
#include <fstream>
#include <chrono>
#include <thread>
#include <mutex>
#include <queue>
#include <algorithm>
#include <random>
#include <Eigen/Dense>
#include <sys/stat.h>

using namespace std;
using namespace Eigen;
using namespace std::string_literals;

constexpr size_t BLUE   =  0;
constexpr size_t GREEN  =  1;
constexpr size_t RED    =  2;
constexpr size_t RE1    =  3;
constexpr size_t RE2    =  4;
constexpr size_t RE3    =  5;
constexpr size_t NIR    =  6;
constexpr size_t SWIR1  =  7;
constexpr size_t SWIR2  =  8;
constexpr size_t CLOUDS =  9;
constexpr size_t SHAPE  = 10;

constexpr size_t NUM_THREADS = 8;
constexpr size_t BANDS = 6;

const array<float, 3> QUANTILES {0.2f, 0.5f, 0.8f};
const size_t QS = QUANTILES.size();

typedef array<float, BANDS> Point;
typedef vector<Point> PointCloud;
typedef pair<vector<float>, vector<PointCloud>> timeseries;

constexpr float START_DAY = 31 + 28; // Only use images starting in March
constexpr float MAX_DAY   = 365 - START_DAY - 31; // End with November

time_t basetime;

struct result
{
  string number;
  char type;
  vector<float> features;
};

array<thread, NUM_THREADS> threads;
array<string, NUM_THREADS> current;
array<vector<result>, NUM_THREADS> results;
mutex queue_lock;
queue<string> todo;

#define DATA(b) half_to_float(raw.data<uint16_t>()[ b * W * H * T + x * H * T + y * T + t ])

float clamp(float x, float min=-100, float max=100)
{
  if (x > max)
    return max;
  else if (x < min)
    return min;
  else if (isnormal(x))
    return x;
  else
    return 0;
}

timeseries parse(string filename)
{
  vector<float> timestamps;
  vector<PointCloud> all_points;

  cnpy::NpyArray raw = cnpy::npy_load(filename + ".npy"s);
  ifstream indexfile(filename + ".idx"s);
  if(indexfile.is_open())
  {
    string line;
    // Skip two lines
    getline(indexfile, line);
    getline(indexfile, line);
    // Skip the label
    getline(indexfile, line, '~');
    // Get entries
    while(getline(indexfile, line, ','))
    {
      struct tm tm;
      strptime(line.c_str(), "%Y-%m-%dT%H:%M:%S", &tm);
      float diff = difftime(mktime(&tm), basetime) / (60 * 60 * 24) - START_DAY;
      timestamps.push_back(diff);
    }
    indexfile.close();
  }

  size_t W = raw.shape[1];
  size_t H = raw.shape[2];
  size_t T = raw.shape[3];
  vector<float> used_timestamps;

  assert(timestamps.size() == T);

  for(size_t t = 0; t < T; ++t)
  {
    if(timestamps[t] <= 0)
      continue;
    PointCloud points;
    size_t glitchy_points = 0,
           cloudy_points = 0,
           total_points = 0;
    for(size_t x = 0; x < W; ++x) for(size_t y = 0; y < H; ++y)
    {
      if(DATA(SHAPE) > 0)
      {
        ++total_points;
        if(DATA(CLOUDS) > 0)
          ++cloudy_points;
        float red  = DATA(RED),
              grn  = DATA(GREEN),
              blu  = DATA(BLUE),
              nir  = DATA(NIR),
              swir1= DATA(SWIR1),
              swir2= DATA(SWIR2),
              re1  = DATA(RE1),
              re2  = DATA(RE2),
              re3  = DATA(RE3);
        if(red == 0 && grn == 0 && blu == 0)
          ++glitchy_points;
        float ndvi = (nir - red) / (nir + red); // Normalized Difference Vegetation Index
        float cai = (swir1 - swir2) / (swir1 + swir2); // Cellulose Absorption index
        float n = (2 * (nir * nir - red * red) + 1.5f * nir + 0.5 * red ) / (nir + red + 0.5f);
        float gemi = n * (1.0f - 0.25f * n) - (red - 0.125f) / (1 - red); // Global Environment Monitoring Index
        float gli = (2 * grn - red - blu) / (2 * grn + red + blu); // Green Leaf Index
        float cvi = (nir * red) / (grn * grn); // Chlorophyll vegetation index
        float ccci = ((nir - re1) / (nir + re1)) / ndvi; // Canopy Chlorophyll Content Index
        float dswi = (re3 - grn) / (swir1 + red); // Disease-Water Stress Index 5
        float nd790_670 = (re3 - red) / (re3 + red); // Normalized Difference 790 / 670
        float ndwi = (nir - swir2) / (nir + swir2); // Normalized Difference Water Index
        points.push_back({
            clamp(ndvi),
            // clamp(cai),
            clamp(gemi),
            clamp(gli),
            clamp(cvi),
            // clamp(ccci),
            clamp(dswi),
            // clamp(nd790_670),
            clamp(ndwi),
        });
      }
    }
    if(cloudy_points > 0.2 * total_points)
      continue;
    if(glitchy_points > 0.2 * total_points)
      continue;
    all_points.push_back(points);
    used_timestamps.push_back(timestamps[t] / MAX_DAY);
  }

  return make_pair(used_timestamps, all_points);
}

inline float hat(float t, float center, float width)
{
  return max(1 - abs(t-center) / width, 0.0f) / width;
}

const vector<float> get_features(timeseries data, float subsample=1.0f)
{
  vector<float> time = data.first;
  vector<PointCloud> point_series  = data.second;
  int count = point_series[0].size();

  vector<bool> used(count, true);
  if(subsample < 1.0f)
  {
    count = 0;
    random_device rd;
    mt19937 random(rd());
    uniform_real_distribution<> real(0.0, 1.0);
    for(auto &&b : used)
    {
      b = real(random) < subsample;
      count += b;
    }
  }

  vector<float> features;
  array<vector<float>, BANDS * QS> Q;

  for(PointCloud point_cloud : point_series)
  {
    for(size_t band = 0; band < BANDS; ++band)
    {
      vector<float> points;
      for(size_t i = 0; i < point_cloud.size(); ++i)
      {
        if(used[i] && !isnan(point_cloud[i][band]))
        {
          points.push_back(point_cloud[i][band]);
        }
      }
      if(points.size() > 0)
      {
        sort(points.begin(), points.end());
        for(size_t q = 0; q < QS; ++q)
          Q[band * QS + q].push_back(points[size_t(points.size() * QUANTILES[q])]);
      }
      else
      {
        for(float q : QUANTILES)
          Q[band * QS + q].push_back(0);
      }
    }
  }

  for(auto q : Q)
  {
    constexpr size_t DIM = 8;
    // vector<complex<float>> coefficients(NUM_FOURIER, 0);
    Eigen::Matrix<float, Eigen::Dynamic, DIM> model(q.size(), DIM);
    Eigen::Matrix<float, Eigen::Dynamic, 1> target(q.size(), 1);
    for(size_t k = 0; k < q.size(); ++k)
    {
      target(k, 0) = q[k];
      model(k, 0)  = hat(time[k], 0.0f  , 1.0f  ); // Left Edge
      model(k, 1)  = hat(time[k], 1.0f  , 1.0f  ); // Right Edge
      model(k, 2)  = hat(time[k], 0.5f  , 0.5f  ); // 1/2
      model(k, 3)  = hat(time[k], 0.25f , 0.25f ); // 1/4
      model(k, 4)  = hat(time[k], 0.75f , 0.25f ); // 3/4
      model(k, 5)  = hat(time[k], 0.125f, 0.125f); // 1/8
      model(k, 6)  = hat(time[k], 0.375f, 0.125f); // 3/8
      model(k, 7)  = hat(time[k], 0.625f, 0.125f); // 5/8
      // model(k, 8)  = hat(time[k], 0.875f, 0.125f); // 7/8
    }
    Eigen::Matrix<float, DIM, DIM> H = model.transpose() * model;
    H = H.completeOrthogonalDecomposition().pseudoInverse();
    Eigen::Matrix<float, DIM, 1> weight = H * model.transpose() * target;
    for(size_t k = 0; k < DIM; ++k)
    {
      features.push_back(weight(k, 0));
    }
  }
  return features;
}

inline bool file_exists (const std::string& name)
{
  struct stat buffer;
  return (stat (name.c_str(), &buffer) == 0);
}

void work(size_t thread_id)
{
  current[thread_id] = "Initializing";
  bool running = true;
  string number = "";
  while(running)
  {
    {
      lock_guard<mutex> lock(queue_lock);
      if(todo.empty())
      {
        break;
      } else {
        number = todo.front();
        current[thread_id] = number;
        todo.pop();
      }
    }

    string filename = "/home/konrad/dev/remote_sensing/ibiss_processed/cubes/" + number;
    if(!file_exists(filename + ".npy"s))
      continue;
    timeseries data = parse(filename);

    results[thread_id].push_back({ number, 'F', get_features(data, 0.5) });
  }
  current[thread_id] = "Done";
}

int main(int argc, char **argv)
{
  struct tm tm;
  strptime("2017-01-01T00:00:00", "%Y-%m-%dT%H:%M:%S", &tm);
  basetime = mktime(&tm);

  size_t count = 0;
  if(argc == 1)
  {
    string number = "";
    while(getline(cin, number))
    {
      todo.push(number);
      ++count;
    }
  }
  else
  {
    string number = "";
    ifstream input(argv[1]);
    if(!input.is_open())
      return 1;
    while(getline(input, number))
    {
      todo.push(number);
      ++count;
    }
    input.close();
  }

  cout << "Beginning feature extraction" << endl;
  auto features = vector<pair<string, vector<float>>>(count);
  size_t num = features.size();

  for(size_t i = 0; i < 100; ++i)
    cout << '_';
  cout << endl;

  for(size_t i = 0; i < NUM_THREADS; ++i)
  {
    threads[i] = thread(work, i);
  }

  size_t progress = 100;
  while(progress > 0)
  {
    if(progress * num > todo.size() * 100)
    {
      cout << '#' << flush;
      --progress;
    }
    else
    {
      this_thread::sleep_for(100ms);
    }
  }

  cout << endl;
  for(size_t i = 0; i < NUM_THREADS; ++i)
  {
    cout << "Thread (" << i << ") processed " << results[i].size() << " elements" << endl;
  }

  for(size_t i = 0; i < NUM_THREADS; ++i)
  {
    threads[i].join();
  }
  cout << "\nCollected all threads" << endl;

  cout << "\nWriting CSV..." << endl;

  ofstream out("/home/konrad/dev/remote_sensing/data/cfeatures.csv");

  for(size_t i = 0; i < 8*NUM_THREADS; ++i)
    cout << '_';
  cout << endl;

  for(size_t i = 0; i < NUM_THREADS; ++i)
  {
    for(auto p : results[i])
    {
      out << p.number << ',' << p.type;
      for(auto val : p.features)
        out << "," << val;
      out << '\n';
    }
    cout << "########" << flush;
    results[i].clear();
  }
  cout << endl;
  out.close();

  return 0;
}


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

#include <boost/simd/toolbox/reduction/functions/any.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/constants/allbits.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::any_, boost::simd::tag::avx_,
                        (A0),
                        ((simd_<fundamental_<A0>,boost::simd::tag::avx_>))
                       )
  {
    typedef typename meta::scalar_of<A0>::type sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::as_arithmetic<A0>::type aA0;
      typedef typename dispatch::meta::as_integer<aA0>::type iA0;
      return result_type( _mm256_testz_si256( bitwise_cast<iA0>(a0)
                                            , Allbits<iA0>()
                                            ) == 0
                        );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::any_, boost::simd::tag::avx_,
                        (A0),
                        ((simd_<floating_<A0>,boost::simd::tag::avx_>))
                       )
  {
    typedef typename meta::scalar_of<A0>::type sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<A0>::type iA0;
      return result_type(boost::simd::any(bitwise_cast<iA0>(boost::simd::abs(a0))));
    }
  };
} } }

#endif
#endif


#pragma once

#include <px4_posix.h>
#include <controllib/uorb/blocks.hpp>
#include <mathlib/mathlib.h>
#include <systemlib/perf_counter.h>
#include <lib/geo/geo.h>

#ifdef USE_MATRIX_LIB
#include "matrix/Matrix.hpp"
using namespace matrix;
#else
#include <Eigen/Eigen>
using namespace Eigen;
#endif

// uORB Subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>

// uORB Publications
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
//#include <uORB/topics/filtered_bottom_flow.h>
#include <uORB/topics/estimator_status.h>

#define CBRK_NO_VISION_KEY	328754

using namespace control;

//static const size_t HIST_LEN = 2; // each step is 100 ms, gps has 200 ms delay

enum fault_t {
	FAULT_NONE = 0,
	FAULT_MINOR,
	FAULT_SEVERE
};

enum sensor_t {
	SENSOR_BARO = 0,
	SENSOR_GPS,
	SENSOR_LIDAR,
	SENSOR_FLOW,
	SENSOR_SONAR,
	SENSOR_VISION,
	SENSOR_MOCAP
};

class BlockLocalPositionEstimator : public control::SuperBlock
{
//
// The purpose of this estimator is to provide a robust solution for
// indoor flight.
//
// dynamics:
//
//	x(+) = A * x(-) + B * u(+)
//	y_i = C_i*x
//
// kalman filter
//
//	E[xx'] = P
//	E[uu'] = W
//	E[y_iy_i'] = R_i
//
//	prediction
//		x(+|-) = A*x(-|-) + B*u(+)
//		P(+|-) = A*P(-|-)*A' + B*W*B'
//
//	correction
//		x(+|+) =  x(+|-) + K_i * (y_i - H_i * x(+|-) )
//
//
// input:
// 	ax, ay, az (acceleration NED)
//
// states:
// 	px, py, pz , ( position NED)
// 	vx, vy, vz ( vel NED),
// 	bx, by, bz ( TODO accelerometer bias)
// 	tz (TODO terrain altitude)
//
// measurements:
//
// 	sonar: pz (measured d*cos(phi)*cos(theta))
//
// 	baro: pz
//
// 	flow: vx, vy (flow is in body x, y frame)
//
// 	gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
//
// 	lidar: px (actual measured d*cos(phi)*cos(theta))
//
// 	vision: px, py, pz, vx, vy, vz
//
// 	mocap: px, py, pz
//
public:

	// constants
	enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz, X_tz, n_x};
	enum {U_ax = 0, U_ay, U_az, n_u};
	enum {Y_baro_z = 0, n_y_baro};
	enum {Y_lidar_z = 0, n_y_lidar};
	enum {Y_flow_x = 0, Y_flow_y, n_y_flow};
	enum {Y_sonar_z = 0, n_y_sonar};
	enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
	enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
	enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
	enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM, n_poll};

	BlockLocalPositionEstimator();
	void update();
	virtual ~BlockLocalPositionEstimator();

private:
	// prevent copy and assignment
	BlockLocalPositionEstimator(const BlockLocalPositionEstimator &);
	BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &);

	// methods
	// ----------------------------
	void initP();

	// predict the next state
	void predict();

	// correct the state prediction with a measurement
	void correctBaro();
	void correctGps();
	void correctLidar();
	void correctFlow();
	void correctSonar();
	void correctVision();
	void correctMocap();

	// sensor timeout checks
	void checkTimeouts();

	// sensor initialization
	void updateHome();
	void initBaro();
	void initGps();
	void initLidar();
	void initSonar();
	void initFlow();
	void initVision();
	void initMocap();

	// publications
	void publishLocalPos();
	void publishGlobalPos();
	//void publishFilteredFlow();
	void publishEstimatorStatus();

	// attributes
	// ----------------------------

	// subscriptions
	uORB::Subscription<vehicle_status_s> _sub_status;
	uORB::Subscription<actuator_armed_s> _sub_armed;
	uORB::Subscription<vehicle_control_mode_s> _sub_control_mode;
	uORB::Subscription<vehicle_attitude_s> _sub_att;
	uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp;
	uORB::Subscription<optical_flow_s> _sub_flow;
	uORB::Subscription<sensor_combined_s> _sub_sensor;
	uORB::Subscription<parameter_update_s> _sub_param_update;
	uORB::Subscription<manual_control_setpoint_s> _sub_manual;
	uORB::Subscription<home_position_s> _sub_home;
	uORB::Subscription<vehicle_gps_position_s> _sub_gps;
	uORB::Subscription<vision_position_estimate_s> _sub_vision_pos;
	uORB::Subscription<att_pos_mocap_s> _sub_mocap;
	uORB::Subscription<distance_sensor_s> *_distance_subs[ORB_MULTI_MAX_INSTANCES];
	uORB::Subscription<distance_sensor_s> *_sub_lidar;
	uORB::Subscription<distance_sensor_s> *_sub_sonar;

	// publications
	uORB::Publication<vehicle_local_position_s> _pub_lpos;
	uORB::Publication<vehicle_global_position_s> _pub_gpos;
	//uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow;
	uORB::Publication<estimator_status_s> _pub_est_status;

	// map projection
	struct map_projection_reference_s _map_ref;

	// general parameters
	BlockParamInt  _integrate;

	// sonar parameters
	BlockParamFloat  _sonar_z_stddev;
	BlockParamFloat  _sonar_z_offset;

	// lidar parameters
	BlockParamFloat  _lidar_z_stddev;
	BlockParamFloat  _lidar_z_offset;

	// accel parameters
	BlockParamFloat  _accel_xy_stddev;
	BlockParamFloat  _accel_z_stddev;

	// baro parameters
	BlockParamFloat  _baro_stddev;

	// gps parameters
	BlockParamFloat  _gps_xy_stddev;
	BlockParamFloat  _gps_z_stddev;
	BlockParamFloat  _gps_vxy_stddev;
	BlockParamFloat  _gps_vz_stddev;
	BlockParamFloat  _gps_eph_max;

	// vision parameters
	BlockParamFloat  _vision_xy_stddev;
	BlockParamFloat  _vision_z_stddev;
	BlockParamInt    _no_vision;

	// mocap parameters
	BlockParamFloat  _mocap_p_stddev;

	// flow parameters
	BlockParamFloat  _flow_z_offset;
	BlockParamFloat  _flow_xy_stddev;
	//BlockParamFloat  _flow_board_x_offs;
	//BlockParamFloat  _flow_board_y_offs;
	BlockParamInt    _flow_min_q;

	// process noise
	BlockParamFloat  _pn_p_noise_power;
	BlockParamFloat  _pn_v_noise_power;
	BlockParamFloat  _pn_b_noise_power;
	BlockParamFloat  _pn_t_noise_power;

	// flow gyro filter
	BlockHighPass _flow_gyro_x_high_pass;
	BlockHighPass _flow_gyro_y_high_pass;

	// stats
	BlockStats<float, 1> _baroStats;
	BlockStats<float, 1> _sonarStats;
	BlockStats<float, 1> _lidarStats;
	BlockStats<float, 1> _flowQStats;
	BlockStats<float, 3> _visionStats;
	BlockStats<float, 3> _mocapStats;
	BlockStats<double, 3> _gpsStats;

	// delay blocks
	//BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
	//BlockDelay<float, n_x, n_x, HIST_LEN> _PDelay;
	//BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;

	// misc
	px4_pollfd_struct_t _polls[3];
	uint64_t _timeStamp;
	//uint64_t _time_last_hist;
	uint64_t _time_last_xy;
	uint64_t _time_last_z;
	uint64_t _time_last_tz;
	uint64_t _time_last_flow;
	uint64_t _time_last_baro;
	uint64_t _time_last_gps;
	uint64_t _time_last_lidar;
	uint64_t _time_last_sonar;
	uint64_t _time_last_vision_p;
	uint64_t _time_last_mocap;
	int 	 _mavlink_fd;

	// initialization flags
	bool _baroInitialized;
	bool _gpsInitialized;
	bool _lidarInitialized;
	bool _sonarInitialized;
	bool _flowInitialized;
	bool _visionInitialized;
	bool _mocapInitialized;

	// reference altitudes
	float _altHome;
	bool _altHomeInitialized;
	float _baroAltHome;
	float _gpsAltHome;
	Vector3f _visionHome;
	Vector3f _mocapHome;

	// flow integration
	float _flowX;
	float _flowY;
	float _flowMeanQual;

	// referene lat/lon
	double _gpsLatHome;
	double _gpsLonHome;

	// status
	bool _canEstimateXY;
	bool _canEstimateZ;
	bool _canEstimateT;
	bool _xyTimeout;
	bool _zTimeout;
	bool _tzTimeout;

	// sensor faults
	fault_t _baroFault;
	fault_t _gpsFault;
	fault_t _lidarFault;
	fault_t _flowFault;
	fault_t _sonarFault;
	fault_t _visionFault;
	fault_t _mocapFault;

	// performance counters
	perf_counter_t _loop_perf;
	perf_counter_t _interval_perf;
	perf_counter_t _err_perf;

	// state space
	Vector<float, n_x>  _x; // state vector
	Vector<float, n_u>  _u; // input vector
	Matrix<float, n_x, n_x>  _P; // state covariance matrix
};


//==================================================================================================
/**
  Copyright 2017 NumScale SAS

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

#ifndef BOOST_SIMD_FUNCTION_SCALAR_ACOSD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_ACOSD_HPP_INCLUDED

#include <boost/simd/function/definition/acosd.hpp>
#include <boost/simd/arch/common/scalar/function/acosd.hpp>

#endif


#include <iostream>
///#include <io.h>
#include <math.h>
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>

#include <math.h>

//add scene retrieving.
#include "scene_retrieve.h"
#include "LoopClosingManager.h"
#include "nlohmann/json.hpp"

using namespace std;
using namespace nlohmann;
//Scene Pointer
//Scene* pScene = NULL;

cv::Ptr<cv::ORB> orb;
void ExtractFeaturesFromCertainKeyPoints(const cv::Mat& img_in,const vector<cv::Point2f> kps_in,vector<cv::Mat>& desp_output)
{
    //reference loop closing manager,extract orb feature of these points.
    cv::Mat gray;
    cv::Mat desp;
    cv::cvtColor(img_in,gray,cv::COLOR_RGB2GRAY);
    vector<cv::KeyPoint> kp_vec;
    cv::KeyPoint::convert(kps_in,kp_vec);
    orb->compute(gray,kp_vec,desp);
    cout<<"kps_in.size():"<<kps_in.size()<<"\tdesp.rows:"<<desp.rows<<endl;
    desp_output.resize(desp.rows);
    for(int r=0;r<desp.rows;r++) desp_output[r]=desp.rowRange(r,r+1).clone();
    return;
}

std::shared_ptr<Scene> MakeSceneFromPath(const string& path)
{
    std::shared_ptr<Scene> pScene(new Scene());
    LoopClosingManager lcm();
    //step<1> parse json.
    string jsonpath(path.c_str());
    jsonpath += "/reconstruction.json";
    cout<<"Using json path: "<<jsonpath<<endl;

    std::ifstream ifstr_json( jsonpath);

//     std::ifstream ifstr_json( jsonpath.c_str() );

    json reconstruction_j;

    ifstr_json >> reconstruction_j;

    auto shots = reconstruction_j[0]["shots"];

    auto img2Rotation = new map<string, cv::Mat>;
    auto img2Translation = new map<string, cv::Mat>;

    for (json::iterator it = shots.begin(); it != shots.end(); ++it)
    {
        cv::Mat rotation_mat,translation_mat;
        string img_filename = it.key();
        cv::Mat rvec(1,3,CV_32F);
        rvec.at<float>(0,0) = (float)shots[img_filename]["rotation"][0].get<double>();
        rvec.at<float>(0,1) = (float)shots[img_filename]["rotation"][1].get<double>();
        rvec.at<float>(0,2) = (float)shots[img_filename]["rotation"][2].get<double>();
        cv::Rodrigues(rvec,rotation_mat); // get 3x3 rotation mat.

        //cv::Mat cur_image;
        //cv::imread(path+"images"+img_filename,cur_image);
        //ptr_frameinfo p = lcm.extractFeature(cur_image);
        (*img2Rotation)[img_filename] = rotation_mat;
        (*img2Translation)[img_filename] = translation_mat;
        //std::cout << it.key() << " : " << it.value() << "\n";
    }

    cout<<"??? 6"<<endl;

    //step<2> build graph,do triangulate via these features.Load "features/xxxnpz and get keypoints info."
    map<string, vector<cv::Mat> >* pMapdesp = new map<string, vector<cv::Mat> >;
    map<string, vector<cv::Mat> >& img2kpDesps = *pMapdesp;
    //map<string, vector<cv::Mat> > img2kpDesps;// = *pMapdesp;

    //map<string, vector<cv::KeyPoint> >* pMap2d = new map<string, vector<cv::KeyPoint> >;
    //map<string, vector<cv::KeyPoint> > &img2Kp2Ds = *pMap2d;
    map<string, vector<cv::KeyPoint> > img2Kp2Ds;

    map<string,vector<cv::Point3d> >* pMap3d = new map<string,vector<cv::Point3d> >;
    map<string,vector<cv::Point3d>> &img2Kp3ds = *pMap3d;
    for(json::iterator it = shots.begin();it!=shots.end();++it)
    {
        string img_filename = it.key();
        std::ifstream ifstr_img_keypoints_json((path+"/features/"+img_filename+".npz.json").c_str());
        json img_kp_json;
        ifstr_img_keypoints_json>> img_kp_json;
        vector<cv::Point2f> kps;
        vector<cv::Mat> current_img_desps;
        for(json::iterator it2 = img_kp_json.begin();it2!=img_kp_json.end();++it2)
        {
            string point_index = it2.key();//make keypoints.
            string keypoint_x_str,keypoint_y_str;
	    /*for(json::iterator it_inner = img_kp_json[point_index].begin();it_inner!=img_kp_json[point_index].end();++it_inner)
	    {
	      cout<<"Inner object:"<<*it_inner;
	    }*/
            keypoint_x_str = img_kp_json[point_index][0].get<string>();
            keypoint_y_str = img_kp_json[point_index][1].get<string>();
	    float kpx,kpy;
	    kpx = std::stof(keypoint_x_str);
	    kpy = std::stof(keypoint_y_str);
	    //cout<<"kpx,kpy:"<<kpx<<" "<<kpy<<endl;
            kps.push_back(cv::Point2f(kpx,kpy));
        }
        cv::Mat curr_img = cv::imread(path+"/images/"+img_filename);
        ExtractFeaturesFromCertainKeyPoints(curr_img,kps,current_img_desps);
        img2kpDesps[img_filename] = current_img_desps;
	/*
	std::vector<cv::KeyPoint> *kps_ = new vector<cv::KeyPoint>;
	cv::KeyPoint::convert(kps,*kps_);
        img2Kp2Ds[img_filename] = *kps_;*/
	std::vector<cv::KeyPoint> kps_;
	cv::KeyPoint::convert(kps,kps_);
        img2Kp2Ds[img_filename] = kps_;

        img2Kp3ds[img_filename] = vector<cv::Point3d>();
        img2Kp3ds[img_filename].resize(img2Kp2Ds[img_filename].size());
    }
    //step<3>.Get 3d position of these points,and make a frame of scene.
    //open undistorted_tracks.csv. actually it's a tsv file.
    ifstream track_ifstr((path+"/undistorted_tracks.csv").c_str());
    vector<vector <string> >fields;
    string line;
    while(getline(track_ifstr,line))
    {
        stringstream sep(line);
        string field;
        fields.push_back(vector<string>());
        while(getline(sep,field,'\t'))
        {
            fields.back().push_back(field);
        }
    }
    std::map<string,std::vector<int> > map_index_with_3d_pos;
    const auto &points_obj = reconstruction_j[0]["points"];
    for(int line_index=0;line_index<fields.size();line_index++) // traverse all tracks to find point 3d coordinate.
    {
        vector<string>& curr_line = fields[line_index];
        string img_filename,track_id_str,feature_id_str;
        int track_id,feature_id;
        img_filename = curr_line[0];
        track_id_str = curr_line[1];
        feature_id_str = curr_line[2];
        track_id = std::stoi(track_id_str);
        feature_id = std::stoi(feature_id_str);
	//string px,py,pz;
	cout<<endl;
	cout<<"curr_line[0]:"<<curr_line[0]<<endl;
        cout<<"at line_index:"<<line_index<<endl;
	cout<<"track_id:"<<track_id_str<<endl;
	cout<<"feature_id:"<<feature_id_str<<endl;
	//auto z = reconstruction_j[0];
	//auto a = reconstruction_j[0]["points"];
	cout <<"bb"<<endl;
	//auto b = reconstruction_j[0]["points"][track_id_str];
	cout <<"cc"<<endl;
	//auto c = reconstruction_j[0]["points"][track_id_str]["coordinates"];
        cout <<"0"<<endl;

	if(points_obj.find(track_id_str)==points_obj.end() )
	{
	  //is a outlier.
	  cout <<"is a outlier"<<endl;
	  continue;
	}
	else
	{
	  cout<<"is a inlier!"<<endl;
	}
	double px = reconstruction_j[0]["points"][track_id_str]["coordinates"][0].get<double>();
	cout<<"1"<<endl;
	double py = reconstruction_j[0]["points"][track_id_str]["coordinates"][1].get<double>();
	cout<<"2"<<endl;
	double pz = reconstruction_j[0]["points"][track_id_str]["coordinates"][2].get<double>();
	cout<<"3"<<endl;
	cout <<"img2Kp3ds[img_filename].size:"<<img2Kp3ds[img_filename].size()<<"\timg2Kp2Ds[img_filename].size():"<<img2Kp2Ds[img_filename].size()<<endl;
	if(img2Kp2Ds[img_filename].size() == 0)
	{
	  cout <<"this image is not inside of reconstruction[0],ignored!"<<endl;
	  continue;
	}
        img2Kp3ds[img_filename][feature_id] = cv::Point3d(px,py,pz);

	cout<<"point added!"<<endl;
	//map_index_with_3d_pos[img_filename].push_back(feature_id);//Unused now.
    }
    cout<<"3d points loaded."<<endl;
    for (json::iterator it = shots.begin(); it != shots.end(); ++it)
    {
        //cv::Mat rotation_mat,translation_mat;
        string img_filename = it.key();
        cv::Mat temp_desp_mat(img2kpDesps[img_filename].size(),//rows
                    32,//cols
                        0
        );
        cout<<"temp_desp_mat:"<<temp_desp_mat.cols<<"|"<<temp_desp_mat.rows<<endl;
        cout<<"Image:"<<img_filename<<img2kpDesps[img_filename].size()<<";"<<img2kpDesps[img_filename][0].rows<<","<<img2kpDesps[img_filename][0].cols<<endl;

        for(int index = 0;index< img2kpDesps[img_filename].size();index++)
        {
            cout<<"index:"<<index<<endl;
            cv::Mat& temp = img2kpDesps[img_filename][index];
            cout<<temp.cols<<"|"<<temp.rows<<endl;
            //temp_desp_mat.rowRange(index,index+1) = img2kpDesps[img_filename][index].row(0);//img2kpDesps[img_filename][index].rowRange(0,1);//.clone();
            temp_desp_mat.row(index) = img2kpDesps[img_filename][index].row(0);//img2kpDesps[img_filename][index].rowRange(0,1);//.clone();
        }

        //NOTE method 1, this will serialize kps, mps and desp;
        //pScene->addFrame(img2Kp2Ds[img_filename], img2Kp3ds[img_filename], temp_desp_mat.clone());

        //NOTE method 2, this will serialize kps, mps, desps, R and t
        cv::Mat R = (cv::Mat_<double>(3,3) << 0, -1.1, 0, -1.2, 5.3, -1.4, 0, -1.5, 0);
        cv::Mat T = (cv::Mat_<double>(1,3) << 0, -1.5, 0);
        cout<<"Current R and T are: "<<endl<<R<<endl<<T<<endl;

        pScene->addFrame(img2Kp2Ds[img_filename], img2Kp3ds[img_filename], temp_desp_mat.clone(), R, T);

    }
    //step<4>.See if this scene has scale factor.

    //pScene->hasScale = false;// for default.
    return pScene;
}

int main(int argc,char** argv)
{
    cout<<"CAUTION:Do not forget set opensfm config feature_type to 'ORB'."<<endl;
    if(argc<3)
    {
        cout<<"Usage: MakeSceneFromOpenSfMModel OpenSfM_project_dir voc_path"<<endl;
        return -1;
    }
    string project_path(argv[1]);
    string voc_path(argv[2]);
    orb = cv::ORB::create();
    std::shared_ptr<Scene>* pp;
    pp = new std::shared_ptr<Scene>;
    *pp= MakeSceneFromPath(project_path);
    (*pp)->saveFile("scene.scene");
    return 0;
}


// Copyright (c) 2012 Christopher Lux <christopherlux@gmail.com>
// Distributed under the Modified BSD License, see license.txt.

#include "fullscreen_triangle.h"

#include <cassert>

#include <boost/assign/list_of.hpp>

#include <scm/core/memory.h>

#include <scm/gl_core/render_device.h>
#include <scm/gl_core/buffer_objects/buffer.h>
#include <scm/gl_core/render_device/context.h>
#include <scm/gl_core/buffer_objects/vertex_array.h>
#include <scm/gl_core/buffer_objects/vertex_format.h>

namespace {
    struct vertex {
        scm::math::vec2f pos;
        scm::math::vec2f tex;
    };
} // namespace

namespace scm {
namespace gl {

fullscreen_triangle::fullscreen_triangle(const render_device_ptr& in_device)
  : geometry(in_device)
{
    using namespace scm::math;

    int num_vertices = 3;

    _vertices = in_device->create_buffer(BIND_VERTEX_BUFFER, USAGE_STATIC_DRAW, num_vertices * sizeof(vertex), 0);

    render_context_ptr ctx = in_device->main_context();
    vertex* data = static_cast<vertex*>(ctx->map_buffer(_vertices, ACCESS_WRITE_INVALIDATE_BUFFER));

#if 0
    data[0].pos = vec3f(0.0f, 0.0f, 0.0f); data[0].tex = vec2f(0.0f, 0.0f);
    data[1].pos = vec3f(2.0f, 0.0f, 0.0f); data[1].tex = vec2f(2.0f, 0.0f);
    data[2].pos = vec3f(0.0f, 2.0f, 0.0f); data[2].tex = vec2f(0.0f, 2.0f);

    ctx->unmap_buffer(_vertices);

    using namespace scm::gl;
    using boost::assign::list_of;

    _vertex_array = in_device->create_vertex_array(vertex_format(0, 0, TYPE_VEC3F, sizeof(vertex))  // position
                                                                (0, 2, TYPE_VEC2F, sizeof(vertex)), // texture coord
                                                   list_of(_vertices));
#else
    data[0].pos = vec2f(0.0f, 0.0f); data[0].tex = vec2f(0.0f, 0.0f);
    data[1].pos = vec2f(2.0f, 0.0f); data[1].tex = vec2f(2.0f, 0.0f);
    data[2].pos = vec2f(0.0f, 2.0f); data[2].tex = vec2f(0.0f, 2.0f);

    ctx->unmap_buffer(_vertices);

    using namespace scm::gl;
    using boost::assign::list_of;

    _vertex_array = in_device->create_vertex_array(vertex_format(0, 0, TYPE_VEC2F, sizeof(vertex))  // position
                                                                (0, 2, TYPE_VEC2F, sizeof(vertex)), // texture coord
                                                   list_of(_vertices));
#endif
}

fullscreen_triangle::~fullscreen_triangle()
{
    _vertex_array.reset();
    _vertices.reset();
}

void
fullscreen_triangle::draw(const render_context_ptr& in_context,
                          const draw_mode in_draw_mode) const
{
    context_vertex_input_guard vig(in_context);

    in_context->bind_vertex_array(_vertex_array);
    in_context->apply();

    if (in_draw_mode == MODE_SOLID) {
        in_context->draw_arrays(PRIMITIVE_TRIANGLE_LIST, 0, 3);
    }
    else if (in_draw_mode == MODE_WIRE_FRAME) {
        in_context->draw_arrays(PRIMITIVE_LINE_LOOP, 0, 3);
    }
    else {
        return;
    }
}

} // namespace gl
} // namespace scm


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2016 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014, 2017.
// Modifications copyright (c) 2014-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_DETAIL_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_DETAIL_HPP

#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Point-point distance approximation taking flattening into account
\ingroup distance
\tparam Spheroid The reference spheroid model
\tparam CalculationType \tparam_calculation
\author After Andoyer, 19xx, republished 1950, republished by Meeus, 1999
\note Although not so well-known, the approximation is very good: in all cases the results
are about the same as Vincenty. In my (Barend's) testcases the results didn't differ more than 6 m
\see http://nacc.upc.es/tierra/node16.html
\see http://sci.tech-archive.net/Archive/sci.geo.satellite-nav/2004-12/2724.html
\see http://home.att.net/~srschmitt/great_circle_route.html (implementation)
\see http://www.codeguru.com/Cpp/Cpp/algorithms/article.php/c5115 (implementation)
\see http://futureboy.homeip.net/frinksamp/navigation.frink (implementation)
\see http://www.voidware.com/earthdist.htm (implementation)
\see http://www.dtic.mil/docs/citations/AD0627893
\see http://www.dtic.mil/docs/citations/AD703541
*/
template
<
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class andoyer
    : public strategy::distance::geographic
        <
            strategy::andoyer, Spheroid, CalculationType
        >
{
    typedef strategy::distance::geographic
        <
            strategy::andoyer, Spheroid, CalculationType
        > base_type;

public :
    inline andoyer()
        : base_type()
    {}

    explicit inline andoyer(Spheroid const& spheroid)
        : base_type(spheroid)
    {}
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Spheroid, typename CalculationType>
struct tag<andoyer<Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct return_type<andoyer<Spheroid, CalculationType>, P1, P2>
    : andoyer<Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template <typename Spheroid, typename CalculationType>
struct comparable_type<andoyer<Spheroid, CalculationType> >
{
    typedef andoyer<Spheroid, CalculationType> type;
};

template <typename Spheroid, typename CalculationType>
struct get_comparable<andoyer<Spheroid, CalculationType> >
{
    static inline andoyer<Spheroid, CalculationType> apply(andoyer<Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct result_from_distance<andoyer<Spheroid, CalculationType>, P1, P2>
{
    template <typename T>
    static inline typename return_type<andoyer<Spheroid, CalculationType>, P1, P2>::type
        apply(andoyer<Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_DETAIL_HPP


#pragma once
#include "include/seastarkv.hh"
#include "include/reply_builder.hh"
#include <cstdio>
#include <fstream>
#include <chrono>
#include <boost/thread/shared_mutex.hpp>

using namespace redis;
using namespace std;

// Thomas Wang, Integer Hash Functions.
// http://www.concentric.net/~Ttwang/tech/inthash.htm
inline uint32_t get_hash(uint32_t key, uint32_t seed) {
    uint32_t hash = key;
    hash = hash ^ seed;
    hash = ~hash + (hash << 15);  // hash = (hash << 15) - hash - 1;
    hash = hash ^ (hash >> 12);
    hash = hash + (hash << 2);
    hash = hash ^ (hash >> 4);
    hash = hash * 2057;  // hash = (hash + (hash << 3)) + (hash << 11);
    hash = hash ^ (hash >> 16);
    return hash & 0x3fffffff;
}

class database;

struct db_val {
    uint32_t key;
    struct db_val *next;
    void* data;
    uint32_t length;
}__attribute__((__packed__));

typedef struct db_val db_val_t;

struct node
{
    void* data;
    node *next;
};

class database {
public:
    typedef std::chrono::time_point<std::chrono::high_resolution_clock> ts;
    typedef std::chrono::duration<size_t, std::nano> ns;

    struct hashtable {
        boost::shared_mutex _access;
        uint32_t size;
        struct db_val **table;
    };

    typedef struct hashtable hashtable_t;

    void hashtable_init(uint32_t size);

    bool ht_set(db_val_t* val, int version);

    db_val_t* ht_get(uint32_t key);

    database()
    {
        ht.table = NULL;
	hashtable_init(1000*1000);
    }

    ~database() {
    };

    db_val** get_table_direct(void);

private:
    hashtable_t ht;
};

extern unordered_map<string, database*> db_map;

inline database* get_db(std::string s) {
    if (db_map.find(s) == db_map.end())
        return NULL;
    return db_map[s];
}

inline bool create_db(std::string s) {
    auto db = new database;
    if (!db)
        return false;
    db_map[s] = db;
    return true;
}


#ifndef __VSIM_POSE_HPP__
#define __VSIM_POSE_HPP__

#include <Eigen/Geometry>
#include <memory>

namespace vsim {

struct Frame ;
typedef std::shared_ptr<Frame> FramePtr ;

struct Pose {
public:

    Pose(): mat_(Eigen::Matrix4f::Identity()) {}

    Eigen::Matrix4f absolute() const ; // accumulate transform of parent frame

    Eigen::Affine3f mat_ ;                 // transformation matrix to apply to child nodes and attached geometries
    FramePtr frame_ ;
};

}
#endif


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

#pragma once

#include <backendsCommon/CpuTensorHandle.hpp>
#include <backendsCommon/Workload.hpp>
#include <backendsCommon/WorkloadData.hpp>
#include <boost/log/trivial.hpp>
#include "TNpuWorkloads.hpp"

namespace armnn {
template <typename armnn::DataType... DataTypes>
class NpuSoftmaxWorkload : public TNpuWorkload<SoftmaxQueueDescriptor, DataTypes...> {
   public:
    using base_type = TNpuWorkload<SoftmaxQueueDescriptor, DataTypes...>;
    explicit NpuSoftmaxWorkload(const SoftmaxQueueDescriptor& descriptor, const WorkloadInfo& info)
        : TNpuWorkload<SoftmaxQueueDescriptor, DataTypes...>(descriptor, info),
          m_Beta(descriptor.m_Parameters.m_Beta),
          m_Axis(descriptor.m_Parameters.m_Axis) {
        // Add inputs operand
        std::vector<uint32_t> inOperandIds;
        // ONLY 1 input
        NpuTensorHandler* inputTensorHandle =
            dynamic_cast<NpuTensorHandler*>(descriptor.m_Inputs[0]);
        uint32_t inputOperandId = this->AddOperandAndSetValue(
            inputTensorHandle->GetTensorInfo(), inputTensorHandle->GetShape(), nullptr);
        inOperandIds.push_back(inputOperandId);

        // Add beta operand
        inOperandIds.push_back(this->AddOperandAndSetValue(m_Beta));

        // Add axis operand
        inOperandIds.push_back(this->AddOperandAndSetValue(m_Axis));

        std::vector<uint32_t> outOperandIds;
        NpuTensorHandler* outputTensorHandle =
            dynamic_cast<NpuTensorHandler*>(descriptor.m_Outputs[0]);
        uint32_t outputTensorId = this->AddOperandAndSetValue(
            outputTensorHandle->GetTensorInfo(), outputTensorHandle->GetShape(), nullptr);
        outOperandIds.push_back(outputTensorId);

        this->AddOperation(nnrt::OperationType::SOFTMAX,
                           inOperandIds.size(),
                           inOperandIds.data(),
                           outOperandIds.size(),
                           outOperandIds.data());
    }

   private:
    float m_Beta;
    int m_Axis;
};
using NpuSoftmaxFloat32Workload = NpuSoftmaxWorkload<armnn::DataType::Float32>;
using NpuSoftmaxFloat16Workload = NpuSoftmaxWorkload<armnn::DataType::Float16>;
using NpuSoftmaxUint8Workload = NpuSoftmaxWorkload<armnn::DataType::QuantisedAsymm8>;
}  // namespace armnn


#include <cstdint>
#include "test.h"
#include "baldr/json.h"
#include <set>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

namespace {

void TestJsonSerialize() {

  using namespace std;
  using namespace valhalla::baldr;
  stringstream result; result << *json::map
  ({
    {"hint_data", json::map
      ({
        {"locations", json::array
          ({
            string("_____38_SADaFQQAKwEAABEAAAAAAAAAdgAAAFfLwga4tW0C4P6W-wAARAA"),
            string("fzhIAP____8wFAQA1AAAAC8BAAAAAAAAAAAAAP____9Uu20CGAiX-wAAAAA")
          })
        },
        {"checksum", static_cast<uint64_t>(2875622111)}
      })
    },
    {"route_name", json::array({string("West 26th Street"), string("Madison Avenue")})},
    {"via_indices", json::array({uint64_t(0), uint64_t(9)})},
    {"found_alternative", bool(false)},
    {"route_summary", json::map
      ({
        {"end_point", string("West 29th Street")},
        {"start_point", string("West 26th Street")},
        {"total_time", uint64_t(145)},
        {"total_distance", uint64_t(878)}
      })
    },
    {"via_points", json::array
      ({
        json::array({json::fp_t{40.744377, 3}, json::fp_t{-73.990433, 3}}),
        json::array({json::fp_t{40.745811, 3}, json::fp_t{-73.988075, 3}})
      })
    },
    {"route_instructions", json::array
      ({
        json::array({ string("10"), string("West 26th Street"), uint64_t(216), uint64_t(0), uint64_t(52), string("215m"), string("SE"), uint64_t(118) }),
        json::array({ string("1"), string("East 26th Street"), uint64_t(153), uint64_t(2), uint64_t(29), string("153m"), string("SE"), uint64_t(120) }),
        json::array({ string("7"), string("Madison Avenue"), uint64_t(237), uint64_t(3), uint64_t(25), string("236m"), string("NE"), uint64_t(29) }),
        json::array({ string("7"), string("East 29th Street"), uint64_t(155), uint64_t(6), uint64_t(29), string("154m"), string("NW"), uint64_t(299) }),
        json::array({ string("1"), string("West 29th Street"), uint64_t(118), uint64_t(7), uint64_t(21), string("117m"), string("NW"), uint64_t(299) }),
        json::array({ string("15"), string(""), uint64_t(0), uint64_t(8), uint64_t(0), string("0m"), string("N"), uint64_t(0) })
      })
    },
    {"route_geometry", string("ozyulA~p_clCfc@ywApTar@li@ybBqe@c[ue@e[ue@i[ci@dcB}^rkA")},
    {"status_message", string("Found route between points")},
    {"status", uint64_t(0)},
    {"escaped_string", string("\"\t\r\n\\\a")}
  });

  stringstream answer; answer << "{\"escaped_string\":\"\\\"\\t\\r\\n\\\\\\u0007\",\"hint_data\":{\"checksum\":2875622111,\"locations\":[\"_____38_SADaFQQAKwEAABEAAAAAAAAAdgAAAFfLwga4tW0C4P6W-wAARAA\",\"fzhIAP____8wFAQA1AAAAC8BAAAAAAAAAAAAAP____9Uu20CGAiX-wAAAAA\"]},\"route_name\":[\"West 26th Street\",\"Madison Avenue\"],\"found_alternative\":false,\"route_summary\":{\"total_distance\":878,\"total_time\":145,\"start_point\":\"West 26th Street\",\"end_point\":\"West 29th Street\"},\"via_points\":[[40.744,-73.990],[40.746,-73.988]],\"route_instructions\":[[\"10\",\"West 26th Street\",216,0,52,\"215m\",\"SE\",118],[\"1\",\"East 26th Street\",153,2,29,\"153m\",\"SE\",120],[\"7\",\"Madison Avenue\",237,3,25,\"236m\",\"NE\",29],[\"7\",\"East 29th Street\",155,6,29,\"154m\",\"NW\",299],[\"1\",\"West 29th Street\",118,7,21,\"117m\",\"NW\",299],[\"15\",\"\",0,8,0,\"0m\",\"N\",0]],\"route_geometry\":\"ozyulA~p_clCfc@ywApTar@li@ybBqe@c[ue@e[ue@i[ci@dcB}^rkA\",\"status_message\":\"Found route between points\",\"via_indices\":[0,9],\"status\":0}";

  boost::property_tree::ptree res,ans;
  boost::property_tree::read_json(result, res);
  boost::property_tree::read_json(answer, ans);
  res.sort();
  ans.sort();

  stringstream res_formatted,ans_formatted;
  boost::property_tree::write_json(res_formatted, res);
  boost::property_tree::write_json(ans_formatted, ans);

  multiset<string> res_set, ans_set;
  for(string line; getline(res_formatted, line);)
    res_set.insert(line);
  for(string line; getline(ans_formatted, line);)
    ans_set.insert(line);
  if(res_set.size() != ans_set.size())
    throw std::logic_error("Wrong json!");

  auto res_itr = res_set.cbegin(), ans_itr = ans_set.cbegin();
  for(;res_itr != res_set.cend() && ans_itr != ans_set.cend(); ++res_itr, ++ans_itr)
    if(*res_itr != *ans_itr)
      throw std::runtime_error("Wrong json!");
}

}

int main() {
  test::suite suite("json");

  suite.test(TEST_CASE(TestJsonSerialize));

  return suite.tear_down();
}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef STATFILE_FILTERING_STATSOURCE_HPP
#define STATFILE_FILTERING_STATSOURCE_HPP

#include <boost/bimap.hpp>
#include "statfile/BuiltInTypeStatSource.hpp"
#include "statfile/BuiltInTypeStatSourceChain.hpp"
#include "genfile/VariantEntry.hpp"

namespace statfile {
	namespace impl {
		//
		// Homogenise types for value comparison.
		// We do this:
		// * 1. convert any ints to doubles
		// * 2. of one operand is int or double and the other operand is string then we cast the string to a double if possible.
		//
		void cast_types_for_comparison( genfile::VariantEntry& value1, genfile::VariantEntry& value2 ) ;
	}

	struct Constraint {
		enum Type { eEqualTo, eNotEqualTo, eLessThan, eLessThanOrEqualTo, eGreaterThan, eGreaterThanOrEqualTo, eBetween, eNotBetween } ;

		static Constraint equal_to( genfile::VariantEntry const& value ) ;
		static Constraint not_equal_to( genfile::VariantEntry const& value ) ;
		static Constraint less_than( genfile::VariantEntry const& value ) ;
		static Constraint less_than_or_equal_to( genfile::VariantEntry const& value ) ;
		static Constraint greater_than( genfile::VariantEntry const& value ) ;
		static Constraint greater_than_or_equal_to( genfile::VariantEntry const& value ) ;
		static Constraint between( genfile::VariantEntry const& value1, genfile::VariantEntry const& value2 ) ;
		static Constraint not_between( genfile::VariantEntry const& value1, genfile::VariantEntry const& value2 ) ;
		static Constraint construct( std::string const& op, genfile::VariantEntry const& value ) ;

		Constraint() ;
		Constraint( Constraint const& other ) ;
		Constraint& operator=( Constraint const& other ) ;
		bool test( genfile::VariantEntry value ) const ;

		std::string get_spec() const ;
		Constraint negation() const ;

	private:
		Type m_type ;
		genfile::VariantEntry m_value1 ;
		genfile::VariantEntry m_value2 ;

	private:
		Constraint( genfile::VariantEntry const& value1, genfile::VariantEntry const& value2, Type type ) ;
	} ;

	struct BoundConstraint {
	public:
		static BoundConstraint parse( std::string const& spec ) ;
	public:
		BoundConstraint() ;
		BoundConstraint( std::string const& variable, Constraint const& constraint ) ;
		BoundConstraint( BoundConstraint const& other ) ;
		BoundConstraint& operator=( BoundConstraint const& other ) ;

		std::string const& variable() const { return m_variable ; }
		Constraint const& constraint() const { return m_constraint ; }
		std::string get_spec() const { return m_variable + " " + m_constraint.get_spec() ; }
		BoundConstraint negation() const ;

	private:
		std::string m_variable ;
		Constraint m_constraint ;
	} ;

	class FilteringStatSource: public BuiltInTypeStatSource {
	public:
		static UniquePtr create(
			BuiltInTypeStatSource::UniquePtr source,
			BoundConstraint const& constraint
		) ;

	public:
		FilteringStatSource(
			BuiltInTypeStatSource::UniquePtr source,
			BoundConstraint const& constraint
		) ;
		operator bool() const ;
		std::string get_source_spec() const ;

	private:
		OptionalCount number_of_rows() const { return OptionalCount() ; }
		std::size_t number_of_columns() const { return m_source->number_of_columns() ; }
		std::vector< std::string > column_names() const { return m_source->column_names() ; }
		std::string get_descriptive_text() const { return m_source->get_descriptive_text() ; }
		using Base::read_value ;
		void read_value( int32_t& value ) ;
		void read_value( uint32_t& value ) ;
		void read_value( std::string& value ) ;
		void read_value( double& value ) ;
		void ignore_value() ;
		void ignore_all() ;
		void end_row() ;
		void restart_row() ;
		void move_to_next_row_impl() ;
		void move_to_next_matching_row() ;
		bool row_matches() ;

	private:
		BuiltInTypeStatSource::UniquePtr m_source ;
		typedef boost::bimap< std::string, std::size_t > ColumnMap ;
		ColumnMap m_columns ;
		BoundConstraint m_constraint ;
	} ;
}

#endif