#include <iostream>
#include <vector>
#include <set>
#include <algorithm>
#include <iterator>
#include <fstream>
#include <sstream>
#include <list>
#include <map>
#include <ext/algorithm>
#include <numeric>
#include <sys/timeb.h>
#include <time.h>
#include <cstdlib>
#include <cstdio>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/utility.hpp>

#include "rbt_coarse.h"
#include "rbt_dense.h"
#include "rbt_pose.h"

#include "CGAL/Cartesian.h"
#include "CGAL/Segment_tree_k.h"
#include "CGAL/Range_segment_tree_traits.h"

#include "CGAL/basic.h"
#include "CGAL/Point_3.h"
#include "CGAL/Range_tree_k.h"

typedef CGAL::Cartesian<double> CTP_Representation;
typedef CGAL::Range_tree_map_traits_3<CTP_Representation, unsigned short> CTP_Traits;
typedef CGAL::Range_tree_3<CTP_Traits> CTP_Range_tree_3_type;
typedef CTP_Traits::Key CTP_Key;
typedef CTP_Traits::Pure_key CTP_Pure_key;
typedef CTP_Traits::Interval CTP_Interval;

using namespace std;

//typedef double RotationMatrix[3][3];
//typedef vector<vector<double> > RotationMatrix(3, vector<double>(3, 0));

template <class FirstType, class SecondType>
struct less_than_univ{
    bool operator()(pair<FirstType, SecondType> pm_A, pair<FirstType, SecondType> pm_B)
    {
        if (pm_A.first < pm_B.first) return true;
        else return false;
    }
};

template <class T>
T rdl_dot_3d(const vector<T>& pm_v1, const vector<T>& pm_v2)
{
    T     r;
    r  = pm_v1[0] * pm_v2[0];
    r += pm_v1[1] * pm_v2[1];
    r += pm_v1[2] * pm_v2[2];

    return r;
}

template <class T>
void rdl_rotate(vector<T> const &pm_axis, \
                vector<T> const &pm_old_point, \
                T  	            pm_angle, \
                vector<T>       &pm_new_point)
{
    pm_new_point[0] = 0.0;
    pm_new_point[1] = 0.0;
    pm_new_point[2] = 0.0;
    double costheta, sintheta;

    costheta = cos(pm_angle);
    sintheta = sin(pm_angle);

    pm_new_point[0] += (costheta + (1 - costheta) * pm_axis[0] * pm_axis[0]) * pm_old_point[0];
    pm_new_point[0] += ((1 - costheta) * pm_axis[0] * pm_axis[1] - pm_axis[2] * sintheta) * pm_old_point[1];
    pm_new_point[0] += ((1 - costheta) * pm_axis[0] * pm_axis[2] + pm_axis[1] * sintheta) * pm_old_point[2];

    pm_new_point[1] += ((1 - costheta) * pm_axis[0] * pm_axis[1] + pm_axis[2] * sintheta) * pm_old_point[0];
    pm_new_point[1] += (costheta + (1 - costheta) * pm_axis[1] * pm_axis[1]) * pm_old_point[1];
    pm_new_point[1] += ((1 - costheta) * pm_axis[1] * pm_axis[2] - pm_axis[0] * sintheta) * pm_old_point[2];

    pm_new_point[2] += ((1 - costheta) * pm_axis[0] * pm_axis[2] - pm_axis[1] * sintheta) * pm_old_point[0];
    pm_new_point[2] += ((1 - costheta) * pm_axis[1] * pm_axis[2] + pm_axis[0] * sintheta) * pm_old_point[1];
    pm_new_point[2] += (costheta + (1 - costheta) * pm_axis[2] * pm_axis[2]) * pm_old_point[2];

    return;
}

void normalize(vector<double>& v)
{
	double n;
	n = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
	if (n < 0.5){
		cerr << "n is very small! = " << n << endl;
	}
	n = sqrt(n);
	v[0] /= n;
	v[1] /= n;
	v[2] /= n;
}

struct MyBall{
	CTP_Range_tree_3_type*      m_index;

	MyBall()
	{
		m_index = NULL;
	}

	void query_local(const vector<double>&	pm_point, double pm_r, vector<unsigned short>& pm_out)
	{
		pm_out.clear();

		vector<CTP_Key> OutputList;

		CTP_Pure_key a = CTP_Pure_key(pm_point[0] - pm_r, pm_point[1] - pm_r, pm_point[2] - pm_r);
		CTP_Pure_key b = CTP_Pure_key(pm_point[0] + pm_r, pm_point[1] + pm_r, pm_point[2] + pm_r);
		CTP_Interval win = CTP_Interval(CTP_Key(a, 0), CTP_Key(b, 0));
		m_index->window_query(win, back_inserter(OutputList));

		if (OutputList.size() == 0){
			cerr << "ERROR: OutputList.size() == 0 in query_nnb(...)! " << endl;
			cerr << "\twhen query (" << pm_point[0] << ", " << pm_point[1] << ", " << pm_point[2] << ")" << endl;
			exit(1);
		}

		vector<pair<double, unsigned short> >	tmp_buffer;
		double tmp_dist;
		double tmp_rsq = pm_r * pm_r;
		size_t i;
		for (i = 0; i < OutputList.size(); i++){
			tmp_dist  = (pm_point[0] - OutputList[i].first.x()) * (pm_point[0] - OutputList[i].first.x());
			tmp_dist += (pm_point[1] - OutputList[i].first.y()) * (pm_point[1] - OutputList[i].first.y());
			tmp_dist += (pm_point[2] - OutputList[i].first.z()) * (pm_point[2] - OutputList[i].first.z());
			if (tmp_dist <= tmp_rsq){
				pm_out.push_back(OutputList[i].second);
			}
		}
	}

	unsigned short query_nn(const vector<double>&	pm_point)
	{
		vector<CTP_Key> OutputList;

		double	tmp_local_dist = 0.09;
		CTP_Pure_key a = CTP_Pure_key(pm_point[0] - tmp_local_dist, pm_point[1] - tmp_local_dist, pm_point[2] - tmp_local_dist);
		CTP_Pure_key b = CTP_Pure_key(pm_point[0] + tmp_local_dist, pm_point[1] + tmp_local_dist, pm_point[2] + tmp_local_dist);
		CTP_Interval win = CTP_Interval(CTP_Key(a, 0), CTP_Key(b, 0));
		m_index->window_query(win, back_inserter(OutputList));

		if (OutputList.size() == 0){
			cerr << "ERROR: OutputList.size() == 0 in query_nnb(...)! " << endl;
			cerr << "\twhen query (" << pm_point[0] << ", " << pm_point[1] << ", " << pm_point[2] << ")" << endl;
			exit(1);
		}

		vector<pair<double, unsigned short> >	tmp_buffer;
		double tmp_dist;
		size_t i;
		for (i = 0; i < OutputList.size(); i++){
			tmp_dist  = (pm_point[0] - OutputList[i].first.x()) * (pm_point[0] - OutputList[i].first.x());
			tmp_dist += (pm_point[1] - OutputList[i].first.y()) * (pm_point[1] - OutputList[i].first.y());
			tmp_dist += (pm_point[2] - OutputList[i].first.z()) * (pm_point[2] - OutputList[i].first.z());
			tmp_buffer.push_back(pair<double, unsigned short>(tmp_dist, OutputList[i].second));
		}

		sort(tmp_buffer.begin(), tmp_buffer.end(), less_than_univ<double, unsigned short>());

		return tmp_buffer[0].second;
	}
};

struct MyBall_Coarse: public MyBall{

	void init()
	{
		vector<CTP_Key>             InputList;
		vector<double>              tmp_point(3, 0);
		size_t i;

		for (i = 0; i < points_default_coarse; i ++){
			InputList.push_back(CTP_Key(CTP_Pure_key(vertex_default_coarse[i][0], vertex_default_coarse[i][1], vertex_default_coarse[i][2]), i));
		}

		m_index = new CTP_Range_tree_3_type(InputList.begin(), InputList.end());
	}
};

struct MyBall_Dense: public MyBall{

	void init()
	{
		vector<CTP_Key>             InputList;
		vector<double>              tmp_point(3, 0);
		size_t i;

		for (i = 0; i < points_default_dense; i ++){
			InputList.push_back(CTP_Key(CTP_Pure_key(vertex_default_dense[i][0], vertex_default_dense[i][1], vertex_default_dense[i][2]), i));
		}

		m_index = new CTP_Range_tree_3_type(InputList.begin(), InputList.end());
	}
};

struct MyBall_Pose: public MyBall{
	vector<vector<vector<double> > >		m_arcs;

	void init()
	{
		vector<CTP_Key>             InputList;
		vector<double>              tmp_point(3, 0);
		size_t i;

		for (i = 0; i < points_default_pose; i ++){
			InputList.push_back(CTP_Key(CTP_Pure_key(vertex_default_pose[i][0], vertex_default_pose[i][1], vertex_default_pose[i][2]), i));
		}

		m_index = new CTP_Range_tree_3_type(InputList.begin(), InputList.end());
	}

	void init_pose()
	{
		size_t i, j;
		vector<double>	curr_point(3, 0);
		vector<double>	rand_point(3, 0);
		double			dist;
		double			theta;
		vector<double>	tmp_Z(3, 0);
		vector<double>	tmp_Y(3, 0);
		vector<double>	tmp_X(3, 0);

		srand(time(NULL));
		vector<vector<double> >	tmp_buff;
		vector<vector<double> >	tmp_coord;

		m_arcs.clear();
		for (i = 0; i < points_default_pose; i ++){
			curr_point[0] = vertex_default_pose[i][0];
			curr_point[1] = vertex_default_pose[i][1];
			curr_point[2] = vertex_default_pose[i][2];
			j = rand() % points_default_pose;
			rand_point[0] = vertex_default_pose[j][0];
			rand_point[1] = vertex_default_pose[j][1];
			rand_point[2] = vertex_default_pose[j][2];
			dist  = (curr_point[0] - rand_point[0]) * (curr_point[0] - rand_point[0]);
			dist += (curr_point[1] - rand_point[1]) * (curr_point[1] - rand_point[1]);
			dist += (curr_point[2] - rand_point[2]) * (curr_point[2] - rand_point[2]);
			while(dist < 1 || dist > 3){
				j = rand() % points_default_pose;
				rand_point[0] = vertex_default_pose[j][0];
				rand_point[1] = vertex_default_pose[j][1];
				rand_point[2] = vertex_default_pose[j][2];
				dist  = (curr_point[0] - rand_point[0]) * (curr_point[0] - rand_point[0]);
				dist += (curr_point[1] - rand_point[1]) * (curr_point[1] - rand_point[1]);
				dist += (curr_point[2] - rand_point[2]) * (curr_point[2] - rand_point[2]);
			}

			tmp_Y[0] = curr_point[1] * rand_point[2] - curr_point[2] * rand_point[1];
			tmp_Y[1] = curr_point[2] * rand_point[0] - curr_point[0] * rand_point[2];
			tmp_Y[2] = curr_point[0] * rand_point[1] - curr_point[1] * rand_point[0];
			normalize(tmp_Y);
			tmp_X[0] = tmp_Y[1] * curr_point[2] - tmp_Y[2] * curr_point[1];
			tmp_X[1] = tmp_Y[2] * curr_point[0] - tmp_Y[0] * curr_point[2];
			tmp_X[2] = tmp_Y[0] * curr_point[1] - tmp_Y[1] * curr_point[0];
			normalize(tmp_X);
			tmp_buff.clear();
			for (j = 0; j < 360; j ++){
				theta = 2.0*i*M_PI/360;
				rdl_rotate(curr_point, tmp_X, theta, rand_point);
				tmp_buff.push_back(rand_point);
			}
			random_shuffle(tmp_buff.begin(), tmp_buff.end());
			rand_point = tmp_buff[rand()%tmp_buff.size()];

			tmp_Y[0] = curr_point[1] * rand_point[2] - curr_point[2] * rand_point[1];
			tmp_Y[1] = curr_point[2] * rand_point[0] - curr_point[0] * rand_point[2];
			tmp_Y[2] = curr_point[0] * rand_point[1] - curr_point[1] * rand_point[0];
			normalize(tmp_Y);
			tmp_X[0] = tmp_Y[1] * curr_point[2] - tmp_Y[2] * curr_point[1];
			tmp_X[1] = tmp_Y[2] * curr_point[0] - tmp_Y[0] * curr_point[2];
			tmp_X[2] = tmp_Y[0] * curr_point[1] - tmp_Y[1] * curr_point[0];
			normalize(tmp_X);
			tmp_Z = curr_point;

			tmp_coord.clear();
			tmp_coord.push_back(tmp_X);
			tmp_coord.push_back(tmp_Y);
			tmp_coord.push_back(tmp_Z);
			m_arcs.push_back(tmp_coord);

            //cout << "X*Y = " << rdl_dot_3d(tmp_X, tmp_Y) << endl;
            //cout << "Y*Z = " << rdl_dot_3d(tmp_Y, tmp_Z) << endl;
            //cout << "Z*X = " << rdl_dot_3d(tmp_Z, tmp_X) << endl;

			tmp_coord.clear();
			tmp_X[0] = 0 - tmp_X[0];
			tmp_X[1] = 0 - tmp_X[1];
			tmp_X[2] = 0 - tmp_X[2];

			tmp_Y[0] = 0 - tmp_Y[0];
			tmp_Y[1] = 0 - tmp_Y[1];
			tmp_Y[2] = 0 - tmp_Y[2];

			tmp_coord.push_back(tmp_X);
			tmp_coord.push_back(tmp_Y);
			tmp_coord.push_back(tmp_Z);
			m_arcs.push_back(tmp_coord);

            //cout << "X*Y = " << rdl_dot_3d(tmp_X, tmp_Y) << endl;
            //cout << "Y*Z = " << rdl_dot_3d(tmp_Y, tmp_Z) << endl;
            //cout << "Z*X = " << rdl_dot_3d(tmp_Z, tmp_X) << endl << endl;
		}
	}
};

struct MyUnitBall{
	vector<vector<double> >		m_unitball_coarse;
	vector<vector<double> >		m_unitball_dense;

	MyBall_Coarse			m_coarse;
	MyBall_Dense				m_dense;
	MyBall_Pose				m_pose;
	vector<vector<unsigned short> >				m_table;		//[12560*2][1256]
	vector<vector<pair<unsigned short, unsigned short> > >	m_table_sort;		//[12560*2][1256]
	vector<vector<unsigned short> >				m_table_coarse2dense;
	vector<vector<unsigned short> >				m_table_dense2coarse;
	vector<vector<vector<double> > >			m_rmatrix;

	void ssave_matching_table(string pm_filename)
	{
		std::ofstream ofs(pm_filename.c_str());
		boost::archive::text_oarchive oa(ofs);
		oa & m_table;
	}

	void ssave_matching_table_sort(string pm_filename)
	{
		std::ofstream ofs(pm_filename.c_str());
		boost::archive::text_oarchive oa(ofs);
		oa & m_table_sort;
	}

	void ssave_matching_table_coarse2dense(string pm_filename)
	{
		std::ofstream ofs(pm_filename.c_str());
		boost::archive::text_oarchive oa(ofs);
		oa & m_table_coarse2dense;
	}

	void ssave_matching_table_dense2coarse(string pm_filename)
	{
		std::ofstream ofs(pm_filename.c_str());
		boost::archive::text_oarchive oa(ofs);
		oa & m_table_dense2coarse;
	}

	void ssave_matching_table_rmatrix(string pm_filename)
	{
		std::ofstream ofs(pm_filename.c_str());
		boost::archive::text_oarchive oa(ofs);
		oa & m_rmatrix;
	}

	void sload_matching_table(string pm_filename)
	{
		m_table.clear();
		std::ifstream ifs(pm_filename.c_str(), std::ios::binary);
		boost::archive::text_iarchive la(ifs);
		la & m_table;
	}

	void sload_matching_table_sort(string pm_filename)
	{
		m_table_sort.clear();
		std::ifstream ifs(pm_filename.c_str(), std::ios::binary);
		boost::archive::text_iarchive la(ifs);
		la & m_table_sort;
	}

	void sload_matching_table_coarse2dense(string pm_filename)
	{
		m_table_coarse2dense.clear();
		std::ifstream ifs(pm_filename.c_str(), std::ios::binary);
		boost::archive::text_iarchive la(ifs);
		la & m_table_coarse2dense;
	}

	void sload_matching_table_dense2coarse(string pm_filename)
	{
		m_table_dense2coarse.clear();
		std::ifstream ifs(pm_filename.c_str(), std::ios::binary);
		boost::archive::text_iarchive la(ifs);
		la & m_table_dense2coarse;
	}

	void sload_matching_table_rmatrix(string pm_filename)
	{
		m_rmatrix.clear();
		std::ifstream ifs(pm_filename.c_str(), std::ios::binary);
		boost::archive::text_iarchive la(ifs);
		la & m_rmatrix;
	}

	void check_table_sort()
	{
		size_t i, j;

		for (i = 0; i < m_table_sort.size(); i ++){
			for (j = 0; j < m_table_sort[i].size(); j ++){
				if (m_table[i][m_table_sort[i][j].first] != m_table_sort[i][j].second){
					cout << "ERROR: " << m_table[i][m_table_sort[i][j].first] << " " << m_table_sort[i][j].second << endl;
 				}
			}
		}
	}

	void write_bin_matching_table_sort(string pm_filename)
	{
        ofstream out_file (pm_filename.c_str(), ios::out|ios::binary);
		unsigned short* out_buffer = new unsigned short [m_table_sort.size() * m_table_sort[0].size() * 2];
		char* head_buffer = reinterpret_cast<char *>(out_buffer);
		size_t i, j;

		for (i = 0; i < m_table_sort.size(); i ++){
			for (j = 0; j < m_table_sort[i].size(); j ++){
				*(out_buffer++) = m_table_sort[i][j].first;
				*(out_buffer++) = m_table_sort[i][j].second;
			}
		}

		unsigned int len = m_table_sort.size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));
		len = m_table_sort[0].size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));

		out_file.write(head_buffer, m_table_sort.size() * m_table_sort[0].size() * 2 * sizeof(unsigned short));
		out_file.close();
	}

	void read_bin_matching_table_sort(string pm_filename)
	{
		m_table_dense2coarse.clear();
        ifstream in_file (pm_filename.c_str(), ios::in | ios::binary | ios::ate);
		ifstream::pos_type in_size = in_file.tellg();
		char* head_buffer = new char [in_size];
		in_file.seekg(0, ios::beg);
		in_file.read(head_buffer, in_size);
		in_file.close();

		unsigned int num_row = *(reinterpret_cast<unsigned int *>(head_buffer));
		unsigned int num_column = *(reinterpret_cast<unsigned int *>(head_buffer+sizeof(unsigned int)));
		m_table_sort.insert(m_table_sort.end(), num_row, vector<pair<unsigned short, unsigned short> >(num_column, pair<unsigned short, unsigned short>(0, 0)));

		size_t i, j;

		for (i = 0; i < num_row; i ++){
			for (j = 0; j < num_column; j ++){
				m_table_sort[i][j].first = (reinterpret_cast<unsigned short *>(head_buffer+2*sizeof(unsigned int)))[i*num_column*2 + j*2];
				m_table_sort[i][j].second = (reinterpret_cast<unsigned short *>(head_buffer+2*sizeof(unsigned int)))[i*num_column*2 + j*2 + 1];
			}
		}
		delete[] head_buffer;
	}

	void write_bin_matching_table_dense2coarse(string pm_filename)
	{
        ofstream out_file (pm_filename.c_str(), ios::out|ios::binary);
		unsigned short* out_buffer = new unsigned short [m_table_dense2coarse.size() * m_table_dense2coarse[0].size()];
		char* head_buffer = reinterpret_cast<char *>(out_buffer);
		size_t i, j;

		for (i = 0; i < m_table_dense2coarse.size(); i ++){
			for (j = 0; j < m_table_dense2coarse[i].size(); j ++){
				*(out_buffer) = m_table_dense2coarse[i][j];
				out_buffer ++;
			}
		}

		unsigned int len = m_table_dense2coarse.size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));
		len = m_table_dense2coarse[0].size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));

		out_file.write(head_buffer, m_table_dense2coarse.size() * m_table_dense2coarse[0].size() * sizeof(unsigned short));
		out_file.close();
	}

	void read_bin_matching_table_dense2coarse()
	{
		m_table_dense2coarse.clear();
		ifstream in_file ("matching_table_dense2coarse.bin", ios::in | ios::binary | ios::ate);
		ifstream::pos_type in_size = in_file.tellg();
		char* head_buffer = new char [in_size];
		in_file.seekg(0, ios::beg);
		in_file.read(head_buffer, in_size);
		in_file.close();

		unsigned int num_row = *(reinterpret_cast<unsigned int *>(head_buffer));
		unsigned int num_column = *(reinterpret_cast<unsigned int *>(head_buffer+sizeof(unsigned int)));
		m_table_dense2coarse.insert(m_table_dense2coarse.end(), num_row, vector<unsigned short>(num_column, 0));

		size_t i, j;

		for (i = 0; i < num_row; i ++){
			for (j = 0; j < num_column; j ++){
				m_table_dense2coarse[i][j] = (reinterpret_cast<unsigned short *>(head_buffer+2*sizeof(unsigned int)))[i*num_column + j];
			}
		}
		delete[] head_buffer;
	}

	void write_bin_matching_table_coarse2dense(string pm_filename)
	{
        ofstream out_file (pm_filename.c_str(), ios::out|ios::binary);
		unsigned short* out_buffer = new unsigned short [m_table_coarse2dense.size() * m_table_coarse2dense[0].size()];
		char* head_buffer = reinterpret_cast<char *>(out_buffer);
		size_t i, j;

		for (i = 0; i < m_table_coarse2dense.size(); i ++){
			for (j = 0; j < m_table_coarse2dense[i].size(); j ++){
				*(out_buffer) = m_table_coarse2dense[i][j];
				out_buffer ++;
			}
		}

		unsigned int len = m_table_coarse2dense.size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));
		len = m_table_coarse2dense[0].size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));

		out_file.write(head_buffer, m_table_coarse2dense.size() * m_table_coarse2dense[0].size() * sizeof(unsigned short));
		out_file.close();
	}

	void read_bin_matching_table_coarse2dense()
	{
		m_table_coarse2dense.clear();
		ifstream in_file ("matching_table_coarse2dense.bin", ios::in | ios::binary | ios::ate);
		ifstream::pos_type in_size = in_file.tellg();
		char* head_buffer = new char [in_size];
		in_file.seekg(0, ios::beg);
		in_file.read(head_buffer, in_size);
		in_file.close();

		unsigned int num_row = *(reinterpret_cast<unsigned int *>(head_buffer));
		unsigned int num_column = *(reinterpret_cast<unsigned int *>(head_buffer+sizeof(unsigned int)));
		m_table_coarse2dense.insert(m_table_coarse2dense.end(), num_row, vector<unsigned short>(num_column, 0));

		size_t i, j;

		for (i = 0; i < num_row; i ++){
			for (j = 0; j < num_column; j ++){
				m_table_coarse2dense[i][j] = (reinterpret_cast<unsigned short *>(head_buffer+2*sizeof(unsigned int)))[i*num_column + j];
			}
		}
		delete[] head_buffer;
	}

	void write_bin_matching_table(string pm_filename)
	{
        ofstream out_file (pm_filename.c_str(), ios::out|ios::binary);
		unsigned short* out_buffer = new unsigned short [m_table.size() * m_table[0].size()];
		char* head_buffer = reinterpret_cast<char *>(out_buffer);
		size_t i, j;

		for (i = 0; i < m_table.size(); i ++){
			for (j = 0; j < m_table[i].size(); j ++){
				*(out_buffer) = m_table[i][j];
				out_buffer ++;
			}
		}

		unsigned int len = m_table.size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));
		len = m_table[0].size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));

		out_file.write(head_buffer, m_table.size() * m_table[0].size() * sizeof(unsigned short));
		out_file.close();
	}

	void read_bin_matching_table()
	{
		m_table.clear();
		ifstream in_file ("matching_table.bin", ios::in | ios::binary | ios::ate);
		ifstream::pos_type in_size = in_file.tellg();
		char* head_buffer = new char [in_size];
		in_file.seekg(0, ios::beg);
		in_file.read(head_buffer, in_size);
		in_file.close();

		unsigned int num_row = *(reinterpret_cast<unsigned int *>(head_buffer));
		unsigned int num_column = *(reinterpret_cast<unsigned int *>(head_buffer+sizeof(unsigned int)));
		m_table.insert(m_table.end(), num_row, vector<unsigned short>(num_column, 0));

		size_t i, j;

		for (i = 0; i < num_row; i ++){
			for (j = 0; j < num_column; j ++){
				m_table[i][j] = (reinterpret_cast<unsigned short *>(head_buffer+2*sizeof(unsigned int)))[i*num_column + j];
			}
		}

		delete[] head_buffer;
	}

	void write_bin_rotation_matrix(string pm_filename)
	{
        ofstream out_file (pm_filename.c_str(), ios::out|ios::binary);
		double* out_buffer = new double [m_rmatrix.size() * 9];
		char* head_buffer = reinterpret_cast<char *>(out_buffer);
		size_t i, j, k;

		for (i = 0; i < m_rmatrix.size(); i ++){
			for (j = 0; j < m_rmatrix[i].size(); j ++){
				for (k = 0; k < m_rmatrix[i][j].size(); k ++){
					*(out_buffer) = m_rmatrix[i][j][k];
					out_buffer ++;
				}
			}
		}
		unsigned int len = m_rmatrix.size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));
		out_file.write(head_buffer, m_rmatrix.size() * 9 * sizeof(double));
		out_file.close();
	}

	void read_bin_rotation_matrix()
	{
		m_rmatrix.clear();
		ifstream in_file ("rotation_matrix.bin", ios::in | ios::binary | ios::ate);
		ifstream::pos_type in_size = in_file.tellg();
		char* head_buffer = new char [in_size];
		in_file.seekg(0, ios::beg);
		in_file.read(head_buffer, in_size);
		in_file.close();

		size_t num_matrix = *(reinterpret_cast<unsigned int *>(head_buffer));
		m_rmatrix.insert(m_rmatrix.end(), num_matrix, vector<vector<double> >(3, vector<double>(3, 0)));
		size_t i, j, k;

		for (i = 0; i < num_matrix; i ++){
			for (j = 0; j < 3; j ++){
				for (k = 0; k < 3; k ++){
					m_rmatrix[i][j][k] = (reinterpret_cast<double *>(head_buffer+sizeof(unsigned int)))[i*9 + j*3 + k];
				}
			}
		}
		delete[] head_buffer;
	}

	void print_rotation_matrix()
	{
		size_t i;
		//stringstream out_buffer;
		//ofstream out_file("rotation_matrix.txt");
		//out_buffer << m_rmatrix.size() << endl;
		FILE*	f = fopen ("rotation_matrix.txt","w");
		fprintf(f, "%d\n", m_rmatrix.size());
		for (i = 0; i < m_rmatrix.size(); i ++){
			fprintf(f, "%+1.16e %+1.16e %+1.16e ",m_rmatrix[i][0][0], m_rmatrix[i][0][1], m_rmatrix[i][0][2]);
			fprintf(f, "%+1.16e %+1.16e %+1.16e ",m_rmatrix[i][1][0], m_rmatrix[i][1][1], m_rmatrix[i][1][2]);
			fprintf(f, "%+1.16e %+1.16e %+1.16e\n",m_rmatrix[i][2][0], m_rmatrix[i][2][1], m_rmatrix[i][2][2]);

			//out_buffer << m_rmatrix[i][0][0] << " ";
			//out_buffer << m_rmatrix[i][0][1] << " ";
			//out_buffer << m_rmatrix[i][0][2] << " ";
			//out_buffer << m_rmatrix[i][1][0] << " ";
			//out_buffer << m_rmatrix[i][1][1] << " ";
			//out_buffer << m_rmatrix[i][1][2] << " ";
			//out_buffer << m_rmatrix[i][2][0] << " ";
			//out_buffer << m_rmatrix[i][2][1] << " ";
			//out_buffer << m_rmatrix[i][2][2] << endl;
		}
		//out_file << out_buffer.str();
		//out_file.close();
	}

	void load_rotation_matrix()
	{
		m_rmatrix.clear();

		ifstream in_file("rotation_matrix.txt");
		int num;
		in_file >> num;

		vector<vector<double> > tmp_matrix(3, vector<double>(3, 0));
		int i;

		for (i = 0; i < num; i ++){
			in_file >> tmp_matrix[0][0]
			        >> tmp_matrix[0][1]
					>> tmp_matrix[0][2]
					>> tmp_matrix[1][0]
			        >> tmp_matrix[1][1]
					>> tmp_matrix[1][2]
					>> tmp_matrix[2][0]
			        >> tmp_matrix[2][1]
					>> tmp_matrix[2][2];
			m_rmatrix.push_back(tmp_matrix);
		}

		in_file.close();
	}

	void load_matching_table()
	{
		m_table.clear();

		ifstream in_file("matching_table.txt");
		int num_pose, num_coarse;
		in_file >> num_pose >> num_coarse;

		vector<unsigned short>	tmp_row;
		unsigned short id_ray;
		int i, j;
		for (i = 0; i < num_pose; i ++){
			tmp_row.clear();
			for (j = 0; j < num_coarse; j ++){
				in_file >> id_ray;
				tmp_row.push_back(id_ray);
			}
			m_table.push_back(tmp_row);
		}
		in_file.close();
	}

	void load_matching_table_coarse2dense()
	{
		m_table_coarse2dense.clear();

		ifstream in_file("matching_table_coarse2dense.txt");
		int num_pose, num_coarse;
		in_file >> num_pose >> num_coarse;

		vector<unsigned short>	tmp_row;
		unsigned short id_ray;
		int i, j;
		for (i = 0; i < num_pose; i ++){
			tmp_row.clear();
			for (j = 0; j < num_coarse; j ++){
				in_file >> id_ray;
				tmp_row.push_back(id_ray);
			}
			m_table_coarse2dense.push_back(tmp_row);
		}
		in_file.close();
	}

	void load_matching_table_dense2coarse()
	{
		m_table_dense2coarse.clear();

		ifstream in_file("matching_table_dense2coarse.txt");
		int num_pose, num_coarse;
		in_file >> num_pose >> num_coarse;

		vector<unsigned short>	tmp_row;
		unsigned short id_ray;
		int i, j;
		for (i = 0; i < num_pose; i ++){
			tmp_row.clear();
			for (j = 0; j < num_coarse; j ++){
				in_file >> id_ray;
				tmp_row.push_back(id_ray);
			}
			m_table_dense2coarse.push_back(tmp_row);
		}
		in_file.close();
	}

	void init()
	{
		load_unitball();
		m_coarse.init();
		m_dense.init();
		m_pose.init();
//cerr << "m_pose.init_pose();" << endl;
		m_pose.init_pose();
	}

	void load_unitball()
	{
		vector<double>	tmp_point(3, 0);
		size_t i;

		m_unitball_coarse.clear();
		for (i = 0; i < points_default_coarse; i ++){
			tmp_point[0] = vertex_default_coarse[i][0];
			tmp_point[1] = vertex_default_coarse[i][1];
			tmp_point[2] = vertex_default_coarse[i][2];
			m_unitball_coarse.push_back(tmp_point);
		}

		m_unitball_dense.clear();
		for (i = 0; i < points_default_dense; i ++){
			tmp_point[0] = vertex_default_dense[i][0];
			tmp_point[1] = vertex_default_dense[i][1];
			tmp_point[2] = vertex_default_dense[i][2];
			m_unitball_dense.push_back(tmp_point);
		}

	}

	void count_matching_table()
	{
		vector<int>	tmp_counter(points_default_dense, 0);
		size_t i, j;

		for (i = 0; i < m_table.size(); i ++){
			for (j = 0; j < m_table[i].size(); j ++){
				tmp_counter[m_table[i][j]] += 1;
			}
		}

		sort(tmp_counter.begin(), tmp_counter.end());
		copy(tmp_counter.begin(), tmp_counter.end(), ostream_iterator<unsigned short>(cout, "\n"));
	}

	void print_matching_table_coarse2dense()
	{
		size_t i, j;
		stringstream out_buffer;
		out_buffer << m_table_coarse2dense.size() << " " << m_table_coarse2dense[0].size() << endl;
		for (i = 0; i < m_table_coarse2dense.size(); i ++){
			for (j = 0; j < m_table_coarse2dense[i].size(); j ++){
				out_buffer << m_table_coarse2dense[i][j] << " ";
			}
			out_buffer << endl;
		}

		ofstream out_file("matching_table_coarse2dense.txt");
		out_file << out_buffer.str();
		out_file.close();
	}

	void print_matching_table_dense2coarse()
	{
		size_t i, j;
		stringstream out_buffer;
		out_buffer << m_table_dense2coarse.size() << " " << m_table_dense2coarse[0].size() << endl;
		for (i = 0; i < m_table_dense2coarse.size(); i ++){
			for (j = 0; j < m_table_dense2coarse[i].size(); j ++){
				out_buffer << m_table_dense2coarse[i][j] << " ";
			}
			out_buffer << endl;
		}

		ofstream out_file("matching_table_dense2coarse.txt");
		out_file << out_buffer.str();
		out_file.close();
	}

	void print_matching_table()
	{
		size_t i, j;
		stringstream out_buffer;
		out_buffer << m_table.size() << " " << m_table[0].size() << endl;
		for (i = 0; i < m_table.size(); i ++){
			for (j = 0; j < m_table[i].size(); j ++){
				out_buffer << m_table[i][j] << " ";
			}
			out_buffer << endl;
		}

		ofstream out_file("matching_table.txt");
		out_file << out_buffer.str();
		out_file.close();
	}

	void compute_matching_table()
	{
		m_table.clear();
		m_rmatrix.clear();

		vector<unsigned short>	tmp_pose;
		vector<pair<double, pair<unsigned short, unsigned short> > >	tmp_pose_sort;
		vector<pair<unsigned short, unsigned short> >	tmp_pose_sort_row;
		vector<unsigned short>	tmp_coarse2dense;
		vector<unsigned short>	tmp_dense2coarse;
		unsigned short tmp_id_dense;
		size_t i, j;
		//double M[3][3];
		//RotationMatrix M;
		vector<vector<double> > M(3, vector<double>(3, 0));
		vector<double>	tmp_point_old(3, 0);
		vector<double>	tmp_point_new(3, 0);
		double			tmp_local_r_sq = 0.4;
		double			tmp_dist;

		for (i = 0; i < m_pose.m_arcs.size(); i ++){
if (i/100*100 == i) cerr << "i = " << i << endl;

			M[0][0] = m_pose.m_arcs[i][0][0];
			M[1][0] = m_pose.m_arcs[i][0][1];
			M[2][0] = m_pose.m_arcs[i][0][2];

			M[0][1] = m_pose.m_arcs[i][1][0];
			M[1][1] = m_pose.m_arcs[i][1][1];
			M[2][1] = m_pose.m_arcs[i][1][2];

			M[0][2] = m_pose.m_arcs[i][2][0];
			M[1][2] = m_pose.m_arcs[i][2][1];
			M[2][2] = m_pose.m_arcs[i][2][2];

			tmp_dist  = M[0][2] * M[0][2];
			tmp_dist += M[1][2] * M[1][2];
			tmp_dist += (M[2][2] - 1) * (M[2][2] - 1);
			if (tmp_dist > tmp_local_r_sq) continue;

			m_rmatrix.push_back(M);
			tmp_pose.clear();
			tmp_pose_sort.clear();

			for (j = 0; j < points_default_coarse; j++){
				tmp_point_old[0] = vertex_default_coarse[j][0];
				tmp_point_old[1] = vertex_default_coarse[j][1];
				tmp_point_old[2] = vertex_default_coarse[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_id_dense = m_dense.query_nn(tmp_point_new);
				tmp_pose.push_back(tmp_id_dense);
				tmp_pose_sort.push_back(pair<double, pair<unsigned short, unsigned short> >(2 * tmp_point_new[2] - tmp_point_old[2], pair<unsigned short, unsigned short>(j, tmp_id_dense)));
			}
			m_table.push_back(tmp_pose);

			sort(tmp_pose_sort.begin(), tmp_pose_sort.end(), less_than_univ<double, pair<unsigned short, unsigned short> >());
			tmp_pose_sort_row.clear();
			for (j = 0; j < tmp_pose_sort.size(); j ++){
				tmp_pose_sort_row.push_back(tmp_pose_sort[j].second);
			}
			m_table_sort.push_back(tmp_pose_sort_row);

			tmp_coarse2dense.clear();
			for (j = 0; j < points_default_pose; j++){
				tmp_point_old[0] = vertex_default_pose[j][0];
				tmp_point_old[1] = vertex_default_pose[j][1];
				tmp_point_old[2] = vertex_default_pose[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_coarse2dense.push_back(m_pose.query_nn(tmp_point_new));
			}
			m_table_coarse2dense.push_back(tmp_coarse2dense);
			tmp_dense2coarse = tmp_coarse2dense;
			for (j = 0; j < tmp_coarse2dense.size(); j ++){
				tmp_dense2coarse[tmp_coarse2dense[j]] = j;
			}
			m_table_dense2coarse.push_back(tmp_dense2coarse);
		}
	}

    //will generate full size table
	void compute_matching_table_v6()
	{
		m_table.clear();
		m_rmatrix.clear();

		vector<unsigned short>	tmp_pose;
		vector<unsigned short>	tmp_coarse2dense;
		vector<unsigned short>	tmp_dense2coarse;
		unsigned short tmp_id_dense;
		size_t i, j;
		vector<vector<double> > M(3, vector<double>(3, 0));
		vector<double>	tmp_point_old(3, 0);
		vector<double>	tmp_point_new(3, 0);

		for (i = 0; i < m_pose.m_arcs.size(); i ++){
if (i/100*100 == i) cerr << "i = " << i << endl;

			M[0][0] = m_pose.m_arcs[i][0][0];
			M[1][0] = m_pose.m_arcs[i][0][1];
			M[2][0] = m_pose.m_arcs[i][0][2];

			M[0][1] = m_pose.m_arcs[i][1][0];
			M[1][1] = m_pose.m_arcs[i][1][1];
			M[2][1] = m_pose.m_arcs[i][1][2];

			M[0][2] = m_pose.m_arcs[i][2][0];
			M[1][2] = m_pose.m_arcs[i][2][1];
			M[2][2] = m_pose.m_arcs[i][2][2];

			m_rmatrix.push_back(M);
			tmp_pose.clear();

			for (j = 0; j < points_default_coarse; j++){
				tmp_point_old[0] = vertex_default_coarse[j][0];
				tmp_point_old[1] = vertex_default_coarse[j][1];
				tmp_point_old[2] = vertex_default_coarse[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_id_dense = m_dense.query_nn(tmp_point_new);
				tmp_pose.push_back(tmp_id_dense);
			}
			m_table.push_back(tmp_pose);

			tmp_coarse2dense.clear();
			for (j = 0; j < points_default_dense; j++){
				tmp_point_old[0] = vertex_default_dense[j][0];
				tmp_point_old[1] = vertex_default_dense[j][1];
				tmp_point_old[2] = vertex_default_dense[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_coarse2dense.push_back(m_dense.query_nn(tmp_point_new));
			}
			m_table_coarse2dense.push_back(tmp_coarse2dense);
			tmp_dense2coarse = tmp_coarse2dense;
			for (j = 0; j < tmp_coarse2dense.size(); j ++){
				tmp_dense2coarse[tmp_coarse2dense[j]] = j;
			}
			m_table_dense2coarse.push_back(tmp_dense2coarse);
		}

        cout << "m_table.size() = " << m_table.size() << endl;
        cout << "m_rmatrix.size() = " << m_rmatrix.size() << endl;
        cout << "m_table_coarse2dense.size() = " << m_table_coarse2dense.size() << endl;
        cout << "m_table_dense2coarse.size() = " << m_table_dense2coarse.size() << endl;
	}

	void compute_matching_table_v6_2(double pm_angle_degree)
	{
		m_table.clear();
		m_rmatrix.clear();

		vector<pair<double, pair<unsigned short, unsigned short> > >	tmp_pose_sort(points_default_coarse, pair<double, pair<unsigned short, unsigned short> >(0, pair<unsigned short, unsigned short>(0, 0)));
		vector<pair<unsigned short, unsigned short> >	tmp_pose_sort_row(points_default_coarse, pair<unsigned short, unsigned short>(0, 0));

		vector<unsigned short>	tmp_pose(points_default_coarse, 0);
		vector<unsigned short>	tmp_coarse2dense(points_default_dense, 0);
		vector<unsigned short>	tmp_dense2coarse(points_default_dense, 0);
		unsigned short tmp_id_dense;
		size_t i, j;
		vector<vector<double> > M(3, vector<double>(3, 0));
		vector<double>	tmp_point_old(3, 0);
		vector<double>	tmp_point_new(3, 0);

		double			tmp_local_r_sq = sin(M_PI*pm_angle_degree/360)*sin(M_PI*pm_angle_degree/360)*4;; //0.268 = 30 degree, 0.362 = 35, 0.468 = 40, 0.586 = 45,
		double			tmp_dist;

		m_table_sort.clear();

        cout << "Please wait ..." << endl;
		for (i = 0; i < m_pose.m_arcs.size(); i ++){
//if (i/100*100 == i) cerr << "i = " << i << endl;

			M[0][0] = m_pose.m_arcs[i][0][0];
			M[1][0] = m_pose.m_arcs[i][0][1];
			M[2][0] = m_pose.m_arcs[i][0][2];

			M[0][1] = m_pose.m_arcs[i][1][0];
			M[1][1] = m_pose.m_arcs[i][1][1];
			M[2][1] = m_pose.m_arcs[i][1][2];

			M[0][2] = m_pose.m_arcs[i][2][0];
			M[1][2] = m_pose.m_arcs[i][2][1];
			M[2][2] = m_pose.m_arcs[i][2][2];

			tmp_dist  = M[0][2] * M[0][2];
			tmp_dist += M[1][2] * M[1][2];
			tmp_dist += (M[2][2] - 1) * (M[2][2] - 1);
			if (tmp_dist > tmp_local_r_sq) continue;

			m_rmatrix.push_back(M);

			for (j = 0; j < points_default_coarse; j++){
				tmp_point_old[0] = vertex_default_coarse[j][0];
				tmp_point_old[1] = vertex_default_coarse[j][1];
				tmp_point_old[2] = vertex_default_coarse[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_id_dense = m_dense.query_nn(tmp_point_new);
				tmp_pose[j] = tmp_id_dense;
				tmp_pose_sort[j] = pair<double, pair<unsigned short, unsigned short> >(2 * tmp_point_new[2] - tmp_point_old[2], pair<unsigned short, unsigned short>(j, tmp_id_dense));
			}
			m_table.push_back(tmp_pose);

			sort(tmp_pose_sort.begin(), tmp_pose_sort.end(), less_than_univ<double, pair<unsigned short, unsigned short> >());
			for (j = 0; j < tmp_pose_sort.size(); j ++){
				tmp_pose_sort_row[j] = tmp_pose_sort[j].second;
			}
			m_table_sort.push_back(tmp_pose_sort_row);

			for (j = 0; j < points_default_dense; j++){
				tmp_point_old[0] = vertex_default_dense[j][0];
				tmp_point_old[1] = vertex_default_dense[j][1];
				tmp_point_old[2] = vertex_default_dense[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_coarse2dense[j] = m_dense.query_nn(tmp_point_new);
			}
			m_table_coarse2dense.push_back(tmp_coarse2dense);

			for (j = 0; j < tmp_coarse2dense.size(); j ++){
				tmp_dense2coarse[tmp_coarse2dense[j]] = j;
			}
			m_table_dense2coarse.push_back(tmp_dense2coarse);
		}

        cout << "m_table.size() = " << m_table.size() << endl;
        cout << "m_rmatrix.size() = " << m_rmatrix.size() << endl;
        cout << "m_table_coarse2dense.size() = " << m_table_coarse2dense.size() << endl;
        cout << "m_table_dense2coarse.size() = " << m_table_dense2coarse.size() << endl;
	}

};

void task_map_sphere2circle(int argc, char *argv[])
{
	if (argc < 3){
		cerr << "USAGE: " << argv[0] << " <out_vtk_filename>" << endl;
		return;
	}

    MyUnitBall myBall;

    myBall.init();

	//m_unitball_dense
	size_t i;
	double x, y, z, r, s, u, v;
	vector<vector<double> >	tmp_points;
	vector<double>			tmp_point(2, 0);

	for (i = 0; i < myBall.m_unitball_dense.size(); i ++){
		x = myBall.m_unitball_dense[i][0];
		y = myBall.m_unitball_dense[i][1];
		z = myBall.m_unitball_dense[i][2];
		if (y < 0 || z < 0 || x < 0 || x == -1) continue;
		s = 0.5 * (1 - x);
		r = 2 * sqrt(1 - s);
		u = y / r;
		v = z / r;
		tmp_point[0] = u;
		tmp_point[1] = v;
		tmp_points.push_back(tmp_point);
	}

	//write to vtk file
	string out_fn = argv[2];
	ofstream out_file(out_fn.c_str());
	stringstream out_buffer;

	out_buffer << "# vtk DataFile Version 3.0" << endl;
	out_buffer << argv[2] << endl;
	out_buffer << "ASCII" << endl;
	out_buffer << "DATASET POLYDATA" << endl;
	out_buffer << "POINTS " << tmp_points.size() << " float" << endl;

	size_t j;
	for (j = 0; j < tmp_points.size(); j ++){
		out_buffer << tmp_points[j][0] << " "
				   << tmp_points[j][1] << " "
			       << 0 << endl;
	}

	out_buffer << "VERTICES " << tmp_points.size() << " " << tmp_points.size() * 2 << endl;
	for(j = 0; j < tmp_points.size(); j ++){
		out_buffer << "1 " << j << endl;
	}

	out_file << out_buffer.str();
	out_file.close();

}

void task_mtgen_v6(int argc, char *argv[])
{
    MyUnitBall myBall;

    myBall.init();
cerr << "myBall.compute_matching_table_v6(); " << endl;
	myBall.compute_matching_table_v6();

cerr << "serializing the tables..." << endl;
	myBall.ssave_matching_table("ssave_matching_table_v6.txt");
	myBall.ssave_matching_table_rmatrix("ssave_matching_table_v6_rmatrix.txt");
	//myBall.ssave_matching_table_sort("ssave_matching_table_v6_sort.txt");
	myBall.ssave_matching_table_dense2coarse("ssave_matching_table_v6_dense2coarse.txt");
	myBall.ssave_matching_table_coarse2dense("ssave_matching_table_v6_coarse2dense.txt");

//cerr << "myBall.write_bin_rotation_matrix();" << endl;
//	myBall.write_bin_rotation_matrix("matching_table_rotation_matrix_v6.bin");
//
//cerr << "myBall.write_bin_matching_table();" << endl;
//	myBall.write_bin_matching_table("matching_table_v6.bin");
//
//cerr << "myBall.write_bin_matching_table_coarse2dense();" << endl;
//	myBall.write_bin_matching_table_coarse2dense("matching_table_coarse2dense_v6.bin");
//
//cerr << "myBall.write_bin_matching_table_dense2coarse();" << endl;
//	myBall.write_bin_matching_table_dense2coarse("matching_table_dense2coarse_v6.bin");

}

void task_mtgen_v6_2(int argc, char *argv[])
{
    MyUnitBall myBall;

    myBall.init();
//cerr << "myBall.compute_matching_table_v6_2(); " << endl;
	myBall.compute_matching_table_v6_2(atof(argv[1]));

		string fn_table = string("ssave_matching_table_") + string(argv[1]) + string(".txt");
		string fn_table_rmatrix = string("ssave_matching_table_") + string(argv[1]) + string("_rmatrix.txt");
		string fn_table_sort = string("ssave_matching_table_") + string(argv[1]) + string("_sort.txt");
		string fn_table_dense2coarse = string("ssave_matching_table_") + string(argv[1]) + string("_dense2coarse.txt");
		string fn_table_coarse2dense = string("ssave_matching_table_") + string(argv[1]) + string("_coarse2dense.txt");

cerr << "serializing the tables..." << endl;
	myBall.ssave_matching_table(fn_table.c_str());
	myBall.ssave_matching_table_rmatrix(fn_table_rmatrix.c_str());
	myBall.ssave_matching_table_sort(fn_table_sort.c_str());
	myBall.ssave_matching_table_dense2coarse(fn_table_dense2coarse.c_str());
	myBall.ssave_matching_table_coarse2dense(fn_table_coarse2dense.c_str());
cerr << "Done!" << endl;

//cerr << "myBall.write_bin_rotation_matrix();" << endl;
//	myBall.write_bin_rotation_matrix("matching_table_rotation_matrix_v6.bin");
//
//cerr << "myBall.write_bin_matching_table();" << endl;
//	myBall.write_bin_matching_table("matching_table_v6.bin");
//
//cerr << "myBall.write_bin_matching_table_coarse2dense();" << endl;
//	myBall.write_bin_matching_table_coarse2dense("matching_table_coarse2dense_v6.bin");
//
//cerr << "myBall.write_bin_matching_table_dense2coarse();" << endl;
//	myBall.write_bin_matching_table_dense2coarse("matching_table_dense2coarse_v6.bin");

}

void task_mtgen(int argc, char *argv[])
{
	MyUnitBall myBall;

    myBall.init();
cerr << "myBall.compute_matching_table(); " << endl;
	myBall.compute_matching_table();

cerr << "serializing the tables..." << endl;
	myBall.ssave_matching_table("ssave_matching_table_full.txt");
	myBall.ssave_matching_table_rmatrix("ssave_matching_table_full_rmatrix.txt");
	myBall.ssave_matching_table_sort("ssave_matching_table_full_sort.txt");
	myBall.ssave_matching_table_dense2coarse("ssave_matching_table_full_dense2coarse.txt");
	myBall.ssave_matching_table_coarse2dense("ssave_matching_table_full_coarse2dense.txt");

//cerr << "myBall.write_bin_rotation_matrix();" << endl;
//	myBall.write_bin_rotation_matrix("rotation_matrix.bin");
//
//cerr << "myBall.write_bin_matching_table();" << endl;
//	myBall.write_bin_matching_table("matching_table.bin");
//
//cerr << "myBall.write_bin_matching_table_coarse2dense();" << endl;
//	myBall.write_bin_matching_table_coarse2dense("matching_table_coarse2dense.bin");
//
//cerr << "myBall.write_bin_matching_table_dense2coarse();" << endl;
//	myBall.write_bin_matching_table_dense2coarse("matching_table_dense2coarse.bin");
//
//cerr << "myBall.write_bin_matching_table_sort();" << endl;
//	myBall.write_bin_matching_table_sort("matching_table_sort.bin");

}

int main(int argc, char *argv[])
{
    //MyUnitBall myBall;
    //myBall.init();

		if (argc != 2){
				cerr << "Usage: " << argv[0] << " <angle>" << endl;
				return -1;
		}

		task_mtgen_v6_2(argc, argv);

/*
    if (argc < 2){
        task_mtgen(argc, argv);
    }
    else if (atoi(argv[1]) == 6){
        task_mtgen_v6(argc, argv);
    }
    else if (atoi(argv[1]) == 62){
        task_mtgen_v6_2(argc, argv);
    }
		else if (atoi(argv[1]) == 5){
				task_map_sphere2circle(argc, argv);
		}
*/

/*
	myBall.load_rotation_matrix();
	myBall.load_matching_table();
	myBall.load_matching_table_coarse2dense();
	myBall.load_matching_table_dense2coarse();

	myBall.write_bin_rotation_matrix();
	myBall.write_bin_matching_table();
	myBall.write_bin_matching_table_coarse2dense();
	myBall.write_bin_matching_table_dense2coarse();
*/

/*
	cout << "myBall.read_bin_matching_table();" << endl;
	myBall.read_bin_matching_table();

	//cout << "myBall.print_matching_table();" << endl;
	//myBall.print_matching_table();

	cout << "myBall.read_bin_matching_table();" << endl;
	myBall.read_bin_matching_table_sort();

	cout << "myBall.read_bin_matching_table_coarse2dense();" << endl;
	myBall.read_bin_matching_table_coarse2dense();

	//cout << "myBall.print_matching_table_coarse2dense();" << endl;
	//myBall.print_matching_table_coarse2dense();

	cout << "myBall.read_bin_matching_table_dense2coarse();" << endl;
	myBall.read_bin_matching_table_dense2coarse();

	//cout << "myBall.print_matching_table_dense2coarse();" << endl;
	//myBall.print_matching_table_dense2coarse();

	cout << "myBall.read_bin_rotation_matrix();" << endl;
	myBall.read_bin_rotation_matrix();

	//cout << "myBall.print_rotation_matrix();" << endl;
	//myBall.print_rotation_matrix();

	myBall.check_table_sort();
*/

    return 0;
}


/* Copyright 2020 Gopinath Chennupati, Raviteja Vangara, Namita Kharat, Erik Skau and Boian Alexandrov,
Triad National Security, LLC. All rights reserved
This program was produced under U.S. Government contract 89233218CNA000001 for Los Alamos National Laboratory (LANL), which is operated by Triad National Security, LLC for the U.S. Department of Energy/National Nuclear Security Administration. All rights in the program are reserved by Triad National Security, LLC, and the U.S. Department of Energy/National Nuclear Security Administration. The Government is granted for itself and others acting on its behalf a nonexclusive, paid-up, irrevocable worldwide license in this material to reproduce, prepare derivative works, distribute copies to the public, perform publicly and display publicly, and to permit others to do so.
*/

#ifndef DISTNMF_DISTREORDER_HPP_
#define DISTNMF_DISTREORDER_HPP_

#include <unistd.h>
#include <armadillo>
#include <string>
#include "../planc-master/common/utils.hpp"
#include "../planc-master/common/distutils.hpp"
#include "../planc-master/distnmf/mpicomm.hpp"

/**
 * File name formats
 */

namespace planc {

template <class MATTYPE>
class DistReOrder {
 private:
  const MPICommunicator& m_mpicomm;
  int m_ownedm;
  int m_ownedn;
  int m_globalm;
  int m_globaln;

  int m_pr;
  int m_pc;
  int m_k;

  UROWVEC localMaxIDx; // vector of max element indices from each proc
  ROWVEC localMax;     // vector of max elements from each proc

  UROWVEC globalMaxIDx; // vector of max element indices from all proc(s)
  ROWVEC globalMax;     // vector of max elements from all proc(s)

  UROWVEC finalIDx;     //vector of final max element indices;
                        //still contains the local indices
  UROWVEC globalIDx;    //Contains the global row indexes for m_k columns
  UVEC sortIDx;         //sort indices of W
  UVEC sharedSortIDx;   //rank 0 initialized with same sortIDx for pr * pc
  UVEC globalSortIDx;   // global sort indices of W

  UMAT globalIMx;       // matrix of final max-val indices across all proc(s)
  MAT globalM;          // matrix of final max elements across all proc(s)

  MAT W, H;             //Both the low rank factors
  MAT new_W, new_H;     //reordered low rank factors

  /**
   * Allocates matrices and vectors
   */
  void allocateData() {
    // Init vectors
    /*
    DISTPRINTINFO("k::"<< this->k <<"::localm::"<< this->m_ownedm <<"::localn::"
                    << this->m_ownedn << "::globalm::" << this->m_globalm <<
                    "::globaln::"<< this->m_globaln <<"::MPI_SIZE::"<< MPI_SIZE);
    */
    localMax.zeros(this->m_k);
    localMaxIDx.zeros(this->m_k);
    globalMaxIDx.zeros(this->m_pr * this->m_pc * this->m_k);
    globalMax.zeros(this->m_pr * this->m_pc * this->m_k);
    finalIDx.zeros(this->m_k);
    globalIDx.zeros(this->m_k);
    sortIDx.zeros(this->m_k);
    sharedSortIDx.zeros(this->m_pr * this->m_pc, 1);
    globalSortIDx.zeros(this->m_k);

    // Init matrices
    globalIMx.zeros(this->m_pr * this->m_pc, this->m_k);
    globalM.zeros(this->m_pr * this->m_pc, this->m_k);

    // low-rank factor matrices
    new_W.zeros(this->W.n_rows, this->W.n_cols);
    new_H.zeros(this->H.n_rows, this->H.n_cols);
  }

  void freeMatrices() {
    localMax.clear();
    localMaxIDx.clear();
    globalMaxIDx.clear();
    globalMax.clear();
    finalIDx.clear();
    globalIDx.clear();
    globalIMx.clear();
    sortIDx.clear();
    globalSortIDx.clear();
    globalM.clear();
    W.clear();
    H.clear();
    new_W.clear();
    new_H.clear();
  }

 public:
    DistReOrder<MATTYPE>(const MAT &leftlowrankfactor, const MAT &rightlowrankfactor,
            const MPICommunicator& communicator, const int k)
        : m_mpicomm(communicator) {
        assert(leftlowrankfactor.n_cols == rightlowrankfactor.n_cols);
        this->m_k = k;
        this->m_pr = NUMROWPROCS;
        this->m_pc = NUMCOLPROCS;
        //Assign the low-rank factors
        this->W = leftlowrankfactor;
        this->H = rightlowrankfactor;
        this->m_ownedm  = leftlowrankfactor.n_rows;
        this->m_ownedn = rightlowrankfactor.n_cols;
        allocateData();
        PRINTROOT("distreorder()::constructor succesful");
    }

    ~DistReOrder() {
      //freeMatrices();
    }

    void finalGlobalSortIDx() {
        /* local Max and IDx */
        localMax = arma::max(this->W,0);
        localMaxIDx = arma::index_max(this->W, 0) + this->m_ownedm;
        //TODO! Need to take care of the topk maxima and indices here
        /* Global Max and IDx */
        MPITIC;  // gather globalMax, globalMaxIDx
        int sendcnt = this->m_k;
        int recvcnt = this->m_k;
        globalMax.zeros();
        MPI_Gather(localMax.memptr(), sendcnt, MPI_DOUBLE, globalMax.memptr(),
                recvcnt, MPI_DOUBLE, 0, MPI_COMM_WORLD);
        globalMaxIDx.zeros();
        MPI_Gather(localMaxIDx.memptr(), sendcnt, MPI_DOUBLE, globalMaxIDx.memptr(),
                recvcnt, MPI_DOUBLE, 0, MPI_COMM_WORLD);
        double temp = MPITOC;  // gather globalMax, globalMaxIDx
        //TODO! Need to report time later
        /* finalIDx, globalM, globalIMx and globalIDx */
        if(MPI_RANK == 0)   {
            globalIMx = arma::reshape(globalMaxIDx.t(), this->m_k,
                                (this->m_pr * this->m_pc)).t();
            globalM = arma::reshape(globalMax, this->m_k,
                                (this->m_pr * this->m_pc)).t();
            //globalM.print("globalM = ");
            //globalIMx.print("globalIMx = ");
            finalIDx = arma::index_max(globalM, 0);
            for(int ki = 0; ki < this->m_k; ki++)   {
                globalIDx(ki) = globalIMx(finalIDx(ki),ki);
            }
            //globalIDx.print("globalIDx = ");
            sortIDx = arma::sort_index(globalIDx);
            sortIDx.print("Rank 0 sortIDx = ");
            sharedSortIDx = arma::repmat(sortIDx, this->m_pr * this->m_pc, 1);
            //H.print("H in rank 0 = ");
            //globalSortIDx.print("Rank 0 globalSortIDx = ");
        }
        //Scatter the sortIDx to all the procs in MPI_COMM_WORLD
        globalSortIDx.zeros();
        MPI_Scatter(this->sharedSortIDx.memptr(), sendcnt, MPI_DOUBLE, this->globalSortIDx.memptr(),
                    recvcnt, MPI_DOUBLE, 0, MPI_COMM_WORLD);
    }

    void reorderW()   {
        finalGlobalSortIDx();
        MPI_Barrier(MPI_COMM_WORLD);
        //Now is the time for reordering the columns in W
        this->sortIDx.print("local sortIDx = ");
        this->globalSortIDx.print("globalSortIDX = ");
        //W.print("local W = ");
        //new_W.zeros();
        for(int col = 0; col < this->m_k; col++)    {
            //W is stored as row-major order
            new_W.col(col) = W.col(globalSortIDx(col));
        }
        if(MPI_RANK == 0) { new_W.print("local new_W = "); std::cout<<"current k "<<this->m_k<<std::endl; }
    }

    void reorderH()   {
        /*
        if(arma::sum(sortIDx) == 0){
            finalGlobalSortIDx();
        }*/
        MPI_Barrier(MPI_COMM_WORLD);
        //Now is the time for reordering the rows in H
        //new_H.zeros();
        for(int col = 0; col < this->m_k; col++)    {
            //In H also, swapping the cols, because, it is in column-major order
            new_H.col(col) = H.col(this->globalSortIDx(col));
        }
    }

    /// Returns the left low rank factor matrix W
    MAT getLeftLowRankFactor() { return new_W; }
    /// Returns the right low rank factor matrix H
    MAT getRightLowRankFactor() { return new_H; }

  }; //class DistReOrder

}  // namespace planc

// run with mpi run 3.
void testDistClust(char argc, char* argv[]) {
  planc::MPICommunicator mpicomm(argc, argv);
  planc::DistReOrder<MAT> dc(arma::randn(10, 4),
                  arma::randu(4,8), mpicomm, 8);;

  dc.reorderW();
}

#endif  // DISTNMF_DISTREORDER_HPP_


#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <Eigen/Dense>

#include <algorithm>
#include <cstdint>
#include <limits>
#include <map>
#include <memory>
#include <random>
#include <stdexcept>
#include <utility>
#include <vector>

namespace py = pybind11;

namespace ote {

const double EPS = 1e-8;
const double EPS2 = 1e-5;
const double EPS3 = 1e-3;

inline int32_t sign(float x) {
  if (fabs(x) < EPS) {
    throw std::logic_error("computing sign of ~0");
  }
  if (x > 0) return 1;
  return -1;
}

class OTEstimators {
 public:
  using NumPyFloatArray = py::array_t<float, py::array::c_style>;
  using NumPyIntArray = py::array_t<int32_t, py::array::c_style>;

  using EigenVector = Eigen::Matrix<float, 1, Eigen::Dynamic, Eigen::RowMajor>;
  using EigenMatrix =
      Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  using Matrix = Eigen::Map<EigenMatrix>;

  OTEstimators() : stage(0) {}

  void load_vocabulary(NumPyFloatArray points) {
    if (stage != 0) {
      throw std::logic_error(
          "load_vocabulary() should be called once in the beginning");
    }
    stage = 1;
    py::buffer_info buf = points.request();
    if (buf.ndim != 2) {
      throw std::logic_error(
          "load_vocabulary() expects a two-dimensional NumPy array");
    }
    auto n = buf.shape[0];
    auto d = buf.shape[1];
    dictionary = std::make_unique<Matrix>(static_cast<float *>(buf.ptr), n, d);
    auto cmin = std::numeric_limits<float>::max();
    auto cmax = std::numeric_limits<float>::min();
    for (ssize_t i = 0; i < n; ++i) {
      for (ssize_t j = 0; j < d; ++j) {
        cmin = std::min(cmin, (*dictionary)(i, j));
        cmax = std::max(cmax, (*dictionary)(i, j));
      }
    }
    auto delta = cmax - cmin;
    cmin -= delta;
    std::random_device rd;
    std::mt19937_64 gen(rd());
    std::uniform_real_distribution<float> shift_gen(0.0, delta);
    std::vector<std::pair<float, float>> bounding_box;
    for (ssize_t i = 0; i < d; ++i) {
      auto s = shift_gen(gen);
      bounding_box.push_back(std::make_pair(cmin + s, cmax + s));
    }
    std::vector<int32_t> all;
    for (ssize_t i = 0; i < n; ++i) {
      all.push_back(i);
    }
    leaf.resize(n);
    build_quadtree(all, bounding_box, 0, -1);
    num_queries = 0;
    marked.resize(parents.size());
    for (auto &x : marked) {
      x = -1;
    }
    node_id.resize(parents.size());
  }

  void load_dataset(
      const std::vector<std::vector<std::pair<int32_t, float>>> &dataset) {
    if (stage != 1) {
      throw std::logic_error(
          "load_dataset() should be called once after calling "
          "load_vocabulary()");
    }
    stage = 2;
    if (dataset.empty()) {
      throw std::logic_error("the dataset can't be empty");
    }
    for (auto &measure : dataset) {
      check_measure(measure);
      dataset_embedding.push_back(compute_embedding(measure));
    }

    raw_dataset = dataset;
    for (auto &measure : raw_dataset) {
      std::sort(measure.begin(), measure.end());
    }

    means.resize(dataset.size(), dictionary->cols());
    for (size_t i = 0; i < dataset.size(); ++i) {
      means.row(i) =
          dictionary->row(dataset[i][0].first) * dataset[i][0].second;
      for (size_t j = 1; j < dataset[i].size(); ++j) {
        means.row(i) +=
            dictionary->row(dataset[i][j].first) * dataset[i][j].second;
      }
    }
    query_mean.resize(dictionary->cols());
    distances.resize(dataset.size());
  }

  void means_rank(const std::vector<std::pair<int32_t, float>> &query,
                  NumPyIntArray input_ids, NumPyIntArray output_ids,
                  NumPyFloatArray output_scores, bool to_sort) {
    check_stage();
    check_measure(query);
    check_input_output_arrays(input_ids, output_ids, output_scores);
    query_mean = dictionary->row(query[0].first) * query[0].second;
    for (size_t j = 1; j < query.size(); ++j) {
      query_mean =
          query_mean + dictionary->row(query[j].first) * query[j].second;
    }
    auto input_ids_buf = input_ids.request();
    auto input_ids_raw = static_cast<int32_t *>(input_ids_buf.ptr);
    int32_t k1 = input_ids_buf.shape[0];
    for (int32_t i = 0; i < k1; ++i) {
      float score = (query_mean - means.row(input_ids_raw[i])).squaredNorm();
      distances[i] = std::make_pair(score, input_ids_raw[i]);
    }
    select_topk_aux(k1, output_ids, output_scores, to_sort);
  }

  void overlap_rank(const std::vector<std::pair<int32_t, float>> &query,
                    NumPyIntArray input_ids, NumPyIntArray output_ids,
                    NumPyFloatArray output_scores, bool to_sort) {
    check_stage();
    check_measure(query);
    auto query_copy = query;
    std::sort(query_copy.begin(), query_copy.end());
    auto input_ids_buf = input_ids.request();
    auto input_ids_raw = static_cast<int32_t *>(input_ids_buf.ptr);
    int32_t k1 = input_ids_buf.shape[0];
    for (int32_t i = 0; i < k1; ++i) {
      auto &point = raw_dataset[input_ids_raw[i]];
      float score = 0.0;
      size_t qp = 0;
      size_t dp = 0;

      while (qp < query_copy.size() && dp < point.size()) {
        if (query_copy[qp].first < point[dp].first) {
          ++qp;
        } else if (query_copy[qp].first > point[dp].first) {
          ++dp;
        } else {
          score += 1;
          ++dp;
          ++qp;
        }
      }
      distances[i] = std::make_pair(-score, input_ids_raw[i]);
    }
    select_topk_aux(k1, output_ids, output_scores, to_sort);
  }

  void quadtree_rank(const std::vector<std::pair<int32_t, float>> &query,
                     NumPyIntArray input_ids, NumPyIntArray output_ids,
                     NumPyFloatArray output_scores, bool to_sort) {
    check_stage();
    check_measure(query);
    check_input_output_arrays(input_ids, output_ids, output_scores);
    auto query_embedding = compute_embedding(query);
    auto input_ids_buf = input_ids.request();
    auto input_ids_raw = static_cast<int32_t *>(input_ids_buf.ptr);
    int32_t k1 = input_ids_buf.shape[0];
    for (int32_t i = 0; i < k1; ++i) {
      auto &point_embedding = dataset_embedding[input_ids_raw[i]];
      float score = 0.0;
      size_t qp = 0;
      size_t dp = 0;
      while (qp < query_embedding.size() || dp < point_embedding.size()) {
        if (qp == query_embedding.size()) {
          score += point_embedding[dp].second;
          ++dp;
        } else if (dp == point_embedding.size()) {
          score += query_embedding[qp].second;
          ++qp;
        } else if (query_embedding[qp].first < point_embedding[dp].first) {
          score += query_embedding[qp].second;
          ++qp;
        } else if (point_embedding[dp].first < query_embedding[qp].first) {
          score += point_embedding[dp].second;
          ++dp;
        } else {
          score +=
              fabs(query_embedding[qp].second - point_embedding[dp].second);
          ++qp;
          ++dp;
        }
      }
      distances[i] = std::make_pair(score, input_ids_raw[i]);
    }
    select_topk_aux(k1, output_ids, output_scores, to_sort);
  }

  void flowtree_rank(const std::vector<std::pair<int32_t, float>> &query,
                     NumPyIntArray input_ids, NumPyIntArray output_ids,
                     NumPyFloatArray output_scores, bool to_sort) {
    check_stage();
    check_measure(query);
    check_input_output_arrays(input_ids, output_ids, output_scores);
    auto input_ids_buf = input_ids.request();
    auto input_ids_raw = static_cast<int32_t *>(input_ids_buf.ptr);
    int32_t k1 = input_ids_buf.shape[0];
    for (int32_t i = 0; i < k1; ++i) {
      auto cur_id = input_ids_raw[i];
      auto score = flowtree_query(query, raw_dataset[cur_id]);
      distances[i] = std::make_pair(score, cur_id);
    }
    select_topk_aux(k1, output_ids, output_scores, to_sort);
  }

  void select_topk(NumPyIntArray input_ids, NumPyFloatArray input_scores,
                   NumPyIntArray output_ids, NumPyFloatArray output_scores,
                   bool to_sort) {
    check_input_output_arrays(input_ids, input_scores, output_ids,
                              output_scores);
    auto input_scores_buf = input_scores.request();
    auto input_ids_buf = input_ids.request();
    auto input_scores_raw = static_cast<float *>(input_scores_buf.ptr);
    auto input_ids_raw = static_cast<int32_t *>(input_ids_buf.ptr);
    int32_t k1 = input_ids_buf.shape[0];
    for (int32_t i = 0; i < k1; ++i) {
      distances[i] = std::make_pair(input_scores_raw[i], input_ids_raw[i]);
    }
    select_topk_aux(k1, output_ids, output_scores, to_sort);
  }

 private:
  std::vector<int32_t> parents;
  std::vector<int32_t> leaf;
  std::vector<int32_t> marked;
  int32_t num_queries;
  std::vector<int32_t> node_id;
  std::vector<int32_t> id_node;
  std::vector<std::vector<int32_t>> subtree;
  std::vector<std::vector<std::pair<float, int32_t>>> excess;
  std::vector<float> delta_node;
  std::unique_ptr<Matrix> dictionary;
  std::vector<int32_t> unleaf;
  std::vector<std::vector<std::pair<int32_t, float>>> dataset_embedding;
  std::vector<std::vector<std::pair<int32_t, float>>> raw_dataset;
  EigenMatrix means;
  EigenVector query_mean;
  std::vector<std::pair<float, int32_t>> distances;
  int32_t stage;

  void build_quadtree(const std::vector<int32_t> &subset,
                      const std::vector<std::pair<float, float>> &bounding_box,
                      int32_t depth, int32_t parent) {
    int32_t node_id(parents.size());
    parents.push_back(parent);
    if (subset.size() == 1) {
      leaf[subset[0]] = node_id;
      return;
    }
    int32_t d = dictionary->cols();
    std::vector<float> mid(d);
    for (int32_t i = 0; i < d; ++i) {
      mid[i] = (bounding_box[i].first + bounding_box[i].second) / 2.0;
    }
    std::map<std::vector<uint8_t>, std::vector<int32_t>> parts;
    for (auto ind : subset) {
      std::vector<uint8_t> code((d + 7) / 8, 0);
      for (int32_t i = 0; i < d; ++i) {
        if ((*dictionary)(ind, i) > mid[i]) {
          code[i / 8] |= 1 << (i % 8);
        }
      }
      parts[code].push_back(ind);
    }
    std::vector<std::pair<float, float>> new_bounding_box(d);
    for (const auto &part : parts) {
      for (int32_t i = 0; i < d; ++i) {
        uint8_t bit = (part.first[i / 8] >> (i % 8)) & 1;
        if (bit) {
          new_bounding_box[i] = std::make_pair(mid[i], bounding_box[i].second);
        } else {
          new_bounding_box[i] = std::make_pair(bounding_box[i].first, mid[i]);
        }
      }
      build_quadtree(part.second, new_bounding_box, depth + 1, node_id);
    }
  }

  std::vector<std::pair<int32_t, float>> compute_embedding(
      const std::vector<std::pair<int32_t, float>> &a) {
    std::vector<std::pair<int32_t, float>> result;
    for (auto x : a) {
      auto id = leaf[x.first];
      int32_t level = 0;
      while (id != -1) {
        ++level;
        id = parents[id];
      }
      id = leaf[x.first];
      while (id != -1) {
        --level;
        result.push_back(std::make_pair(id, x.second / (1 << level)));
        id = parents[id];
      }
    }
    std::sort(result.begin(), result.end());
    std::vector<std::pair<int32_t, float>> ans;
    for (auto x : result) {
      if (ans.empty() || ans.back().first != x.first) {
        ans.push_back(x);
      } else {
        ans.back().second += x.second;
      }
    }
    return ans;
  }

  float flowtree_query(const std::vector<std::pair<int32_t, float>> &a,
                       const std::vector<std::pair<int32_t, float>> &b) {
    int32_t num_nodes = 0;
    id_node.clear();
    for (auto x : a) {
      auto id = leaf[x.first];
      while (id != -1) {
        if (marked[id] != num_queries) {
          id_node.push_back(id);
          node_id[id] = num_nodes++;
        }
        marked[id] = num_queries;
        id = parents[id];
      }
    }
    for (auto x : b) {
      auto id = leaf[x.first];
      while (id != -1) {
        if (marked[id] != num_queries) {
          id_node.push_back(id);
          node_id[id] = num_nodes++;
        }
        marked[id] = num_queries;
        id = parents[id];
      }
    }
    if (static_cast<int32_t>(subtree.size()) < num_nodes) {
      subtree.resize(num_nodes);
    }
    for (int32_t i = 0; i < num_nodes; ++i) {
      subtree[i].clear();
    }
    for (int32_t i = 0; i < num_nodes; ++i) {
      int32_t u = parents[id_node[i]];
      if (u != -1) {
        subtree[node_id[u]].push_back(i);
      }
    }
    if (static_cast<int32_t>(excess.size()) < num_nodes) {
      excess.resize(num_nodes);
    }
    delta_node.assign(num_nodes, 0.0);
    unleaf.resize(num_nodes);
    for (auto x : a) {
      delta_node[node_id[leaf[x.first]]] += x.second;
      unleaf[node_id[leaf[x.first]]] = x.first;
    }
    for (auto x : b) {
      delta_node[node_id[leaf[x.first]]] -= x.second;
      unleaf[node_id[leaf[x.first]]] = x.first;
    }
    float res = run_query(0, node_id[0]);
    if (!excess[node_id[0]].empty()) {
      float unassigned = 0.0;
      for (auto x : excess[node_id[0]]) {
        unassigned += x.first;
      }
      if (unassigned > EPS2) {
        throw std::logic_error("too much unassigned flow");
      }
    }
    ++num_queries;
    return res;
  }

  float run_query(int32_t depth, int32_t nd) {
    float res = 0.0;
    for (auto x : subtree[nd]) {
      res += run_query(depth + 1, x);
    }
    excess[nd].clear();
    if (subtree[nd].empty()) {
      if (fabs(delta_node[nd]) > EPS) {
        excess[nd].push_back(std::make_pair(delta_node[nd], unleaf[nd]));
      }
    } else {
      for (auto x : subtree[nd]) {
        if (excess[x].empty()) {
          continue;
        }
        bool same = false;
        if (excess[nd].empty()) {
          same = true;
        } else if (sign(excess[x][0].first) == sign(excess[nd][0].first)) {
          same = true;
        }
        if (same) {
          for (auto y : excess[x]) {
            excess[nd].push_back(y);
          }
        } else {
          while (!excess[x].empty() && !excess[nd].empty()) {
            auto u = excess[nd].back();
            auto v = excess[x].back();

            float dist =
                (dictionary->row(u.second) - dictionary->row(v.second)).norm();
            if (fabs(u.first + v.first) < EPS) {
              excess[nd].pop_back();
              excess[x].pop_back();
              res += dist * fabs(u.first);
            } else if (fabs(u.first) < fabs(v.first)) {
              excess[nd].pop_back();
              excess[x].back().first += u.first;
              res += dist * fabs(u.first);
            } else {
              excess[x].pop_back();
              excess[nd].back().first += v.first;
              res += dist * fabs(v.first);
            }
          }
          if (!excess[x].empty()) {
            excess[x].swap(excess[nd]);
          }
        }
      }
    }
    return res;
  }

  void select_topk_aux(int32_t k1, NumPyIntArray output_ids,
                       NumPyFloatArray output_scores, bool to_sort) {
    auto output_ids_buf = output_ids.request();
    auto output_ids_raw = static_cast<int32_t *>(output_ids_buf.ptr);
    auto output_scores_buf = output_scores.request();
    auto output_scores_raw = static_cast<float *>(output_scores_buf.ptr);
    int32_t k2 = output_ids_buf.shape[0];
    std::nth_element(distances.begin(), distances.begin() + k2 - 1,
                     distances.begin() + k1);
    if (to_sort) {
      std::sort(distances.begin(), distances.begin() + k2);
    }
    for (int32_t i = 0; i < k2; ++i) {
      output_scores_raw[i] = distances[i].first;
      output_ids_raw[i] = distances[i].second;
    }
  }

  void check_measure(const std::vector<std::pair<int32_t, float>> &measure) {
    float sum = 0.0;
    auto n = dictionary->rows();
    for (auto &atom : measure) {
      if (atom.first < 0 || atom.first >= n) {
        throw std::logic_error("invalid index in the measure");
      }
      if (atom.second < -EPS) {
        throw std::logic_error("negative mass");
      }
      sum += atom.second;
    }
    if (fabs(sum - 1.0) > EPS3) {
      throw std::logic_error("the masses don't sum to 1");
    }
  }

  void check_stage() {
    if (stage != 2) {
      throw std::logic_error(
          "need to call load_vocabulary() and load_dataset() first");
    }
  }

  template <typename T>
  void check_dimension(T x) {
    auto buf = x.request();
    if (buf.ndim != 1) {
      throw std::logic_error(
          "input_ids, output_ids, output_scores must be one-dimensional");
    }
  }

  template <typename T>
  ssize_t get_length(T x) {
    return x.request().shape[0];
  }

  void check_input_output_arrays(NumPyIntArray input_ids,
                                 NumPyIntArray output_ids,
                                 NumPyFloatArray output_scores) {
    check_dimension(input_ids);
    check_dimension(output_ids);
    check_dimension(output_scores);
    auto l1 = get_length(input_ids);
    auto l2 = get_length(output_ids);
    auto l3 = get_length(output_scores);
    if (l2 != l3) {
      throw std::logic_error(
          "output_ids and output_scores must be of the same length");
    }
    if (l2 > l1) {
      throw std::logic_error(
          "output_ids and output_scores must be no longer than input_ids");
    }
    auto buf = static_cast<int32_t *>(input_ids.request().ptr);
    for (ssize_t i = 0; i < l1; ++i) {
      auto val = buf[i];
      if (val < 0 || val >= static_cast<int32_t>(raw_dataset.size())) {
        throw std::logic_error("input_ids contain an invalid index");
      }
    }
  }

  void check_input_output_arrays(NumPyIntArray input_ids,
                                 NumPyFloatArray input_scores,
                                 NumPyIntArray output_ids,
                                 NumPyFloatArray output_scores) {
    check_input_output_arrays(input_ids, output_ids, output_scores);
    check_dimension(input_scores);
    if (get_length(input_ids) != get_length(input_scores)) {
      throw std::logic_error(
          "input_ids and input_scores must be of the same length");
    }
  }
};
}  // namespace ote

PYBIND11_MODULE(ot_estimators, m) {
  using ote::OTEstimators;
  py::class_<OTEstimators>(m, "OTEstimators")
      .def(py::init<>())
      .def("load_vocabulary", &OTEstimators::load_vocabulary)
      .def("load_dataset", &OTEstimators::load_dataset)
      .def("means_rank", &OTEstimators::means_rank)
      .def("overlap_rank", &OTEstimators::overlap_rank)
      .def("quadtree_rank", &OTEstimators::quadtree_rank)
      .def("flowtree_rank", &OTEstimators::flowtree_rank)
      .def("select_topk", &OTEstimators::select_topk);
}


#include <iostream>
#include <cassert>
#include <string>
#include <fstream>
#include <set>
#include <vector>
#include <map>
#include <random>
#include <chrono>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

using namespace std;
using namespace boost;

// Typedefs for Input and output graphs
struct vertex_p
{
    int name;
    float color;
    string fillcolor;
};

struct edge_p
{
    double weight;
    unsigned penwidth;
};

typedef adjacency_list <listS, vecS, undirectedS, vertex_p, edge_p, no_property > graph_p;

typedef graph_traits<graph_p>::vertex_descriptor vd_p;
typedef graph_traits<graph_p>::edge_descriptor ed_p;

// Typedefs for intermediate graphs

struct vertex_q
{
    unsigned name;
    unsigned label;
    float color;
};

struct edge_q
{
    double weight;
};

typedef adjacency_list <listS, vecS, undirectedS, vertex_q, edge_q, no_property > graph_q;
typedef graph_traits<graph_q>::vertex_descriptor vd_q;
typedef graph_traits<graph_q>::edge_descriptor ed_q;

// Global Variables
string input_filename, output_filename;
float multicast_fraction = 0.0;
unsigned vertex_count = 0;
set<unsigned> multicast_vertices;

int main(int argc, char* argv[])
{
    if(argc != 4)
    {
        cerr << "Usage: " << argv[0] << " <Multicast Fraction> <Input.dot> <Output.dot>\n";
        return 1;
    }

    try
    {
        multicast_fraction = stof(string(argv[1]));
        if(multicast_fraction < 0 || multicast_fraction > 1)
            throw invalid_argument("Fraction should be > 0 and < 1.");
    }
    catch(std::exception &err)
    {
        cerr << err.what() << endl;
        cerr << "Invalid fraction argument\n";
        return 1;
    }

    input_filename.assign(argv[2]);
    output_filename.assign(argv[3]);

#ifdef DEBUG
    cout << "Running in debug mode\n";
    ifstream input_file("steiner.dot", ios::in);
    ofstream output_file("debug.dot", ios::out);
#else
    ifstream input_file(input_filename.c_str(), ios::in);
    ofstream output_file(output_filename.c_str(), ios::out);
#endif

    assert(output_file.is_open() && input_file.is_open() && "Error opening files");

    graph_p network(0);
    dynamic_properties dp;
    auto name = get(&vertex_p::name, network);
    dp.property("node_id", name);
    auto fillcolor = get(&vertex_p::fillcolor, network);
    dp.property("fillcolor", fillcolor);
    auto weight = get(&edge_p::weight, network);
    dp.property("weight", weight);
    auto penwidth = get(&edge_p::penwidth, network);
    dp.property("penwidth", penwidth);

    try
    {
        read_graphviz(input_file, network, dp, "node_id");
    }
    catch(std::exception &err)
    {
        cerr << err.what() << endl;
        cerr << "read_graphviz failed for " << input_filename << "\n";
        return 1;
    }

    // Initialize all dynamic properties

    BGL_FORALL_VERTICES(v, network, graph_p)
    {
        fillcolor[v] = "white";
    }

    BGL_FORALL_EDGES(e, network, graph_p)
    {
        penwidth[e] = 1;
    }

    // TODO: Add timing code

    vertex_count = num_vertices(network);
    unsigned multicast_count = unsigned(multicast_fraction * vertex_count);

#ifndef DEBUG
    // Assume that vertices are numbered 0 -- N-1 (generated by BRITE)
    // Randomly select multicast nodes using uniform distribution

    std::default_random_engine generator;
    std::uniform_int_distribution<unsigned> distribution(0, multicast_count - 1);

    multicast_count = multicast_count == 0 ? 1 : multicast_count;

    do
    {
        // Insert vd_p Names ( == vd_p descriptors )
        unsigned value = distribution(generator) % multicast_count;
        multicast_vertices.insert(value);
    } while( multicast_vertices.size() < multicast_count);

#else
    // debug for steiner.dot
    multicast_count = 4;
    cout << "Num vd_p: " << vertex_count << endl;
    cout << "Num MCast: " << multicast_count << endl;
    multicast_vertices.insert(0);
    multicast_vertices.insert(1);
    multicast_vertices.insert(2);
    multicast_vertices.insert(3);

    // Debug
    ostream dbg(std::cout.rdbuf());
    write_graphviz_dp(dbg, network, dp);
#endif

    graph_q G1(0);
    map<unsigned, vd_q> vd_p_to_vd_q_map;

    for(auto &V : multicast_vertices)
    {
        vd_q Vx = add_vertex(G1);
        //cout << Vx << " " << V << "\n";
        G1[Vx].label = V;
        G1[Vx].name = Vx;
        vd_p_to_vd_q_map.insert(make_pair(V,Vx));
    }

#ifdef DEBUG
    // Debug
    dynamic_properties dp_q;
    auto name_q = get(&vertex_q::name, G1);
    dp_q.property("node_id", name_q);
    auto label_q = get(&vertex_q::label, G1);
    dp_q.property("label", label_q);
    auto weight_q = get(&edge_q::weight, G1);
    dp_q.property("weight", weight_q);
    write_graphviz_dp(dbg, G1, dp_q);
    cout << "Step1-----------------------------------\n";
#endif

    map<vd_p, vector<vd_p> > AllPreds;
    map<vd_p, vector<unsigned> > AllDistances;

    auto start_step_1 = chrono::steady_clock::now();

    // Step 1 -- Construct undirected distance graph G1, G and S.
    for(auto V = multicast_vertices.begin(), E = multicast_vertices.end(); V != E; V++)
    {
        vd_p Vx = *V;
        // Get the shortest path between Vx,All
        vector<unsigned> D(vertex_count);

        AllPreds.insert(pair<vd_p,vector<vd_p> >(Vx, vector<vd_p>(vertex_count)));
        AllDistances.insert(pair<vd_p,vector<unsigned> >(Vx, vector<unsigned>(vertex_count)));

        dijkstra_shortest_paths(network, Vx,
                weight_map(get(&edge_p::weight,network))
                .distance_map(make_iterator_property_map(AllDistances[Vx].begin(), get(vertex_index, network)))
                .predecessor_map(make_iterator_property_map(AllPreds[Vx].begin(), get(vertex_index, network))));

        for(auto W = std::next(V); W != E; W++)
        {
            vd_p Wx = *W;
            //// Debug
            //cout << "Adding ed_p (Vx, Wx, D[Wx]): " << Vx << "," << Wx << "," << D[Wx] << endl;
            ed_q e; bool found;
            tie(e,found) = add_edge(vd_p_to_vd_q_map[*V], vd_p_to_vd_q_map[*W], G1);
            G1[e].weight = AllDistances[Vx][Wx];
        }
    }

    auto end_step_1 = chrono::steady_clock::now();
    cout << "[KMB] Step1: "<< chrono::duration <double, milli> (end_step_1-start_step_1).count() << " ms" << endl;

#ifdef DEBUG
    // Debug
    write_graphviz_dp(dbg, G1, dp_q);
    cout << "Step2-----------------------------------\n";
#endif

    // Step 2a -- Construct Minimum Spanning Tree from G1

    auto start_step_2 = chrono::steady_clock::now();

    vector<ed_q> st;
    kruskal_minimum_spanning_tree(G1, back_inserter(st),
            weight_map(get(&edge_q::weight, G1)));

    // Step 2b -- Trim G1 based on MST

    set<vd_q> vKeep, vRemove;
    set<ed_q> eKeep, eRemove;

    for(auto &e : st)
    {
        vKeep.insert(source(e, G1)); vKeep.insert(target(e, G1));
        eKeep.insert(e);
    }

    auto end_step_2 = chrono::steady_clock::now();
    cout << "[KMB] Step2: "<< chrono::duration <double, milli> (end_step_2-start_step_2).count() << " ms" << endl;

#ifdef DEBUG
    BGL_FORALL_VERTICES(v, G1, graph_q)
    {
        if(vKeep.count(v) == 0)
            vRemove.insert(v);
    }

    BGL_FORALL_EDGES(e, G1, graph_q)
    {
        if(eKeep.count(e) == 0)
            eRemove.insert(e);
    }

    for(auto Ex : eRemove) remove_edge(Ex, G1);
    for(auto Vx : vRemove) remove_vertex(Vx, G1);

    write_graphviz_dp(dbg, G1, dp_q);
    cout << "Step3-----------------------------------\n";
#endif

    auto start_step_3 = chrono::steady_clock::now();
    // Step 3 -- Add the shortest paths back to G1 (expand each edge)

    graph_q G2(0);

    // For each edge in the MST, get the path we saved earlier
    // For each vertex in path, if vertex is not added, add vertex, add edge
    // else check if edge is added else add edge. Set weight

    set<unsigned> added_labels;
    map<unsigned, vd_q> label_map;

    for(auto &Ex : eKeep)
    {

#ifdef DEBUG
        cout << "Source: " << G1[source(Ex,G1)].label << "\n";
        cout << "Target: " << G1[target(Ex,G1)].label << "\n";
#endif

        // Traverse from sink to source

        auto predecessor_map = AllPreds[G1[source(Ex,G1)].label];
        vd_p v = G1[target(Ex, G1)].label;
        for(vd_p u = predecessor_map[v];          // Start by setting 'u' to the destintaion node's predecessor
                u != v;                          // Keep tracking the path until we get to the source
                v = u, u = predecessor_map[v])   // Set the current vertex to the current predecessor, and the predecessor to one level up
        {
            vd_q Vx, Ux;
            if(added_labels.count(v) == 0)
            {
                Vx = add_vertex(G2);
                G2[Vx].label = v;
                G2[Vx].name = Vx;
                added_labels.insert(v);
                label_map.insert(make_pair(v,Vx));
            }
            else
            {
                Vx = label_map[v];
            }

            if(added_labels.count(u) == 0)
            {
                Ux = add_vertex(G2);
                G2[Ux].label = u;
                G2[Ux].name = Ux;
                added_labels.insert(u);
                label_map.insert(make_pair(u,Ux));
            }
            else
            {
                Ux = label_map[u];
            }

            ed_q eq; bool fq = false;
            tie(eq, fq) = edge(Vx, Ux, G2);
            if(!fq)
            {
                ed_p e; bool f = false;
                tie(e, f) = edge(u, v, network);
                assert(f && "Could not find edge in original graph!");
                double wt = network[e].weight;

                tie(eq, fq) = add_edge(Vx, Ux, G2);
                G2[eq].weight = wt;
            }
        }
    }

    auto end_step_3 = chrono::steady_clock::now();
    cout << "[KMB] Step3: "<< chrono::duration <double, milli> (end_step_3-start_step_3).count() << " ms" << endl;

#ifdef DEBUG
    dynamic_properties dp_q2;
    auto name_q2 = get(&vertex_q::name, G2);
    dp_q2.property("node_id", name_q2);
    auto label_q2 = get(&vertex_q::label, G2);
    dp_q2.property("label", label_q2);
    auto weight_q2 = get(&edge_q::weight, G2);
    dp_q2.property("weight", weight_q2);

    write_graphviz_dp(dbg, G2, dp_q2);

    cout << "Step4-----------------------------------\n";
#endif

    auto start_step_4 = chrono::steady_clock::now();
    // Step 4a -- Construct Minimum Spanning Tree from G2

    vector<ed_q> st2;
    kruskal_minimum_spanning_tree(G2, back_inserter(st2),
            weight_map(get(&edge_q::weight, G2)));

    // Step 4b -- Trim G2 based on MST

    set<vd_q> vKeep2, vRemove2;
    set<ed_q> eKeep2, eRemove2;

    for(auto &e : st2)
    {
        vKeep2.insert(source(e, G2)); vKeep2.insert(target(e, G2));
        eKeep2.insert(e);
    }

    BGL_FORALL_VERTICES(v, G2, graph_q)
    {
        if(vKeep2.count(v) == 0)
            vRemove2.insert(v);
    }

    BGL_FORALL_EDGES(e, G2, graph_q)
    {
        if(eKeep2.count(e) == 0)
            eRemove2.insert(e);
    }

    for(auto Ex : eRemove2) remove_edge(Ex, G2);
    for(auto Vx : vRemove2) remove_vertex(Vx, G2);

    auto end_step_4 = chrono::steady_clock::now();
    cout << "[KMB] Step4: "<< chrono::duration <double, milli> (end_step_4-start_step_4).count() << " ms" << endl;

#ifdef DEBUG
    write_graphviz_dp(dbg, G2, dp_q2);
    cout << "Step5-----------------------------------\n";
#endif

    auto start_step_5 = chrono::steady_clock::now();

    // Remove non-steiner leaf nodes
    // Possible optimization : Start from leaf node and delete upwards instead
    // of iterating over and over the graph and examining each time.

    set<vd_q> to_remove;
    do
    {
        for(auto v : to_remove)
        {
            clear_vertex(v, G2);
            remove_vertex(v, G2);
        }

        to_remove.clear();

        BGL_FORALL_VERTICES(v, G2, graph_q)
        {
            if(multicast_vertices.count(G2[v].label) == 0 && degree(v, G2) == 1)
            {
                //cout << "Removing: " << v << " " << G2[v].label << "\n";
                to_remove.insert(v);
            }
        }
    } while(to_remove.size() > 0);

    auto end_step_5 = chrono::steady_clock::now();
    cout << "[KMB] Step5: "<< chrono::duration <double, milli> (end_step_5-start_step_5).count() << " ms" << endl;

#ifdef DEBUG
    write_graphviz_dp(dbg, G2, dp_q2);
    cout << "Step6-----------------------------------\n";
#endif

    // Update the fillcolor and penwidth in the original graph and print it out.

    for(auto &m : multicast_vertices)
    {
        network[m].fillcolor = string("black");
    }

    BGL_FORALL_EDGES(ed, G2, graph_q)
    {
        vd_p vx = G2[source(ed, G2)].label;
        vd_p vy = G2[target(ed, G2)].label;

        //cout << vx << " " << vy << "\n";
        ed_p e; bool f = false;
        tie(e,f) = edge(vx, vy, network);
        assert(f && "ed_p not found in original graph!");
        network[e].penwidth = 5;
    }

    try
    {
        write_graphviz_dp(output_file, network, dp);
    }
    catch(std::exception &err)
    {
        cerr << err.what() << endl;
        cerr << "write_graphviz failed for " << output_filename << "\n";
        return 1;
    }

    return 0;
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __LMA_OPT2_BA_CREATE_HESSIAN_HPP__
#define __LMA_OPT2_BA_CREATE_HESSIAN_HPP__

#include "make_type.hpp"

#include <libv/lma/ttt/mpl/for.hpp>
#include <libv/lma/lm/function/function.hpp>
#include <libv/lma/ttt/fusion/pair.hpp>
#include <libv/lma/ttt/mpl/naming.hpp>
#include <libv/lma/ttt/mpl/cat.hpp>
#include <boost/mpl/unique.hpp>
#include <boost/mpl/count_if.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/replace.hpp>
#include <boost/mpl/copy_if.hpp>
#include <boost/type_traits/is_same.hpp>

namespace lma
{

  // extract parameters
  template<class L1> using ListOfListOfParameters = mpl::transform<L1,Function<mpl::_1>>;

  template<class A, class B> struct MakePair
  {
    typedef bf::pair<A,B> type;
  };

  template<class Int_, class List, class Int, class Result> struct CreatePairs3_
  : mpl::push_back<
                  Result,
                  typename MakePair<typename mpl::at<List,Int_>::type,typename mpl::at<List,Int>::type>::type
                  > {};

  template<class Int> using CreatePairs3 = CreatePairs3_<Int,mpl::_1,mpl::_2,mpl::_3>;

  template<class List, class Int, class Result> struct CreatePairs2_ : For<Int::value+1,mpl::size<List>::value,List,CreatePairs3<Int>,Result> {};

  template<class L> using CreatePairs2 = CreatePairs2_<L,mpl::_2,mpl::_3>;

  template<class T> struct Unique : mpl::unique<T, boost::is_same<mpl::_1,mpl::_2> > {};// ne retire que les doublons contigus

  // L ne doit pas contenir de doublon
  template<class L> struct CreatePairs : For<0,mpl::size<L>::value,L,CreatePairs2<L>> {};

  // get cross parameters
  template<class L2> using CrossParameters = mpl::transform<typename mpl::transform<L2,Unique<mpl::_1>>::type,CreatePairs<mpl::_1>>;

  template<class T> struct Double {};
  template<class T> struct Single {};

  template<class T, class L> struct SingleOrDouble : mpl::if_c<(mpl::count_if<L,boost::is_same<mpl::_1,T>>::value>1),Double<T>,Single<T>> {};

  template<class L> struct DiagHessianFunctor : mpl::transform<typename Unique<L>::type,SingleOrDouble<mpl::_1,L>> {};

  template<class L2> using ToSingleOrDouble = mpl::transform<L2,DiagHessianFunctor<mpl::_1>>;

  template<class L3, class L4> using CatCrossAndDiag = mpl::transform<L4,L3,Cat<mpl::_1,mpl::_2>>;

  template<class L, class T> struct AddIf;

  template<class L, class A, class B> struct AddIf<L,bf::pair<A,B>>
  : mpl::if_<
              mpl::or_<
                      mpl::contains<L,bf::pair<A,B>>,
                      mpl::contains<L,bf::pair<B,A>>
                      >,
              L,
              typename mpl::push_back<L,bf::pair<A,B>>::type
            > {};

  template<class L, class T> struct AddIf<L,Single<T>>
  : mpl::if_<
              mpl::or_<
                        mpl::contains<L,Single<T>>,
                        mpl::contains<L,Double<T>>
                      >,
              L,
              typename mpl::push_front<L,Single<T>>::type
            > {};

  template<class L, class T> struct AddIf<L,Double<T>>
  :  mpl::if_<
              boost::mpl::contains<L,Single<T>>,
              typename mpl::replace<L,Single<T>,Double<T>>::type,
              typename mpl::push_front<L,Double<T>>::type
            > {};

  template<class List, class Int, class Result> struct ForEachParameters : AddIf<Result,typename mpl::at<List,Int>::type> {};

  template<class List, class Int, class Result> struct ForEachFunctor :
  For<0,mpl::size<typename mpl::at<List,Int>::type>::value,typename mpl::at<List,Int>::type,ForEachParameters<mpl::_1,mpl::_2,mpl::_3>,Result> {};

  template<class L> using TypesContainers = For<0,mpl::size<L>::value,L,ForEachFunctor<mpl::_1,mpl::_2,mpl::_3>>;

  template<class T, class Flt> struct ToTable;

  template<class A, class Flt> struct ToTable<Single<A>,Flt>// : MakeTupleTable<A,A,Flt> {
  {
    typedef bf::pair<bf::pair<A,A>,Table<A,A,Flt,Diagonal>> type;
  };

  template<class A, class Flt> struct ToTable<Double<A>,Flt>
  {
    typedef bf::pair<bf::pair<A,A>,Table<A,A,Flt,Symetric>> type;
  };

  template<class A, class B, class Flt> struct ToTable<bf::pair<A,B>,Flt> : MakeTupleTable<A,B,Flt> {};

  template<class L, class Flt> using Containers = mpl::transform<L,ToTable<mpl::_1,Flt>>;

  template<class Result, class A, class B> struct add_ { typedef Result type; };
  template<class Result, class A> struct add_<Result,A,Single<A>> : mpl::push_back<Result,Single<A>> {};
  template<class Result, class A> struct add_<Result,A,Double<A>> : mpl::push_back<Result,Double<A>> {};
  template<class Result, class A, class B> struct add_<Result,A,bf::pair<A,B>> : mpl::push_back<Result,bf::pair<A,B>> {};
  template<class Elt, class L, class Int, class Result> struct Ordering2 : add_<Result,Elt,typename mpl::at<L,Int>::type> {};

  template<class L, class Ordre, class Int, class Result> struct Ordering : For<0,mpl::size<L>::value,L,Ordering2<typename mpl::at<Ordre,Int>::type,mpl::_1,mpl::_2,mpl::_3>,Result> {};
  template<class L, class Ordre> struct Order : For<0,mpl::size<Ordre>::value,Ordre,Ordering<L,mpl::_1,mpl::_2,mpl::_3>> {};

  template<class Keys, class T> struct IsKeyUs : mpl::false_ { };
  template<class Keys, class Key, class P, class Q, class T, class Tag> struct IsKeyUs<Keys, bf::pair< Key, Table<P,Q,T,Tag> >>
    : mpl::and_< mpl::contains<Keys,P>, mpl::contains<Keys,Q> > { };

  template<class T> struct SymetricToDiagonal { typedef T type; };
  template<class Key, class P, class T> struct SymetricToDiagonal<bf::pair< Key, Table<P,P,T,Diagonal> >>  { typedef bf::pair< Key, Table<P,P,T,Symetric> > type; };

  template<class H, class KeyUs> struct ListS
  {
    typedef typename mpl::copy_if<H, IsKeyUs<KeyUs,mpl::_1>>::type type0;
    typedef typename mpl::transform<type0, SymetricToDiagonal<mpl::_1>>::type type;
  };

  template<class Bundle, class flt> struct ListH
  {
    typedef typename Bundle::ListFunction ListFunction;
    typedef typename Bundle::ListeParam ListeParam;
    typedef typename Bundle::ParamFonctor ParamFonctor;
    typedef ListFunction L1;

//       typedef typename CreateListParam<ListFunction,ParamFonctor>::type parameters;

      typedef typename ListOfListOfParameters<L1>::type L2_;
      typedef typename mpl::transform<L2_,ParamFonctor>::type L2;

      typedef typename CrossParameters<L2>::type L3;

      typedef typename ToSingleOrDouble<L2>::type L4;

      typedef typename CatCrossAndDiag<L3,L4>::type L5;

      typedef typename TypesContainers<L5>::type L6;

      typedef typename Order<L6,ListeParam>::type L7;

      typedef typename Containers<L7,flt>::type type;

      static void disp()
      {
        std::cout << " Functors : " << ttt::name<L1>() << std::endl;
        std::cout << " ListOfListOfParameters : " << ttt::name<L2>() << std::endl;
        std::cout << " ToSingleOrDouble : " << ttt::name<L4>() << std::endl;
        std::cout << std::endl;
        std::cout << " CrossAndDiag : " << ttt::name<L5>() << std::endl;
        std::cout << std::endl;
        std::cout << " Types : " << ttt::name<L6>() << std::endl;
        std::cout << " Order  : " << ttt::name<L7>() << std::endl;
        std::cout << " Containers  : " << ttt::name<type>() << std::endl;
//         std::cout << " Parmaeters " << ttt::name<ListeParam>() << std::endl;
      }
  };

}

namespace ttt
{
  template<class T> struct Name<lma::Single<T>> { static std::string name() { return std::string("Single<") + ttt::name<T>() + ">"; } };
  template<class T> struct Name<lma::Double<T>> { static std::string name() { return std::string("Double<") + ttt::name<T>() + ">"; } };
}

#endif