/*
(c) 2012 Fengtao Fan
*/
#include "FilesOutputForOptimalCycles.h"
#include <iostream>
#include <fstream>
#include <sstream>
#include <map>

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

using namespace boost::numeric::ublas;
using namespace std;

/* Matrix inversion routine.
Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
template<class T>
bool InvertMatrix(const matrix<T> &input, matrix<T> &inverse) {
    typedef permutation_matrix<std::size_t> pmatrix;

    // create a working copy of the input
    matrix<T> A(input);

    // create a permutation matrix for the LU-factorization
    pmatrix pm(A.size1());

    // perform LU-factorization
    int res = lu_factorize(A, pm);
    if (res != 0)
        return false;

    // create identity matrix of "inverse"
    inverse.assign(identity_matrix<T>(A.size1()));

    // backsubstitute to get the inverse
    lu_substitute(A, pm, inverse);

    return true;
}

void AssembleMatrix(matrix<double> &A, std::vector<std::vector<std::pair<int, int> > > &sparseLinkMat) {
    for (unsigned int i = 0; i < sparseLinkMat.size(); i++) {
        for (unsigned int j = 0; j < sparseLinkMat.size(); j++) {
            A(i, j) = 0.0;
        }
    }
    for (unsigned int i = 0; i < sparseLinkMat.size(); i++) {
        for (unsigned int j = 0; j < sparseLinkMat[i].size(); j++) {
            A(i, sparseLinkMat[i][j].first) = sparseLinkMat[i][j].second;
        }
    }
    return;
}

void FilesOutputForOptimalCycles::WriteOrientedComplex(const char *out_file_name) {
    // edge orientation is taken as v0-->--v1 in the vecEdge
    std::cout << "writing file ... " << out_file_name << std::endl;

    std::ofstream ofile;
    ofile.open(out_file_name, std::ifstream::out);

    //
    int vertexNum = int(inMeshPtr->vecVertex.size());
    int triangleNum = int(inMeshPtr->vecTriangle.size());
    int edgeNum = int(inMeshPtr->vecEdge.size());
    //
    std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //
    //std::stringstream locSstr(std::stringstream::in | std::stringstream::out);
    if (ofile.is_open()) {
        // total number of simplices
        sstr << vertexNum + triangleNum + edgeNum << std::endl;
        // number of skeletons
        sstr << 3 << std::endl;
        // number of vertices
        sstr << vertexNum << std::endl;
        // write all coordinates
        for (int i = 0; i < vertexNum; i++) {
            sstr << inMeshPtr->vecVertex[i].x << " "
                 << inMeshPtr->vecVertex[i].y << " "
                 << inMeshPtr->vecVertex[i].z << std::endl;
        }
        // write the number of edges
        sstr << edgeNum << std::endl;
        // write the edge endpoints in the orientation of v0-->--v1 in the vecEdge
        for (int i = 0; i < edgeNum; i++) {
            sstr << inMeshPtr->vecEdge[i].v0 << " " << inMeshPtr->vecEdge[i].v1 << std::endl;
        }
        // write the number of triangles
        sstr << triangleNum << std::endl;
        // write the triangle endpoints in the orientation of v0--v1--v2 in the vecTriangle
        for (int i = 0; i < triangleNum; i++) {
            sstr << inMeshPtr->vecTriangle[i].e01 << " "
                 << inMeshPtr->vecTriangle[i].e12 << " "
                 << inMeshPtr->vecTriangle[i].e02 << std::endl;
        }

        //
        //ofile.write(sstr.str().c_str(), 10);
        ofile << sstr.rdbuf();
        //
        // delete string stream content
        sstr.str("");
        sstr.clear();
        // close file
        ofile.close();
    } else {
        std::cout << "Can NOT open file " << out_file_name << std::endl;
        exit(0);
    }
    std::cout << "Done... " << vertexNum << " " << edgeNum << " " << triangleNum << std::endl;
    //
    return;
}

void FilesOutputForOptimalCycles::WritePolygonOut(const char *out_file_name, std::vector<Vector3> &inPly) {
    std::cout << "writing file ... " << out_file_name << std::endl;

    std::ofstream ofile;
    ofile.open(out_file_name, std::ifstream::out);

    //
    int vertexNum = int(inPly.size());
    //
    std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //
    //std::stringstream locSstr(std::stringstream::in | std::stringstream::out);
    if (ofile.is_open()) {
        // total number of edges
        sstr << vertexNum << " ";
        for (int i = 0; i < vertexNum; i++) {
            sstr << inPly[i][0] << " "
                 << inPly[i][1] << " "
                 << inPly[i][2] << std::endl;
        }

        //
        //ofile.write(sstr.str().c_str(), 10);
        ofile << sstr.rdbuf();
        //
        // delete string stream content
        sstr.str("");
        sstr.clear();
        // close file
        ofile.close();
    } else {
        std::cout << "Can NOT open file " << out_file_name << std::endl;
        exit(0);
    }
    std::cout << "Done... " << vertexNum << " " << std::endl;
}

void FilesOutputForOptimalCycles::ReadPolygonIn(const char *in_file_name, std::vector<Vector3> &wantPly) {
    std::cout << "reading... " << std::endl;

    std::ifstream ifile;
    ifile.open(in_file_name, std::ifstream::in);
    long long fileSize = 0;
    char *fBuf = NULL;

    std::string sBuf;
    //
    int vertexNum = 0;
    //
    std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //
    Vector3 tempVec;
    if (ifile.is_open()) {
        ifile.seekg(0, std::ios::end);
        fileSize = ifile.tellg();
        // move pointer back to beginning
        ifile.seekg(0, std::ios::beg);
        //
        //copy whole file into file buffer
        fBuf = new char[fileSize + 1];
        ifile.read(fBuf, fileSize);
        // add extra symbol
        fBuf[fileSize] = '\n';
        //
        sBuf.assign(fBuf);
        sstr.str(sBuf);

        sBuf.clear();
        // close file
        ifile.close();

        //deallocate memory
        delete[] fBuf;

        sstr >> vertexNum;
        for (int i = 0; i < vertexNum; i++) {
            sstr >> tempVec[0] >> tempVec[1] >> tempVec[2];
        }

        sstr.clear();
    } else {
        std::cout << "Can NOT open file " << in_file_name << std::endl;
        exit(0);
    }
    std::cout << "Done... " << vertexNum << " " << std::endl;
    return;

}

void FilesOutputForOptimalCycles::WriteOrientedCycle(const int in_cycle_id, const char *out_file_name) {
    // edge orientation is taken as v0-->--v1 in the vecEdge
    int cycle_id = in_cycle_id;
    std::vector<std::vector<int> > *vecVertex_Ptr = vecInitVerticalLoops_Vertex_Ptr;
    std::vector<std::vector<int> > *vecEdge_Ptr = vecInitVerticalLoops_Edge_Ptr;

    if (cycle_id >= vecInitVerticalLoops_Vertex_Ptr->size()) {
        cycle_id -= int(vecInitVerticalLoops_Vertex_Ptr->size());
        vecVertex_Ptr = vecInitHorizontalLoops_Vertex_Ptr;
        vecEdge_Ptr = vecInitHorizontalLoops_Edge_Ptr;
    }

    //if (in_cycle_id == 1)
    //{
    //	int own_orientation = -1;
    //	std::cout << "writing file ... " << out_file_name << std::endl;

    //	std::ofstream ofile;
    //	ofile.open(out_file_name, std::ifstream::out);

    //	//
    //	int vertexNum = int((*vecVertex_Ptr)[cycle_id].size());
    //	int edgeNum = int ((*vecEdge_Ptr)[cycle_id].size());
    //	//
    //	std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //	//
    //	//std::stringstream locSstr(std::stringstream::in | std::stringstream::out);
    //	if (ofile.is_open())
    //	{
    //		// total number of edges
    //		sstr << edgeNum + (*vecInitHorizontalLoops_Edge_Ptr)[1].size() << " ";
    //		for (int i = 0; i < edgeNum; i++)
    //		{
    //			// vi--ei--v(i+1)
    //			// check the orientation
    //			if( (inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][i]].v0 != (*vecVertex_Ptr)[cycle_id][i] &&
    //				inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][i]].v0 != (*vecVertex_Ptr)[cycle_id][i+1] ) ||
    //				(inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][i]].v1 != (*vecVertex_Ptr)[cycle_id][i] &&
    //				inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][i]].v1 != (*vecVertex_Ptr)[cycle_id][i+1] ) )
    //			{
    //				std::cout << "EDGE doesn't match vertices " << std::endl;
    //				exit(0);
    //			}
    //			if (inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][i]].v0 == (*vecVertex_Ptr)[cycle_id][i])
    //			{// vi-->---v(i+1) has the same orientation as ei
    //				sstr << 1 * own_orientation << " " ;
    //			}
    //			else
    //			{// opposite orientation
    //				sstr << -1 * own_orientation << " ";
    //			}
    //			// write the edge
    //			sstr << (*vecEdge_Ptr)[cycle_id][i] << " ";
    //		}

    //		for (int i = 0; i < (*vecInitHorizontalLoops_Edge_Ptr)[1].size(); i++)
    //		{
    //			// vi--ei--v(i+1)
    //			// check the orientation

    //			if (inMeshPtr->vecEdge[(*vecInitHorizontalLoops_Edge_Ptr)[1][i]].v0 == (*vecInitHorizontalLoops_Vertex_Ptr)[1][i])
    //			{// vi-->---v(i+1) has the same orientation as ei
    //				sstr << 1  << " " ;
    //			}
    //			else
    //			{// opposite orientation
    //				sstr << -1  << " ";
    //			}
    //			// write the edge
    //			sstr << (*vecInitHorizontalLoops_Edge_Ptr)[1][i] << " ";
    //		}
    //
    //		//
    //		//ofile.write(sstr.str().c_str(), 10);
    //		ofile << sstr.rdbuf();
    //		//
    //		// delete string stream content
    //		sstr.str("");
    //		sstr.clear();
    //		// close file
    //		ofile.close();
    //	}
    //	else
    //	{
    //		std::cout << "Can NOT open file " << out_file_name << std::endl;
    //		exit(0);
    //	}
    //	std::cout << "Done... " << vertexNum << " "  << edgeNum << " " << std::endl;
    //	return;

    //}
    if (in_cycle_id >= 0 && in_cycle_id < 2 * vecInitVerticalLoops_Vertex_Ptr->size()) {
        std::cout << "writing file ... " << out_file_name << std::endl;

        std::ofstream ofile;
        ofile.open(out_file_name, std::ifstream::out);

        //
        int vertexNum = int((*vecVertex_Ptr)[cycle_id].size());
        int edgeNum = int((*vecEdge_Ptr)[cycle_id].size());
        //
        std::stringstream sstr(std::stringstream::in | std::stringstream::out);
        //
        //std::stringstream locSstr(std::stringstream::in | std::stringstream::out);
        if (ofile.is_open()) {
            // total number of edges
            sstr << edgeNum << " ";
            for (int i = 0; i < edgeNum; i++) {
                // vi--ei--v(i+1)
                // check the orientation
                if ((inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][i]].v0 != (*vecVertex_Ptr)[cycle_id][i] &&
                     inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][i]].v0 != (*vecVertex_Ptr)[cycle_id][i + 1]) ||
                    (inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][i]].v1 != (*vecVertex_Ptr)[cycle_id][i] &&
                     inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][i]].v1 != (*vecVertex_Ptr)[cycle_id][i + 1])) {
                    std::cout << "EDGE doesn't match vertices " << std::endl;
                    exit(0);
                }
                if (inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][i]].v0 ==
                    (*vecVertex_Ptr)[cycle_id][i]) {// vi-->---v(i+1) has the same orientation as ei
                    sstr << 1 << " ";
                } else {// opposite orientation
                    sstr << -1 << " ";
                }
                // write the edge
                sstr << (*vecEdge_Ptr)[cycle_id][i] << " ";
            }

            //
            //ofile.write(sstr.str().c_str(), 10);
            ofile << sstr.rdbuf();
            //
            // delete string stream content
            sstr.str("");
            sstr.clear();
            // close file
            ofile.close();
        } else {
            std::cout << "Can NOT open file " << out_file_name << std::endl;
            exit(0);
        }
        std::cout << "Done... " << vertexNum << " " << edgeNum << " " << std::endl;
    } else {
        std::cout << "cycle index " << cycle_id << " out of RANGE " << std::endl;
    }
    //
    return;
}

void FilesOutputForOptimalCycles::WriteCoefficientMatrix(const char *out_file_name) {
    //int tot_genus = coeffMatrix->size() / 2 ;

    //std::cout << "M=zeros("<< tot_genus * 2 << "," << tot_genus * 2 << ");" << std::endl;
    //for (int i = 0; i < tot_genus; i++)
    //{
    //	//std::cout << "Vert Loop " << i << " link against " << std::endl;
    //	for (unsigned int j = 0; j < (*coeffMatrix)[i].size(); j++)
    //	{
    //		std::cout << "M(" << i + 1 << "," << (*coeffMatrix)[i][j].first + 1 << ")=" << (*coeffMatrix)[i][j].second << ";";
    //
    //	}
    //	std::cout << std::endl;
    //}
    //for (int i = 0; i < tot_genus; i++)
    //{
    //	//std::cout << "Hori Loop " << i + tot_genus << " link against " << std::endl;
    //	for (unsigned int j = 0; j < (*coeffMatrix)[i + tot_genus].size(); j++)
    //	{
    //		std::cout << "M(" << i + tot_genus + 1 << "," << (*coeffMatrix)[i + tot_genus][j].first + 1<< ")=" << (*coeffMatrix)[i + tot_genus][j].second << ";";
    //
    //	}
    //	std::cout << std::endl;
    //}
}

void FilesOutputForOptimalCycles::OrderCycles(std::set<int> &edge_set, std::map<int, int> &vertex_ordering) {
    vertex_ordering.clear();
    //
    int verCounter = 0;
    std::map<int, int>::iterator mFindIter;
    for (std::set<int>::iterator sIter = edge_set.begin();
         sIter != edge_set.end(); sIter++) {
        int node_a = inMeshPtr->vecEdge[*sIter].v0;
        int node_b = inMeshPtr->vecEdge[*sIter].v1;
        //
        mFindIter = vertex_ordering.find(node_a);
        if (mFindIter == vertex_ordering.end()) {
            vertex_ordering[node_a] = verCounter++;
        }
        //
        mFindIter = vertex_ordering.find(node_b);
        if (mFindIter == vertex_ordering.end()) {
            vertex_ordering[node_b] = verCounter++;
        }
    }
    return;
}

bool FilesOutputForOptimalCycles::OrderCycles(std::set<int> &edge_set, std::vector<int> &vertex_set) {
    int bSuccess = true;
    vertex_set.clear();
    vertex_set.reserve(edge_set.size() + 1);
    //
    std::map<int, std::pair<int, int> > vertex_edge_conn;
    std::map<int, std::pair<int, int> >::iterator mFindIter;
    for (std::set<int>::iterator sIter = edge_set.begin();
         sIter != edge_set.end(); sIter++) {
        int node_a = inMeshPtr->vecEdge[*sIter].v0;
        int node_b = inMeshPtr->vecEdge[*sIter].v1;
        //
        mFindIter = vertex_edge_conn.find(node_a);
        if (mFindIter == vertex_edge_conn.end()) {
            vertex_edge_conn[node_a] = std::pair<int, int>(*sIter, -1);
        } else {
            mFindIter->second.second = *sIter;
        }
        //
        mFindIter = vertex_edge_conn.find(node_b);
        if (mFindIter == vertex_edge_conn.end()) {
            vertex_edge_conn[node_b] = std::pair<int, int>(*sIter, -1);
        } else {
            mFindIter->second.second = *sIter;
        }
    }
    //
    std::set<int>::iterator sFindIter;
    int current_edge = *edge_set.begin();
    int end_vertex = inMeshPtr->vecEdge[current_edge].v0;
    int current_vertex = inMeshPtr->vecEdge[current_edge].v1;
    //
    while (current_vertex != end_vertex) {
        vertex_set.push_back(current_vertex);

        mFindIter = vertex_edge_conn.find(current_vertex);
        if (mFindIter->second.second < 0) {
            //std::cout << "NOT a closed loop" << std::endl;
            bSuccess = false;
            vertex_set.clear();
            break;
        }
        if (mFindIter->second.first == current_edge) {
            current_edge = mFindIter->second.second;
        } else {
            current_edge = mFindIter->second.first;
        }
        //
        if (current_vertex == inMeshPtr->vecEdge[current_edge].v0)
            current_vertex = inMeshPtr->vecEdge[current_edge].v1;
        else
            current_vertex = inMeshPtr->vecEdge[current_edge].v0;
    }
    if (bSuccess)
        vertex_set.push_back(current_vertex);
    return bSuccess;
}

void FilesOutputForOptimalCycles::WriteCyclesInformation(const char *out_file_prefix,
                                                         std::vector<std::set<int> > &v_basis_loops,
                                                         std::vector<std::set<int> > &h_basis_loops) {
//
    std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //
    string file_prefix(out_file_prefix);
    string file_name;
    // assemble the matrix
    const int tot_genus = v_basis_loops.size();
    //
    file_name = "loops_" + file_prefix + ".lop";
    //
    std::cout << file_name << " \n";
    //
    std::ofstream ofile;
    ofile.open(file_name.c_str(), std::ifstream::out);
    //ofile.open(file_name.c_str());
    if (ofile.is_open()) {
        sstr << "# there are " << tot_genus << " handle loops" << std::endl << std::endl;
        std::vector<int> cycleVertex;
        for (unsigned int i = 0; i < tot_genus; i++) {

            //if (!OrderCycles(h_basis_loops[i], cycleVertex))
            {
                //Vertices are not ordered
                //Only the set of vertices are there
                std::set<int> verSet;
                for (std::set<int>::iterator sIter = h_basis_loops[i].begin();
                     sIter != h_basis_loops[i].end(); sIter++) {
                    verSet.insert(inMeshPtr->vecEdge[*sIter].v0);
                    verSet.insert(inMeshPtr->vecEdge[*sIter].v1);
                }
                cycleVertex.assign(verSet.begin(), verSet.end());
            }
            sstr << "handle loop " << i << "size(" << cycleVertex.size() << "): ";
            for (unsigned int vid = 0; vid < cycleVertex.size(); vid++) {
                sstr << cycleVertex[vid] << ",";
            }
            sstr << std::endl;
        }
        //
        sstr << std::endl << "# there are " << tot_genus << " tunnel loops" << std::endl << std::endl;
        for (unsigned int i = 0; i < tot_genus; i++) {

            //if (!OrderCycles(v_basis_loops[i], cycleVertex))
            {
                //Vertices are not ordered
                //Only the set of vertices are there
                std::set<int> verSet;
                for (std::set<int>::iterator sIter = v_basis_loops[i].begin();
                     sIter != v_basis_loops[i].end(); sIter++) {
                    verSet.insert(inMeshPtr->vecEdge[*sIter].v0);
                    verSet.insert(inMeshPtr->vecEdge[*sIter].v1);
                }
                cycleVertex.assign(verSet.begin(), verSet.end());
            }
            sstr << "tunnel loop " << i << "size(" << cycleVertex.size() << "): ";
            for (unsigned int vid = 0; vid < cycleVertex.size(); vid++) {
                sstr << cycleVertex[vid] << ",";
            }
            sstr << std::endl;
        }
        //
        ofile << sstr.rdbuf();
        //
        ofile.close();
        ofile.clear();
        //
        sstr.str("");
        sstr.clear();
    } else {
        std::cout << "can not open " << file_name << std::endl;
        exit(0);
    }
    return;
}

void FilesOutputForOptimalCycles::WriteGeomviewListFormat(const char *out_file_prefix,
                                                          std::vector<std::set<int> > &v_basis_loops,
                                                          std::vector<std::set<int> > &h_basis_loops,
                                                          std::vector<int> &vec_oriented_triangles,
                                                          const int orgVertexSize, const int orgTriangleSize,
                                                          const float fEnlargeFactor) {
    std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //
    string file_prefix(out_file_prefix);
    string file_name;
    // assemble the matrix
    const int tot_genus = v_basis_loops.size();
    //
    file_name = "loops_" + file_prefix + ".list";
    std::cout << file_name << std::endl;
    std::ofstream ofile;
    bool full_mesh_flag = false;
    ofile.open(file_name.c_str(), std::ifstream::out);
    if (ofile.is_open()) {
        sstr << "LIST" << std::endl;
        sstr << "{" << std::endl;
        sstr << "# there are " << tot_genus << " handle loops" << std::endl << std::endl;
        std::map<int, int> cycleVertex;
        for (unsigned int i = 0; i < tot_genus; i++) {

            OrderCycles(h_basis_loops[i], cycleVertex);
            sstr << "# handle loop " << i << std::endl;
            sstr << "appearance {linewidth 6}" << std::endl;
            sstr << "{" << std::endl;
            sstr << "OFF" << std::endl;
            sstr << cycleVertex.size() << " " << h_basis_loops[i].size() << " 0" << std::endl;
            //
            std::vector<int> vecCycVertex(cycleVertex.size());
            for (std::map<int, int>::iterator mIter = cycleVertex.begin();
                 mIter != cycleVertex.end(); mIter++) {
                vecCycVertex[mIter->second] = mIter->first;
                if (!full_mesh_flag && mIter->first >= orgVertexSize)
                    full_mesh_flag = true;
            }
            for (std::vector<int>::iterator vIter = vecCycVertex.begin();
                 vIter != vecCycVertex.end(); vIter++) {
                sstr << inMeshPtr->vecVertex[*vIter].x * fEnlargeFactor << " ";
                sstr << inMeshPtr->vecVertex[*vIter].y * fEnlargeFactor << " ";
                sstr << inMeshPtr->vecVertex[*vIter].z * fEnlargeFactor << std::endl;
            }
            for (std::set<int>::iterator sIter = h_basis_loops[i].begin();
                 sIter != h_basis_loops[i].end(); sIter++) {
                sstr << "2 ";
                sstr << cycleVertex[inMeshPtr->vecEdge[*sIter].v0] << " ";
                sstr << cycleVertex[inMeshPtr->vecEdge[*sIter].v1] << " ";
                sstr << "0.000000 0.800000 0.000000 1.0" << std::endl;
            }
            sstr << "}" << std::endl;
        }
        //
        sstr << std::endl << "# there are " << tot_genus << " tunnel loops" << std::endl << std::endl;
        for (unsigned int i = 0; i < tot_genus; i++) {

            OrderCycles(v_basis_loops[i], cycleVertex);
            sstr << "# tunnel loop " << i << std::endl;
            sstr << "appearance {linewidth 6}" << std::endl;
            sstr << "{" << std::endl;
            sstr << "OFF" << std::endl;
            sstr << cycleVertex.size() << " " << v_basis_loops[i].size() << " 0" << std::endl;
            //
            std::vector<int> vecCycVertex(cycleVertex.size());
            for (std::map<int, int>::iterator mIter = cycleVertex.begin();
                 mIter != cycleVertex.end(); mIter++) {
                vecCycVertex[mIter->second] = mIter->first;
                if (!full_mesh_flag && mIter->first >= orgVertexSize)
                    full_mesh_flag = true;
            }
            for (std::vector<int>::iterator vIter = vecCycVertex.begin();
                 vIter != vecCycVertex.end(); vIter++) {
                sstr << inMeshPtr->vecVertex[*vIter].x * fEnlargeFactor << " ";
                sstr << inMeshPtr->vecVertex[*vIter].y * fEnlargeFactor << " ";
                sstr << inMeshPtr->vecVertex[*vIter].z * fEnlargeFactor << std::endl;
            }
            for (std::set<int>::iterator sIter = v_basis_loops[i].begin();
                 sIter != v_basis_loops[i].end(); sIter++) {
                sstr << "2 ";
                sstr << cycleVertex[inMeshPtr->vecEdge[*sIter].v0] << " ";
                sstr << cycleVertex[inMeshPtr->vecEdge[*sIter].v1] << " ";
                sstr << "0.800000 0.000000 0.000000 1.0" << std::endl;
            }
            //
            sstr << "}" << std::endl;
        }
        //
        // write the mesh back to file
        //
        sstr << "# the mesh" << std::endl;
        sstr << "appearance {+transparent linewidth 1}" << std::endl;
        sstr << "{" << std::endl;
        // vertices
        int outVertexSize = orgVertexSize;
        int outTriangleSize = orgTriangleSize;
        if (full_mesh_flag) {
            outVertexSize = inMeshPtr->vecVertex.size();
            outTriangleSize = inMeshPtr->vecTriangle.size();
        }
        sstr << "OFF" << std::endl;
        sstr << outVertexSize << " " << outTriangleSize << " 0" << std::endl;

        for (unsigned int i = 0; i < outVertexSize; i++) {
            sstr << inMeshPtr->vecVertex[i].x * fEnlargeFactor << " ";
            sstr << inMeshPtr->vecVertex[i].y * fEnlargeFactor << " ";
            sstr << inMeshPtr->vecVertex[i].z * fEnlargeFactor << std::endl;
        }
        // triangles
        for (unsigned int i = 0; i < outTriangleSize; i++) {
            sstr << "3 ";
            sstr << vec_oriented_triangles[3 * i] << " ";
            sstr << vec_oriented_triangles[3 * i + 1] << " ";
            sstr << vec_oriented_triangles[3 * i + 2] << " ";
            sstr << "0.9 0.9 0.9 0.4" << std::endl;
        }
        sstr << "}" << std::endl;
        sstr << "}" << std::endl; // pair with LIST
        ofile << sstr.rdbuf();
        //
        ofile.close();
        ofile.clear();
        //
        sstr.str("");
        sstr.clear();
    } else {
        std::cout << "can not open " << file_name << std::endl;
        exit(0);
    }
}

void FilesOutputForOptimalCycles::WriteAllCyclesOut(std::vector<bool> &inVertLoopType, const char *out_file_prefix) {
    std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //
    string file_prefix(out_file_prefix);
    string file_name;
    // assemble the matrix
    const int tot_genus = vecInitVerticalLoops_Vertex_Ptr->size();
    //
    file_name = file_prefix + "_all_v.txt";
    std::ofstream ofile;
    ofile.open(file_name.c_str(), std::ifstream::out);
    if (ofile.is_open()) {
        sstr << tot_genus << std::endl;
        for (unsigned int i = 0; i < tot_genus; i++) {
            int edges_num = 0;
            int row_num = i;
            //
            if (!inVertLoopType[i])
                row_num += tot_genus;
            //
            for (std::list<std::pair<int, int> >::iterator listIter = (*coeffMatrix)[row_num].begin();
                 listIter != (*coeffMatrix)[row_num].end(); listIter++) {
                int col_num = listIter->first;
                int counter = abs(listIter->second);
                if (col_num >= tot_genus) {// it is a horizontal loop
                    edges_num += counter * (*vecInitHorizontalLoops_Edge_Ptr)[col_num - tot_genus].size();
                } else {
                    edges_num += counter * (*vecInitVerticalLoops_Edge_Ptr)[col_num].size();
                }
            }
            //
            sstr << edges_num << std::endl;
            for (std::list<std::pair<int, int> >::iterator listIter = (*coeffMatrix)[row_num].begin();
                 listIter != (*coeffMatrix)[row_num].end(); listIter++) {
                int col_num = listIter->first;
                int counter = abs(listIter->second);
                if (col_num >= tot_genus) {// it is a horizontal loop
                    for (int j = 0; j < counter; j++) {
                        for (std::vector<int>::iterator vIter = (*vecInitHorizontalLoops_Edge_Ptr)[col_num -
                                                                                tot_genus].begin();
                             vIter != (*vecInitHorizontalLoops_Edge_Ptr)[col_num - tot_genus].end(); vIter++) {
                            sstr << *vIter << " ";
                        }
                    }
                } else {
                    for (int j = 0; j < counter; j++) {
                        for (std::vector<int>::iterator vIter = (*vecInitVerticalLoops_Edge_Ptr)[col_num].begin();
                             vIter != (*vecInitVerticalLoops_Edge_Ptr)[col_num].end(); vIter++) {
                            sstr << *vIter << " ";
                        }
                    }
                }
            }
            sstr << std::endl;
        }
        //
        ofile << sstr.rdbuf();
        //
        ofile.close();
        ofile.clear();
        //
        sstr.str("");
        sstr.clear();
    } else {
        std::cout << "can not open " << file_name << std::endl;
        exit(0);
    }
    //
    file_name = file_prefix + "_all_h.txt";
    ofile.open(file_name.c_str(), std::ifstream::out);
    if (ofile.is_open()) {
        sstr << tot_genus << std::endl;
        for (unsigned int i = 0; i < tot_genus; i++) {
            int edges_num = 0;
            int row_num = i;
            //
            if (inVertLoopType[i])
                row_num += tot_genus;
            //
            for (std::list<std::pair<int, int> >::iterator listIter = (*coeffMatrix)[row_num].begin();
                 listIter != (*coeffMatrix)[row_num].end(); listIter++) {
                int col_num = listIter->first;
                int counter = abs(listIter->second);
                if (col_num >= tot_genus) {// it is a horizontal loop
                    edges_num += counter * (*vecInitHorizontalLoops_Edge_Ptr)[col_num - tot_genus].size();
                } else {
                    edges_num += counter * (*vecInitVerticalLoops_Edge_Ptr)[col_num].size();
                }
            }
            //
            sstr << edges_num << std::endl;
            for (std::list<std::pair<int, int> >::iterator listIter = (*coeffMatrix)[row_num].begin();
                 listIter != (*coeffMatrix)[row_num].end(); listIter++) {
                int col_num = listIter->first;
                int counter = abs(listIter->second);
                if (col_num >= tot_genus) {// it is a horizontal loop
                    for (int j = 0; j < counter; j++) {
                        for (std::vector<int>::iterator vIter = (*vecInitHorizontalLoops_Edge_Ptr)[col_num -
                                                                                tot_genus].begin();
                             vIter != (*vecInitHorizontalLoops_Edge_Ptr)[col_num - tot_genus].end(); vIter++) {
                            sstr << *vIter << " ";
                        }
                    }
                } else {
                    for (int j = 0; j < counter; j++) {
                        for (std::vector<int>::iterator vIter = (*vecInitVerticalLoops_Edge_Ptr)[col_num].begin();
                             vIter != (*vecInitVerticalLoops_Edge_Ptr)[col_num].end(); vIter++) {
                            sstr << *vIter << " ";
                        }
                    }
                }
            }
            sstr << std::endl;
        }
        //
        ofile << sstr.rdbuf();
        //
        ofile.close();
        ofile.clear();
        //
        sstr.str("");
        sstr.clear();
    } else {
        std::cout << "can not open " << file_name << std::endl;
        exit(0);
    }
    //
    return;
}

void FilesOutputForOptimalCycles::WriteAllCyclesOut(std::vector<bool> &inVertLoopType, const char *out_file_name,
                                                    const int loop_type_num) {
    std::vector<std::vector<int> > vertLoops_Edge;
    std::vector<std::vector<int> > horiLoops_Edge;
    std::vector<int> *activeResLoopPtr;
    //

    std::stringstream locSstr(std::stringstream::in | std::stringstream::out);
    //
    string v_file_name = "v_loop_";
    string h_file_name = "h_loop_";
    string file_name;
    // assemble the matrix
    const int tot_genus = vecInitVerticalLoops_Vertex_Ptr->size();
    matrix<double> linkNumMat(2 * tot_genus, 2 * tot_genus);
    matrix<double> invLinkNumMat(2 * tot_genus, 2 * tot_genus);
    //
    //AssembleMatrix(linkNumMat, *coeffMatrix);
    //InvertMatrix(linkNumMat, invLinkNumMat);
    //
    //for (unsigned int i = 0; i < 2 * tot_genus; i++)
    //{		for (unsigned int j = 0; j < 2 * tot_genus; j++)
    //		{
    //			std::cout << invLinkNumMat(i , j ) << " " ;
    //		}
    //		std::cout << std::endl;
    //}
    //std::cout << std::endl;
    //
    // reserve memory for loops
    vertLoops_Edge.resize(tot_genus);
    horiLoops_Edge.resize(tot_genus);
    //
    for (unsigned int i = 0; i < 2 * tot_genus; i++) {
        //
        if (i < tot_genus) {
            locSstr << i << ".txt";
            file_name = v_file_name + locSstr.str();
            locSstr.str("");
            locSstr.clear();

            // set the active loop
            activeResLoopPtr = &vertLoops_Edge[i];
        } else {
            locSstr << i - tot_genus << ".txt";
            file_name = h_file_name + locSstr.str();
            locSstr.str("");
            locSstr.clear();

            // set the active loop
            activeResLoopPtr = &horiLoops_Edge[i - tot_genus];
        }
        //
        std::cout << "writing file ... " << file_name << std::endl;

        std::ofstream ofile;
        ofile.open(file_name.c_str(), std::ifstream::out);
        if (ofile.is_open()) {
            int totEdgeNum = 0;
            for (unsigned int col = 0; col < 2 * tot_genus; col++) {
                if (invLinkNumMat(i, col) < -0.5 || invLinkNumMat(i, col) > 0.5) {
                    //std::cout << invLinkNumMat(i, col)   << " " ;
                    if (col < tot_genus)
                        totEdgeNum += (*vecInitVerticalLoops_Edge_Ptr)[col].size();
                    else
                        totEdgeNum += (*vecInitHorizontalLoops_Edge_Ptr)[col - tot_genus].size();
                }
            }
            //std::cout << std::endl;
            std::stringstream sstr(std::stringstream::in | std::stringstream::out);

            // total number of edges
            sstr << totEdgeNum << " ";
            for (unsigned int col = 0; col < 2 * tot_genus; col++) {
                int coeff = int(float(invLinkNumMat(i, col)));
                std::cout << coeff << " ";
                if (coeff != 0) {// add this into the out
                    int cycle_id = col;
                    std::vector<std::vector<int> > *vecVertex_Ptr = vecInitVerticalLoops_Vertex_Ptr;
                    std::vector<std::vector<int> > *vecEdge_Ptr = vecInitVerticalLoops_Edge_Ptr;

                    if (cycle_id >= vecInitVerticalLoops_Vertex_Ptr->size()) {
                        cycle_id -= int(vecInitVerticalLoops_Vertex_Ptr->size());
                        vecVertex_Ptr = vecInitHorizontalLoops_Vertex_Ptr;
                        vecEdge_Ptr = vecInitHorizontalLoops_Edge_Ptr;
                    }
                    //
                    int vertexNum = int((*vecVertex_Ptr)[cycle_id].size());
                    int edgeNum = int((*vecEdge_Ptr)[cycle_id].size());
                    //

                    for (int eid = 0; eid < edgeNum; eid++) {
                        // vi--ei--v(i+1)
                        // check the orientation
                        if ((inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][eid]].v0 != (*vecVertex_Ptr)[cycle_id][eid] &&
                             inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][eid]].v0 !=
                             (*vecVertex_Ptr)[cycle_id][eid + 1]) ||
                            (inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][eid]].v1 != (*vecVertex_Ptr)[cycle_id][eid] &&
                             inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][eid]].v1 !=
                             (*vecVertex_Ptr)[cycle_id][eid + 1])) {
                            std::cout << "EDGE doesn't match vertices " << std::endl;
                            exit(0);
                        }
                        if (inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][eid]].v0 ==
                            (*vecVertex_Ptr)[cycle_id][eid]) {// vi-->---v(i+1) has the same orientation as ei
                            sstr << 1 * coeff << " ";
                        } else {// opposite orientation
                            sstr << -1 * coeff << " ";
                        }
                        // write the edge
                        sstr << (*vecEdge_Ptr)[cycle_id][eid] << " ";

                        // save the edge to the vector
                        for (unsigned int nCounter = 0; nCounter < abs(coeff); nCounter++)
                            activeResLoopPtr->push_back((*vecEdge_Ptr)[cycle_id][eid]);
                    }

                } //coeff != 0
            } // col
            std::cout << std::endl;
            //
            //ofile.write(sstr.str().c_str(), 10);
            ofile << sstr.rdbuf();
            //
            // delete string stream content
            sstr.str("");
            sstr.clear();
            // close file
            ofile.close();
        }//
        else {
            std::cout << "Can NOT open file " << file_name << std::endl;
            exit(0);
        }

    }// i
    //
    if (loop_type_num == 1)
        WriteAllCyclesInOneFile(vertLoops_Edge, horiLoops_Edge, inVertLoopType, out_file_name, true);
    if (loop_type_num == 2) {
        WriteAllCyclesInOneFile(vertLoops_Edge, horiLoops_Edge, inVertLoopType, "all_h_loops.txt", false);
        WriteAllCyclesInOneFile(vertLoops_Edge, horiLoops_Edge, inVertLoopType, "all_v_loops.txt", true);
    }
    if (loop_type_num == 3)
        WriteAllCyclesInOneFile(vertLoops_Edge, horiLoops_Edge, inVertLoopType, out_file_name, false);
    //
    return;
}

void FilesOutputForOptimalCycles::WriteAllCyclesOut() {
    std::vector<std::vector<int> > vertLoops_Edge;
    std::vector<std::vector<int> > horiLoops_Edge;
    //

    std::stringstream locSstr(std::stringstream::in | std::stringstream::out);
    string v_file_name = "v_loop_";
    string h_file_name = "h_loop_";
    string file_name;
    // assemble the matrix
    const int tot_genus = vecInitVerticalLoops_Vertex_Ptr->size();
    matrix<double> linkNumMat(2 * tot_genus, 2 * tot_genus);
    matrix<double> invLinkNumMat(2 * tot_genus, 2 * tot_genus);
    //
    //AssembleMatrix(linkNumMat, *coeffMatrix);
    //InvertMatrix(linkNumMat, invLinkNumMat);
    ////////////////////////////////////////////
    //
    //for (unsigned int i = 0; i < 2 * tot_genus; i++)
    //{		for (unsigned int j = 0; j < 2 * tot_genus; j++)
    //		{
    //			std::cout << invLinkNumMat(i , j ) << " " ;
    //		}
    //		std::cout << std::endl;
    //}
    //std::cout << std::endl;
    //
    for (unsigned int i = 0; i < 2 * tot_genus; i++) {
        //
        if (i < tot_genus) {
            locSstr << i << ".txt";
            file_name = v_file_name + locSstr.str();
            locSstr.str("");
            locSstr.clear();

        } else {
            locSstr << i - tot_genus << ".txt";
            file_name = h_file_name + locSstr.str();
            locSstr.str("");
            locSstr.clear();
        }
        //
        std::cout << "writing file ... " << file_name << std::endl;

        std::ofstream ofile;
        ofile.open(file_name.c_str(), std::ifstream::out);
        if (ofile.is_open()) {
            int totEdgeNum = 0;
            for (unsigned int col = 0; col < 2 * tot_genus; col++) {
                if (invLinkNumMat(i, col) < -0.5 || invLinkNumMat(i, col) > 0.5) {
                    //std::cout << invLinkNumMat(i, col)   << " " ;
                    if (col < tot_genus)
                        totEdgeNum += (*vecInitVerticalLoops_Edge_Ptr)[col].size();
                    else
                        totEdgeNum += (*vecInitHorizontalLoops_Edge_Ptr)[col - tot_genus].size();
                }
            }
            //std::cout << std::endl;
            std::stringstream sstr(std::stringstream::in | std::stringstream::out);

            // total number of edges
            sstr << totEdgeNum << " ";
            for (unsigned int col = 0; col < 2 * tot_genus; col++) {
                int coeff = int(float(invLinkNumMat(i, col)));
                //std::cout << coeff << " " ;
                if (coeff != 0) {// add this into the out
                    int cycle_id = col;
                    std::vector<std::vector<int> > *vecVertex_Ptr = vecInitVerticalLoops_Vertex_Ptr;
                    std::vector<std::vector<int> > *vecEdge_Ptr = vecInitVerticalLoops_Edge_Ptr;

                    if (cycle_id >= vecInitVerticalLoops_Vertex_Ptr->size()) {
                        cycle_id -= int(vecInitVerticalLoops_Vertex_Ptr->size());
                        vecVertex_Ptr = vecInitHorizontalLoops_Vertex_Ptr;
                        vecEdge_Ptr = vecInitHorizontalLoops_Edge_Ptr;
                    }
                    //
                    int vertexNum = int((*vecVertex_Ptr)[cycle_id].size());
                    int edgeNum = int((*vecEdge_Ptr)[cycle_id].size());
                    //

                    for (int eid = 0; eid < edgeNum; eid++) {
                        // vi--ei--v(i+1)
                        // check the orientation
                        if ((inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][eid]].v0 != (*vecVertex_Ptr)[cycle_id][eid] &&
                             inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][eid]].v0 !=
                             (*vecVertex_Ptr)[cycle_id][eid + 1]) ||
                            (inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][eid]].v1 != (*vecVertex_Ptr)[cycle_id][eid] &&
                             inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][eid]].v1 !=
                             (*vecVertex_Ptr)[cycle_id][eid + 1])) {
                            std::cout << "EDGE doesn't match vertices " << std::endl;
                            exit(0);
                        }
                        if (inMeshPtr->vecEdge[(*vecEdge_Ptr)[cycle_id][eid]].v0 ==
                            (*vecVertex_Ptr)[cycle_id][eid]) {// vi-->---v(i+1) has the same orientation as ei
                            sstr << 1 * coeff << " ";
                        } else {// opposite orientation
                            sstr << -1 * coeff << " ";
                        }
                        // write the edge
                        sstr << (*vecEdge_Ptr)[cycle_id][eid] << " ";
                    }

                } //coeff != 0
            } // col
            //std::cout << std::endl;
            //
            //ofile.write(sstr.str().c_str(), 10);
            ofile << sstr.rdbuf();
            //
            // delete string stream content
            sstr.str("");
            sstr.clear();
            // close file
            ofile.close();
        }//
        else {
            std::cout << "Can NOT open file " << file_name << std::endl;
            exit(0);
        }

    }// i
    return;
}

void FilesOutputForOptimalCycles::WriteAllCyclesInOneFile(std::vector<std::vector<int> > &vertLoops,
                                                          std::vector<std::vector<int> > &horiLoops,
                                                          const char *out_file_name) {// loop_type ---- true for vertical loop
    // loop_type ---- false for horizontal loop
    std::cout << "writing file ... " << out_file_name << std::endl;

    std::ofstream ofile;
    ofile.open(out_file_name, std::ifstream::out);

    //
    int tot_num = vertLoops.size();
    //
    std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //
    if (ofile.is_open()) {
        // total number of edges
        sstr << "h " << tot_num << std::endl;;
        for (int i = 0; i < tot_num; i++) {
            sstr << horiLoops[i].size() << std::endl;
            for (std::vector<int>::iterator sIter = horiLoops[i].begin();
                 sIter != horiLoops[i].end();
                 sIter++) {
                sstr << *sIter << " ";
            }
            sstr << std::endl;
        }
        sstr << std::endl << "v " << tot_num << std::endl;
        for (int i = 0; i < tot_num; i++) {
            sstr << vertLoops[i].size() << std::endl;
            for (std::vector<int>::iterator sIter = vertLoops[i].begin();
                 sIter != vertLoops[i].end();
                 sIter++) {
                sstr << *sIter << " ";
            }
            sstr << std::endl;
        }
        //
        //ofile.write(sstr.str().c_str(), 10);
        ofile << sstr.rdbuf();
        //
        // delete string stream content
        sstr.str("");
        sstr.clear();
        // close file
        ofile.close();
    } else {
        std::cout << "Can NOT open file " << out_file_name << std::endl;
        exit(0);
    }
    std::cout << "Done... " << tot_num << std::endl;
    return;

}

void FilesOutputForOptimalCycles::WriteAllCyclesInOneFile(std::vector<std::vector<int> > &vertLoops,
                                                          std::vector<std::vector<int> > &horiLoops,
                                                          std::vector<bool> &initVerticalLoopsType,
                                                          const char *out_file_name,
                                                          const bool loop_type) {// loop_type ---- true for vertical loop
    // loop_type ---- false for horizontal loop
    std::cout << "writing file ... " << out_file_name << std::endl;

    std::ofstream ofile;
    ofile.open(out_file_name, std::ifstream::out);

    //
    int tot_num = initVerticalLoopsType.size();
    int edgeNum = 0;
    //
    std::vector<std::vector<int> > *posLoops;
    std::vector<std::vector<int> > *negLoops;
    if (loop_type) {
        posLoops = &vertLoops;
        negLoops = &horiLoops;
    } else {
        posLoops = &horiLoops;
        negLoops = &vertLoops;
    }
    std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //
    if (ofile.is_open()) {
        // total number of edges
        sstr << tot_num << std::endl;;
        for (int i = 0; i < tot_num; i++) {
            // for each loop
            std::vector<int> *curEdgeLoopPtr = NULL;
            if (loop_type) {
                if (initVerticalLoopsType[i] == true) { //vertical
                    curEdgeLoopPtr = &((vertLoops)[i]);
                } else {// horizontal
                    curEdgeLoopPtr = &((horiLoops)[i]);
                }
            } else {
                if (initVerticalLoopsType[i] == true) { //vertical
                    curEdgeLoopPtr = &((horiLoops)[i]);
                } else {// horizontal
                    curEdgeLoopPtr = &((vertLoops)[i]);
                }
            }
            sstr << curEdgeLoopPtr->size() << std::endl;
            for (std::vector<int>::iterator sIter = curEdgeLoopPtr->begin();
                 sIter != curEdgeLoopPtr->end();
                 sIter++) {
                sstr << *sIter << " ";
            }
            sstr << std::endl;
        }

        //
        //ofile.write(sstr.str().c_str(), 10);
        ofile << sstr.rdbuf();
        //
        // delete string stream content
        sstr.str("");
        sstr.clear();
        // close file
        ofile.close();
    } else {
        std::cout << "Can NOT open file " << out_file_name << std::endl;
        exit(0);
    }
    std::cout << "Done... " << tot_num << std::endl;
    return;

}

void FilesOutputForOptimalCycles::WriteCriticalPoints(const char *out_file_name,
                                                      std::vector<std::pair<int, int> > &critSet) {
    std::cout << "writing file ... " << out_file_name << std::endl;

    std::ofstream ofile;
    ofile.open(out_file_name, std::ifstream::out);
    //
    std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //
    if (ofile.is_open()) {
        sstr << critSet.size() << std::endl;

        for (unsigned int i = 0; i < critSet.size(); i++) {
            sstr << critSet[i].first << " " << critSet[i].second << std::endl;
        }
        //ofile.write(sstr.str().c_str(), 10);
        ofile << sstr.rdbuf();
        //
        // delete string stream content
        sstr.str("");
        sstr.clear();
        // close file
        ofile.close();
    } else {
        std::cout << "Can NOT open file " << out_file_name << std::endl;
        exit(0);
    }
    std::cout << "Done... " << critSet.size() << std::endl;
    return;
}

void FilesOutputForOptimalCycles::WriteCriticalPoints(const char *out_file_name, std::set<int> &critSet,
                                                      const bool bVertical) {
    std::cout << "writing file ... " << out_file_name << std::endl;

    std::ofstream ofile;
    ofile.open(out_file_name, std::ifstream::out);

    std::vector<std::vector<int> > *activeLoops;
    if (bVertical)
        activeLoops = vecInitHorizontalLoops_Vertex_Ptr;
    else
        activeLoops = vecInitVerticalLoops_Vertex_Ptr;
    //
//	int tot_num = critSet.size();
    int tot_num = 0;
    for (unsigned int i = 0; i < vecInitHorizontalLoops_Edge_Ptr->size(); i++) {
        tot_num += (*activeLoops)[i].size();
    }
    //
    std::stringstream sstr(std::stringstream::in | std::stringstream::out);
    //
    if (ofile.is_open()) {
        // total number of edges
        //sstr << tot_num << std::endl;
        //for (std::set<int>::iterator sIter = critSet.begin();
        //	sIter != critSet.end();
        //	sIter++)
        //{
        //	sstr << *sIter << " " ;
        //}
        //sstr << std::endl;

        sstr << tot_num << std::endl;
        std::set<int> uniqueHoriVertex;
        for (unsigned int i = 0; i < activeLoops->size(); i++) {
            for (unsigned int k = 0; k < (*activeLoops)[i].size(); k++) {
                uniqueHoriVertex.insert((*activeLoops)[i][k]);
            }
        }
        for (std::set<int>::iterator sIter = uniqueHoriVertex.begin();
             sIter != uniqueHoriVertex.end();
             sIter++) {
            sstr << *sIter << " ";
        }
        sstr << std::endl;
        //
        //ofile.write(sstr.str().c_str(), 10);
        ofile << sstr.rdbuf();
        //
        // delete string stream content
        sstr.str("");
        sstr.clear();
        // close file
        ofile.close();
    } else {
        std::cout << "Can NOT open file " << out_file_name << std::endl;
        exit(0);
    }
    std::cout << "Done... " << tot_num << std::endl;
}


#ifndef I_SEE_PEE_ITERATOR_HPP
#define I_SEE_PEE_ITERATOR_HPP

#include <Eigen/Dense>
#include <cmath>

namespace i_see_pee {

template<typename T>
struct centered_submap_iterator {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  using data_t = Eigen::Matrix<T, 2ul, 1ul>;
  using bound_t = Eigen::Matrix<size_t, 2ul, 1ul>;

  explicit centered_submap_iterator(const bound_t &_dim) noexcept :
          size_(_dim.cast<T>()), curr_(data_t::Zero()), prod_(size_.prod()) {}

  centered_submap_iterator &operator++() noexcept {
    ++curr_(1);
    if (curr_(1) >= size_(1)) {
      curr_(1) = 0;
      ++curr_(0);
    }
    return *this;
  }

  inline const data_t &operator*() const noexcept {
    return curr_;
  }

  inline const data_t *operator->() const noexcept {
    return &curr_;
  }

  inline bool past_end() const noexcept {
    return curr_(0) >= size_(0) || !prod_;
  }

private:
  data_t size_, curr_;
  size_t prod_;
};

template<typename T>
struct submap_iterator {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  using centered_submap_iterator_t = centered_submap_iterator<T>;
  using data_t = typename centered_submap_iterator_t::data_t;
  using bound_t = typename centered_submap_iterator_t::bound_t;

  submap_iterator() noexcept :
          iter_(bound_t::Zero()), begin_(data_t::Zero()), curr_(data_t::Zero()) {}

  submap_iterator(const data_t &_begin, const bound_t &_size) noexcept :
          iter_(_size), begin_(_begin), curr_(_begin) {}

  submap_iterator &operator++() noexcept {
    ++iter_;
    curr_ = begin_ + *iter_;
    return *this;
  }

  inline const data_t &operator*() const noexcept {
    return curr_;
  }

  inline const data_t *operator->() const noexcept {
    return &curr_;
  }

  inline bool past_end() const noexcept {
    return iter_.past_end();
  }

private:
  data_t begin_, curr_;
  centered_submap_iterator_t iter_;
};

template<typename T>
struct circle_iterator {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  using submap_iterator_t = submap_iterator<T>;
  using data_t = typename submap_iterator_t::data_t;
  using bound_t = typename submap_iterator_t::bound_t;

  circle_iterator(const data_t &_center, T _radius) noexcept :
          center_(_center), radius_(_radius) {
    // get the positive radius
    _radius = std::abs(_radius);

    // get the submap parameters with 1 pixel at the center
    const data_t rad(_radius, _radius);
    const data_t begin = _center - rad;
    const bound_t size = rad.template cast<size_t>() * 2 + bound_t::Ones();
    iter_ = submap_iterator_t(begin, size);

    // advance till valid position to allow instant access
    while (!valid(*iter_)) {
      ++iter_;
    }
  }

  circle_iterator &operator++() noexcept {
    do {
      ++iter_;
    } while (!iter_.past_end() && !valid(*iter_));
    return *this;
  }

  inline const data_t &operator*() const noexcept {
    return *iter_;
  }

  inline const data_t *operator->() const noexcept {
    return iter_.operator->();
  }

  bool past_end() const noexcept {
    return iter_.past_end();
  }

private:

  bool valid(const data_t &_d) const noexcept {
    return (center_ - _d).norm() <= radius_;
  }

  const data_t center_;
  const T radius_;
  submap_iterator_t iter_;
};

} // namespace i_see_pee

#endif //I_SEE_PEE_ITERATOR_HPP


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#ifndef LIMBO_ACQUI_EI_HPP
#define LIMBO_ACQUI_EI_HPP

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

#include <limbo/opt/optimizer.hpp>
#include <limbo/tools/macros.hpp>

namespace limbo {
    namespace defaults {
        struct acqui_ei {
            /// @ingroup acqui_defaults
            BO_PARAM(double, jitter, 0.0);
        };
    }
    namespace acqui {
        /** @ingroup acqui
        \rst
        Classic EI (Expected Improvement). See :cite:`brochu2010tutorial`, p. 14

          .. math::
            EI(x) = (\mu(x) - f(x^+) - \xi)\Phi(Z) + \sigma(x)\phi(Z),\\\text{with } Z = \frac{\mu(x)-f(x^+) - \xi}{\sigma(x)}.

        Parameters:
          - ``double jitter`` - :math:`\xi`
        \endrst
        */
        template <typename Params, typename Model>
        class EI {
        public:
            EI(const Model& model, int iteration = 0) : _model(model), _nb_samples(-1) {}

            size_t dim_in() const { return _model.dim_in(); }

            size_t dim_out() const { return _model.dim_out(); }

            template <typename AggregatorFunction>
            opt::eval_t operator()(const Eigen::VectorXd& v, const AggregatorFunction& afun, bool gradient)
            {
                assert(!gradient);

                Eigen::VectorXd mu;
                double sigma_sq;
                std::tie(mu, sigma_sq) = _model.query(v);
                double sigma = std::sqrt(sigma_sq);

                // If \sigma(x) = 0 or we do not have any observation yet we return 0
                if (sigma < 1e-10 || _model.samples().size() < 1)
                    return opt::no_grad(0.0);

                // Compute EI(x)
                // First find the best so far (predicted) observation -- if needed
                if (_nb_samples != _model.nb_samples()) {
                    std::vector<double> rewards;
                    for (auto s : _model.samples()) {
                        rewards.push_back(afun(_model.mu(s)));
                    }

                    _nb_samples = _model.nb_samples();
                    _f_max = *std::max_element(rewards.begin(), rewards.end());
                }
                // Calculate Z and \Phi(Z) and \phi(Z)
                double X = afun(mu) - _f_max - Params::acqui_ei::jitter();
                double Z = X / sigma;
                double phi = std::exp(-0.5 * std::pow(Z, 2.0)) / std::sqrt(2.0 * M_PI);
                double Phi = 0.5 * std::erfc(-Z / std::sqrt(2)); //0.5 * (1.0 + std::erf(Z / std::sqrt(2)));

                return opt::no_grad(X * Phi + sigma * phi);
            }

        protected:
            const Model& _model;
            int _nb_samples;
            double _f_max;
        };
    }
}

#endif


#pragma once
#include <boost/multiprecision/cpp_int.hpp>
#include <fc/log/logger.hpp>

namespace EOS {

   using namespace boost::multiprecision;

   typedef number<cpp_int_backend<8, 8, unsigned_magnitude, unchecked, void> >     UInt8;
   typedef number<cpp_int_backend<16, 16, unsigned_magnitude, unchecked, void> >   UInt16;
   typedef number<cpp_int_backend<32, 32, unsigned_magnitude, unchecked, void> >   UInt32;
   typedef number<cpp_int_backend<64, 64, unsigned_magnitude, unchecked, void> >   UInt64;
   typedef boost::multiprecision::uint128_t UInt128;
   typedef boost::multiprecision::uint256_t UInt256;
   typedef number<cpp_int_backend<8, 8, signed_magnitude, unchecked, void> >       Int8;
   typedef number<cpp_int_backend<16, 16, signed_magnitude, unchecked, void> >     Int16;
   typedef number<cpp_int_backend<32, 32, signed_magnitude, unchecked, void> >     Int32;
   typedef number<cpp_int_backend<64, 64, signed_magnitude, unchecked, void> >     Int64;
   typedef boost::multiprecision::int128_t Int128;
   typedef boost::multiprecision::int256_t Int256;

   template<typename Stream, typename Number>
   void fromBinary( Stream& st, boost::multiprecision::number<Number>& value ) {
      unsigned char data[(std::numeric_limits<decltype(value)>::digits+1)/8];
      st.read( (char*)data, sizeof(data) );
      boost::multiprecision::import_bits( value, data, data + sizeof(data), 1 );
   }
   template<typename Stream, typename Number>
   void toBinary( Stream& st, const boost::multiprecision::number<Number>& value ) {
      unsigned char data[(std::numeric_limits<decltype(value)>::digits+1)/8];
      boost::multiprecision::export_bits( value, data, 1 );
      st.write( (const char*)data, sizeof(data) );
   }
} // namespace EOS


//
// Licensed to Green Energy Corp (www.greenenergycorp.com) under one
// or more contributor license agreements. See the NOTICE file
// distributed with this work for additional information
// regarding copyright ownership.  Green Enery Corp licenses this file
// to you under the Apache License, Version 2.0 (the
// "License"); you may not use this file except in compliance
// with the License.  You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing,
// software distributed under the License is distributed on an
// "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied.  See the License for the
// specific language governing permissions and limitations
// under the License.
//
#define BOOST_TEST_MODULE apl
#include <boost/test/unit_test.hpp>

#include <opendnp3/APL/Types.h>
#include <limits>

using namespace std;
using namespace apl;

#undef max
#undef min

template<class T>
void TestType(T min, T max, size_t aBytes)
{
	BOOST_CHECK_EQUAL(sizeof(T), aBytes);

	T val = min;
	T limit = numeric_limits<T>::min();
	BOOST_CHECK_EQUAL(val, limit);

	val = max;
	limit = numeric_limits<T>::max();
	BOOST_CHECK_EQUAL(val, limit);
}

BOOST_AUTO_TEST_SUITE(TestTypes)

BOOST_AUTO_TEST_CASE( UByte )
{
	TestType<boost::uint8_t>(0, 255, 1);
}

BOOST_AUTO_TEST_CASE( Int16 )
{
	TestType<boost::int16_t>(-32768, 32767, 2);
}

BOOST_AUTO_TEST_CASE( UInt16 )
{
	TestType<boost::uint16_t>(0, 65535, 2);
}

BOOST_AUTO_TEST_CASE( Int32 )
{
	TestType<boost::int32_t>(0x80000000, 2147483647L, 4);
}

BOOST_AUTO_TEST_CASE( UInt32 )
{
	TestType<boost::uint32_t>(0, 4294967295UL, 4);
}

BOOST_AUTO_TEST_CASE( Int64)
{
	TestType<boost::int64_t>(0x8000000000000000LL, 9223372036854775807LL, 8);
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <boost/hana/assert.hpp>
#include <boost/hana/ext/boost/mpl/integral_c.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/long.hpp>
using namespace boost::hana;
namespace mpl = boost::mpl;

int main() {

{

//! [constant]
static_assert(value(mpl::integral_c<int, 3>{}) == 3, "");

BOOST_HANA_CONSTANT_CHECK(equal(mpl::integral_c<int, 3>{}, mpl::int_<3>{}));
BOOST_HANA_CONSTANT_CHECK(equal(mpl::integral_c<int, 3>{}, mpl::long_<3>{}));
BOOST_HANA_CONSTANT_CHECK(not_equal(mpl::integral_c<int, 3>{}, mpl::int_<0>{}));
//! [constant]

}

}


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

#include <boost/hana.hpp>

#include <boost/hana/range.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/core/tag_of.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/optional.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/comparable.hpp>
#include <laws/foldable.hpp>
#include <laws/iterable.hpp>
#include <laws/searchable.hpp>
#include <test/cnumeric.hpp>

#include <type_traits>
namespace hana = boost::hana;
namespace test = hana::test;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Setup for the laws below
    //////////////////////////////////////////////////////////////////////////
    auto ranges = hana::make_tuple(
          hana::make_range(hana::int_c<0>, hana::int_c<0>)
        , hana::make_range(hana::int_c<0>, hana::int_c<1>)
        , hana::make_range(hana::int_c<0>, hana::int_c<2>)
        , hana::make_range(hana::int_c<1>, hana::int_c<1>)
        , hana::make_range(hana::int_c<1>, hana::int_c<2>)
        , hana::make_range(hana::int_c<1>, hana::int_c<3>)
        , hana::make_range(hana::int_c<50>, hana::int_c<60>)

        , hana::make_range(hana::int_c<50>, hana::long_c<60>)
        , hana::make_range(hana::long_c<50>, hana::int_c<60>)
    );

    auto integers = hana::tuple_c<int, 0, 1, 900>; (void)integers;

#if BOOST_HANA_TEST_PART == 1
    //////////////////////////////////////////////////////////////////////////
    // range_c
    //////////////////////////////////////////////////////////////////////////
    {
        using T = int;
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::range_c<T, 0, 0>,
            hana::make_range(hana::integral_c<T, 0>, hana::integral_c<T, 0>)
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::range_c<T, 0, 1>,
            hana::make_range(hana::integral_c<T, 0>, hana::integral_c<T, 1>)
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::range_c<T, 0, 2>,
            hana::make_range(hana::integral_c<T, 0>, hana::integral_c<T, 2>)
        ));

        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::range_c<T, 1, 1>,
            hana::make_range(hana::integral_c<T, 1>, hana::integral_c<T, 1>)
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::range_c<T, 1, 2>,
            hana::make_range(hana::integral_c<T, 1>, hana::integral_c<T, 2>)
        ));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::range_c<T, 1, 3>,
            hana::make_range(hana::integral_c<T, 1>, hana::integral_c<T, 3>)
        ));
    }

    //////////////////////////////////////////////////////////////////////////
    // make<range_tag>
    //////////////////////////////////////////////////////////////////////////
    {
        // make sure make<range_tag> works with arbitrary Constants
        BOOST_HANA_CONSTANT_CHECK(hana::equal(
            hana::make<hana::range_tag>(test::_constant<1>{}, test::_constant<4>{}),
            hana::make_range(hana::integral_c<int, 1>, hana::integral_c<int, 4>)
        ));
    }

    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    {
        // equal
        {
            BOOST_HANA_CONSTANT_CHECK(
                hana::make_range(hana::int_c<0>, hana::int_c<0>) ==
                hana::make_range(hana::int_c<0>, hana::int_c<0>)
            );
            BOOST_HANA_CONSTANT_CHECK(
                hana::make_range(hana::int_c<0>, hana::int_c<4>) !=
                hana::make_range(hana::int_c<0>, hana::int_c<0>)
            );

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::make_range(hana::int_c<0>, hana::int_c<0>),
                hana::make_range(hana::int_c<0>, hana::int_c<0>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::equal(
                hana::make_range(hana::int_c<0>, hana::int_c<0>),
                hana::make_range(hana::int_c<0>, hana::int_c<1>)
            )));
            BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::equal(
                hana::make_range(hana::int_c<0>, hana::int_c<1>),
                hana::make_range(hana::int_c<0>, hana::int_c<0>)
            )));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::make_range(hana::int_c<0>, hana::int_c<1>),
                hana::make_range(hana::int_c<0>, hana::int_c<1>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::equal(
                hana::make_range(hana::int_c<0>, hana::int_c<2>),
                hana::make_range(hana::int_c<0>, hana::int_c<1>)
            )));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::make_range(hana::int_c<0>, hana::int_c<2>),
                hana::make_range(hana::int_c<0>, hana::int_c<2>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::make_range(hana::int_c<0>, hana::int_c<0>),
                hana::make_range(hana::int_c<2>, hana::int_c<2>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::make_range(hana::int_c<2>, hana::int_c<4>),
                hana::make_range(hana::int_c<2>, hana::int_c<4>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::make_range(hana::int_c<-4>, hana::int_c<-3>),
                hana::make_range(hana::int_c<-4>, hana::int_c<-3>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::make_range(hana::int_c<-4>, hana::int_c<2>),
                hana::make_range(hana::int_c<-4>, hana::int_c<2>)
            ));
        }

        // laws
        test::TestComparable<hana::range_tag>{ranges};
    }
#elif BOOST_HANA_TEST_PART == 2
    //////////////////////////////////////////////////////////////////////////
    // Foldable
    //////////////////////////////////////////////////////////////////////////
    {
        test::_injection<0> f{};

        // unpack
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::unpack(hana::make_range(hana::int_c<0>, hana::int_c<0>), f),
                f()
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::unpack(hana::make_range(hana::int_c<0>, hana::int_c<1>), f),
                f(hana::int_c<0>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::unpack(hana::make_range(hana::int_c<0>, hana::int_c<2>), f),
                f(hana::int_c<0>, hana::int_c<1>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::unpack(hana::make_range(hana::int_c<0>, hana::int_c<3>), f),
                f(hana::int_c<0>, hana::int_c<1>, hana::int_c<2>)
            ));

            // Previously, we would only unpack with `std::size_t`s. Make
            // sure this does not happen.
            hana::unpack(hana::make_range(hana::int_c<0>, hana::int_c<1>), [](auto x) {
                using T = hana::tag_of_t<decltype(x)>;
                static_assert(std::is_same<typename T::value_type, int>{}, "");
            });
        }

        // length
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::length(hana::make_range(hana::int_c<0>, hana::int_c<0>)), hana::size_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::length(hana::make_range(hana::int_c<0>, hana::int_c<1>)), hana::size_c<1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::length(hana::make_range(hana::int_c<0>, hana::int_c<2>)), hana::size_c<2>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::length(hana::make_range(hana::int_c<4>, hana::int_c<4>)), hana::size_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::length(hana::make_range(hana::int_c<4>, hana::int_c<10>)), hana::size_c<6>
            ));
        }

        // minimum
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::minimum(hana::make_range(hana::int_c<3>, hana::int_c<4>)), hana::int_c<3>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::minimum(hana::make_range(hana::int_c<3>, hana::int_c<5>)), hana::int_c<3>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::minimum(hana::make_range(hana::int_c<-1>, hana::int_c<5>)), hana::int_c<-1>
            ));
        }

        // maximum
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::maximum(hana::make_range(hana::int_c<3>, hana::int_c<4>)), hana::int_c<3>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::maximum(hana::make_range(hana::int_c<3>, hana::int_c<5>)), hana::int_c<4>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::maximum(hana::make_range(hana::int_c<-1>, hana::int_c<6>)), hana::int_c<5>
            ));
        }

        // sum
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<-3>, hana::int_c<-3>)), hana::int_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<-3>, hana::int_c<-2>)), hana::int_c<-3>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<-3>, hana::int_c<-1>)), hana::int_c<-3 + -2>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<-3>, hana::int_c<0>)), hana::int_c<-3 + -2 + -1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<-3>, hana::int_c<1>)), hana::int_c<-3 + -2 + -1 + 0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<-3>, hana::int_c<2>)), hana::int_c<-3 + -2 + -1 + 0 + 1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<-3>, hana::int_c<3>)), hana::int_c<-3 + -2 + -1 + 0 + 1 + 2>
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<0>, hana::int_c<0>)), hana::int_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<0>, hana::int_c<1>)), hana::int_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<0>, hana::int_c<2>)), hana::int_c<0 + 1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<0>, hana::int_c<3>)), hana::int_c<0 + 1 + 2>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<0>, hana::int_c<4>)), hana::int_c<0 + 1 + 2 + 3>
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<3>, hana::int_c<3>)), hana::int_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<3>, hana::int_c<4>)), hana::int_c<3>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<3>, hana::int_c<5>)), hana::int_c<3 + 4>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<3>, hana::int_c<6>)), hana::int_c<3 + 4 + 5>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::sum<>(hana::make_range(hana::int_c<3>, hana::int_c<7>)), hana::int_c<3 + 4 + 5 + 6>
            ));
        }

        // product
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<-3>, hana::int_c<-3>)), hana::int_c<1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<-3>, hana::int_c<-2>)), hana::int_c<-3>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<-3>, hana::int_c<-1>)), hana::int_c<-3 * -2>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<-3>, hana::int_c<0>)), hana::int_c<-3 * -2 * -1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<-3>, hana::int_c<1>)), hana::int_c<-3 * -2 * -1 * 0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<-3>, hana::int_c<2>)), hana::int_c<-3 * -2 * -1 * 0 * 1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<-3>, hana::int_c<3>)), hana::int_c<-3 * -2 * -1 * 0 * 1 * 2>
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<1>, hana::int_c<1>)), hana::int_c<1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<1>, hana::int_c<2>)), hana::int_c<1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<1>, hana::int_c<3>)), hana::int_c<1 * 2>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<1>, hana::int_c<4>)), hana::int_c<1 * 2 * 3>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<1>, hana::int_c<5>)), hana::int_c<1 * 2 * 3 * 4>
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<3>, hana::int_c<3>)), hana::int_c<1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<3>, hana::int_c<4>)), hana::int_c<3>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<3>, hana::int_c<5>)), hana::int_c<3 * 4>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<3>, hana::int_c<6>)), hana::int_c<3 * 4 * 5>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::product<>(hana::make_range(hana::int_c<3>, hana::int_c<7>)), hana::int_c<3 * 4 * 5 * 6>
            ));
        }

        // laws
        test::TestFoldable<hana::range_tag>{ranges};
    }
#elif BOOST_HANA_TEST_PART == 3

    //////////////////////////////////////////////////////////////////////////
    // Iterable
    //////////////////////////////////////////////////////////////////////////
    {
        // is_empty
        {
            BOOST_HANA_CONSTANT_CHECK(hana::is_empty(hana::make_range(hana::int_c<0>, hana::int_c<0>)));
            BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::is_empty(hana::make_range(hana::int_c<0>, hana::int_c<1>))));
            BOOST_HANA_CONSTANT_CHECK(hana::not_(hana::is_empty(hana::make_range(hana::int_c<0>, hana::int_c<2>))));
        }

        // front
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::front(hana::make_range(hana::int_c<0>, hana::int_c<1>)), hana::int_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::front(hana::make_range(hana::int_c<0>, hana::int_c<2>)), hana::int_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::front(hana::make_range(hana::int_c<2>, hana::int_c<5>)), hana::int_c<2>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::front(hana::make_range(hana::int_c<5>, hana::int_c<6>)), hana::int_c<5>
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::front(hana::make_range(hana::int_c<5>, hana::long_c<6>)), hana::long_c<5>
            ));
        }

        // tail
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::tail(hana::make_range(hana::int_c<0>, hana::int_c<1>)),
                hana::make_range(hana::int_c<1>, hana::int_c<1>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::tail(hana::make_range(hana::int_c<0>, hana::int_c<2>)),
                hana::make_range(hana::int_c<1>, hana::int_c<2>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::tail(hana::make_range(hana::int_c<0>, hana::int_c<3>)),
                hana::make_range(hana::int_c<1>, hana::int_c<3>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::tail(hana::make_range(hana::int_c<4>, hana::int_c<7>)),
                hana::make_range(hana::int_c<5>, hana::int_c<7>)
            ));
        }

        // at
        {
            BOOST_HANA_CONSTANT_CHECK(
                hana::make_range(hana::int_c<0>, hana::int_c<10>)[hana::int_c<2>] == hana::int_c<2>
            );

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::at(hana::make_range(hana::int_c<0>, hana::int_c<1>), hana::int_c<0>),
                hana::int_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::at(hana::make_range(hana::int_c<0>, hana::int_c<2>), hana::int_c<0>),
                hana::int_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::at(hana::make_range(hana::int_c<0>, hana::int_c<2>), hana::int_c<1>),
                hana::int_c<1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::at(hana::make_range(hana::int_c<4>, hana::int_c<90>), hana::int_c<46>),
                hana::int_c<50>
            ));
        }

        // back
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::back(hana::make_range(hana::int_c<0>, hana::int_c<1>)),
                hana::int_c<0>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::back(hana::make_range(hana::int_c<0>, hana::int_c<2>)),
                hana::int_c<1>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::back(hana::make_range(hana::int_c<0>, hana::int_c<3>)),
                hana::int_c<2>
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::back(hana::make_range(hana::int_c<3>, hana::int_c<6>)),
                hana::int_c<5>
            ));
        }

        // drop_front
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<0>), hana::int_c<0>),
                hana::make_range(hana::int_c<0>, hana::int_c<0>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<0>), hana::int_c<1>),
                hana::make_range(hana::int_c<0>, hana::int_c<0>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<0>), hana::int_c<2>),
                hana::make_range(hana::int_c<0>, hana::int_c<0>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<1>), hana::int_c<0>),
                hana::make_range(hana::int_c<0>, hana::int_c<1>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<1>), hana::int_c<1>),
                hana::make_range(hana::int_c<1>, hana::int_c<1>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<1>), hana::int_c<2>),
                hana::make_range(hana::int_c<1>, hana::int_c<1>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<2>), hana::int_c<0>),
                hana::make_range(hana::int_c<0>, hana::int_c<2>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<2>), hana::int_c<1>),
                hana::make_range(hana::int_c<1>, hana::int_c<2>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<2>), hana::int_c<2>),
                hana::make_range(hana::int_c<2>, hana::int_c<2>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<3>), hana::int_c<0>),
                hana::make_range(hana::int_c<0>, hana::int_c<3>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<3>), hana::int_c<1>),
                hana::make_range(hana::int_c<1>, hana::int_c<3>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<3>), hana::int_c<2>),
                hana::make_range(hana::int_c<2>, hana::int_c<3>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<0>, hana::int_c<3>), hana::int_c<3>),
                hana::make_range(hana::int_c<3>, hana::int_c<3>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front(hana::make_range(hana::int_c<20>, hana::int_c<50>), hana::int_c<10>),
                hana::make_range(hana::int_c<30>, hana::int_c<50>)
            ));
        }

        // drop_front_exactly
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<0>, hana::int_c<0>), hana::int_c<0>),
                hana::make_range(hana::int_c<0>, hana::int_c<0>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<0>, hana::int_c<1>), hana::int_c<0>),
                hana::make_range(hana::int_c<0>, hana::int_c<1>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<0>, hana::int_c<1>), hana::int_c<1>),
                hana::make_range(hana::int_c<1>, hana::int_c<1>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<0>, hana::int_c<2>), hana::int_c<0>),
                hana::make_range(hana::int_c<0>, hana::int_c<2>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<0>, hana::int_c<2>), hana::int_c<1>),
                hana::make_range(hana::int_c<1>, hana::int_c<2>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<0>, hana::int_c<2>), hana::int_c<2>),
                hana::make_range(hana::int_c<2>, hana::int_c<2>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<0>, hana::int_c<3>), hana::int_c<0>),
                hana::make_range(hana::int_c<0>, hana::int_c<3>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<0>, hana::int_c<3>), hana::int_c<1>),
                hana::make_range(hana::int_c<1>, hana::int_c<3>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<0>, hana::int_c<3>), hana::int_c<2>),
                hana::make_range(hana::int_c<2>, hana::int_c<3>)
            ));
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<0>, hana::int_c<3>), hana::int_c<3>),
                hana::make_range(hana::int_c<3>, hana::int_c<3>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::drop_front_exactly(hana::make_range(hana::int_c<20>, hana::int_c<50>), hana::int_c<10>),
                hana::make_range(hana::int_c<30>, hana::int_c<50>)
            ));
        }

        // laws
        test::TestIterable<hana::range_tag>{ranges};
    }
#elif BOOST_HANA_TEST_PART == 4
    //////////////////////////////////////////////////////////////////////////
    // Searchable
    //////////////////////////////////////////////////////////////////////////
    {
        // find
        {
            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::find(hana::make_range(hana::int_c<0>, hana::int_c<0>), test::cnumeric<int, 0>),
                hana::nothing
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::find(hana::make_range(hana::int_c<0>, hana::int_c<1>), test::cnumeric<int, 0>),
                hana::just(hana::int_c<0>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::find(hana::make_range(hana::int_c<0>, hana::int_c<10>), test::cnumeric<int, 3>),
                hana::just(hana::int_c<3>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::find(hana::make_range(hana::int_c<0>, hana::int_c<10>), test::cnumeric<int, 9>),
                hana::just(hana::int_c<9>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::find(hana::make_range(hana::int_c<-10>, hana::int_c<10>), test::cnumeric<int, -10>),
                hana::just(hana::int_c<-10>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::find(hana::make_range(hana::int_c<-10>, hana::int_c<10>), test::cnumeric<int, -5>),
                hana::just(hana::int_c<-5>)
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::find(hana::make_range(hana::int_c<-10>, hana::int_c<0>), test::cnumeric<int, 3>),
                hana::nothing
            ));

            BOOST_HANA_CONSTANT_CHECK(hana::equal(
                hana::find(hana::make_range(hana::int_c<0>, hana::int_c<10>), test::cnumeric<int, 15>),
                hana::nothing
            ));
        }

        // contains
        {
            BOOST_HANA_CONSTANT_CHECK(
                hana::not_(hana::contains(hana::make_range(hana::int_c<0>, hana::int_c<0>), test::cnumeric<int, 0>))
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::contains(hana::make_range(hana::int_c<0>, hana::int_c<1>), test::cnumeric<int, 0>)
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::contains(hana::make_range(hana::int_c<0>, hana::int_c<10>), test::cnumeric<int, 3>)
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::contains(hana::make_range(hana::int_c<0>, hana::int_c<10>), test::cnumeric<int, 9>)
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::contains(hana::make_range(hana::int_c<-10>, hana::int_c<10>), test::cnumeric<int, -10>)
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::contains(hana::make_range(hana::int_c<-10>, hana::int_c<10>), test::cnumeric<int, -5>)
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::not_(hana::contains(hana::make_range(hana::int_c<-10>, hana::int_c<0>), test::cnumeric<int, 3>))
            );

            BOOST_HANA_CONSTANT_CHECK(
                hana::not_(hana::contains(hana::make_range(hana::int_c<0>, hana::int_c<10>), test::cnumeric<int, 15>))
            );
        }

        // laws
        test::TestSearchable<hana::range_tag>{ranges, integers};
    }
#endif
}


#ifndef DATA_TYPE_CONVERSION_HPP
#define DATA_TYPE_CONVERSION_HPP

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

#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Pose2D.h>
#include <sensor_msgs/Imu.h>

namespace mros {

namespace data_type_conversion {

    static geometry_msgs::Point EigenPointToGeometryMsgPoint( const Eigen::Vector3d& point ){
        geometry_msgs::Point ret_point;
        ret_point.x = point.x();
        ret_point.y = point.y();
        ret_point.z = point.z();
        return ret_point;
    }

    static Eigen::Vector3d   GeometryMsgPointToEigenPoint( const geometry_msgs::Point& point ){
        Eigen::Vector3d ret_point;
        ret_point.x() = point.x;
        ret_point.y() = point.y;
        ret_point.z() = point.z;
        return ret_point;
    }

    static geometry_msgs::Pose EigenPoseToGeometryMsgPose( const Eigen::Vector3d& position,
                                                           const Eigen::Quaterniond& orientation ) {
        geometry_msgs::Pose  pose;
        pose.position.x = position.x();
        pose.position.y = position.y();
        pose.position.z = position.z();
        pose.orientation.w = orientation.w();
        pose.orientation.x = orientation.x();
        pose.orientation.y = orientation.y();
        pose.orientation.z = orientation.z();
        return pose;
    }

    static geometry_msgs::Quaternion EigenQuaterniondToGeometryMsgQuaternion( const Eigen::Quaterniond& q ){
        geometry_msgs::Quaternion  ret_q;
        ret_q.w = q.w();
        ret_q.x = q.x();
        ret_q.y = q.y();
        ret_q.z = q.z();
        return  ret_q;
    }

    static Eigen::Quaterniond GeometryMsgQuaternionToEigenQuaterniond( const geometry_msgs::Quaternion& q ){
        Eigen::Quaterniond  ret_q;
        ret_q.w() = q.w;
        ret_q.x() = q.x;
        ret_q.y() = q.y;
        ret_q.z() = q.z;
        return  ret_q;
    }

    static geometry_msgs::Vector3 EigenVector3dToGeometryMsgVector3( const Eigen::Vector3d& vector ){
        geometry_msgs::Vector3  ret_vector;
        ret_vector.x = vector.x();
        ret_vector.y = vector.y();
        ret_vector.z = vector.z();
        return  ret_vector;
    }

    static Eigen::Vector3d GeometryMsgVector3ToEigenVectord( const geometry_msgs::Vector3& vector ){
        Eigen::Vector3d  ret_vector;
        ret_vector.x() = vector.x;
        ret_vector.y() = vector.y;
        ret_vector.z() = vector.z;
        return  ret_vector;
    }

    ///std::vector<double> to Eigen::Vector3d
    static Eigen::Vector3d StdVector3dToEigenVector3d( const std::vector<double>& vector ){
        Eigen::Vector3d  ret_vector;
        ret_vector.x() = vector[0];
        ret_vector.y() = vector[1];
        ret_vector.z() = vector[2];
        return  ret_vector;
    }

    static geometry_msgs::Pose2D GetGeometryPose2D( const double x, const double y, const double theta ){
      geometry_msgs::Pose2D pose2d;
      pose2d.x = x;
      pose2d.y = y;
      pose2d.theta = theta;
      return  pose2d;
    }

    static geometry_msgs::Point GetGeometryPoint( const double x, const double y, const double z ){
      geometry_msgs::Point point;
      point.x = x;
      point.y = y;
      point.z = z;
      return point;
    }

}  //end of namespace data_type_conversion

} // end of namespace mros

#endif // DATA_TYPE_CONVERSION_HPP


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NQS_SGD_HPP
#define NQS_SGD_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <iostream>
#include "abstract_optimizer.hpp"

namespace nqs {

class Sgd : public AbstractOptimizer {
  // decay constant
  double eta_;

  int npar_;

  double l2reg_;

  double decay_factor_;

 public:
  explicit Sgd(double learning_rate = 0.001, double l2reg = 0,
               double decay_factor = 1.0)
      : eta_(learning_rate), l2reg_(l2reg) {
    npar_ = -1;
    SetDecayFactor(decay_factor);
    PrintParameters();
  }

  void PrintParameters() {
    InfoMessage() << "Sgd optimizer initialized with these parameters :"
                  << std::endl;
    InfoMessage() << "Learning Rate = " << eta_ << std::endl;
    InfoMessage() << "L2 Regularization = " << l2reg_ << std::endl;
    InfoMessage() << "Decay Factor = " << decay_factor_ << std::endl;
  }

  void Init(int npar) override { npar_ = npar; }

  void Update(const Eigen::VectorXd &grad,
              Eigen::Ref<Eigen::VectorXd> pars) override {
    assert(npar_ > 0);

    eta_ *= decay_factor_;

    for (int i = 0; i < npar_; i++) {
      pars(i) = pars(i) - (grad(i) + l2reg_ * pars(i)) * eta_;
    }
  }

  void SetDecayFactor(double decay_factor) {
    assert(decay_factor <= 1.00001);
    decay_factor_ = decay_factor;
  }

  void Reset() override {}
};

}  // namespace nqs

#endif


//
// Created by redwards on 10/3/18.
// Process every file in the directory and make a count of all the hits
#include <boost/filesystem.hpp>
#include <boost/range/iterator_range.hpp>
#include <iostream>
#include <fstream>
#include <string>
#include <cstring>
#include <sstream>
#include <map>
#include <gzstream>
using namespace std;

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

    if ( argc < 2) {
        cerr << "Usage: " << argv[0] << " <directory to process>\n";
        return 1;
    }

    if (! boost::filesystem::is_directory(argv[1])) {
        cerr << "Please provide a directory with all the files to summarize" << endl;
        return 1;
    }

    map<string, float> counts;
    map<string, int> total;
    for(auto& filename : boost::make_iterator_range(boost::filesystem::directory_iterator(argv[1]), {})) {
        ifstream reader;
        reader.open(filename.path().string());
        if (!reader) {
            cerr << "Unable to open the input file" << filename << endl;
            return 1;
        }
        float c; string peg;
        // read each line and split into two tokens
        while (reader >> peg >> c) {
            counts[peg] += c;
            total[peg]++;
        }
        reader.close();
    }

    for( auto const& x : counts )
    {
        cout << x.first << " " << (float) x.second/ (float) total[x.first] << " " << total[x.first] << endl;
    }

}


#include <nlohmann/json.hpp>
#include <iostream>
#include "gtest/gtest.h"
#include <Eigen/Dense>
#include "json.h"

TEST(JsonTest, MatTest){
  Eigen::MatrixXd test = Eigen::MatrixXd::Random(3, 3);

  nlohmann::json j = test;
  Eigen::MatrixXd test2 = j;

  for (int i = 0; i < test.rows(); i++){
    for (int j = 0; j < test.cols(); j++){
      EXPECT_EQ(test(i, j), test2(i, j));
    }
  }
};

TEST(JsonTest, SizeTest){
  nlohmann::json j;
  std::cout << j.size() << std::endl;

  int test = 1;
  j.push_back({"test", test});
  j.push_back({"test2", test});

  std::cout << j.size() << std::endl;
  std::cout << j[0] << std::endl;
  std::cout << j[1] << std::endl;
}