#include "learning/learningIO_bin.h"
#include <learning/learningMath.h>
#include <processing/graphCut.h>
#include <util/helper.h>
#include <cstdlib>
#include <ctime>
#include <CGAL/Random.h>
#include <boost/filesystem.hpp>

#include <xtensor/xbuilder.hpp>
#include <xtensor/xmath.hpp>
#include <xtensor-io/xnpz.hpp>

namespace fs = boost::filesystem;

///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////// EXPORT FROM POISSON INPUT ///////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
void graphExporter::cellLabel(Delaunay::All_cells_iterator& fci){

    int i = fci->info().global_idx;
    infinite_[i] = Dt_.is_infinite(fci);

    if(options_.ground_truth){
        inside_perc_[i] = fci->info().inside_score;
        outside_perc_[i] = fci->info().outside_score;
    }

}
void graphExporter::cellGeometricFeatures(Delaunay::All_cells_iterator& fci){

    int i = fci->info().global_idx;
    if(!Dt_.is_infinite(fci)){
        auto tet = Dt_.tetrahedron(fci);
        tetFeatures tf = calcTetFeatures(tet);
        radius_[i] = tf.radius;
        vol_[i] = tf.vol;
        longest_edge_[i] = tf.longest_edge;
        shortest_edge_[i] = tf.shortest_edge;
    }
}

void graphExporter::cellVertexFeatures(Delaunay::All_cells_iterator& fci){

//    cbvf << "cb_vertex_inside_count cb_vertex_inside_dist_min cb_vertex_inside_dist_max cb_vertex_inside_dist_sum ";
//    cbvf << "cb_vertex_outside_count cb_vertex_outside_dist_min cb_vertex_outside_dist_max cb_vertex_outside_dist_sum ";
//    cbvf << "cb_vertex_last_count cb_vertex_last_dist_min cb_vertex_last_dist_max cb_vertex_last_dist_sum" << endl;

    // inside
    int i = fci->info().global_idx;
    int vdi = fci->info().cb_vertex_inside.size();
    cb_vertex_inside_count_[i] = vdi;
    if(vdi > 0){
        cb_vertex_inside_dist_min_[i] = *min_element(fci->info().cb_vertex_inside.begin(), fci->info().cb_vertex_inside.end());
        cb_vertex_inside_dist_max_[i] = *max_element(fci->info().cb_vertex_inside.begin(), fci->info().cb_vertex_inside.end());
        cb_vertex_inside_dist_sum_[i] = accumulate(fci->info().cb_vertex_inside.begin(), fci->info().cb_vertex_inside.end(),0.0);
    }

    // outside
    int vdo = fci->info().cb_vertex_outside.size();
    cb_vertex_outside_count_[i] = vdo;
    if(vdo > 0){
        cb_vertex_outside_dist_min_[i] = *min_element(fci->info().cb_vertex_outside.begin(), fci->info().cb_vertex_outside.end());
        cb_vertex_outside_dist_max_[i] = *max_element(fci->info().cb_vertex_outside.begin(), fci->info().cb_vertex_outside.end());
        cb_vertex_outside_dist_sum_[i] = accumulate(fci->info().cb_vertex_outside.begin(), fci->info().cb_vertex_outside.end(),0.0);
    }

    // last
    int vdl = fci->info().cb_vertex_last.size();
    cb_vertex_last_count_[i] = vdl;
    if(vdl > 0){
        cb_vertex_last_dist_min_[i] = *min_element(fci->info().cb_vertex_last.begin(), fci->info().cb_vertex_last.end());
        cb_vertex_last_dist_max_[i] = *max_element(fci->info().cb_vertex_last.begin(), fci->info().cb_vertex_last.end());
        cb_vertex_last_dist_sum_[i] = accumulate(fci->info().cb_vertex_last.begin(), fci->info().cb_vertex_last.end(),0.0);
    }
}

void graphExporter::cellFacetFeatures(Delaunay::All_cells_iterator& fci){

//    cbff << "cb_facet_inside_first_count cb_facet_inside_first_dist_min cb_facet_inside_first_dist_max cb_facet_inside_first_dist_sum ";
//    cbff << "cb_facet_inside_second_count cb_facet_inside_second_dist_min cb_facet_inside_second_dist_max cb_facet_inside_second_dist_sum ";
//    cbff << "cb_facet_outside_first_count cb_facet_outside_first_dist_min cb_facet_outside_first_dist_max cb_facet_outside_first_dist_sum ";
//    cbff << "cb_facet_outside_second_count cb_facet_outside_second_dist_min cb_facet_outside_second_dist_max cb_facet_outside_second_dist_sum ";
//    cbff << "cb_facet_last_first_count cb_facet_last_first_dist_min cb_facet_last_first_dist_max cb_facet_last_first_dist_sum" << endl;
//    cbff << "cb_facet_last_second_count cb_facet_last_second_dist_min cb_facet_last_second_dist_max cb_facet_last_second_dist_sum" << endl;

    // inside first
    int i = fci->info().global_idx;
    int fdif = fci->info().cb_facet_inside_first.size();
    cb_facet_inside_first_count_[i] = fdif;
    if(fdif > 0){
        cb_facet_inside_first_dist_min_[i] = *min_element(fci->info().cb_facet_inside_first.begin(), fci->info().cb_facet_inside_first.end());
        cb_facet_inside_first_dist_max_[i] = *max_element(fci->info().cb_facet_inside_first.begin(), fci->info().cb_facet_inside_first.end());
        cb_facet_inside_first_dist_sum_[i] = accumulate(fci->info().cb_facet_inside_first.begin(), fci->info().cb_facet_inside_first.end(),0.0);
    }
    // inside second
    int fdis = fci->info().cb_facet_inside_second.size();
    cb_facet_inside_second_count_[i] = fdis;
    if(fdis > 0){
        cb_facet_inside_second_dist_min_[i] = *min_element(fci->info().cb_facet_inside_second.begin(), fci->info().cb_facet_inside_second.end());
        cb_facet_inside_second_dist_max_[i] = *max_element(fci->info().cb_facet_inside_second.begin(), fci->info().cb_facet_inside_second.end());
        cb_facet_inside_second_dist_sum_[i] = accumulate(fci->info().cb_facet_inside_second.begin(), fci->info().cb_facet_inside_second.end(),0.0);
    }
    // outside first
    int fdof = fci->info().cb_facet_outside_first.size();
    cb_facet_outside_first_count_[i] = fdof;
    if(fdof > 0){
        cb_facet_outside_first_dist_min_[i] = *min_element(fci->info().cb_facet_outside_first.begin(), fci->info().cb_facet_outside_first.end());
        cb_facet_outside_first_dist_max_[i] = *max_element(fci->info().cb_facet_outside_first.begin(), fci->info().cb_facet_outside_first.end());
        cb_facet_outside_first_dist_sum_[i] = accumulate(fci->info().cb_facet_outside_first.begin(), fci->info().cb_facet_outside_first.end(),0.0);
    }
    // outside second
    int fdos = fci->info().cb_facet_outside_second.size();
    cb_facet_outside_second_count_[i] = fdos;
    if(fdos > 0){
        cb_facet_outside_second_dist_min_[i] = *min_element(fci->info().cb_facet_outside_second.begin(), fci->info().cb_facet_outside_second.end());
        cb_facet_outside_second_dist_max_[i] = *max_element(fci->info().cb_facet_outside_second.begin(), fci->info().cb_facet_outside_second.end());
        cb_facet_outside_second_dist_sum_[i] = accumulate(fci->info().cb_facet_outside_second.begin(), fci->info().cb_facet_outside_second.end(),0.0);
    }
    // last first
    int fdlf = fci->info().cb_facet_last_first.size();
    cb_facet_last_first_count_[i] = fdlf;
    if(fdlf > 0){
        cb_facet_last_first_dist_min_[i] = *min_element(fci->info().cb_facet_last_first.begin(), fci->info().cb_facet_last_first.end());
        cb_facet_last_first_dist_max_[i] = *max_element(fci->info().cb_facet_last_first.begin(), fci->info().cb_facet_last_first.end());
        cb_facet_last_first_dist_sum_[i] = accumulate(fci->info().cb_facet_last_first.begin(), fci->info().cb_facet_last_first.end(),0.0);
    }
    // last second
    int fdls = fci->info().cb_facet_last_second.size();
    cb_facet_last_second_count_[i] = fdls;
    if(fdls > 0){
        cb_facet_last_second_dist_min_[i] = *min_element(fci->info().cb_facet_last_second.begin(), fci->info().cb_facet_last_second.end());
        cb_facet_last_second_dist_max_[i] = *max_element(fci->info().cb_facet_last_second.begin(), fci->info().cb_facet_last_second.end());
        cb_facet_last_second_dist_sum_[i] = accumulate(fci->info().cb_facet_last_second.begin(), fci->info().cb_facet_last_second.end(),0.0);
    }
}

void graphExporter::facetGeometricFeatures(Delaunay::All_cells_iterator& fci, int j, int edge_count){

//    fgeom << "area angle beta dist" << endl;
    // export the area and angle off the current facet as edge feature
    auto fac = make_pair(fci,j);
    area_[edge_count] = computeFacetArea(Dt_,fac);
    angle_[edge_count] = computeCosFacetCellAngle(Dt_,fac);
    beta_[edge_count] = 1-min(computeCosFacetCellAngle(Dt_, fac),
                             computeCosFacetCellAngle(Dt_, Dt_.mirror_facet(fac)));

    auto neighbor = fci->neighbor(j);
    if(!Dt_.is_infinite(neighbor) && !Dt_.is_infinite(fci)){
        auto cen1 = CGAL::centroid(fci->vertex(0)->point(),
                                   fci->vertex(1)->point(),
                                   fci->vertex(2)->point(),
                                   fci->vertex(3)->point());
        auto cen2 = CGAL::centroid(neighbor->vertex(0)->point(),
                       neighbor->vertex(1)->point(),
                       neighbor->vertex(2)->point(),
                       neighbor->vertex(3)->point());
        dist_[edge_count] = CGAL::squared_distance(cen1,cen2);
    }
    else
        dist_[edge_count] = -1.0;
}
void graphExporter::facetVertexFeatures(Delaunay::All_cells_iterator& fci, int j, int edge_count){

//    fbvf << "fb_vertex_inside_count fb_vertex_inside_dist_min fb_vertex_inside_dist_max fb_vertex_inside_dist_sum ";
//    fbvf << "fb_vertex_outside_count fb_vertex_outside_dist_min fb_vertex_outside_dist_max fb_vertex_outside_dist_sum" << endl;
//    fbvf << "fb_vertex_last_count fb_vertex_last_dist_min fb_vertex_last_dist_max fb_vertex_last_dist_sum" << endl;

    // inside
    int vdi = fci->info().fb_vertex_inside[j].size();
    fb_vertex_inside_count_[edge_count] = vdi;
    if(vdi > 0){
        fb_vertex_inside_dist_min_[edge_count] = *min_element(fci->info().fb_vertex_inside[j].begin(), fci->info().fb_vertex_inside[j].end());
        fb_vertex_inside_dist_max_[edge_count] = *max_element(fci->info().fb_vertex_inside[j].begin(), fci->info().fb_vertex_inside[j].end());
        fb_vertex_inside_dist_sum_[edge_count] = accumulate(fci->info().fb_vertex_inside[j].begin(), fci->info().fb_vertex_inside[j].end(),0.0);

    }
    // outside
    int vdo = fci->info().fb_vertex_outside[j].size();
    fb_vertex_outside_count_[edge_count] = vdo;
    if(vdo > 0){
        fb_vertex_outside_dist_min_[edge_count] = *min_element(fci->info().fb_vertex_outside[j].begin(), fci->info().fb_vertex_outside[j].end());
        fb_vertex_outside_dist_max_[edge_count] = *max_element(fci->info().fb_vertex_outside[j].begin(), fci->info().fb_vertex_outside[j].end());
        fb_vertex_outside_dist_sum_[edge_count] = accumulate(fci->info().fb_vertex_outside[j].begin(), fci->info().fb_vertex_outside[j].end(),0.0);
    }
    // last
    int vdl = fci->info().fb_vertex_last[j].size();
    fb_vertex_last_count_[edge_count] = vdl;
    if(vdl > 0){
        fb_vertex_last_dist_min_[edge_count] = *min_element(fci->info().fb_vertex_last[j].begin(), fci->info().fb_vertex_last[j].end());
        fb_vertex_last_dist_max_[edge_count] = *max_element(fci->info().fb_vertex_last[j].begin(), fci->info().fb_vertex_last[j].end());
        fb_vertex_last_dist_sum_[edge_count] = accumulate(fci->info().fb_vertex_last[j].begin(), fci->info().fb_vertex_last[j].end(),0.0);
    }
}

void graphExporter::facetFacetFeatures(Delaunay::All_cells_iterator& fci, int j, int edge_count){

//    fbff << "fb_facet_inside_count fb_facet_inside_dist_min fb_facet_inside_dist_max fb_facet_inside_dist_sum ";
//    fbff << "fb_facet_outside_count fb_facet_outside_dist_min fb_facet_outside_dist_max fb_facet_outside_dist_sum" << endl;
//    fbff << "fb_facet_last_count fb_facet_last_dist_min fb_facet_last_dist_max fb_facet_last_dist_sum" << endl;

    // inside
    int vdi = fci->info().fb_facet_inside[j].size();
    fb_facet_inside_count_[edge_count] = vdi;
    if(vdi > 0){
        fb_facet_inside_dist_min_[edge_count] = *min_element(fci->info().fb_facet_inside[j].begin(), fci->info().fb_facet_inside[j].end());
        fb_facet_inside_dist_max_[edge_count] = *max_element(fci->info().fb_facet_inside[j].begin(), fci->info().fb_facet_inside[j].end());
        fb_facet_inside_dist_sum_[edge_count] = accumulate(fci->info().fb_facet_inside[j].begin(), fci->info().fb_facet_inside[j].end(),0.0);
    }
    // outside
    int vdo = fci->info().fb_facet_outside[j].size();
    fb_facet_outside_count_[edge_count] = vdo;
    if(vdo > 0){
        fb_facet_outside_dist_min_[edge_count] = *min_element(fci->info().fb_facet_outside[j].begin(), fci->info().fb_facet_outside[j].end());
        fb_facet_outside_dist_max_[edge_count] = *max_element(fci->info().fb_facet_outside[j].begin(), fci->info().fb_facet_outside[j].end());
        fb_facet_outside_dist_sum_[edge_count] = accumulate(fci->info().fb_facet_outside[j].begin(), fci->info().fb_facet_outside[j].end(),0.0);
    }
    // last
    int vdl = fci->info().fb_facet_last[j].size();
    fb_facet_last_count_[edge_count] = vdl;
    if(vdl > 0){
        fb_facet_last_dist_min_[edge_count] = *min_element(fci->info().fb_facet_last[j].begin(), fci->info().fb_facet_last[j].end());
        fb_facet_last_dist_max_[edge_count] = *max_element(fci->info().fb_facet_last[j].begin(), fci->info().fb_facet_last[j].end());
        fb_facet_last_dist_sum_[edge_count] = accumulate(fci->info().fb_facet_last[j].begin(), fci->info().fb_facet_last[j].end(),0.0);
    }
}

void graphExporter::run(bool compress){

    auto start = std::chrono::high_resolution_clock::now();
    cout << "\nExport graph..." << endl;
    cout << "\t-to " << "dgnn/"+outfile_+"_X.npz" << endl;
//    string rays = (options_.export_rays) ? "\t-with rays" : "\t-without rays";
//    cout << rays << endl;

    /////////////////////////////////////////////
    //////////////////// CELLS //////////////////
    /////////////////////////////////////////////
    ////// info file
    ofstream info;
    info.open(dir_.path+"dgnn/"+outfile_+"_info.txt");
    info << "vertices " << nv_ << endl;
    info << "facets " << nf_ << endl;
    info << "cells " << nc_ << endl;
    info.close();

    int j, i, edge_count=0;

    Delaunay::All_cells_iterator fci;
    for(fci = Dt_.all_cells_begin(); fci != Dt_.all_cells_end(); fci++){

        i = fci->info().global_idx;

        /////////////////////////////////////////////
        //////////////     LABELS    ////////////////
        /////////////////////////////////////////////
        cellLabel(fci);

        /////////////////////////////////////////////
        ////////////// CELL FEATURES ////////////////
        /////////////////////////////////////////////
        cellGeometricFeatures(fci);
        cellVertexFeatures(fci);
        cellFacetFeatures(fci);

        /////////////////////////////////////////////
        ////////////// EDGE FEATURES ////////////////
        /////////////////////////////////////////////
        for(int c = 0; c < 4; c++){
            // set the edge to the mirror cell
            j = fci->neighbor(c)->info().global_idx;

            if(j == i) // skip self loops
                continue;

            adjacencies_(edge_count,0) = i;
            adjacencies_(edge_count,1) = j;

//            adjacency_ij << i << " ";
//            adjacency_ji << j << " ";

            //// geometric features
            facetGeometricFeatures(fci,c,edge_count);
            facetVertexFeatures(fci,c,edge_count);
            facetFacetFeatures(fci,c,edge_count);

            edge_count++;
        }
    }

    string ilf = dir_.path+"dgnn/"+outfile_+"_labels.npz";
    xt::dump_npz(ilf,"infinite",infinite_,compress,false);
    if(options_.ground_truth){
        xt::dump_npz(ilf,"inside_perc",inside_perc_,compress,true);
        xt::dump_npz(ilf,"outside_perc",outside_perc_,compress,true);
    }

    string cgeom = dir_.path+"dgnn/"+outfile_+"_cgeom.npz";
    xt::dump_npz(cgeom,"radius",radius_,compress,false);
    xt::dump_npz(cgeom,"vol",vol_,compress,true);
    xt::dump_npz(cgeom,"longest_edge",longest_edge_,compress,true);
    xt::dump_npz(cgeom,"shortest_edge",shortest_edge_,compress,true);

    ////// cell based vertex features
    string cvff = dir_.path+"dgnn/"+outfile_+"_cbvf.npz";
    xt::dump_npz(cvff,"cb_vertex_inside_count",cb_vertex_inside_count_,compress,false);
    xt::dump_npz(cvff,"cb_vertex_inside_dist_min",cb_vertex_inside_dist_min_,compress,true);
    xt::dump_npz(cvff,"cb_vertex_inside_dist_max",cb_vertex_inside_dist_max_,compress,true);
    xt::dump_npz(cvff,"cb_vertex_inside_dist_sum",cb_vertex_inside_dist_sum_,compress,true);

    xt::dump_npz(cvff,"cb_vertex_outside_count",cb_vertex_outside_count_,compress,true);
    xt::dump_npz(cvff,"cb_vertex_outside_dist_min",cb_vertex_outside_dist_min_,compress,true);
    xt::dump_npz(cvff,"cb_vertex_outside_dist_max",cb_vertex_outside_dist_max_,compress,true);
    xt::dump_npz(cvff,"cb_vertex_outside_dist_sum",cb_vertex_outside_dist_sum_,compress,true);

    xt::dump_npz(cvff,"cb_vertex_last_count",cb_vertex_last_count_,compress,true);
    xt::dump_npz(cvff,"cb_vertex_last_dist_min",cb_vertex_last_dist_min_,compress,true);
    xt::dump_npz(cvff,"cb_vertex_last_dist_max",cb_vertex_last_dist_max_,compress,true);
    xt::dump_npz(cvff,"cb_vertex_last_dist_sum",cb_vertex_last_dist_sum_,compress,true);

    ////// cell based facet features
    string cbff = dir_.path+"dgnn/"+outfile_+"_cbff.npz";
    xt::dump_npz(cbff,"cb_facet_inside_first_count",cb_facet_inside_first_count_,compress,false);
    xt::dump_npz(cbff,"cb_facet_inside_first_dist_min",cb_facet_inside_first_dist_min_,compress,true);
    xt::dump_npz(cbff,"cb_facet_inside_first_dist_max",cb_facet_inside_first_dist_max_,compress,true);
    xt::dump_npz(cbff,"cb_facet_inside_first_dist_sum",cb_facet_inside_first_dist_sum_,compress,true);

    xt::dump_npz(cbff,"cb_facet_inside_second_count",cb_facet_inside_second_count_,compress,true);
    xt::dump_npz(cbff,"cb_facet_inside_second_dist_min",cb_facet_inside_second_dist_min_,compress,true);
    xt::dump_npz(cbff,"cb_facet_inside_second_dist_max",cb_facet_inside_second_dist_max_,compress,true);
    xt::dump_npz(cbff,"cb_facet_inside_second_dist_sum",cb_facet_inside_second_dist_sum_,compress,true);

    xt::dump_npz(cbff,"cb_facet_outside_first_count",cb_facet_outside_first_count_,compress,true);
    xt::dump_npz(cbff,"cb_facet_outside_first_dist_min",cb_facet_outside_first_dist_min_,compress,true);
    xt::dump_npz(cbff,"cb_facet_outside_first_dist_max",cb_facet_outside_first_dist_max_,compress,true);
    xt::dump_npz(cbff,"cb_facet_outside_first_dist_sum",cb_facet_outside_first_dist_sum_,compress,true);

    xt::dump_npz(cbff,"cb_facet_outside_second_count",cb_facet_outside_second_count_,compress,true);
    xt::dump_npz(cbff,"cb_facet_outside_second_dist_min",cb_facet_outside_second_dist_min_,compress,true);
    xt::dump_npz(cbff,"cb_facet_outside_second_dist_max",cb_facet_outside_second_dist_max_,compress,true);
    xt::dump_npz(cbff,"cb_facet_outside_second_dist_sum",cb_facet_outside_second_dist_sum_,compress,true);

    xt::dump_npz(cbff,"cb_facet_last_first_count",cb_facet_last_first_count_,compress,true);
    xt::dump_npz(cbff,"cb_facet_last_first_dist_min",cb_facet_last_first_dist_min_,compress,true);
    xt::dump_npz(cbff,"cb_facet_last_first_dist_max",cb_facet_last_first_dist_max_,compress,true);
    xt::dump_npz(cbff,"cb_facet_last_first_dist_sum",cb_facet_last_first_dist_sum_,compress,true);

    xt::dump_npz(cbff,"cb_facet_last_second_count",cb_facet_last_second_count_,compress,true);
    xt::dump_npz(cbff,"cb_facet_last_second_dist_min",cb_facet_last_second_dist_min_,compress,true);
    xt::dump_npz(cbff,"cb_facet_last_second_dist_max",cb_facet_last_second_dist_max_,compress,true);
    xt::dump_npz(cbff,"cb_facet_last_second_dist_sum",cb_facet_last_second_dist_sum_,compress,true);

    /////////////////////////////////////////////
    //////////////////// EDGES //////////////////
    /////////////////////////////////////////////
    string adjf = dir_.path+"dgnn/"+outfile_+"_adjacencies.npz";
    xt::dump_npz(adjf,"adjacencies",adjacencies_,compress,false);

    // geometric features
    string fgeom = dir_.path+"dgnn/"+outfile_+"_fgeom.npz";
    xt::dump_npz(fgeom,"area",area_,compress,false);
    xt::dump_npz(fgeom,"angle",angle_,compress,true);
    xt::dump_npz(fgeom,"beta",beta_,compress,true);
    xt::dump_npz(fgeom,"dist",dist_,compress,true);

    // facet based vertex features
    string fbvf = dir_.path+"dgnn/"+outfile_+"_fbvf.npz";
    xt::dump_npz(fbvf,"fb_vertex_inside_count",fb_vertex_inside_count_,compress,false);
    xt::dump_npz(fbvf,"fb_vertex_inside_dist_min",fb_vertex_inside_dist_min_,compress,true);
    xt::dump_npz(fbvf,"fb_vertex_inside_dist_max",fb_vertex_inside_dist_max_,compress,true);
    xt::dump_npz(fbvf,"fb_vertex_inside_dist_sum",fb_vertex_inside_dist_sum_,compress,true);

    xt::dump_npz(fbvf,"fb_vertex_outside_count",fb_vertex_outside_count_,compress,true);
    xt::dump_npz(fbvf,"fb_vertex_outside_dist_min",fb_vertex_outside_dist_min_,compress,true);
    xt::dump_npz(fbvf,"fb_vertex_outside_dist_max",fb_vertex_outside_dist_max_,compress,true);
    xt::dump_npz(fbvf,"fb_vertex_outside_dist_sum",fb_vertex_outside_dist_sum_,compress,true);

    xt::dump_npz(fbvf,"fb_vertex_last_count",fb_vertex_last_count_,compress,true);
    xt::dump_npz(fbvf,"fb_vertex_last_dist_min",fb_vertex_last_dist_min_,compress,true);
    xt::dump_npz(fbvf,"fb_vertex_last_dist_max",fb_vertex_last_dist_max_,compress,true);
    xt::dump_npz(fbvf,"fb_vertex_last_dist_sum",fb_vertex_last_dist_sum_,compress,true);

    // facet based facet features
    string fbff = dir_.path+"dgnn/"+outfile_+"_fbff.npz";
    xt::dump_npz(fbff,"fb_facet_inside_count",fb_facet_inside_count_,compress,false);
    xt::dump_npz(fbff,"fb_facet_inside_dist_min",fb_facet_inside_dist_min_,compress,true);
    xt::dump_npz(fbff,"fb_facet_inside_dist_max",fb_facet_inside_dist_max_,compress,true);
    xt::dump_npz(fbff,"fb_facet_inside_dist_sum",fb_facet_inside_dist_sum_,compress,true);

    xt::dump_npz(fbff,"fb_facet_outside_count",fb_facet_outside_count_,compress,true);
    xt::dump_npz(fbff,"fb_facet_outside_dist_min",fb_facet_outside_dist_min_,compress,true);
    xt::dump_npz(fbff,"fb_facet_outside_dist_max",fb_facet_outside_dist_max_,compress,true);
    xt::dump_npz(fbff,"fb_facet_outside_dist_sum",fb_facet_outside_dist_sum_,compress,true);

    xt::dump_npz(fbff,"fb_facet_last_count",fb_facet_last_count_,compress,true);
    xt::dump_npz(fbff,"fb_facet_last_dist_min",fb_facet_last_dist_min_,compress,true);
    xt::dump_npz(fbff,"fb_facet_last_dist_max",fb_facet_last_dist_max_,compress,true);
    xt::dump_npz(fbff,"fb_facet_last_dist_sum",fb_facet_last_dist_sum_,compress,true);

    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>(stop - start);
    cout << "\t-Exported " << i+1 << " nodes and " << edge_count << " edges" << endl;
    cout << "\t-in " << duration.count() << "s" << endl;
}

bool importOccPoints(dirHolder dir, dataHolder& data){

    fs::path path = fs::path(dir.path) / fs::path(dir.occ_file).replace_extension(".npz");
    path = path.lexically_normal();
    std::ifstream file(path.string());

    cout << "\nLoad occupancy points..." << endl;
    cout << "\t-from " << path.string() << endl;

    // load the occupancy points here and put them in a new vector
    // called data.gt_points

    if(!file){
        cout << "\nFILE DOES NOT EXIST OR IS EMPTY!" << endl;
        return 1;
    }
    auto a = xt::load_npz(path.string());
    if(a.find("points") == a.end()){
        cout << "\nERROR: No points found in NPZ file!" << endl;
        return 1;
    }
    if(a.find("occupancies") == a.end()){
        cout << "\nERROR: No occupancies found in NPZ file!" << endl;
        return 1;
    }

    data.xgt_points = a["points"].cast<float>();
    data.xgt_occupancies = a["occupancies"].cast<uint8_t>();

//    data.gt_points.clear();
//    data.gt_infos.clear();
//    Point p;
//    for(int i = 0; i < points.shape()[0]; i++){
//        p = Point(points(i,0),points(i,1),points(i,2));
//        data.gt_points.push_back(p);
//    }

    return 0;

}
bool point2TetraIndex(dataHolder& data){

    Point p;
    Cell_handle c;
    data.xgt_point2tet.clear();
//    assert(data.xgt_point2tet.size()==0);
    for(int i = 0; i < data.xgt_points.shape()[0]; i++){
        p = Point(data.xgt_points(i,0),data.xgt_points(i,1),data.xgt_points(i,2));
        c = data.Dt.locate(p);
        data.xgt_point2tet.push_back(c->info().global_idx);
    }

};
bool exportOccPoints(dirHolder dir, dataHolder& data){

    // make my own
    fs::path p(dir.write_file);
    string outfile = p.stem().string()+"_eval";

    fs::path path = fs::path(dir.path) / fs::path("dgnn") / fs::path(outfile).replace_extension(".npz");
    path = path.lexically_normal();

    cout << "\nExport GT points..." << endl;
//    cout << "\t-to " << path.string() << endl;
    cout << "\t-to " << "dgnn/"+outfile+".npz" << endl;

    bool compress=true;

    xt::dump_npz(path.string(),"points",data.xgt_points,compress,false);
    xt::dump_npz(path.string(),"occupancies",data.xgt_occupancies,compress,true);
    xt::dump_npz(path.string(),"tetIndex",xt::adapt(data.xgt_point2tet),compress,true);

    return 0;

}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////// IMPORT ////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////
bool importPrediction(dirHolder dir, dataHolder& data, runningOptions options){

    fs::path path = fs::path(dir.path) / fs::path(dir.prediction_file).replace_extension(".npz");
    path = path.lexically_normal();
    ifstream file(path.string());

    cout << "\nLoad prediction score..." << endl;
    cout << "\t-from " << path.string() << endl;

    if(!file){
        cout << "\nFILE DOES NOT EXIST OR IS EMPTY!" << endl;
        return 1;
    }
    auto npz_map = xt::load_npz(path.string());

    Delaunay::All_cells_iterator fci;
    int b = data.Dt.number_of_cells();
    auto nc = npz_map["number_of_cells"].cast<long>();
    const int a = nc(0,0);
    // check for same sampling of input and scan
    if(a != b){
        cout << "ERROR: SAMPLING IS NOT THE SAME!" << endl;
        cout << "\t-" << b << " Delaunay cells" << endl;
        cout << "\t-" << a << " imported cells" << endl;
        return 1;
    }

    int i = 0;
    array<size_t, 2> shape = { a, 2 };
    xt::xtensor<float, 2> pred(shape);
    if(options.occupancy_type == "lo"){
        pred = npz_map["logits"].cast<float>();
    }
    else if(options.occupancy_type == "so"){
        pred = npz_map["softmax"].cast<float>();
    }
    else if(options.occupancy_type == "si"){
        pred = npz_map["sigmoid"].cast<float>();
    }
    else{
        cout << options.occupancy_type << " is not a valid occupancy type, choose either lo or sm" << endl;
        return 1;
    }
    for(fci = data.Dt.all_cells_begin(); fci != data.Dt.all_cells_end(); fci++){

        if(data.Dt.is_infinite(fci)){
            fci->info().outside_score = 1.0;
            fci->info().inside_score = 0.0;
        }
        else{

            // check if there is a camera inside the cell
            for(auto s = data.sensor_map.begin(); s != data.sensor_map.end(); s++){
                if(data.Dt.tetrahedron(fci).has_on_positive_side(s->second)){
                    fci->info().outside_score = 1000;
                    fci->info().inside_score = -1000;
                    break;
                }
            }

//            xt::xtensor_fixed<double, xt::xshape<a, 2>> pred;
            if(pred.shape(1)==1){
//                cout << pred(i,0) << endl;
                if(pred(i,0)<=0.0){
                    fci->info().outside_score = 0;
                    fci->info().inside_score = 1;
                }
                else{
                    fci->info().outside_score = 1;
                    fci->info().inside_score = 0;
                }
            }
            else{
                fci->info().inside_score = pred(i,0);
                fci->info().outside_score = pred(i,1);
                // TODO: maybe it should be
                // fci->info().inside_score = pred(i,0) - pred(i,1);
                // fci->info().outside_score = pred(i,1) - pred(i,0);
                // because in the case of logits pred(i,0) = 1 - pred(i,1)
                // UPDATE: in fact it doesn't matter! what matters in  the graph cut is the difference between the two labelling options.
            }
        }
        i++;

    }
    cout<< "\t-read " << a << " cell scores. " << endl;

    return 0;
}


/*
 * Copyright 2019,
 * LAAS-CNRS,
 * Gepetto team
 *
 * This file is part of sot-talos-balance.
 * See license file
 */

#ifndef __sot_talos_balance_dcm_estimator_H__
#define __sot_talos_balance_dcm_estimator_H__

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined(WIN32)
#if defined(dcm_estimator_EXPORTS)
#define DCMESTIMATOR_EXPORT __declspec(dllexport)
#else
#define DCMESTIMATOR_EXPORT __declspec(dllimport)
#endif
#else
#define DCMESTIMATOR_EXPORT
#endif

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include <pinocchio/fwd.hpp>
#include <sot/core/robot-utils.hh>
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <map>
#include "boost/assign.hpp"
#include <boost/math/distributions/normal.hpp>  // for normal_distribution

/* Pinocchio */
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/kinematics.hpp>

namespace dynamicgraph {
namespace sot {
namespace talos_balance {

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

class DCMESTIMATOR_EXPORT DcmEstimator : public ::dynamicgraph::Entity {
  typedef pinocchio::SE3 SE3;
  typedef Eigen::Vector2d Vector2;
  typedef Eigen::Vector3d Vector3;
  typedef Eigen::Vector4d Vector4;
  typedef Vector6d Vector6;
  typedef Vector7d Vector7;
  typedef Eigen::Matrix3d Matrix3;
  typedef boost::math::normal normal;

  DYNAMIC_GRAPH_ENTITY_DECL();

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /* --- CONSTRUCTOR ---- */
  DcmEstimator(const std::string& name);

  void init(const double& dt, const std::string& urdfFile);
  /* --- SIGNALS --- */
  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);

  DECLARE_SIGNAL_OUT(c, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(dc, dynamicgraph::Vector);

  /* --- COMMANDS --- */
  /* --- ENTITY INHERITANCE --- */
  virtual void display(std::ostream& os) const;

 protected:
  bool m_initSucceeded;  /// true if the entity has been successfully initialized
  RobotUtilShrPtr m_robot_util;
  pinocchio::Data m_data;    /// Pinocchio robot data
  Eigen::VectorXd m_q_pin;   /// robot configuration according to pinocchio convention
  Eigen::VectorXd m_v_pin;   /// robot velocities according to pinocchio convention
  double m_dt;               /// sampling time step
  pinocchio::Model m_model;  /// Pinocchio robot model

};  // class DCMEstimator

}  // namespace talos_balance
}  // namespace sot
}  // namespace dynamicgraph

#endif  // #ifndef __sot_talos_balance_dcm_estimator_H__


/**
 * \ file OutCircularPointerFilter.cpp
 */

#include <cmath>

#include <boost/math/constants/constants.hpp>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutCircularPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#define PROCESSSIZE (512)

BOOST_AUTO_TEST_CASE( OutCircularPointerFloat_sin1k_test )
{
  std::array<float, PROCESSSIZE*200> data;
  for(gsl::index i = 0; i < PROCESSSIZE*200; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, data.size(), false);
  generator.set_output_sampling_rate(48000);

  ATK::OutCircularPointerFilter<float> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);

  auto nb_iterations = 4 + ATK::OutCircularPointerFilter<float>::slice_size * ATK::OutCircularPointerFilter<float>::nb_slices / PROCESSSIZE;
  auto max_zero = std::max(0, int(ATK::OutCircularPointerFilter<float>::out_slice_size));
  auto offset = 0;

  for(int i = 0; i < nb_iterations; ++i)
  {
    output.process(PROCESSSIZE);
    bool process;
    const auto& outdata = output.get_last_slice(process);

    if(process && i!=0)
    {
      max_zero = std::max(0, int(ATK::OutCircularPointerFilter<float>::out_slice_size - (i+1) * PROCESSSIZE));
      offset = std::max(0, (i+1) * PROCESSSIZE - int(ATK::OutCircularPointerFilter<float>::out_slice_size)) - max_zero ;
    }
    for(gsl::index j = 0; j < max_zero; ++j)
    {
      BOOST_REQUIRE_EQUAL(outdata[j], 0);
    }
    for(gsl::index j = max_zero; j < outdata.size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(outdata[j], data[j + offset]);
    }
  }
}

BOOST_AUTO_TEST_CASE( OutCircularPointerFloat_sin1k_full_setup_test )
{
  std::array<float, PROCESSSIZE*200> data;
  for(gsl::index i = 0; i < PROCESSSIZE*200; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, data.size(), false);
  generator.set_output_sampling_rate(48000);

  ATK::OutCircularPointerFilter<float> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);

  auto nb_iterations = 4 + ATK::OutCircularPointerFilter<float>::slice_size * ATK::OutCircularPointerFilter<float>::nb_slices / PROCESSSIZE;

  for(int i = 0; i < nb_iterations; ++i)
  {
    output.process(PROCESSSIZE);
    output.full_setup();
    bool process;
    const auto& outdata = output.get_last_slice(process);

    for(gsl::index j = 0; j < outdata.size(); ++j)
    {
      BOOST_REQUIRE_EQUAL(outdata[j], 0);
    }
  }
}


/*
 * Copyright (c) 2014, The Robot Studio
 *  All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *	* Redistributions of source code must retain the above copyright notice, this
 *	  list of conditions and the following disclaimer.
 *
 *	* Redistributions in binary form must reproduce the above copyright notice,
 *	  this list of conditions and the following disclaimer in the documentation
 *	  and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Sep 08, 2014
 *      Author: Cyril Jourdan (cyril.jourdan@therobotstudio.com)
 */

/*** Includes ***/
#include <ros/ros.h>
#include <ros/package.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <trs_msgs/MotorCmdSet.h>
#include <trs_msgs/MotorDataSet.h>
#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/Bool.h>
#include "trs_control/getMotorCmdSet.h"
#include "trs_control/switchNode.h"
#include <flann/flann.hpp> //used for the kdtree search
#include <stdio.h>
#include "robotDefines.h"

/*** Defines ***/
#define LOOP_RATE					HEART_BEAT //10 //20
#define SPACE_DIM  					3 //XYZ
#define REF_DATA_DIM 				1 //only one reference point in 3d space to find its nearest neighboor.
#define NUM_NN 						5 //number of nearest neighboors
#define DOF_DIM 					13 //degrees of freedom = number of motors //(no rotator and hand)

using namespace flann;

/*** Variables ***/
bool switch_node = false; //disable by default
trs_msgs::MotorCmdSet motorCmdSet;
int dataset_dim = 0; //this will depend on the size of the bag, number of lines of data

//left arm
Matrix<float> dataset_L_object; //matrix for the object
Matrix<int> dataset_L_positions; //matrix for the positions
Matrix<float> query_L(new float[SPACE_DIM*REF_DATA_DIM], REF_DATA_DIM, SPACE_DIM); //matrix for the query, just one line
Matrix<int> indices_L(new int[query_L.rows*SPACE_DIM], query_L.rows, NUM_NN);
Matrix<float> dists_L(new float[query_L.rows*SPACE_DIM], query_L.rows, NUM_NN);

//right arm
Matrix<float> dataset_R_object; //matrix for the object
Matrix<int> dataset_R_positions; //matrix for the positions
Matrix<float> query_R(new float[SPACE_DIM*REF_DATA_DIM], REF_DATA_DIM, SPACE_DIM); //matrix for the query, just one line
Matrix<int> indices_R(new int[query_R.rows*SPACE_DIM], query_R.rows, NUM_NN);
Matrix<float> dists_R(new float[query_R.rows*SPACE_DIM], query_R.rows, NUM_NN);

/*** Functions ***/
//debug function to display a matrix
void displayMatrixFloat(const Matrix<float> matrix, char* name)
{
	std::cout << name << " matrix :" << std::endl;

	int count = 0;
	for(int i=0; i<matrix.rows; i++)
	{
		std::cout << count << " | ";

		for(int j=0; j<matrix.cols; j++)
		{
			std::cout << matrix.ptr()[matrix.cols*i+j] << "\t";
		}

		count += 1;
		std::cerr << std::endl;
	}
}

void displayMatrixInt(const Matrix<int> matrix, char* name)
{
	std::cout << name << " matrix :" << std::endl;

	int count = 0;
	for(int i=0; i<matrix.rows; i++)
	{
		std::cout << count << " | ";

		for(int j=0; j<matrix.cols; j++)
		{
			std::cout << matrix.ptr()[matrix.cols*i+j] << "\t";
		}

		count += 1;
		std::cerr << std::endl;
	}
}

/*** Callback functions ***/
void objectCoord_cb(const std_msgs::Int32MultiArrayConstPtr& coords)
{
	if(switch_node)
	{
		//for left Arm, following 1st object
		if((coords->data[0] != 0) && (coords->data[1] != 0) && (coords->data[2] != 0))
		{
			//update the query
			for(int i=0; i<query_R.cols; i++) //query cols should be 1
			{
				query_L.ptr()[i] = coords->data[i];
			}

			/*** Left ***/
			// construct a randomized kd-tree index using 4 kd-trees
			Index<L2<float> > data_L_index(dataset_L_object, flann::KDTreeIndexParams(SPACE_DIM));
			data_L_index.buildIndex();

			// do a knn search, using 128 checks
			data_L_index.knnSearch(query_L, indices_L, dists_L, SPACE_DIM, flann::SearchParams(128));
			int data_L_idx = indices_L.ptr()[0];

			ROS_INFO("data_L_idx = %d", data_L_idx);
			displayMatrixFloat(query_L, "Query_L");

			//init of the packet
			for(int i=0; i<DOF_DIM; i++)
			{
				//L
				motorCmdSet.motorCmd[i].nodeID = i+1;
				motorCmdSet.motorCmd[i].mode = 0;
				motorCmdSet.motorCmd[i].value = dataset_L_positions.ptr()[dataset_L_positions.cols*data_L_idx+i];
			}
		}

		//for right Arm, following 2nd object
		if((coords->data[3] != 0) && (coords->data[4] != 0) && (coords->data[5] != 0))
		{
			//update the query
			for(int i=0; i<query_R.cols; i++) //query cols should be 1
			{
				query_R.ptr()[i] = coords->data[SPACE_DIM + i];
			}

			/*** Right ***/
			// construct a randomized kd-tree index using 4 kd-trees
			Index<L2<float> > data_R_index(dataset_R_object, flann::KDTreeIndexParams(SPACE_DIM));
			data_R_index.buildIndex();

			// do a knn search, using 128 checks
			data_R_index.knnSearch(query_R, indices_R, dists_R, SPACE_DIM, flann::SearchParams(128));
			int data_R_idx = indices_R.ptr()[0];

			ROS_INFO("data_R_idx = %d", data_R_idx);
			displayMatrixFloat(query_R, "Query_R");

			//init of the packet
			for(int i=0; i<DOF_DIM; i++)
			{
				//R
				motorCmdSet.motorCmd[NUMBER_MAX_EPOS2_PER_SLAVE+i].nodeID = NUMBER_MAX_EPOS2_PER_SLAVE+i+1;
				motorCmdSet.motorCmd[NUMBER_MAX_EPOS2_PER_SLAVE+i].mode = 0;
				motorCmdSet.motorCmd[NUMBER_MAX_EPOS2_PER_SLAVE+i].value = dataset_R_positions.ptr()[dataset_R_positions.cols*data_R_idx+i];
			}
		}
	}
}

/*** Services ***/
bool switchNode(trs_control::switchNode::Request  &req, trs_control::switchNode::Response &res)
{
	switch_node = req.state;
	return true;
}

bool getKdTreeObjectCmd(trs_control::getMotorCmdSet::Request  &req, trs_control::getMotorCmdSet::Response &res)
{
	//send the cmd set by the callback function
	if(switch_node)
	{
		res.motorCmdSet = motorCmdSet;
		return true;
	}
	else
	{
		return false;
	}
}

/*** Main ***/
int main (int argc, char** argv)
{
	// Initialize ROS
	ros::init (argc, argv, "trs_kdtreeObjectToArms_server_node");
	ros::NodeHandle nh;

	ros::Rate r(LOOP_RATE);

	// Create a ROS subscriber for the input objectCoord topic
	//that will be the requests for the kd-trees
	ros::Subscriber sub_objectCoord = nh.subscribe ("/objectCoord", 10, objectCoord_cb);

	//services
	ros::ServiceServer srv_switchNode = nh.advertiseService("switch_kd_tree_object", switchNode);
	ros::ServiceServer srv_getKdTreeObjecCmd = nh.advertiseService("get_kd_tree_object_cmd", getKdTreeObjectCmd);

	//Initialization of the kd tree
	ROS_INFO("Initialization of the 2 kd-tree :");

	//read a bag to generate 2 datasets, one for each arm
	rosbag::Bag bag(ros::package::getPath("trs_control") + "/bag/objectToPosture_1.bag"); //change this bag
	rosbag::View view_object(bag, rosbag::TopicQuery("/objectCoord")); //object info
	rosbag::View view_posture(bag, rosbag::TopicQuery("/motorDataSet")); //motor data position info

	//create the 2 dataset_object matrix
	dataset_dim = view_object.size(); //set the dataset dim equal to the number of lines in the bag file
	Matrix<float> tempL1(new float[SPACE_DIM*dataset_dim], dataset_dim, SPACE_DIM);
	dataset_L_object = tempL1;
	Matrix<float> tempR1(new float[SPACE_DIM*dataset_dim], dataset_dim, SPACE_DIM);
	dataset_R_object = tempR1;

	ROS_INFO("Create object matrix");
	int line = 0;
	BOOST_FOREACH(rosbag::MessageInstance const m, view_object)
	{
		std_msgs::Int32MultiArray::Ptr i = m.instantiate<std_msgs::Int32MultiArray>();

	    if(i != NULL)
	    {
	    	//i->header.stamp = ros::Time::now();

	    	//Build the right dataset_object matrix
	    	for(int j=0; j<SPACE_DIM; j++)
	    	{
	    		dataset_L_object.ptr()[dataset_L_object.cols*line+j] = i->data[j]; //offset of 3 for left
	    		dataset_R_object.ptr()[dataset_R_object.cols*line+j] = i->data[SPACE_DIM + j];
	    	}
	    }
		else
			std::cout << "null" << std::endl;

	    line++;
	}

	ROS_INFO("Create posture matrix");
	//create the dataset_positions matrix
	int dataset_post_dim = view_posture.size(); //set the dataset dim equal to the number of lines in the bag file
	Matrix<int> tempL2(new int[DOF_DIM*dataset_post_dim], dataset_post_dim, DOF_DIM);
	dataset_L_positions = tempL2;
	Matrix<int> tempR2(new int[DOF_DIM*dataset_post_dim], dataset_post_dim, DOF_DIM);
	dataset_R_positions = tempR2;

	line = 0;
	BOOST_FOREACH(rosbag::MessageInstance const m, view_posture) //error compiles ok
	{
		trs_msgs::MotorDataSet::Ptr i = m.instantiate<trs_msgs::MotorDataSet>();

		if(i != NULL)
		{
			//i->header.stamp = ros::Time::now();

			//Build the dataset_positions matrix
			for(int j=0; j<DOF_DIM; j++)
			{
				dataset_L_positions.ptr()[dataset_L_positions.cols*line+j] = i->motorData[j].encPosition;
				dataset_R_positions.ptr()[dataset_R_positions.cols*line+j] = i->motorData[NUMBER_MAX_EPOS2_PER_SLAVE + j].encPosition;
			}
		}
		else
			std::cout << "null" << std::endl;

		line++;
	}

	bag.close();

	//#ifdef TRS_DEBUG
	displayMatrixFloat(dataset_L_object, "dataset_L_object");
	displayMatrixInt(dataset_L_positions, "dataset_L_positions");
	displayMatrixFloat(dataset_R_object, "dataset_R_object");
	displayMatrixInt(dataset_R_positions, "dataset_R_positions");
	//#endif

	ROS_INFO("Create posture matrix");

	while(ros::ok())
	{
		ros::spinOnce();
		r.sleep();
	}

	//free matrix pointers from memory

	delete[] dataset_L_object.ptr();
	delete[] dataset_L_positions.ptr();
	delete[] query_L.ptr();
	delete[] indices_L.ptr();
	delete[] dists_L.ptr();

	delete[] dataset_R_object.ptr();
	delete[] dataset_R_positions.ptr();
	delete[] query_R.ptr();
	delete[] indices_R.ptr();
	delete[] dists_R.ptr();

	return 0;
}


#include <iostream>
#include <fstream>
#include <iterator>
#include <sstream>
#include <vector>
#include <arpa/inet.h>
#include <my_ip_country_detector.hpp>
#include <boost/asio/ip/address_v6.hpp>
#include <regex>
#include <boost/asio/ip/address.hpp>

//
// Get country name from ip address
//
// data format ref https://lite.ip2location.com/database/ip-country
namespace my_ip_country_detector
{
    std::vector<IpAndCountryInfo> contextV4;
    std::vector<IpAndCountryInfo> contextV6;

    void load_data_from_cvs(std::string filepath, std::vector<IpAndCountryInfo> &output);

    IpAndCountryInfo find(std::vector<IpAndCountryInfo> ips, boost::multiprecision::uint128_t ip);

    void setup_context(std::string filepathV4, std::string filepathV6)
    {
        load_data_from_cvs(filepathV4, contextV4);
        load_data_from_cvs(filepathV6, contextV6);
    }

    std::string find_country_from_ip(std::string ip)
    {
        boost::asio::ip::address a(boost::asio::ip::address::from_string(ip));

        if (a.is_v4())
        {
            return find_country_from_ipv4(ip);
        }
        else
        {
            return find_country_from_ipv6(ip);
        }
    }

    std::string find_dns_from_ip(std::string ip)
    {
        boost::asio::ip::address a(boost::asio::ip::address::from_string(ip));
        if (a.is_v4())
        {
            // v4
            struct sockaddr_in sa; /* input */
            socklen_t len;         /* input */
            char hbuf[NI_MAXHOST];
            memset(&sa, 0, sizeof(struct sockaddr_in));
            sa.sin_family = AF_INET;
            sa.sin_addr.s_addr = inet_addr(ip.c_str());
            len = sizeof(struct sockaddr_in);
            if (getnameinfo((struct sockaddr *)&sa, len, hbuf, sizeof(hbuf),
                            NULL, 0, NI_NAMEREQD))
            {
                return "";
            }
            else
            {
                printf("host=%s\n", hbuf);
                return std::string(hbuf);
            }
        }
        else
        {
            struct sockaddr_in6 sa6;
            char hbuf[NI_MAXHOST];
            memset(&sa6, 0, sizeof(struct sockaddr_in6));

            sa6.sin6_family = AF_INET6;
            in6_addr addr6;
            int s = inet_pton(AF_INET6, ip.c_str(), &addr6);
            sa6.sin6_addr = addr6;

            int len = sizeof(struct sockaddr_in6);
            if (getnameinfo((struct sockaddr *)&sa6, len, hbuf, sizeof(hbuf),
                            NULL, 0, NI_NAMEREQD))
            {
                return "";
            }
            else
            {
                return std::string(hbuf);
            }
        }
    }
    std::string find_country_from_ipv6(std::string ip)
    {
        boost::asio::ip::address_v6 v6 = boost::asio::ip::address_v6::from_string(ip);
        boost::multiprecision::uint128_t val{};
        for (uint8_t b : v6.to_bytes())
        {
            (val <<= 8) |= b;
        }
        return find_country_from_ipv6(val);
    }
    std::string find_country_from_ipv4(std::string ip)
    {
        //std::cout << "(9) <<" << ip << std::endl;
        boost::asio::ip::address_v4 i = boost::asio::ip::address_v4::from_string(ip.c_str());
        //std::cout << "(9) <<" << i.to_ulong() << std::endl;

        //std::cout << boost::multiprecision::uint128_t(i.to_ulong()) << std::endl;
        return find_country_from_ipv4(boost::multiprecision::uint128_t(i.to_ulong()));
    }

    std::string find_country_from_ipv4(boost::multiprecision::uint128_t ip)
    {
        return find(contextV4, ip).country_name;
    }

    std::string find_country_from_ipv6(boost::multiprecision::uint128_t ip)
    {
        return find(contextV6, ip).country_name;
    }

    void load_data_from_cvs(std::string filepath, std::vector<IpAndCountryInfo> &output)
    {
        std::fstream f(filepath);
        std::string l;

        while (std::getline(f, l))
        {
            //std::cout << l << std::endl;
            auto v = create_ip_and_country_info(l);
            output.push_back(v);
        }
    }

    IpAndCountryInfo create_ip_and_country_info(std::string line)
    {
        IpAndCountryInfo ret;
        std::stringstream ss;
        ss << line;
        std::string begin_src;
        std::string end_src;

        std::getline(ss, begin_src, ',');
        std::getline(ss, end_src, ',');
        std::getline(ss, ret.country_name, ',');
        if (ret.country_name.length() > 0 && ret.country_name[0] == '"')
        {
            ret.country_name = ret.country_name.substr(1, ret.country_name.length() - 2);
        }
        if (begin_src.length() > 0 && begin_src[0] == '"')
        {
            begin_src = begin_src.substr(1, begin_src.length() - 2);
        }
        ret.begin = boost::multiprecision::uint128_t(begin_src);

        if (end_src.length() > 0 && end_src[0] == '"')
        {
            end_src = end_src.substr(1, end_src.length() - 2);
        }
        ret.end = boost::multiprecision::uint128_t(end_src);

        return ret;
    }

    IpAndCountryInfo find(std::vector<IpAndCountryInfo> ips, boost::multiprecision::uint128_t ip)
    {
        int min = 0;
        int max = ips.size() - 1;
        //std::cout << "(19) <<" << ip << "m:" << min << "s:" << max << std::endl;
        //std::cout << "::" << ips[min].end << std::endl;
        if (ips[min].begin <= ip && ip <= ips[min].end)
        {
            return ips[min];
        }
        if (ips[max].begin <= ip && ip <= ips[max].end)
        {
            return ips[max];
        }
        int index = -1;
        int prev_index = -1;
        do
        {
            prev_index = index;
            index = (max - min) / 2 + min;
            IpAndCountryInfo i = ips[index];
            if (i.begin <= ip && ip <= i.end)
            {
                //std::cout << ":f:" << i.country_name << "::" << std::endl;
                return i;
            }
            if (ip < i.begin)
            {
                max = index;
            }
            else if (i.end < ip)
            {
                min = index;
            }
            //std::cout << "::" << index << "::" << i.begin << "<" << ip << "<" << i.end << "::" << std::endl;
        } while (prev_index != index);

        //for(ipinfo i : ips) {
        //    if(i.begin <= ip && ip<=i.end) {
        //        return i;
        //    }
        //}
        throw std::invalid_argument("ipinfo::find");
    }

} // namespace my_ip_country_detector


#include "storm/storage/expressions/BinaryRelationExpression.h"

#include <boost/variant.hpp>

#include "storm/storage/expressions/BooleanLiteralExpression.h"
#include "storm/utility/macros.h"
#include "storm/utility/constants.h"
#include "storm/exceptions/InvalidTypeException.h"
#include "storm/storage/expressions/ExpressionVisitor.h"

namespace storm {
    namespace expressions {
        BinaryRelationExpression::BinaryRelationExpression(ExpressionManager const& manager, Type const& type, std::shared_ptr<BaseExpression const> const& firstOperand, std::shared_ptr<BaseExpression const> const& secondOperand, RelationType relationType) : BinaryExpression(manager, type, firstOperand, secondOperand), relationType(relationType) {
            // Intentionally left empty.
        }

        storm::expressions::OperatorType BinaryRelationExpression::getOperator() const {
            storm::expressions::OperatorType result = storm::expressions::OperatorType::Equal;
            switch (this->getRelationType()) {
                case RelationType::Equal: result = storm::expressions::OperatorType::Equal; break;
                case RelationType::NotEqual: result = storm::expressions::OperatorType::NotEqual; break;
                case RelationType::Less: result = storm::expressions::OperatorType::Less; break;
                case RelationType::LessOrEqual: result = storm::expressions::OperatorType::LessOrEqual; break;
                case RelationType::Greater: result = storm::expressions::OperatorType::Greater; break;
                case RelationType::GreaterOrEqual: result = storm::expressions::OperatorType::GreaterOrEqual; break;
            }
            return result;
        }

        bool BinaryRelationExpression::evaluateAsBool(Valuation const* valuation) const {
            STORM_LOG_THROW(this->hasBooleanType(), storm::exceptions::InvalidTypeException, "Unable to evaluate expression as boolean.");

            double firstOperandEvaluated = this->getFirstOperand()->evaluateAsDouble(valuation);
            double secondOperandEvaluated = this->getSecondOperand()->evaluateAsDouble(valuation);
            bool result = false;
            switch (this->getRelationType()) {
                case RelationType::Equal: result = firstOperandEvaluated == secondOperandEvaluated; break;
                case RelationType::NotEqual: result = firstOperandEvaluated != secondOperandEvaluated; break;
                case RelationType::Greater: result = firstOperandEvaluated > secondOperandEvaluated; break;
                case RelationType::GreaterOrEqual: result = firstOperandEvaluated >= secondOperandEvaluated; break;
                case RelationType::Less: result = firstOperandEvaluated < secondOperandEvaluated; break;
                case RelationType::LessOrEqual: result = firstOperandEvaluated <= secondOperandEvaluated; break;
            }
            return result;
        }

        std::shared_ptr<BaseExpression const> BinaryRelationExpression::simplify() const {
            std::shared_ptr<BaseExpression const> firstOperandSimplified = this->getFirstOperand()->simplify();
            std::shared_ptr<BaseExpression const> secondOperandSimplified = this->getSecondOperand()->simplify();

            if (firstOperandSimplified->isLiteral() && secondOperandSimplified->isLiteral()) {
                storm::RationalNumber firstOperandEvaluation;
                storm::RationalNumber secondOperandEvaluation;

                if (firstOperandSimplified->hasIntegerType()) {
                    firstOperandEvaluation = storm::utility::convertNumber<storm::RationalNumber>(firstOperandSimplified->evaluateAsInt());
                } else {
                    firstOperandEvaluation = firstOperandSimplified->evaluateAsRational();
                }
                if (secondOperandSimplified->hasIntegerType()) {
                    secondOperandEvaluation = storm::utility::convertNumber<storm::RationalNumber>(secondOperandSimplified->evaluateAsInt());
                } else {
                    secondOperandEvaluation = secondOperandSimplified->evaluateAsRational();
                }

                bool truthValue = false;
                switch (this->getRelationType()) {
                    case RelationType::Equal: truthValue = firstOperandEvaluation == secondOperandEvaluation; break;
                    case RelationType::NotEqual: truthValue = firstOperandEvaluation != secondOperandEvaluation; break;
                    case RelationType::Greater: truthValue = firstOperandEvaluation > secondOperandEvaluation; break;
                    case RelationType::GreaterOrEqual: truthValue = firstOperandEvaluation >= secondOperandEvaluation; break;
                    case RelationType::Less: truthValue = firstOperandEvaluation < secondOperandEvaluation; break;
                    case RelationType::LessOrEqual: truthValue = firstOperandEvaluation <= secondOperandEvaluation; break;
                }
                return std::shared_ptr<BaseExpression>(new BooleanLiteralExpression(this->getManager(), truthValue));
            }

            if (firstOperandSimplified.get() == this->getFirstOperand().get() && secondOperandSimplified.get() == this->getSecondOperand().get()) {
                return this->shared_from_this();
            } else {
                return std::shared_ptr<BaseExpression>(new BinaryRelationExpression(this->getManager(), this->getType(), firstOperandSimplified, secondOperandSimplified, this->getRelationType()));
            }
        }

        boost::any BinaryRelationExpression::accept(ExpressionVisitor& visitor, boost::any const& data) const {
            return visitor.visit(*this, data);
        }

        bool BinaryRelationExpression::isBinaryRelationExpression() const {
            return true;
        }

        BinaryRelationExpression::RelationType BinaryRelationExpression::getRelationType() const {
            return this->relationType;
        }

        void BinaryRelationExpression::printToStream(std::ostream& stream) const {
            stream << "(" << *this->getFirstOperand();
            switch (this->getRelationType()) {
                case RelationType::Equal: stream << " = "; break;
                case RelationType::NotEqual: stream << " != "; break;
                case RelationType::Greater: stream << " > "; break;
                case RelationType::GreaterOrEqual: stream << " >= "; break;
                case RelationType::Less: stream << " < "; break;
                case RelationType::LessOrEqual: stream << " <= "; break;
            }
            stream << *this->getSecondOperand() << ")";
        }
    }
}


#include "entities/Node.hpp"
#include "mapcss/StyleDeclaration.hpp"

#include <boost/test/unit_test.hpp>

#include <cstdio>

#include "test_utils/DependencyProvider.hpp"
#include "test_utils/ElementUtils.hpp"

using namespace utymap::entities;
using namespace utymap::index;
using namespace utymap::mapcss;

namespace {
    struct MapCss_StyleDeclarationFixture
    {
        DependencyProvider dependencyProvider;
    };
}

BOOST_FIXTURE_TEST_SUITE(MapCss_StyleDeclaration, MapCss_StyleDeclarationFixture)

BOOST_AUTO_TEST_CASE(GivenOnlySingleTag_WhenDoubleEvaluate_ThenReturnValue)
{
    StyleDeclaration styleDeclaration(0, "eval(\"tag('height')\")");

    double result = styleDeclaration.evaluate<double>(
        ElementUtils::createElement<Node>(*dependencyProvider.getStringTable(), 0, { { "height", "2.5" } }).tags,
        *dependencyProvider.getStringTable());

    BOOST_CHECK_EQUAL(result, 2.5);
}

BOOST_AUTO_TEST_CASE(GiveTwoTags_WhenDoubleEvaluate_ThenReturnValue)
{
    StyleDeclaration styleDeclaration(0, "eval(\"tag('building:height') - tag('roof:height')\")");

    double result = styleDeclaration.evaluate<double>(ElementUtils::createElement<Node>(*dependencyProvider.getStringTable(),
        0, { { "building:height", "10" }, { "roof:height", "2.5" } }).tags,
         *dependencyProvider.getStringTable());

    BOOST_CHECK_EQUAL(result, 7.5);
}

BOOST_AUTO_TEST_CASE(GiveOneTagOneNumber_WhenDoubleEvaluate_ThenReturnValue)
{
    StyleDeclaration styleDeclaration(0, "eval(\"tag('building:levels') * 3\")");

    double result = styleDeclaration.evaluate<double>(ElementUtils::createElement<Node>(*dependencyProvider.getStringTable(),
        0, { { "building:levels", "5" } }).tags,
    *dependencyProvider.getStringTable());

    BOOST_CHECK_EQUAL(result, 15);
}

BOOST_AUTO_TEST_CASE(GiveRawValue_WhenDoubleEvaluate_ThenThrowsException)
{
    StyleDeclaration styleDeclaration(0, "13");

    BOOST_CHECK_THROW(styleDeclaration.evaluate<double>(ElementUtils::createElement<Node>(*dependencyProvider.getStringTable(),
        0, { { "building:levels", "5" } }).tags, *dependencyProvider.getStringTable()),
    utymap::MapCssException);
}

BOOST_AUTO_TEST_CASE(GiveOneTagOneNumber_WhenStringEvaluate_ThenReturnValue)
{
    StyleDeclaration styleDeclaration(0, "eval(\"tag('color')\")");

    std::string result = styleDeclaration.evaluate<std::string>(ElementUtils::createElement<Node>(*dependencyProvider.getStringTable(),
        0, { { "color", "red" } }).tags,
    *dependencyProvider.getStringTable());

    BOOST_CHECK_EQUAL(result, "red");
}

BOOST_AUTO_TEST_SUITE_END()


#include <vector>
#include <string>
#include <Eigen/Core>
#include <cmath>
#include "ndt_map/ndt_map.h"
#include <fstream>
#include "ndt_map/ndt_cell.h"
#include "ros/ros.h"
class map_converter {
  std::string ndt_map_name;
  std::string occ_map_name;
  double ndt_resolution;
  double occ_resolution;
  double lik_tr;
  double sample_height;
  double floor_deviation;
  double robot_height;
  int width, height, breadth;
  perception_oru::NDTMap *ndtMap;
  perception_oru::LazyGrid *mapGrid;
  bool floor_only;
public:
  map_converter(ros::NodeHandle private_nh){
    private_nh.param<std::string>("ndt_map_name", ndt_map_name, "map.jff");
    private_nh.param<double>("ndt_resolution", ndt_resolution, 0.4);
    private_nh.param<std::string>("occ_map_name", occ_map_name, "occ");
    private_nh.param<double>("occ_resolution", occ_resolution, 0.1);
    private_nh.param<double>("sample_height", sample_height, -0.3);
    private_nh.param<double>("floor_deviation", floor_deviation, 0.15);
    private_nh.param<double>("lik_tr", lik_tr, 0.1);
    private_nh.param<double>("robot_height", robot_height, 1.9);
    private_nh.param<bool>("floor_only", floor_only, false);
    LoadMap();
    BuildMap();
  }

private:
  int LoadMap(){
    FILE * jffin;
    jffin = fopen(ndt_map_name.c_str(), "r+b");
    mapGrid = new perception_oru::LazyGrid(ndt_resolution);
    ndtMap = new perception_oru::NDTMap(mapGrid);
    if(ndtMap->loadFromJFF(jffin) < 0)
      return -1;
    return 0;
  }

  double getAverageLevel(std::vector<perception_oru::NDTCell*> floors){
    double aver = 0;

    for(size_t i = 0; i < floors.size(); i++)
      aver += floors[i]->getMean()[2];
    return aver / double(floors.size());
  }

  void BuildMap(){
    double xs, ys, zs;
    double xc, yc, zc;
    double minx, miny, minz, maxx, maxy, maxz;
    ndtMap->getCentroid(xc, yc, zc);
    ndtMap->getGridSizeInMeters(xs, ys, zs);
    minx = xc - xs / 2.0;
    miny = yc - ys / 2.0;
    minz = zc - zs / 2.0;
    maxx = xc + xs / 2.0;
    maxy = yc + ys / 2.0;
    maxz = zc + zs / 2.0;
    height = abs(ceil(xs / occ_resolution));
    width = abs(ceil(ys / occ_resolution));
    std::vector<std::vector<double> > maps;
    maps.resize(width, std::vector<double>(height));

    ROS_INFO_STREAM(xc<<" "<<yc<<" "<<zc);
    ROS_INFO_STREAM(xs<<" "<<ys<<" "<<zs);
    ROS_INFO_STREAM(minx<<" "<<miny<<" "<<minz);
    ROS_INFO_STREAM(height<<" "<<width);

    perception_oru::NDTCell* ptCell = NULL;
    for (int h=0;h<height;h++){
      for (int w=0;w<width;w++){
		pcl::PointXYZ pt(minx+occ_resolution/2+h*occ_resolution,miny+occ_resolution/2+w*occ_resolution,sample_height);
	if(!ndtMap->getCellAtPoint(pt,ptCell)){
//	  ROS_INFO_STREAM("no cell");
	  //Cell does not exist, it's unexplored (gray)
	  maps[w][h]=1;
	}
	else{
//	  ROS_INFO_STREAM(ptCell->getOccupancy());
	  if(ptCell->getLikelihood(pt)>=lik_tr){
	    //cell exists and the point is likely -> white
	    maps[w][h]=2;
	  }
	  else{
	    if(ptCell->getOccupancy()<0){
	      //the point is not likely, but the cell is not occupied -> white
	      maps[w][h]=2;
	    }
	    else
	      if(ptCell->getLikelihood(pt)<lik_tr)
	      {
		 //occupied
	         maps[w][h]=0;
	     }
	  }
	}
      }
    }
			std::cout << "saving map" << std::endl;
			std::string png_map_file = occ_map_name;
			png_map_file += std::to_string(sample_height);
			png_map_file += ".pgm";
			std::ofstream map_file(png_map_file.c_str());
			if(map_file.is_open()){
				map_file << "P2\n";
				map_file << width << " " << height << std::endl;
				map_file << "3\n";
				for(int w = width - 1; w >= 0; w--){
					for(int h = 0; h < height; h++)
						map_file << maps[w][h] << " ";
					map_file << std::endl;
				}
				map_file.close();
			}else std::cout << "Unable to open file";

  }
};

int main(int argc, char **argv){
  ROS_INFO("Starting node");
  ros::init(argc, argv, "off_line_velodyne_test");
  ros::NodeHandle nh;
  ros::NodeHandle parameters("~");
  map_converter mc(parameters);
}


//
//  Functions_optimize.hpp
//  PAINTOR_V3.1
//
//  Created by Gleb Kichaev on 11/21/17.
//  Copyright © 2017 Gleb Kichaev. All rights reserved.
//

#ifndef Functions_optimize_hpp
#define Functions_optimize_hpp

#include <stdio.h>

#include <fstream>
#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen>

#include "nlopt.hpp"
#include "Functions_model.hpp"

using namespace Eigen;
using namespace std;
using namespace nlopt;

/*
struct optimization_input{
    vector<double> current_posterior;
    vector<vector<double> > all_annotations;
};
*/

double dot_prod(vector<double>& vec1, vector<double>& vec2);
vector<double> vector_scalar_product(const double& scalar, vector<double>& vec);
vector<double> vector_sum(vector<double>& vec1, vector<double>& vec2);
double complete_data_log_liklei(const vector<double> &x, vector<double> &grad, void *data);
vector<double> compute_gradient(vector<double>& gamma, optimization_input *in_data);
void optimize_complete_data_log_liklei(vector<double>& gamma_initial, double lower, double upper, void* in_data);

#endif /* Functions_optimize_hpp */


#include <boost/python.hpp>

#include "../../../def_submodule.hpp"

#include "IceRay/geometry/simple/quadric.hpp"

#include "./simple.hpp"

typedef GS_DDMRM::S_IceRay::S_geometry::GC_quadric   GTs_quadric;

void expose_IceRay_geometry_simple_quadric()
 {
  //MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( core );
  MAKE_SUBMODULE( geometry );

  typedef GS_DDMRM::S_IceRay::S_type::GT_scalar              Tf_scalar;
  typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar     Tf_coord;
  typedef GS_DDMRM::S_IceRay::S_type::S_matrix::GT_scalar3D  Tf_matrix;

  typedef  Tf_matrix const& (GTs_quadric::*Tf_getMatrix  )(void) const;
  typedef  bool (GTs_quadric::*Tf_setMatrix  )(Tf_matrix const&);

  typedef  Tf_coord const& (GTs_quadric::*Tf_getCoord  )(void) const;
  typedef  bool (GTs_quadric::*Tf_setCoord  )(Tf_coord const&);

  typedef  Tf_scalar const& (GTs_quadric::*Tf_getScalar  )(void) const;
  typedef  bool (GTs_quadric::*Tf_setScalar  )(Tf_scalar const&);

  Tf_getMatrix  I_getMatrix = &GTs_quadric::F_matrix;
  Tf_setMatrix  I_setMatrix = &GTs_quadric::F_matrix;
  Tf_getCoord   I_getCoord  = &GTs_quadric::F_coord;
  Tf_setCoord   I_setCoord  = &GTs_quadric::F_coord;
  Tf_getScalar  I_getScalar = &GTs_quadric::F_scalar;
  Tf_setScalar  I_setScalar = &GTs_quadric::F_scalar;

  boost::python::class_< GTs_quadric, boost::python::bases<GTs__base> >( "GeometrySimpleQuadric" )
    .def( boost::python::init<>() )
    .def( boost::python::init<Tf_matrix, Tf_coord, Tf_scalar>() )
    .def("matrix",  I_getMatrix, boost::python::return_value_policy<boost::python::copy_const_reference>()  )
    .def("matrix",  I_setMatrix )
    .def("coord",   I_getCoord,  boost::python::return_value_policy<boost::python::copy_const_reference>()  )
    .def("coord",   I_setCoord  )
    .def("scalar",  I_getScalar, boost::python::return_value_policy<boost::python::copy_const_reference>()  )
    .def("scalar",  I_setScalar )
    ;

 }


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/twelve.hpp>
#include <boost/simd/as.hpp>
#include <stf.hpp>

STF_CASE_TPL( "Check twelve behavior for all types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
              (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::twelve;
  using boost::simd::Twelve;

  STF_TYPE_IS(decltype(Twelve<T>()), T);
  STF_EQUAL(Twelve<T>(), T(12));
  STF_EQUAL(twelve( as(T{}) ),T(12));
}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ReduceAdd
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
// codelets
#include "poplar/Target.hpp"
#include "poplibs_test/Check.hpp"
#include "poplibs_test/Util.hpp"
#include "poplin/Convolution.hpp"
#include "poplin/codelets.hpp"
#include "poputil/VertexTemplates.hpp"
#include <stdexcept>
#include <string.h>

#include <boost/program_options.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poplin;
using namespace poputil;
using namespace poplibs_test::util;
using namespace poplibs_support;

static bool doTest(const DeviceType &deviceType, const Type &partialsType,
                   const Type &outType, unsigned outerDim, unsigned innerDim,
                   bool singleInput, bool constrainPartials, bool profile) {
  auto device = createTestDevice(deviceType);
  auto &target = device.getTarget();
  Graph graph(target);
  poplin::addCodelets(graph);

  // Claim enough space for floats
  std::vector<char> data(innerDim * outerDim * 4);
  std::vector<float> nums(innerDim * outerDim);
  for (unsigned i = 0; i < innerDim * outerDim; ++i) {
    nums[i] = 1.0 * (i % outerDim);
  }
  copy(target, nums.data(), innerDim * outerDim, partialsType, data.data());
  std::vector<float> answers(outerDim + 1);
  std::vector<char> ans_data((outerDim + 1) * 4);
  for (unsigned i = 0; i < outerDim + 1; ++i) {
    answers[i] = 0.0;
  }
  memcpy(ans_data.data(), answers.data(), (outerDim + 1) * 4);

  Sequence prog;

  auto cs = graph.addComputeSet("cs");

  Tensor partials;
  partials = graph.addVariable(partialsType, {innerDim, outerDim});

  auto out = graph.addVariable(outType, {outerDim + 1});

  const auto vertexClass =
      templateVertex("poplin::ReduceAdd", outType, partialsType, singleInput,
                     constrainPartials);
  auto v1 = graph.addVertex(cs, vertexClass);

  if (singleInput) {
    graph.connect(v1["partials"], partials.slice(0, 1, 0).flatten());
    graph.connect(v1["initialPartial"],
                  partials.slice(innerDim == 1 ? 0 : 1, innerDim, 0).flatten());
    graph.setInitialValue(v1["numPartials"], innerDim - 1);
  } else {
    for (unsigned i = 0; i < innerDim; ++i) {
      Tensor Row = partials.slice(i, i + 1, 0);
      graph.connect(v1["partials"][i], Row.reshape({outerDim}));
    }
    graph.setFieldSize(v1["partials"], innerDim);
    graph.setInitialValue(v1["numPartials"], innerDim);
  }
  graph.connect(v1["out"], out.slice(0, outerDim));
  graph.setInitialValue(v1["numElems"], outerDim);

  graph.setTileMapping(v1, 0);
  graph.setTileMapping(partials, 0);
  graph.setTileMapping(out, 0);

  graph.createHostWrite("partials", partials);
  graph.createHostWrite("outw", out);
  graph.createHostRead("out", out);

  prog.add(Execute(cs));

  OptionFlags engineOptions;
  if (profile) {
    engineOptions.set("debug.instrumentCompute", "true");
  }
  Engine e(graph, prog, engineOptions);
  auto outSize = out.numElements() * target.getTypeSize(outType);

  device.bind([&](const Device &d) {
    e.load(d);
    e.writeTensor("partials", data.data(),
                  data.data() + partials.numElements() *
                                    target.getTypeSize(partialsType));
    e.writeTensor("outw", ans_data.data(), ans_data.data() + outSize);
    e.readTensor("out", ans_data.data(), ans_data.data() + outSize);

    e.run();

    e.readTensor("out", ans_data.data(), ans_data.data() + outSize);
  });

  copy(target, outType, ans_data.data(), answers.data(), outerDim + 1);

  if (profile) {
    e.printProfileSummary(std::cout,
                          OptionFlags{{"showExecutionSteps", "true"}});
  }
  bool success = true;
  for (unsigned i = 0; i < outerDim; ++i) {
    CHECK_IF(success, innerDim * 1.0 * i == answers[i]);
    answers[i] = 0; // zero for next iteration
  }
  CHECK_IF(success, answers[outerDim] == 0.0);
  return success;
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  DeviceType deviceType;
  Type partialsType;
  Type outType;
  unsigned outerDim, innerDim;
  bool singleInput = false;
  bool constrainPartials = false;
  bool profile = false;
  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Print help")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("profile",
     po::value<bool>(&profile)->default_value(profile),
     "Show a profile report")
    ("partials-type",
     po::value<Type>(&partialsType)->required(),
     "Partials Type")
    ("out-type",
     po::value<Type>(&outType)->required(),
     "Output Type")
    ("outer-dim",
     po::value<unsigned>(&outerDim)->required(),
     "Outer dimension")
    ("inner-dim",
     po::value<unsigned>(&innerDim)->required(),
     "Inner dimension")
    ("single-input",
     po::value<bool>(&singleInput)->default_value(singleInput),
     "Use single input region variant of the vertex")
    ("constrain-partials",
     po::value<bool>(&constrainPartials)->default_value(constrainPartials),
     "Use variant with constrained partials memory allocation")
    ;
  // clang-format on
  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }
  if (!singleInput && constrainPartials) {
    std::cerr << "Error, vertex without singleInput but with constrained"
                 " partials is not supported\n";
  }
  if (!doTest(deviceType, partialsType, outType, outerDim, innerDim,
              singleInput, constrainPartials, profile))
    return 1;
  return 0;
}


/*!
@file
Forward declares `boost::hana::MonadPlus`.

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

#ifndef BOOST_HANA_FWD_CONCEPT_MONAD_PLUS_HPP
#define BOOST_HANA_FWD_CONCEPT_MONAD_PLUS_HPP

#include <boost/hana/config.hpp>

BOOST_HANA_NAMESPACE_BEGIN
    //! @ingroup group-concepts
    //! @defgroup group-MonadPlus MonadPlus
    //! The `MonadPlus` concept represents Monads with a monoidal structure.
    //!
    //! Intuitively, whereas a Monad can be seen as some kind of container
    //! or context, a MonadPlus can be seen as a container or a context that
    //! can be concatenated with other containers or contexts. There must
    //! also be an identity element for this combining operation. For example,
    //! a tuple is a MonadPlus, because tuples can be concatenated and the
    //! empty tuple would act as an identity for concatenation. How is this
    //! different from a Monad which is also a Monoid? The answer is that the
    //! monoidal structure on a MonadPlus must _not_ depend of the contents
    //! of the structure; it must not require the contents to be a Monoid
    //! in order to work.
    //!
    //! While sequences are not the only possible model for MonadPlus, the
    //! method names used here refer to the MonadPlus of sequences under
    //! concatenation. Several useful functions generalizing operations on
    //! sequences are included with this concept, like `append`, `prepend`
    //! and `filter`.
    //!
    //! @note
    //! This documentation does not go into much details about the nature
    //! of the MonadPlus concept. However, there is a nice Haskell-oriented
    //! [WikiBook][1] going into further details.
    //!
    //!
    //! Minimal complete definition
    //! ---------------------------
    //! `concat` and `empty`
    //!
    //!
    //! Laws
    //! ----
    //! First, a MonadPlus is required to have a monoidal structure. Hence, it
    //! is no surprise that for any MonadPlus `M`, we require `M(T)` to be a
    //! valid monoid. However, we do not enforce that `M(T)` actually models
    //! the Monoid concept provided by Hana. Further, for all objects `a, b, c`
    //! of data type `M(T)`,
    //! @code
    //!     // identity
    //!     concat(empty<M(T)>(), a) == a
    //!     concat(a, empty<M(T)>()) == a
    //!
    //!     // associativity
    //!     concat(a, concat(b, c)) == concat(concat(a, b), c)
    //! @endcode
    //!
    //! Secondly, a MonadPlus is also required to obey the following laws,
    //! which represent the fact that `empty<M(T)>()` must be some kind of
    //! absorbing element for the `chain` operation. For all objects `a` of
    //! data type `M(T)` and functions @f$ f : T \to M(U) @f$,
    //! @code
    //!     chain(empty<M(T)>(), f)         == empty<M(U)>()
    //!     chain(a, always(empty<M(T)>())) == empty<M(U)>()
    //! @endcode
    //!
    //!
    //! Refined concepts
    //! ----------------
    //! `Functor`, `Applicative` and `Monad`
    //!
    //!
    //! Concrete models
    //! ---------------
    //! `hana::optional`, `hana::tuple`
    //!
    //! [1]: https://en.wikibooks.org/wiki/Haskell/MonadPlus
    template <typename M>
    struct MonadPlus;
BOOST_HANA_NAMESPACE_END

#endif // !BOOST_HANA_FWD_CONCEPT_MONAD_PLUS_HPP


/***************************************************************************************************************************
* formation_flight_40uav.cpp
*
* Author: Qyp
*
* Update Time: 2021.3.10
*
*    40架飞机阵型控制程序,仅用于Gazebo仿真.
***************************************************************************************************************************/

//ros头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>
#include <mission_utils.h>

//topic 头文件
#include <geometry_msgs/Point.h>
#include <prometheus_msgs/SwarmCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/PositionReference.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/ActuatorControl.h>
#include <sensor_msgs/Imu.h>
#include <prometheus_msgs/DroneState.h>
#include <prometheus_msgs/AttitudeReference.h>
#include <prometheus_msgs/DroneState.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Int8.h>
#include <math.h>
#include "message_utils.h"

using namespace std;

#define NODE_NAME "formation_flight_40uav"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int controller_num;

prometheus_msgs::SwarmCommand swarm_command;

Eigen::Vector3f virtual_leader_pos;
Eigen::Vector3f virtual_leader_vel;
float virtual_leader_yaw;
float formation_size;
int formation_num = 1;

// 无人机集群控制指令发布声明
ros::Publisher uav1_command_pub,uav2_command_pub,uav3_command_pub,uav4_command_pub,uav5_command_pub;
ros::Publisher uav6_command_pub,uav7_command_pub,uav8_command_pub,uav9_command_pub,uav10_command_pub;
ros::Publisher uav11_command_pub,uav12_command_pub,uav13_command_pub,uav14_command_pub,uav15_command_pub;
ros::Publisher uav16_command_pub,uav17_command_pub,uav18_command_pub,uav19_command_pub,uav20_command_pub;
ros::Publisher uav21_command_pub,uav22_command_pub,uav23_command_pub,uav24_command_pub,uav25_command_pub;
ros::Publisher uav26_command_pub,uav27_command_pub,uav28_command_pub,uav29_command_pub,uav30_command_pub;
ros::Publisher uav31_command_pub,uav32_command_pub,uav33_command_pub,uav34_command_pub,uav35_command_pub;
ros::Publisher uav36_command_pub,uav37_command_pub,uav38_command_pub,uav39_command_pub,uav40_command_pub;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param();
void pub_formation_command();
void publish_same_command(prometheus_msgs::SwarmCommand swarm_command);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void goal_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    cout << "Get a new goal from rviz!"<<endl;
    virtual_leader_pos[0] = msg->pose.position.x;
    virtual_leader_pos[1] = msg->pose.position.y;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "formation_flight_uav40");
    ros::NodeHandle nh("~");

    // 0代表位置追踪模式，１代表速度追踪模式，２代表加速度追踪模式
    nh.param<int>("controller_num", controller_num, 0);
    nh.param<float>("virtual_leader_pos_x", virtual_leader_pos[0], 0.0);
    nh.param<float>("virtual_leader_pos_y", virtual_leader_pos[1], 0.0);
    nh.param<float>("virtual_leader_pos_z", virtual_leader_pos[2], 0.6);
    nh.param<float>("virtual_leader_yaw", virtual_leader_yaw, 0.0);
    nh.param<float>("formation_size", formation_size, 1.0);

    //【订阅】目标点
    ros::Subscriber goal_sub = nh.subscribe<geometry_msgs::PoseStamped>("/prometheus/formation/virtual_leader", 10, goal_cb);

    // 【发布】用于地面站显示的提示消息
    ros::Publisher message_pub = nh.advertise<prometheus_msgs::Message>("/prometheus/message/main", 10);

    //【发布】集群控制指令
    uav1_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav1/prometheus/swarm_command", 10);
    uav2_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav2/prometheus/swarm_command", 10);
    uav3_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav3/prometheus/swarm_command", 10);
    uav4_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav4/prometheus/swarm_command", 10);
    uav5_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav5/prometheus/swarm_command", 10);
    uav6_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav6/prometheus/swarm_command", 10);
    uav7_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav7/prometheus/swarm_command", 10);
    uav8_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav8/prometheus/swarm_command", 10);
    uav9_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav9/prometheus/swarm_command", 10);
    uav10_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav10/prometheus/swarm_command", 10);
    uav11_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav11/prometheus/swarm_command", 10);
    uav12_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav12/prometheus/swarm_command", 10);
    uav13_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav13/prometheus/swarm_command", 10);
    uav14_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav14/prometheus/swarm_command", 10);
    uav15_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav15/prometheus/swarm_command", 10);
    uav16_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav16/prometheus/swarm_command", 10);
    uav17_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav17/prometheus/swarm_command", 10);
    uav18_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav18/prometheus/swarm_command", 10);
    uav19_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav19/prometheus/swarm_command", 10);
    uav20_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav20/prometheus/swarm_command", 10);
    uav21_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav21/prometheus/swarm_command", 10);
    uav22_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav22/prometheus/swarm_command", 10);
    uav23_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav23/prometheus/swarm_command", 10);
    uav24_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav24/prometheus/swarm_command", 10);
    uav25_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav25/prometheus/swarm_command", 10);
    uav26_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav26/prometheus/swarm_command", 10);
    uav27_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav27/prometheus/swarm_command", 10);
    uav28_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav28/prometheus/swarm_command", 10);
    uav29_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav29/prometheus/swarm_command", 10);
    uav30_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav30/prometheus/swarm_command", 10);
    uav31_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav31/prometheus/swarm_command", 10);
    uav32_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav32/prometheus/swarm_command", 10);
    uav33_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav33/prometheus/swarm_command", 10);
    uav34_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav34/prometheus/swarm_command", 10);
    uav35_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav35/prometheus/swarm_command", 10);
    uav36_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav36/prometheus/swarm_command", 10);
    uav37_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav37/prometheus/swarm_command", 10);
    uav38_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav38/prometheus/swarm_command", 10);
    uav39_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav39/prometheus/swarm_command", 10);
    uav40_command_pub = nh.advertise<prometheus_msgs::SwarmCommand>("/uav40/prometheus/swarm_command", 10);

    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(2);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    // Waiting for input
    int start_flag = 0;

    ros::Duration(3.0).sleep();

    printf_param();

    while(start_flag == 0)
    {
        cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>Formation Flight UAV40 Mission<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
        cout << "Please enter 1 to disarm all the UAVs."<<endl;
        cin >> start_flag;

        swarm_command.Mode = prometheus_msgs::SwarmCommand::Idle;
        swarm_command.yaw_ref = 999;

        publish_same_command(swarm_command);
    }

    start_flag = 0;

    while(start_flag == 0)
    {
        cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>Formation Flight Mission<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
        cout << "Please enter 1 to takeoff all the UAVs."<<endl;
        cin >> start_flag;

        swarm_command.Mode = prometheus_msgs::SwarmCommand::Takeoff;
        swarm_command.yaw_ref = 0.0;

        publish_same_command(swarm_command);
    }

    float trajectory_total_time;
    while (ros::ok())
    {
        cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>Formation Flight Mission<<<<<<<<<<<<<<<<<<<<<<<<< 3 for Circle Trajectory Tracking,"<< endl;
        cout << "Please choose the action: 0 for Formation Shape, 1 for Virtual Leader Pos, 2 for Hold, 3 for Land, 4 for Circle, 5 for Disarm..."<<endl;
        cin >> start_flag;

        if (start_flag == 0)
        {
            cout << "Please choose the formation: 1 for One Column, 2 for Triangle, 3 for One Row ..."<<endl;
            cin >> formation_num;

            pub_formation_command();

        }else if (start_flag == 1)
        {
            cout << "Please enter the virtual leader position:"<<endl;
            cout << "virtual_leader_pos: --- x [m] "<<endl;
            cin >> virtual_leader_pos[0];
            cout << "virtual_leader_pos: --- y [m]"<<endl;
            cin >> virtual_leader_pos[1];
            cout << "virtual_leader_pos: --- z [m]"<<endl;
            cin >> virtual_leader_pos[2];
            cout << "virtual_leader_yaw [deg]:"<<endl;
            cin >> virtual_leader_yaw;
            virtual_leader_yaw = virtual_leader_yaw/180.0*M_PI;

            pub_formation_command();

        }else if (start_flag == 2)
        {
            swarm_command.Mode = prometheus_msgs::SwarmCommand::Hold;

            publish_same_command(swarm_command);
        }else if (start_flag == 3)
        {
            swarm_command.Mode = prometheus_msgs::SwarmCommand::Land;

            publish_same_command(swarm_command);
        }else if (start_flag == 4)
        {
            cout << "Input the trajectory_total_time:"<<endl;
            cin >> trajectory_total_time;

            float time_trajectory = 0.0;

            while(time_trajectory < trajectory_total_time)
            {

                const float omega = 0.15;
                const float circle_radius = 1.5;

                virtual_leader_pos[0] = circle_radius * cos(time_trajectory * omega);
                virtual_leader_pos[1] = circle_radius * sin(time_trajectory * omega);
                //virtual_leader_pos[2] = 1.0;
                virtual_leader_vel[0] = - omega * circle_radius * sin(time_trajectory * omega);
                virtual_leader_vel[1] = omega * circle_radius * cos(time_trajectory * omega);
                virtual_leader_vel[2] = 0.0;

                time_trajectory = time_trajectory + 0.01;

                cout << "Trajectory tracking: "<< time_trajectory << " / " << trajectory_total_time  << " [ s ]" <<endl;

                pub_formation_command();
                ros::Duration(0.01).sleep();
            }
        }else if (start_flag == 5)
        {
            swarm_command.Mode = prometheus_msgs::SwarmCommand::Disarm;

            publish_same_command(swarm_command);
        }else
        {
            cout << "Wrong input."<<endl;
        }

        cout << "virtual_leader_pos [X Y] : " << virtual_leader_pos[0] << " [ m ] "<< virtual_leader_pos[1] <<" [ m ] "<< virtual_leader_pos[2] <<" [ m ] "<< endl;
        cout << "virtual_leader_yaw: " << virtual_leader_yaw/M_PI*180.0 <<" [ deg ] "<< endl;

        ros::Duration(2.0).sleep();
    }

    return 0;

}

void pub_formation_command()
{
    if(formation_num == 1)
    {
        cout << "Formation shape: [ One_column ]"<<endl;

        swarm_command.swarm_shape = prometheus_msgs::SwarmCommand::One_column;
    }else if(formation_num == 2)
    {
        cout << "Formation shape: [ Triangle ]"<<endl;

        swarm_command.swarm_shape = prometheus_msgs::SwarmCommand::Triangle;
    }else if(formation_num == 3)
    {
        cout << "todo:  Formation shape: [ One_row ]"<<endl;

        // swarm_command.swarm_shape = prometheus_msgs::SwarmCommand::One_row;
    }else
    {
        cout << "Wrong formation shape!"<<endl;
    }

    if (controller_num == 0)
    {
        swarm_command.Mode = prometheus_msgs::SwarmCommand::Position_Control;
    }else if(controller_num == 1)
    {
        swarm_command.Mode = prometheus_msgs::SwarmCommand::Velocity_Control;
    }else if(controller_num == 2)
    {
        swarm_command.Mode = prometheus_msgs::SwarmCommand::Accel_Control;
    }

    swarm_command.swarm_size = formation_size;
    swarm_command.position_ref[0] = virtual_leader_pos[0] ;
    swarm_command.position_ref[1] = virtual_leader_pos[1] ;
    swarm_command.position_ref[2] = virtual_leader_pos[2] ;
    swarm_command.velocity_ref[0] = virtual_leader_vel[0] ;
    swarm_command.velocity_ref[1] = virtual_leader_vel[1] ;
    swarm_command.velocity_ref[2] = virtual_leader_vel[2] ;
    swarm_command.yaw_ref = virtual_leader_yaw;

    uav1_command_pub.publish(swarm_command);
    uav2_command_pub.publish(swarm_command);
    uav3_command_pub.publish(swarm_command);
    uav4_command_pub.publish(swarm_command);
    uav5_command_pub.publish(swarm_command);

}

void printf_param()
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>> Formation Flight Parameter <<<<<<<<<<<<<<<<<<<<<<" <<endl;

    cout << "controller_num   : "<< controller_num <<endl;

    cout << "virtual_leader_pos_x   : "<< virtual_leader_pos[0]<<" [m] "<<endl;
    cout << "virtual_leader_pos_y   : "<< virtual_leader_pos[1]<<" [m] "<<endl;
    cout << "virtual_leader_pos_z   : "<< virtual_leader_pos[2]<<" [m] "<<endl;
    cout << "virtual_leader_yaw : "<< virtual_leader_yaw << " [rad]  "<< endl;
    cout << "formation_size : "<< formation_size << " [m] "<< endl;
}

void publish_same_command(prometheus_msgs::SwarmCommand swarm_command)
{
    uav1_command_pub.publish(swarm_command);
    uav2_command_pub.publish(swarm_command);
    uav3_command_pub.publish(swarm_command);
    uav4_command_pub.publish(swarm_command);
    uav5_command_pub.publish(swarm_command);
    uav6_command_pub.publish(swarm_command);
    uav7_command_pub.publish(swarm_command);
    uav8_command_pub.publish(swarm_command);
    uav9_command_pub.publish(swarm_command);
    uav10_command_pub.publish(swarm_command);
    uav11_command_pub.publish(swarm_command);
    uav12_command_pub.publish(swarm_command);
    uav13_command_pub.publish(swarm_command);
    uav14_command_pub.publish(swarm_command);
    uav15_command_pub.publish(swarm_command);
    uav16_command_pub.publish(swarm_command);
    uav17_command_pub.publish(swarm_command);
    uav18_command_pub.publish(swarm_command);
    uav19_command_pub.publish(swarm_command);
    uav20_command_pub.publish(swarm_command);
    uav21_command_pub.publish(swarm_command);
    uav22_command_pub.publish(swarm_command);
    uav23_command_pub.publish(swarm_command);
    uav24_command_pub.publish(swarm_command);
    uav25_command_pub.publish(swarm_command);
    uav26_command_pub.publish(swarm_command);
    uav27_command_pub.publish(swarm_command);
    uav28_command_pub.publish(swarm_command);
    uav29_command_pub.publish(swarm_command);
    uav30_command_pub.publish(swarm_command);
    uav31_command_pub.publish(swarm_command);
    uav32_command_pub.publish(swarm_command);
    uav33_command_pub.publish(swarm_command);
    uav34_command_pub.publish(swarm_command);
    uav35_command_pub.publish(swarm_command);
    uav36_command_pub.publish(swarm_command);
    uav37_command_pub.publish(swarm_command);
    uav38_command_pub.publish(swarm_command);
    uav39_command_pub.publish(swarm_command);
    uav40_command_pub.publish(swarm_command);
}


/*
    Copyright 2013 Adobe
    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*/
/****************************************************************************************************/

#ifndef ADOBE_EXTENTS_HPP
#define ADOBE_EXTENTS_HPP

#include <adobe/config.hpp>

#include <boost/array.hpp>
#include <boost/operators.hpp>

#include <vector>

/****************************************************************************************************/

namespace adobe {

/****************************************************************************************************/

template <typename T = int>
struct point_2d : boost::equality_comparable<point_2d<T>> {
    point_2d() : x_m(T()), y_m(T()) {}

    point_2d(const T& x, const T& y) : x_m(x), y_m(y) {}

    T x_m;
    T y_m;

    friend inline void swap(const point_2d& x, const point_2d& y) BOOST_NOEXCEPT {
        swap(x.x_m, y.x_m);
        swap(x.y_m, y.y_m);
    }

    friend inline bool operator==(const point_2d& x, const point_2d& y) {
        return (x.x_m == y.x_m) && (x.y_m == y.y_m);
    }
};

typedef std::pair<int, int> pair_long_t;
typedef point_2d<int> point_2d_t;
typedef std::vector<int> guide_set_t;

// REVISIT (sparent) : points of interest need to be named entities. This will become:

#if 0
struct guide_set_t
{
    name_t name_m;
    int    offset_m;
};
typedef std::vector<guide_set_t> point_of_interest_set_t;
#endif

// REVISIT (sparent) : Open issue - are there "alignment" attributes on POIs?

struct extents_slices_t {
    enum slice_select_t {
        horizontal,
        vertical
    };
};

/****************************************************************************************************/

struct extents_t :
#if !defined(ADOBE_NO_DOCUMENTATION)
    private extents_slices_t,
    boost::equality_comparable<extents_t>
#endif
    {
    struct slice_t : boost::equality_comparable<slice_t> {
        slice_t() : length_m(0) {};

        int length_m;
        pair_long_t outset_m;
        pair_long_t frame_m;
        pair_long_t inset_m;
        guide_set_t guide_set_m;

        friend bool operator==(const slice_t& x, const slice_t& y);
    };

    boost::array<slice_t, 2> slice_m;

    slice_t& vertical() { return slice_m[extents_slices_t::vertical]; }
    slice_t& horizontal() { return slice_m[extents_slices_t::horizontal]; }

    const slice_t& vertical() const { return slice_m[extents_slices_t::vertical]; }
    const slice_t& horizontal() const { return slice_m[extents_slices_t::horizontal]; }

    int& height() { return vertical().length_m; }
    int& width() { return horizontal().length_m; }

    const int& height() const { return vertical().length_m; }
    const int& width() const { return horizontal().length_m; }

    friend bool operator==(const extents_t& x, const extents_t& y);
};

/****************************************************************************************************/

#ifndef NDEBUG
std::ostream& operator<<(std::ostream& s, const extents_t& x);
#endif

#ifndef NDEBUG
std::ostream& operator<<(std::ostream& s, const extents_t::slice_t& x);
#endif

/*************************************************************************************************/

} // namespace adobe

/****************************************************************************************************/

namespace std {

template <>
inline void swap(adobe::extents_t::slice_t& x, adobe::extents_t::slice_t& y) BOOST_NOEXCEPT {
    swap(x.length_m, y.length_m);
    swap(x.outset_m, y.outset_m);
    swap(x.frame_m, y.frame_m);
    swap(x.inset_m, y.inset_m);
    swap(x.guide_set_m, y.guide_set_m);
}

template <>
inline void swap(adobe::extents_t& x, adobe::extents_t& y) BOOST_NOEXCEPT {
    swap(x.slice_m, y.slice_m);
}

} // namespace std

/****************************************************************************************************/

#endif

/****************************************************************************************************/


#include <boost/geometry/util/select_most_precise.hpp>


#pragma once
#include <algorithm>
#include <varalgo/std_variant_traits.hpp>

#include <boost/range.hpp>
#include <boost/range/detail/range_return.hpp>

namespace varalgo
{
	template <class InputIterator, class RandomAccessIterator, class Pred>
	inline RandomAccessIterator partial_sort_copy(
		InputIterator first, InputIterator last,
	    RandomAccessIterator d_first, RandomAccessIterator d_last,
	    Pred && pred)
	{
		auto alg = [&first, &last, &d_first, &d_last](auto && pred)
		{
			return std::partial_sort_copy(first, last, d_first, d_last, std::forward<decltype(pred)>(pred));
		};

		return variant_traits<std::decay_t<Pred>>::visit(std::move(alg), std::forward<Pred>(pred));
	}

	/// range overloads
	template <class SinglePassRange, class RandomAccessRange, class Pred>
	inline auto partial_sort_copy(const SinglePassRange & input, const RandomAccessRange & output, Pred && pred)
	{
		return varalgo::partial_sort_copy(
			boost::begin(input), boost::end(input),
			boost::begin(output), boost::end(output),
			std::forward<Pred>(pred));
	}

	template <class SinglePassRange, class RandomAccessRange, class Pred>
	inline auto partial_sort_copy(const SinglePassRange & input, RandomAccessRange & output, Pred && pred)
	{
		return varalgo::partial_sort_copy(
			boost::begin(input), boost::end(input),
			boost::begin(output), boost::end(output),
			std::forward<Pred>(pred));
	}
}