/* FreeCT_ICD is MBIR CT reconstruction Software */
/* Copyright (C) 2018  John Hoffman, Stefano Young, Frederic Noo */

/* This program is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU General Public License */
/* as published by the Free Software Foundation; either version 2 */
/* of the License, or (at your option) any later version. */

/* This program is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the */
/* GNU General Public License for more details. */

/* You should have received a copy of the GNU General Public License */
/* along with this program; if not, write to the Free Software */
/* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA. */

/* Questions and comments should be directed to */
/* jmhoffman@mednet.ucla.edu with "FreeCT_ICD" in the subject line*/

#include <iostream>
#include <iomanip>
#include <fstream>
#include <cmath>
#include <cstdio>
#include "generate_system_matrix.h"
#include "recon_structs.h"

#include <vector>

//#include <Eigen/Dense>
//#include <Eigen/SparseCore>

#include "spinner.h"

#include "boost/numeric/ublas/vector_sparse.hpp"
#include "boost/numeric/ublas/vector.hpp"
#include "boost/numeric/ublas/io.hpp"
namespace ublas = boost::numeric::ublas;

#define PI 3.14159265359

#define debug_disp(VARIABLE_NAME) std::cout << #VARIABLE_NAME": " << VARIABLE_NAME << std::endl;

struct pair{
    int index;
    float value;
};

void save_system_matrix_first_block(const struct recon_params * rp,std::vector<ublas::compressed_vector<float>> & system_matrix_block);
void       save_system_matrix_block(const struct recon_params * rp,std::vector<ublas::compressed_vector<float>> & system_matrix_block);

void generate_system_matrix(const struct recon_params * rp, struct ct_data * data){
    std::cout << "Generating system matrix (no flying focal spots)..." << std::endl;

    ublas::vector<double> source_position(3);
    ublas::vector<double> direction_cosine(3);
    ublas::vector<double> e_w(3);
    ublas::vector<double> e_u(3);
    ublas::vector<double> e_z(3);

    std::cout << rp->num_voxels_x << " : " << rp->num_voxels_y << std::endl;

    std::vector<ublas::compressed_vector<float>> system_matrix(rp->num_voxels_x*rp->num_voxels_y);
    for (int i = 0; i < system_matrix.size(); i++){
        system_matrix[i].resize((size_t)(rp->Readings*rp->n_channels*rp->Nrows_projection));
    }

    // Report initial information about system matrix
    std::cout << "System matrix size: " << system_matrix.size() << std::endl;
    std::cout << "Allocating system matrix..." << std::endl;

    // Attempts with BOOST
    if ((!rp->Zffs)&(!rp->Phiffs)){
        std::cout << "Matrix generation for SciFFSNone configuration."  << std::endl;

        init_spinner();

        for (int i = 0; i < rp->num_views_for_system_matrix; i++){

            update_spinner(i,rp->num_views_for_system_matrix);

            // tk this could be one place where we take the initial tube angle into account
            double tube_angle = (i - 0.5*rp->num_views_for_system_matrix)*rp->tube_angle_increment;

            //Define local coordinate system for the current view
            e_w(0) = cos(tube_angle);
            e_w(1) = sin(tube_angle);
            e_w(2) = 0.0;

            e_u(0) = -sin(tube_angle);
            e_u(1) = cos(tube_angle);
            e_u(2) = 0.0;

            e_z(0) = 0.0;
            e_z(1) = 0.0;
            e_z(2) = 1.0;

            source_position = rp->focal_spot_radius*e_w + rp->table_direction*((double)i - 0.5*(double)rp->num_views_for_system_matrix)*rp->tube_z_increment*e_z;

            for (int j = 0; j < rp->n_channels; j++){
                double transaxial_position = (j - rp->center_channel_non_ffs)*rp->transaxial_detector_spacing;
                double transaxial_angle = transaxial_position / rp->source_detector_distance;
                double cos_transaxial_angle = cos(transaxial_angle);
                double sin_transaxial_angle = sin(transaxial_angle);

                for (int k = 0; k < rp->Nrows_projection; k++){
                    double axial_position = (k - rp->center_row)*rp->axial_detector_spacing;

                    direction_cosine = (-rp->source_detector_distance*cos_transaxial_angle*e_w  \
                                        -rp->source_detector_distance*sin_transaxial_angle*e_u  \
                                        -axial_position*e_z)/sqrt(axial_position*axial_position \
                                        +rp->source_detector_distance*rp->source_detector_distance);

                    int q = k + rp->Nrows_projection*j + rp->Nrows_projection*rp->n_channels*i;
                    //int q = (rp->Nrows_projection-1-k) + rp->Nrows_projection*j + rp->Nrows_projection*rp->n_channels*i;

                    //Compute the contribution of the ray to the system matrix
                    double x, y,
                        x_hat, y_hat, z_hat,
                        i_hat, j_hat, k_hat;

                    double abs_alpha_x = fabs(direction_cosine(0));
                    double abs_alpha_y = fabs(direction_cosine(1));

                    if (abs_alpha_x > abs_alpha_y){

                        //std::cout << "x focused" << std::endl;

                        for (int i = 0; i < rp->num_voxels_x; i++){
                            x = (i - rp->center_voxel_x)*rp->voxel_size_x;

                            y_hat = source_position(1) + direction_cosine(1) / direction_cosine(0)*(x - source_position(0));
                            j_hat = y_hat / rp->voxel_size_y + rp->center_voxel_y;
                            int j = (int)(floor(j_hat));

                            z_hat = source_position(2) + direction_cosine(2) / direction_cosine(0)*(x - source_position(0));
                            k_hat = z_hat / rp->voxel_size_z + rp->center_voxel_z;
                            int k = (int)(floor(k_hat)); // for negative values of k, casting to int gives the wrong answer so I use floor

                            if ((j >= 0) && (j <= (int)(rp->num_voxels_y - 2))){
                                double scale = rp->voxel_size_x / abs_alpha_x;

                                if (k == -1){
                                    system_matrix[i + rp->num_voxels_x*j].push_back(q, (float)(scale*(k_hat - k)*(1 - (j_hat - j))));
                                    system_matrix[i + rp->num_voxels_x*(j + 1)].push_back(q, (float)(scale*(k_hat - k)*(j_hat - j)));
                                }
                                if (k == 0){
                                    system_matrix[i + rp->num_voxels_x*j].push_back(q, (float)(scale*(1 - (k_hat - k))*(1 - (j_hat - j))));
                                    system_matrix[i + rp->num_voxels_x*(j + 1)].push_back(q, (float)(scale*(1 - (k_hat - k))*(j_hat - j)));
                                }
                            }
                        }
                    }
                    else{

                        //std::cout << "y focused" << std::endl;

                        for (int j = 0; j < rp->num_voxels_y; j++){
                            y = (j - rp->center_voxel_y)*rp->voxel_size_y;

                            x_hat = source_position(0) + direction_cosine(0) / direction_cosine(1)*(y - source_position(1));
                            i_hat = x_hat / rp->voxel_size_x + rp->center_voxel_x;
                            int i = (int)(floor(i_hat));

                            z_hat = source_position(2) + direction_cosine(2) / direction_cosine(1)*(y - source_position(1));
                            k_hat = z_hat / rp->voxel_size_z + rp->center_voxel_z;
                            int k = (int)(floor(k_hat));

                            if ((i >= 0) && (i <= (int)(rp->num_voxels_x - 2))){
                                double scale = rp->voxel_size_y / abs_alpha_y;

                                if (k == -1){
                                    system_matrix[i +       rp->num_voxels_x*j].push_back(q, (float)(scale*(k_hat - k)*(1 - (i_hat - i))));
                                    system_matrix[(i + 1) + rp->num_voxels_x*j].push_back(q, (float)(scale*(k_hat - k)*(i_hat - i)));
                                }
                                if (k == 0){
                                    system_matrix[i +       rp->num_voxels_x*j].push_back(q, (float)(scale*(1 - (k_hat - k))*(1 - (i_hat - i))));
                                    system_matrix[(i + 1) + rp->num_voxels_x*j].push_back(q, (float)(scale*(1 - (k_hat - k))*(i_hat - i)));
                                }
                            }
                        }
                    }
                }
            }

            // Save system matrix chunk to disk (should happen ~5 times throughout generation)
            if (i==300){
                save_system_matrix_first_block(rp,system_matrix);
            }
            else if (i%300==0 && i!=0){
                save_system_matrix_block(rp,system_matrix);
            }

        }

        // Save the final chunk to disk
        if (rp->num_views_for_system_matrix<300)
            save_system_matrix_first_block(rp,system_matrix);
        else
            save_system_matrix_block(rp,system_matrix);

        // Clear our user feeback
        destroy_spinner();

        std::cout << "Done!" << std::endl;
    }
}

void save_system_matrix_first_block(const struct recon_params * rp,std::vector<ublas::compressed_vector<float>> & system_matrix_block){

    std::cout << "Saving first block to disk..." << std::endl;

    // Open the output file
    std::string matrix_filepath=rp->output_dir+"/matrix.bin";
    std::ofstream output_file(matrix_filepath.c_str(),std::ios_base::binary);

    // Loop over all columns of our system matrix
    for (int i=0; i<rp->num_voxels_x*rp->num_voxels_y; i++){

        // Get the information about the column chunk in memory
        size_t num_nonzeros_memory=(size_t)system_matrix_block[i].filled();
        struct pair * col_memory=new struct pair[num_nonzeros_memory];

        size_t nonzero_idx=0;
        for (auto pos = system_matrix_block[i].begin(); pos != system_matrix_block[i].end(); ++pos){
            col_memory[nonzero_idx].index=(int)pos.index();
            col_memory[nonzero_idx].value=(*pos);
            nonzero_idx=nonzero_idx + 1;
        }

        // Flush the data to disk (more lines of code than needed for clarity);
        size_t num_nonzeros=num_nonzeros_memory;
        output_file.write((char*)&num_nonzeros,sizeof(num_nonzeros));
        output_file.write((char*)col_memory,num_nonzeros_memory*sizeof(struct pair));

        // Empty contents, and force reallocation of matrix column
        ublas::compressed_vector<float>(system_matrix_block[i].size(), 0).swap(system_matrix_block[i]);

        delete[] col_memory;
    }

    output_file.close();
}

void save_system_matrix_block(const struct recon_params * rp,std::vector<ublas::compressed_vector<float>> & system_matrix_block){

    std::cout << "Saving next block to disk..." << std::endl;

    // Filepaths
    std::string tmp_matrix_filepath=rp->output_dir + "/matrix_tmp.bin";
    std::string matrix_filepath=rp->output_dir+"/matrix.bin";

    // Open our input and output files
    std::ofstream   output_file(tmp_matrix_filepath.c_str(),std::ios_base::binary);
    std::ifstream existing_file(matrix_filepath.c_str(),std::ios_base::binary);

    for (int i=0; i<rp->num_voxels_x*rp->num_voxels_y; i++){

        // Read the already saved chunk of the current column from disk
        size_t num_nonzeros_existing;
        existing_file.read((char*)&num_nonzeros_existing,sizeof(num_nonzeros_existing));

        struct pair * col_disk = new struct pair[num_nonzeros_existing];
        existing_file.read((char*)col_disk,num_nonzeros_existing*sizeof(struct pair));

        // Get the information about the chunk in memory
        size_t num_nonzeros_memory=(size_t)system_matrix_block[i].filled();
        struct pair * col_memory=new struct pair[num_nonzeros_memory];

        size_t nonzero_idx=0;
        for (auto pos = system_matrix_block[i].begin(); pos != system_matrix_block[i].end(); ++pos){
            col_memory[nonzero_idx].index=(int)pos.index();
            col_memory[nonzero_idx].value=(*pos);
            nonzero_idx=nonzero_idx + 1;
        }

        // Flush the data to disk (more lines of code than needed for clarity);
        size_t num_nonzeros=num_nonzeros_existing+num_nonzeros_memory;
        output_file.write((char*)&num_nonzeros,sizeof(num_nonzeros));
        output_file.write((char*)col_disk,num_nonzeros_existing*sizeof(struct pair));
        output_file.write((char*)col_memory,num_nonzeros_memory*sizeof(struct pair));

        // Empty contents, and force reallocation of matrix column
        ublas::compressed_vector<float>(system_matrix_block[i].size(), 0).swap(system_matrix_block[i]);

        delete[] col_memory;
        delete[] col_disk;
    }

    // Overwrite the existing matrix file with the updated temporary file
    int success=rename(tmp_matrix_filepath.c_str(),matrix_filepath.c_str()); // 0 return means success
    if (success!=0){
        std::cout << "There was an error joining matrix chunks. Exiting."  << std::endl;
        exit(1);
    }

    output_file.close();
    existing_file.close();
}


///////////////////////////////////////////////////////////////////////////////
// importance_sampling::src::main.cpp                                        //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <boost/assert.hpp>
#include <libs/statistics/detail/importance_sampling/example/sampler.h>
#include <libs/statistics/detail/importance_sampling/example/scale_to_finite_sum.h>

int main(){

    example_sampler(std::cout);
    example_scale_to_finite_sum(std::cout);

    return 0;
}


/*-------------------------------------------------------------------*\

  NAME
    kitti2pcd

  DESCRIPTION
    Converts binary Point cloud files from the Kitti dataset
    to a PCD ASCII format

    The KITTI dataset:
    http://www.cvlibs.net/datasets/kitti/

    PCD File format:
    http://pointclouds.org/documentation/tutorials/pcd_file_format.html

  AUTHOR
    Jari Honkanen

\*-------------------------------------------------------------------*/

#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <algorithm>

namespace po = boost::program_options;
namespace boostfs = boost::filesystem;

typedef struct
{
    float x;
    float y;
    float z;
    float i;
} PointT;

enum OperModeE
{
    CONVERT_SINGLE_FILE = 0,
    CONVERT_DIRECTORY = 1
};

static OperModeE operMode = CONVERT_DIRECTORY;
static uint outputPrecision = 6;

/*---------------------------------------------------------------*\
    Read KITTI file
\*---------------------------------------------------------------*/
int readKittiFile(std::string inFileName, std::vector<PointT> &pointCloudVector, bool bDebugPrint)
{
    std::ifstream pcInFile(inFileName, std::ios::binary);
    if (!pcInFile)
    {
        std::cerr << "*** Error: readKittiFile(): Point Cloud file'" << inFileName << "' not found." << std::endl;
        return 0;
    }

    // get length of input file:
    pcInFile.seekg(0, pcInFile.end);
    int length = (int)pcInFile.tellg();
    pcInFile.seekg(0, pcInFile.beg);

    int numValues = length / sizeof(float);
    std::cout << "Number of values: " << numValues << std::endl;
    // Read data into buffer vector of floats
    std::vector<float> buffer(numValues);
    pcInFile.read(reinterpret_cast<char *>(&buffer[0]), numValues * sizeof(float));
    pcInFile.close();

    PointT point;
    std::vector<float> angles;
    for (uint32_t i = 0; i < buffer.size(); i += 5)
    {
        point.x = buffer[i];
        point.y = buffer[i + 1];
        point.z = buffer[i + 2];
        point.i = buffer[i + 3];

        float dist_xy = sqrt(point.x * point.x + point.y * point.y);
        float anglez_rad = atan2(point.z, dist_xy) * 180 / M_PI;
        if ((anglez_rad < -15.1) || (anglez_rad < -11.5 && anglez_rad > -12.5) || (anglez_rad < -9.5 && anglez_rad > -10.5) || (anglez_rad < -7.5 && anglez_rad > -8.5) || (anglez_rad < -5.2 && anglez_rad > -6.5) || (anglez_rad < -3.2 && anglez_rad > -4.8) || (anglez_rad < -1.2 && anglez_rad > -2.8) || (anglez_rad < -0.2 && anglez_rad > -0.8) || (anglez_rad > 0.1 && anglez_rad < 0.8) || (anglez_rad > 1.2 && anglez_rad < 1.8))
        {
            continue;
        }
        pointCloudVector.push_back(point);
        if (bDebugPrint)
        {
            angles.push_back(int(anglez_rad * 50) / 50.);
        }
        // std::cout << std::setprecision(outputPrecision) << point.x << " " << point.y << " " << point.z << " " << point.i << " " << dist_xy << " " << anglez_rad << std::endl;
    }
    if (bDebugPrint)
    {
        int c;
        std::sort(angles.begin(), angles.end()); //对于无序的数组需要先排序
        c = (std::unique(angles.begin(), angles.end()) - angles.begin());
        std::cout << "c = " << c << std::endl;
        //打印去重后的数组成员
        for (int i = 0; i < c; i++)
            std::cout << "a = [" << i << "] = " << angles[i] << std::endl;

        std::cout << "File '" << inFileName << "' contains " << length << " bytes and " << pointCloudVector.size() << " points" << std::endl;
    }

    return 1;
}

int WriteKittiFile(std::string outFileName, std::vector<PointT> &pointCloudVector, bool bDebugPrint)
{
    std::ofstream pcOutFile(outFileName, std::ios::binary | std::ios::out | std::ios::app);
    if (!pcOutFile)
    {
        std::cerr << "*** Error: openKittiFile():Couldn't open" << outFileName << "!" << std::endl;
        return 0;
    }

    // get length of input file:
    // pcInFile.seekg(0, pcInFile.end);
    // int length = (int)pcInFile.tellg();
    // pcInFile.seekg(0, pcInFile.beg);

    for (uint32_t i = 0; i < pointCloudVector.size(); i++)
    {
        pcOutFile.write((char *)&pointCloudVector[i].x, 3 * sizeof(float));
        pcOutFile.write((char *)&pointCloudVector[i].i, sizeof(float));

        // std::cout << std::setprecision(outputPrecision) << point.x << " " << point.y << " " << point.z << " " << point.i << " " << dist_xy << " " << anglez_rad << std::endl;
    }

    pcOutFile.close();

    return 1;
}

/*---------------------------------------------------------------*\
    Write PCD file
\*---------------------------------------------------------------*/
int writePcdFile(std::string outFileName, std::vector<PointT> &pointCloudVector, bool bDebugPrint)
{
    std::ofstream pcOutFile(outFileName);
    if (!pcOutFile)
    {
        std::cerr << "*** Error: writePcdFile(): Could not open output file '" << outFileName << "'" << std::endl;
        return 0;
    }

    // Write the PCD Header
    pcOutFile << "# .PCD v.7 - Point Cloud Data file format" << std::endl;
    pcOutFile << "VERSION .7" << std::endl;
    pcOutFile << "FIELDS x y z intensity" << std::endl;
    pcOutFile << "SIZE 4 4 4 4" << std::endl;
    pcOutFile << "TYPE F F F F" << std::endl;
    pcOutFile << "COUNT 1 1 1 1" << std::endl;
    pcOutFile << "WIDTH " << pointCloudVector.size() << std::endl;
    pcOutFile << "HEIGHT 1" << std::endl;
    pcOutFile << "POINTS " << pointCloudVector.size() << std::endl;
    pcOutFile << "DATA ASCII" << std::endl;
    pcOutFile << std::setprecision(outputPrecision);
    // Write points
    for (uint32_t i = 0; i < pointCloudVector.size(); i++)
    {
        pcOutFile << pointCloudVector[i].x << " " << pointCloudVector[i].y << " " << pointCloudVector[i].z << " " << pointCloudVector[i].i << std::endl;
    }

    if (bDebugPrint)
    {
        std::cout << "Wrote " << pointCloudVector.size() << " points to '" << outFileName << "'" << std::endl;
    }

    pcOutFile.close();

    return 1;
}

void resucurePath(boost::filesystem::path src_path, boost::filesystem::path dest_path)
{
  using namespace boost::filesystem;
  if (is_directory(src_path))
  {
    directory_iterator tmp_directory_end;
    directory_iterator tmp_dir_it(src_path);

    for (tmp_dir_it; tmp_dir_it != tmp_directory_end; tmp_dir_it++)
    {
      path child_dest_path = dest_path;
      if (is_directory(*tmp_dir_it))
      {
        std::string tmp_dir_name = (*tmp_dir_it).path().filename().string();
        child_dest_path.append(tmp_dir_name.begin(), tmp_dir_name.end());

        if (!exists(child_dest_path))
        {
          create_directory(child_dest_path);
        }
        std::cout << child_dest_path << std::endl;
      }
      else
      {
        std::string tmp_dir_name = (*tmp_dir_it).path().stem().string();
        child_dest_path.append(tmp_dir_name.begin(), tmp_dir_name.end());
      }
      resucurePath((*tmp_dir_it).path(), child_dest_path);
    }
  }
  else if (is_regular_file(src_path))
  {
    std::string src_path_string = src_path.string();
    std::string src_filename_string = src_path.filename().string();
    std::string src_parent_path_string = src_path.parent_path().string();
    std::string src_parent_dir_string = src_path.parent_path().filename().string();
    std::string dest_path_string = dest_path.parent_path().string()+"/"+src_filename_string;
    std::cout << "src_path_string: " << src_path_string << std::endl;
    std::cout << "src_filename_string: " << src_filename_string << std::endl;
    std::cout << "src_parent_path_string: " << src_parent_path_string << std::endl;
    std::cout << "src_parent_dir_string: " << src_parent_dir_string << std::endl;
    std::cout << "dest_path_string: " << dest_path_string << std::endl;
    if(src_parent_dir_string=="lidar_roof"){
        std::vector<PointT> pointCloudVector;
        readKittiFile(src_path_string, pointCloudVector, true);
        WriteKittiFile(dest_path_string, pointCloudVector, false);
    }
    // FILE *fp(NULL);
    // fp = fopen(dest_path.string().c_str(), "w+b");
    // fclose(fp);
    // fp = NULL;
    // std::cout <<src_path<<" "<< dest_path << std::endl;
  }

}

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

    /*------------------------------------------------------------------------*\
       Process Command Line Options
    \*------------------------------------------------------------------------*/

    // Optional options:
    po::options_description optsDesc("Optional Parameters");

    optsDesc.add_options()("precision,p", po::value<unsigned int>()->default_value(6), "Floating point precision for outputted values, default = 6")("help,h", "Print this help message");

    std::string srcPath;
    std::string destPath;

    // Required source directory or file
    po::options_description positional_params("Required Paramaters");
    positional_params.add_options()("src", po::value<std::string>(&srcPath)->required(), "Source Directory with KITTI bin files or a single KITTI bin file")("dest", po::value<std::string>(&destPath)->required(), "Destination Directory with PCD files or a single PCD file");

    optsDesc.add(positional_params);

    po::positional_options_description posOptsDesc;
    posOptsDesc.add("src", 1);  // Expect one argument
    posOptsDesc.add("dest", 1); // Expect one argument

    int unix_style = boost::program_options::command_line_style::unix_style |
                     boost::program_options::command_line_style::short_allow_next;

    po::variables_map vm;
    try
    {
        po::store(
            po::command_line_parser(argc, argv)
                .options(optsDesc)
                .positional(posOptsDesc)
                .style(unix_style)
                .run(),
            vm);

        po::notify(vm);

        if (argc < 3 || vm.count("help"))
        {
            std::cout << "USAGE: " << argv[0] << "\n"
                      << optsDesc << std::endl;
        }
    }
    catch (po::error &poe)
    {
        std::cerr << poe.what() << "\n"
                  << "USAGE: " << argv[0] << "\n"
                  << optsDesc << std::endl;

        return EXIT_FAILURE;
    }

    if (vm.count("precision"))
    {
        outputPrecision = vm["precision"].as<unsigned int>();
        std::cout << "Setting output precision to " << outputPrecision << std::endl;
    }

    // Check if 'srcPath' paramater is a file or a directory
    boostfs::path ps(srcPath);
    if (boostfs::exists(ps))
    {

        if (is_regular_file(ps))
        { // is source path just a single file?
            operMode = CONVERT_SINGLE_FILE;
        }
        else if (is_directory(ps))
        { // is source path a directory?
            operMode = CONVERT_DIRECTORY;
        }
        else
        {
            std::cerr << "*** ERROR: '" << ps << "' is not a regular file or directory!" << std::endl;
            std::cerr << "USAGE: " << argv[0] << "\n"
                      << optsDesc << std::endl;

            return EXIT_FAILURE;
        }
    }
    else
    {
        std::cerr << "*** ERROR: Source path '" << ps << "' does not exist!" << std::endl;
        std::cerr << "USAGE: " << argv[0] << "\n"
                  << optsDesc << std::endl;

        return EXIT_FAILURE;
    }

    // Check if destination directory exist
    if (operMode == CONVERT_DIRECTORY)
    {
        boostfs::path pd(destPath);
        if (is_regular_file(pd))
        { // Looking for a directory, put destPath is a file
            std::cerr << "*** ERROR: File '" << pd << "' is not a directory!" << std::endl;
            return EXIT_FAILURE;
        }
        if (!is_directory(pd))
        { // if desth path does not exist, create it
            boostfs::create_directories(pd);
        }
    }

    /*------------------------------------------------------------------------*\
       Read and Write Files
    \*------------------------------------------------------------------------*/
    std::vector<PointT> pointCloudVector;

    if (operMode == CONVERT_SINGLE_FILE)
    {
        readKittiFile(srcPath, pointCloudVector, true);
        WriteKittiFile(destPath, pointCloudVector, true);
        // writePcdFile(destPath, pointCloudVector, true);
    }
    else if (operMode == CONVERT_DIRECTORY)
    {

        resucurePath(boost::filesystem::path(srcPath), boost::filesystem::path(destPath));

    }

    return EXIT_SUCCESS;
}


#ifndef WARP_POINT_YAW
#define WARP_POINT_YAW

#pragma once

#include <pcl/registration/warp_point_rigid.h>
#include <boost/shared_ptr.hpp>

namespace pcl {
namespace registration {
  /** \brief @b WarpPointYaw enables 1D rotation around yaw
   * transformations for points.
   *
   * \note The class is templated on the source and target point types as well as on the
   * output scalar of the transformation matrix (i.e., float or double). Default: float.
   * \author Radu B. Rusu
   * \ingroup registration
   */
  template<typename PointSourceT, typename PointTargetT, typename Scalar = float>
  class WarpPointYaw : public WarpPointRigid<PointSourceT, PointTargetT, Scalar>
  {
  public:
    using Matrix4 = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::Matrix4;
    using VectorX = typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::VectorX;

    using Ptr = boost::shared_ptr<WarpPointYaw<PointSourceT, PointTargetT, Scalar>>;
    using ConstPtr =
      boost::shared_ptr<const WarpPointYaw<PointSourceT, PointTargetT, Scalar>>;

    /** \brief Constructor. */
    WarpPointYaw() : WarpPointRigid<PointSourceT, PointTargetT, Scalar>(1) {}

    /** \brief Empty destructor */
    ~WarpPointYaw() {}

    /** \brief Set warp parameters.
     * \param[in] p warp parameters (tx ty rz)
     */
    void setParam(const VectorX &p) override
    {
      assert(p.rows() == this->getDimension());
      Matrix4 &trans = this->transform_matrix_;

      trans = Matrix4::Identity();

      // Copy the rotation and translation components
      trans.block(0, 3, 4, 1) = Eigen::Matrix<Scalar, 4, 1>(0, 0, 0, 1.0);

      // Add only yaw rotation
      Eigen::AngleAxis<Scalar> yawAngle(p[0], Eigen::Matrix<Scalar, 3, 1>(0, 0, 1));
      Eigen::Matrix3f m;
      m = yawAngle;
      trans.topLeftCorner(3, 3) = m;
    }
  };
}// namespace registration
}// namespace pcl

#endif /* WARP_POINT_YAW */


/**
 * A Blackjack/21 example showing how non-deterministic (probabilistic)
 * Episodic Q-Learning works.
 *
 * @version 0.1.0
 * @author Alex Giokas
 * @date 19.11.2016
 *
 * @see the header (blackjack_header.hpp) for implementation details
 */
#include "blackjack_header.hpp"
#include <boost/predef.h>

// create aliases for state and action:
// - a state is the current hand held by a player
// - an action is draw(true) or stay(false)
//
using state = relearn::state<hand>;
using action = relearn::action<bool>;
// policy memory
relearn::policy<state,action> policies;

// catching CTRL-C works only on Linux for now
#if BOOST_OS_LINUX
#include <signal.h>
#include <fstream>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/vector.hpp>
volatile sig_atomic_t flag = 0;
void signal_handler(int sig) {
    flag = 1;
}
#endif

// TODO: add BOOST_OS_WINDOWS
//       and BOOST_OS_MACOS

int main()
{
    signal(SIGINT, signal_handler);
    // PRNG needed for, magic random voodoo
    std::mt19937 gen(static_cast<std::size_t>(std::chrono::high_resolution_clock::now()
                                                           .time_since_epoch().count()));
    // create the dealer, and two players...
    auto dealer = std::make_shared<house>(cards, gen);
    auto agent = std::make_shared<client>();

    using link = relearn::link<state,action>;
    std::deque<std::deque<link>>  experience;

    float sum  = 0;
    float wins = 0;
    std::cout << "starting! Press CTRL-C to stop at any time!"
              << std::endl;
    start:
    // play 10 rounds - then stop
    for (int i = 0; i < 100; i++) {
        sum++;
        std::deque<link> episode;
        // one card to dealer/house
        dealer->reset_deck();
        dealer->insert(dealer->deal());
        // two cards to player
        agent->insert(dealer->deal());
        agent->insert(dealer->deal());
        // root state is starting hand
        auto s_t = agent->state();

        play:
        // if agent's hand is burnt skip all else
        if (agent->min_value() && agent->max_value() > 21) {
            goto cmp;
        }
        // agent decides to draw
        if (agent->draw(gen, s_t, policies)) {
            episode.push_back(link{s_t, action(true)});
            agent->insert(dealer->deal());
            s_t = agent->state();
            goto play;
        }
        // agent decides to stay
        else {
            episode.push_back(link{s_t, action(false)});
        }
        // dealer's turn
        while (dealer->draw()) {
            dealer->insert(dealer->deal());
        }

        cmp:
        // compare hands, assign rewards!
        if (hand_compare(*agent, *dealer)) {
            if (!episode.empty()) {
                episode.back().state.set_reward(1);
            }
            wins++;
        }
        else {
            if (!episode.empty()) {
                episode.back().state.set_reward(-1);
            }
        }

        // clear current hand for both players
        agent->clear();
        dealer->clear();
        experience.push_back(episode);

        std::cout << "\twin ratio: " << wins / sum << std::endl;
        std::cout << "\ton-policy ratio: "
                  << agent->policy_actions / (agent->policy_actions + agent->random_actions)
                  << std::endl;
    }

    // CTRL-C/SIGINT => save and exit
    if (flag) {
    #if USING_BOOST_SERIALIZATION
        std::cout << "save & exit\r\n";
        std::ofstream ofs("blackjack.policy");
        boost::archive::text_oarchive oa(ofs);
        oa << policies;
        ofs.close();
        return 0;
    #else
        std::cout << "exiting...\r\n";
    #endif
    }

    // at this point, we have some playing experience, which we're going to use
    // in order to train the agent.
    relearn::q_probabilistic<state,action> learner;
    for (auto & episode : experience) {
        for (int i = 0; i < 10; i++) {
            learner(episode, policies);
        }
    }

    // clear experience - we'll add new ones!
    experience.clear();
    agent->reset();
    // restart - please don't use goto's in modern C++
    goto start;

    return 0;
}


/*
*
*    data: 2019-5-20
*    auther : Ma Mengyu@National University of Defense Technology
*    e-mail : mamengyu10@nudt.edu.cn
*    description :
*    Hybird-Parallel Visualization Engine
*    run :
*
*/

#include "ogrsf_frmts.h"
#include "ogr_p.h"
#include "cpl_conv.h"
#include "cpl_string.h"
#include "Redis.h"
#include <stdio.h>
#include <cstring>
#include <string>
#include <iostream>
#include <sstream>
#include <vector>
#include <dirent.h>
#include <omp.h>
#include <sys/time.h>
#include <regex.h>
#include <math.h>
#include <mpi.h>
#include <stdlib.h>
#include <queue>
#include <utility>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/foreach.hpp>
#include <boost/interprocess/managed_mapped_file.hpp>
#include <boost/lambda/lambda.hpp>
#include <png.h>
#include <stdlib.h>

#define MAX_NODE_SIZE 8
#define MAX_TILE_PARAMS 256
#define TILE_SIZE 256
#define L 20037508.34

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
namespace bgm = boost::geometry::model;
namespace bi = boost::interprocess;

using namespace std;

typedef bgm::d2::point_xy<double> point;
typedef bgm::box<point> box;
typedef bgm::segment<point> segment;
typedef boost::tuple<segment,unsigned long,bool> polygon_segment;
typedef std::pair<box,unsigned long> polygon_box;
typedef bgi::quadratic<MAX_NODE_SIZE> params;
typedef bgi::indexable<point> indexable_point;
typedef bgi::equal_to<point> equal_to_point;
typedef bi::allocator<point, bi::managed_mapped_file::segment_manager> allocator_point;
typedef bgi::rtree<point, params, indexable_point, equal_to_point, allocator_point> rtree_point;
typedef bgi::indexable<segment> indexable_segment;
typedef bgi::equal_to<segment> equal_to_segment;
typedef bi::allocator<segment, bi::managed_mapped_file::segment_manager> allocator_segment;
typedef bgi::rtree<segment, params, indexable_segment, equal_to_segment, allocator_segment> rtree_segment;
typedef bgi::indexable<polygon_segment> indexable_psegment;
typedef bgi::equal_to<polygon_segment> equal_to_psegment;
typedef bi::allocator<polygon_segment, bi::managed_mapped_file::segment_manager> allocator_psegment;
typedef bgi::rtree<polygon_segment, params, indexable_psegment, equal_to_psegment, allocator_psegment> rtree_psegment;
typedef bgi::indexable<polygon_box> indexable_box;
typedef bgi::equal_to<polygon_box> equal_to_box;
typedef bi::allocator<polygon_box, bi::managed_mapped_file::segment_manager> allocator_box;
typedef bgi::rtree<polygon_box, params, indexable_box, equal_to_box, allocator_box> rtree_box;

/**
 * @brief Compare x span of two polygons
 * @param a  	Input polygon
 * @param b  	Input polygon
 *
 * @return 		If the x span of polygon a is smaller than than the x span of polygon b, return true; otherwise, return true.
 */
bool SortPolygon(polygon_box a, polygon_box b){
	return (bg::get<1,0>(a.first)-bg::get<0,0>(a.first))<(bg::get<1,0>(b.first)-bg::get<0,0>(b.first));
}

/**
 * @brief Split a string into a list where each word is a list item
 * @param argv   	Input string
 * @param result[]  Output Word list
 * @param flag  	Separator used to split the string
 * @param count  	Output Word list length
 *
 */
void GetList(char* argv, char* result[], char* flag, int& count)
{
	char* string = strdup(argv);
	char* p;
	int i = 0;
	while((p = strsep(&string, flag)) != NULL)
	{
		result[i] = p;
		i++;
	}
	result[i] = string;
	count = i;
}

/**
 * @brief Visualize Linestring Objects
 * @param z  			Zoom level of the tile
 * @param x  			x coordinate of the tile
 * @param y  			y coordinate of the tile
 * @param indexPath  	Spatial index path
 * @param dataId  		Dataset id
 * @param tile_area  	Matrix indentifing different regions(e.g. background region, color transition region, the zones of rasterized spatial objects)
 *
 */
void  LineVision(int z,int x,int y,char* indexPath, char *dataId,char *tile_area)
{
	char* tmp=new char[256];
	double Rz,R,R1,R2,r,Rz4;
	rtree_segment * rtree_segment_ptr;
	Rz=L/(128<<z); // Calculate the pixel resolution of zoom level z
	R=2*Rz;		   // R=2*Rz ,we set the visualizaiton radius N to 2 (pixels)
	R1=1.6464*Rz;  // R1=R-sqrt(2)/4*Rz=(2-sqrt(2)/4)*Rz
	R2=2.3536*Rz;  // R2=R+sqrt(2)/4*Rz=(2+sqrt(2)/4)*Rz
	r=0.7071*R1;   // r=sqrt(2)/2*R1
	Rz4=0.25*Rz;   // Rz4=1/4*Rz
	sprintf(tmp,"%s%s",indexPath, dataId);
	bi::managed_mapped_file file(bi::open_only, tmp);
	rtree_segment_ptr = file.find<rtree_segment>("rtree").first;
	#pragma omp parallel for num_threads(2)
	for(int i = 0; i < 256; i++)
	{
		for(int j = 0; j < 256; j ++)
		{
			int tile_index=i*256+j;
			tile_area[tile_index]= 0;
			//Calculate the web mercartor coordinate of the pixel
			double web_mercartor_x = (256*x+j+0.5)*Rz - L;
			double web_mercartor_y = L-(256*y+i-0.5)*Rz;
			//Query the spatial objects with InnerBox1(if there are lots of spatial objects within the distance R1 from the pixel, we query the spatial objects intersects the InnerBox1)
			box InnerBox1(point(web_mercartor_x-r,web_mercartor_y-r),point(web_mercartor_x+r,web_mercartor_y+r));
			rtree_segment::const_query_iterator it=rtree_segment_ptr->qbegin(bgi::intersects(InnerBox1));
			if(it!=rtree_segment_ptr->qend())
			{
				tile_area[tile_index] += 4;
			}
			else
			{
				//Query the spatial objects with OuterBox1(if there are few spatial objects in the neighbor of the pixel, we use the OuterBox1 to filter out the spatial objects which are far from the pixel)
				box OuterBox1(point(web_mercartor_x-R1,web_mercartor_y-R1),point(web_mercartor_x+R1,web_mercartor_y+R1));
				std::vector<segment> segment_result1;
				rtree_segment_ptr->query(bgi::intersects(OuterBox1)&&bgi::nearest(point(web_mercartor_x,web_mercartor_y),1),
				std::back_inserter(segment_result1));
				if(segment_result1.size()>0)
				{
					tile_area[tile_index] += 4;
				}
				else
				{
					//Determine whether the pixel belongs to the color transition regions (calculate the number of sub-pixels that are in the plotting region)
					box OuterBox2(point(web_mercartor_x-R2,web_mercartor_y-R2),point(web_mercartor_x+R2,web_mercartor_y+R2));
					std::vector<segment> segment_result2;
					rtree_segment_ptr->query(bgi::intersects(OuterBox2)&&bgi::nearest(point(web_mercartor_x,web_mercartor_y),1),
					std::back_inserter(segment_result2));
					if(segment_result2.size()>0)
					{
						if(bg::distance(point(web_mercartor_x+Rz4,web_mercartor_y+Rz4),segment_result2.front())<R)
							tile_area[tile_index] += 1;
						if(bg::distance(point(web_mercartor_x-Rz4,web_mercartor_y-Rz4),segment_result2.front())<R)
							tile_area[tile_index] += 1;
						if(bg::distance(point(web_mercartor_x+Rz4,web_mercartor_y-Rz4),segment_result2.front())<R)
							tile_area[tile_index] += 1;
						if(bg::distance(point(web_mercartor_x-Rz4,web_mercartor_y+Rz4),segment_result2.front())<R)
							tile_area[tile_index] += 1;
					}
				}
			}
		}
	}
}

/**
 * @brief Visualize Point Objects
 * @param z  			Zoom level of the tile
 * @param x  			x coordinate of the tile
 * @param y  			y coordinate of the tile
 * @param indexPath  	Spatial index path
 * @param dataId  		Dataset id
 * @param tile_area  	Matrix indentifing different regions(e.g. background region, color transition region, the zones of rasterized spatial objects)
 *
 */
void  PointVision(int z,int x,int y,char* indexPath, char *dataId,char *tile_area)
{
	char* tmp=new char[256];
	double Rz,R,R1,R2,r,Rz4;
	rtree_point * rtree_point_ptr;
	Rz=L/(128<<z); // Calculate the pixel resolution of zoom level z
	R=2*Rz;		   // R=2*Rz ,we set the visualizaiton radius N to 2 (pixels)
	R1=1.6464*Rz;  // R1=R-sqrt(2)/4*Rz=(2-sqrt(2)/4)*Rz
	R2=2.3536*Rz;  // R2=R+sqrt(2)/4*Rz=(2+sqrt(2)/4)*Rz
	r=0.7071*R1;   // r=sqrt(2)/2*R1
	Rz4=0.25*Rz;   // Rz4=1/4*Rz
	sprintf(tmp,"%s%s",indexPath,dataId);
	bi::managed_mapped_file file(bi::open_only, tmp);
	rtree_point_ptr = file.find<rtree_point>("rtree").first;
	#pragma omp parallel for num_threads(2)
	for(int i = 0; i < 256; i++)
	{
		for(int j = 0; j < 256; j ++)
		{
			int tile_index=i*256+j;
			tile_area[tile_index]= 0;
			//Calculate the web mercartor coordinate of the pixel
			double web_mercartor_x = (256*x+j+0.5)*Rz - L;
			double web_mercartor_y = L-(256*y+i-0.5)*Rz;
			//Query the spatial objects with InnerBox1(if there are lots of spatial objects within the distance R1 from the pixel, we query the spatial objects intersects the InnerBox1)
			box InnerBox1(point(web_mercartor_x-r,web_mercartor_y-r),point(web_mercartor_x+r,web_mercartor_y+r));
			rtree_point::const_query_iterator it=rtree_point_ptr->qbegin(bgi::intersects(InnerBox1));
			if(it!=rtree_point_ptr->qend())
			{
				tile_area[tile_index] += 4;
			}
			else
			{
				//Query the spatial objects with OuterBox1(if there are few spatial objects in the neighbor of the pixel, we use the OuterBox1 to filter out the spatial objects which are far from the pixel)
				box OuterBox1(point(web_mercartor_x-R1,web_mercartor_y-R1),point(web_mercartor_x+R1,web_mercartor_y+R1));
				std::vector<point> point_result1;
				rtree_point_ptr->query(bgi::intersects(OuterBox1)&&bgi::nearest(point(web_mercartor_x,web_mercartor_y),1),
				std::back_inserter(point_result1));
				if(point_result1.size()>0)
				{
					tile_area[tile_index] += 4;
				}
				else
				{
					//Determine whether the pixel belongs to the color transition regions (calculate the number of sub-pixels that are in the plotting region)
					box OuterBox2(point(web_mercartor_x-R2,web_mercartor_y-R2),point(web_mercartor_x+R2,web_mercartor_y+R2));
					std::vector<point> point_result2;
					rtree_point_ptr->query(bgi::intersects(OuterBox2)&&bgi::nearest(point(web_mercartor_x,web_mercartor_y),1),
					std::back_inserter(point_result2));
					if(point_result2.size()>0)
					{
						if(bg::distance(point(web_mercartor_x+Rz4,web_mercartor_y+Rz4),point_result2.front())<R)
							tile_area[tile_index] += 1;
						if(bg::distance(point(web_mercartor_x-Rz4,web_mercartor_y-Rz4),point_result2.front())<R)
							tile_area[tile_index] += 1;
						if(bg::distance(point(web_mercartor_x+Rz4,web_mercartor_y-Rz4),point_result2.front())<R)
							tile_area[tile_index] += 1;
						if(bg::distance(point(web_mercartor_x-Rz4,web_mercartor_y+Rz4),point_result2.front())<R)
							tile_area[tile_index] += 1;
					}
				}
			}

		}
	}
}

/**
 * @brief Visualize Pologon Objects
 * @param z  			Zoom level of the tile
 * @param x  			x coordinate of the tile
 * @param y  			y coordinate of the tile
 * @param indexPath  	Spatial index path
 * @param dataId  		Dataset id
 * @param tile_area  	Matrix indentifing different regions(e.g. background region, color transition region, the zones of rasterized spatial objects)
 *
 */
void  PologonVision(int z,int x,int y,char* indexPath, char *dataId,char *tile_area)
{
	char* tmp=new char[256];
	double Rz,R,R1,R2,r,Rz4;
	rtree_psegment * rtree_psegment_ptr;
	rtree_box * rtree_box_ptr;
	Rz=L/(128<<z); // Calculate the pixel resolution of zoom level z
	R=2*Rz;		   // R=2*Rz ,we set the visualizaiton radius N to 2 (pixels)
	R1=1.6464*Rz;  // R1=R-sqrt(2)/4*Rz=(2-sqrt(2)/4)*Rz
	R2=2.3536*Rz;  // R2=R+sqrt(2)/4*Rz=(2+sqrt(2)/4)*Rz
	r=0.7071*R1;   // r=sqrt(2)/2*R1
	Rz4=0.25*Rz;   // Rz4=1/4*Rz
	sprintf(tmp,"%s%s",indexPath,dataId);
	bi::managed_mapped_file file(bi::open_only, tmp);
	rtree_psegment_ptr=file.find<rtree_psegment>("rtree").first;
	sprintf(tmp,"%s%s_mbr",indexPath,dataId);
	bi::managed_mapped_file file_mbr(bi::open_only, tmp);
	rtree_box_ptr=file_mbr.find<rtree_box>("rtree").first;
	#pragma omp parallel for num_threads(2)
	for(int i = 0; i < 256; i++)
	{
		for(int j = 0; j < 256; j ++)
		{
			int tile_index=i*256+j;
			tile_area[tile_index]= 0;
			//Calculate the web mercartor coordinate of the pixel
			double web_mercartor_x = (256*x+j+0.5)*Rz - L;
			double web_mercartor_y = L-(256*y+i-0.5)*Rz;
			//Spatial-Index-Based Filling
			std::vector<polygon_box> box_result;
			rtree_box_ptr->query(bgi::intersects(point(web_mercartor_x,web_mercartor_y)),std::back_inserter(box_result));// Find the candidate polygons
			sort(box_result.begin(),box_result.end(),SortPolygon);//Sort the polygons(Polygon with smaller x span has higher priority)
			BOOST_FOREACH(polygon_box const &v, box_result)// measure the spatial relationship between the pixel and each candidate polygon one by one until the polygon which contains the pixel is found
			{
				unsigned long pID= v.second;
				box pBox= v.first;
				double minx= bg::get<0,0>(pBox);
				double maxx= bg::get<1,0>(pBox);
				int lcount=0;
				std::vector<polygon_segment> psegment_result;
				if(web_mercartor_x-minx<maxx-web_mercartor_x)
					rtree_psegment_ptr->query(bgi::intersects(segment(point(minx,web_mercartor_y),point(web_mercartor_x,web_mercartor_y))),
					std::back_inserter(psegment_result));
				else
					rtree_psegment_ptr->query(bgi::intersects(segment(point(web_mercartor_x,web_mercartor_y),point(maxx,web_mercartor_y))),
				std::back_inserter(psegment_result));
				BOOST_FOREACH(polygon_segment const &sv, psegment_result)
				{
					bool level=boost::get<2>(sv);
					unsigned long pIDtmp= boost::get<1>(sv);
					if (level&&pID==pIDtmp)
						lcount++;
				}
				if(lcount%2>0)//pixel is classified as ’inside the polygon’ if the number of crossings is odd, or ’outside’ if it is an even number
				{
					tile_area[tile_index]=4;
					break;
				}
			}

			//Query the spatial objects with InnerBox1(if there are lots of spatial objects within the distance R1 from the pixel, we query the spatial objects intersects the InnerBox1)
			box InnerBox1(point(web_mercartor_x-r,web_mercartor_y-r),point(web_mercartor_x+r,web_mercartor_y+r));
			rtree_psegment::const_query_iterator it=rtree_psegment_ptr->qbegin(bgi::intersects(InnerBox1));
			if(it!=rtree_psegment_ptr->qend())
			{
				if(tile_area[tile_index]>0)
					tile_area[tile_index] += 4;
				else
					tile_area[tile_index] -= 4;
			}
			else
			{
				//Query the spatial objects with OuterBox1(if there are few spatial objects in the neighbor of the pixel, we use the OuterBox1 to filter out the spatial objects which are far from the pixel)
				box OuterBox1(point(web_mercartor_x-R1,web_mercartor_y-R1),point(web_mercartor_x+R1,web_mercartor_y+R1));
				std::vector<polygon_segment> segment_result1;
				rtree_psegment_ptr->query(bgi::intersects(OuterBox1)&&bgi::nearest(point(web_mercartor_x,web_mercartor_y),1),
				std::back_inserter(segment_result1));
				if(segment_result1.size()>0)
				{
					if(tile_area[tile_index]>0)
						tile_area[tile_index] += 4;
					else
						tile_area[tile_index] -= 4;
				}
				else
				{
					//Determine whether the pixel belongs to the color transition regions (calculate the number of sub-pixels that are in the plotting region)
					box OuterBox2(point(web_mercartor_x-R2,web_mercartor_y-R2),point(web_mercartor_x+R2,web_mercartor_y+R2));
					std::vector<polygon_segment> segment_result2;
					rtree_psegment_ptr->query(bgi::intersects(OuterBox2)&&bgi::nearest(point(web_mercartor_x,web_mercartor_y),1),
					std::back_inserter(segment_result2));
					if(segment_result2.size()>0)
					{
						segment pSegment= boost::get<0>(segment_result2.front());
						if(tile_area[tile_index]>0)
						{
							if(bg::distance(point(web_mercartor_x+Rz4,web_mercartor_y+Rz4),pSegment)<R)
								tile_area[tile_index] += 1;
							if(bg::distance(point(web_mercartor_x-Rz4,web_mercartor_y-Rz4),pSegment)<R)
								tile_area[tile_index] += 1;
							if(bg::distance(point(web_mercartor_x+Rz4,web_mercartor_y-Rz4),pSegment)<R)
								tile_area[tile_index] += 1;
							if(bg::distance(point(web_mercartor_x-Rz4,web_mercartor_y+Rz4),pSegment)<R)
								tile_area[tile_index] += 1;
						}else
						{
							if(bg::distance(point(web_mercartor_x+Rz4,web_mercartor_y+Rz4),pSegment)<R)
								tile_area[tile_index] -= 1;
							if(bg::distance(point(web_mercartor_x-Rz4,web_mercartor_y-Rz4),pSegment)<R)
								tile_area[tile_index] -= 1;
							if(bg::distance(point(web_mercartor_x+Rz4,web_mercartor_y-Rz4),pSegment)<R)
								tile_area[tile_index] -= 1;
							if(bg::distance(point(web_mercartor_x-Rz4,web_mercartor_y+Rz4),pSegment)<R)
								tile_area[tile_index] -= 1;
						}
					}
				}
			}

		}
	}
}

/**
 * @brief HiVision visualization engine main function
 * @param nArgc   	Input parameters copunt
 * @param papszArgv Input startup parameters
 *
 */
int main( int nArgc, char ** papszArgv )
{
	int myId, numProcs;
	MPI_Init(&nArgc,&papszArgv);
	MPI_Comm_rank(MPI_COMM_WORLD,&myId);
	MPI_Comm_size(MPI_COMM_WORLD,&numProcs);
	double t1,t2;
	char* indexPath = papszArgv[1]; 	// Path to store the spatial indexes
	char* redisHost=papszArgv[2];   	// Host IP of Redis
	int redisPort=atoi(papszArgv[3]);	// Host Port of Redis
	CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO");
	CPLSetConfigOption("SHAPE_ENCODING", "UTF-8");
	//Connect Redis
	Redis *redis = new Redis();
	if(!redis->connect(redisHost, redisPort))
	{
		printf("connect redis error!\n");
		exit(0);
		MPI_Finalize();
	}
	char *tile_area = (char*)malloc(TILE_SIZE*TILE_SIZE);
	char *buffer_tmp = (char*)malloc(TILE_SIZE*TILE_SIZE);
	char* task= new char[256];
	char* tile_params[MAX_TILE_PARAMS];
	int count=0;
	int x,y,z;
	if(myId==0)
	{
		printf("indexPath:%s\n",indexPath);
    	printf("Service Start. cores:%d\n",numProcs);
    }
    while(1){
		//Get tasks from Task Pool
		sprintf(task,"%s",redis->brpop("HiVisiontasklist").c_str());
		try{
			if (strlen(task)>0)
			{
				t1=MPI_Wtime();
				//Parse the parameters
				GetList(task, tile_params, (char*)"/", count);
				z=atoi(tile_params[1]);
				x=atoi(tile_params[2]);
				y=atoi(tile_params[3]);
			    if (tile_params[0][0]=='l')//Linestring visualizaiton
					LineVision(z,x,y,indexPath,tile_params[0],tile_area);
				else if(tile_params[0][0]=='p')//Point visualizaiton
					PointVision(z,x,y,indexPath,tile_params[0],tile_area);
				else if(tile_params[0][0]=='a')//Polygon visulization
					PologonVision(z,x,y,indexPath,tile_params[0],tile_area);
				redis->zset(task,tile_area,TILE_SIZE*TILE_SIZE);//Write the result to the Result Pool
				while (! redis->zget(task,buffer_tmp))
					redis->zset(task,tile_area,TILE_SIZE*TILE_SIZE);
				redis->expire(task,"1000");//Set expire time to 1000s(expired results will be cleaned up if memory usage reaches the upper limit)
			    redis->pub("HiVisiontiles",task);//Send task completion message
				t2 = MPI_Wtime();
				printf("tile-%s-%d-%f\n",task, myId, t2-t1);
			}
		}
		catch(...)
		{
			printf("Error task %s \n",task);
		}
	}
	MPI_Finalize();
}


#include <ros/ros.h>
#include <iostream>
#include <string>
#include <Eigen/Geometry>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
#include <lightweight_vicon_bridge/MocapState.h>

typedef Eigen::Affine3d Pose;

std::unique_ptr<tf::TransformListener> g_transform_listener;

ros::Publisher g_left_arm_pose_pub;
ros::Publisher g_right_arm_pose_pub;
std::string g_robot_object_name;
std::string g_robot_segment_name;
std::string g_left_arm_object;
std::string g_right_arm_object;
std::string g_left_arm_segment;
std::string g_right_arm_segment;

void PublishLeftArmPose(tf::StampedTransform& torso_transform, geometry_msgs::Transform& mocap_base_pose, geometry_msgs::Transform& mocap_left_arm_pose)
{
    // Make the Eigen transform for the torso
    Eigen::Translation3d torso_to_base_translation(torso_transform.getOrigin().x(), torso_transform.getOrigin().y(), torso_transform.getOrigin().z());
    Eigen::Quaterniond torso_to_base_rotation(torso_transform.getRotation().w(), torso_transform.getRotation().x(), torso_transform.getRotation().y(), torso_transform.getRotation().z());
    Pose torso_to_base = torso_to_base_translation * torso_to_base_rotation;
    // Make the Eigen transforms for the base
    Eigen::Translation3d world_to_base_translation(mocap_base_pose.translation.x, mocap_base_pose.translation.y, mocap_base_pose.translation.z);
    Eigen::Quaterniond world_to_base_rotation(mocap_base_pose.rotation.w, mocap_base_pose.rotation.x, mocap_base_pose.rotation.y, mocap_base_pose.rotation.z);
    Pose world_to_base = world_to_base_translation * world_to_base_rotation;
    Pose world_to_base_inv = world_to_base.inverse();
    // Make the Eigen transform for the hand
    Eigen::Translation3d world_to_left_hand_translation(mocap_left_arm_pose.translation.x, mocap_left_arm_pose.translation.y, mocap_left_arm_pose.translation.z);
    Eigen::Quaterniond world_to_left_hand_rotation(mocap_left_arm_pose.rotation.w, mocap_left_arm_pose.rotation.x, mocap_left_arm_pose.rotation.y, mocap_left_arm_pose.rotation.z);
    Pose world_to_left_hand = world_to_left_hand_translation * world_to_left_hand_rotation;
    // Make the fixed hand->wrist offset transform
    Eigen::Translation3d left_hand_to_left_wrist_translation(-0.12, 0.0, -0.048);
    Eigen::Quaterniond left_hand_to_left_wrist_rotation(1.0, 0.0, 0.0, 0.0);
    Pose left_hand_to_left_wrist = left_hand_to_left_wrist_translation * left_hand_to_left_wrist_rotation;
    // Compose the transforms together
    Pose torso_to_left_wrist = torso_to_base * world_to_base_inv * world_to_left_hand * left_hand_to_left_wrist;
    // Extract from Eigen and publish
    Eigen::Quaterniond torso_to_left_wrist_rotation(torso_to_left_wrist.rotation());
    geometry_msgs::PoseStamped left_arm_pose;
    left_arm_pose.header.frame_id = "/torso_lift_link";
    left_arm_pose.pose.position.x = torso_to_left_wrist.translation().x();
    left_arm_pose.pose.position.y = torso_to_left_wrist.translation().y();
    left_arm_pose.pose.position.z = torso_to_left_wrist.translation().z();
    left_arm_pose.pose.orientation.x = torso_to_left_wrist_rotation.x();
    left_arm_pose.pose.orientation.y = torso_to_left_wrist_rotation.y();
    left_arm_pose.pose.orientation.z = torso_to_left_wrist_rotation.z();
    left_arm_pose.pose.orientation.w = torso_to_left_wrist_rotation.w();
    g_left_arm_pose_pub.publish(left_arm_pose);
}

void PublishRightArmPose(tf::StampedTransform& torso_transform, geometry_msgs::Transform& mocap_base_pose, geometry_msgs::Transform& mocap_right_arm_pose)
{
    // Make the Eigen transform for the torso
    Eigen::Translation3d torso_to_base_translation(torso_transform.getOrigin().x(), torso_transform.getOrigin().y(), torso_transform.getOrigin().z());
    Eigen::Quaterniond torso_to_base_rotation(torso_transform.getRotation().w(), torso_transform.getRotation().x(), torso_transform.getRotation().y(), torso_transform.getRotation().z());
    Pose torso_to_base = torso_to_base_translation * torso_to_base_rotation;
    // Make the Eigen transforms for the base
    Eigen::Translation3d world_to_base_translation(mocap_base_pose.translation.x, mocap_base_pose.translation.y, mocap_base_pose.translation.z);
    Eigen::Quaterniond world_to_base_rotation(mocap_base_pose.rotation.w, mocap_base_pose.rotation.x, mocap_base_pose.rotation.y, mocap_base_pose.rotation.z);
    Pose world_to_base = world_to_base_translation * world_to_base_rotation;
    Pose world_to_base_inv = world_to_base.inverse();
    // Make the Eigen transform for the hand
    Eigen::Translation3d world_to_right_hand_translation(mocap_right_arm_pose.translation.x, mocap_right_arm_pose.translation.y, mocap_right_arm_pose.translation.z);
    Eigen::Quaterniond world_to_right_hand_rotation(mocap_right_arm_pose.rotation.w, mocap_right_arm_pose.rotation.x, mocap_right_arm_pose.rotation.y, mocap_right_arm_pose.rotation.z);
    Pose world_to_right_hand = world_to_right_hand_translation * world_to_right_hand_rotation;
    // Make the fixed hand->wrist offset transform
    Eigen::Translation3d right_hand_to_right_wrist_translation(-0.12, 0.0, -0.048);
    Eigen::Quaterniond right_hand_to_right_wrist_rotation(1.0, 0.0, 0.0, 0.0);
    Pose right_hand_to_right_wrist = right_hand_to_right_wrist_translation * right_hand_to_right_wrist_rotation;
    // Now that we have all the transforms, compose the transforms together
    Pose torso_to_right_wrist = torso_to_base * world_to_base_inv * world_to_right_hand * right_hand_to_right_wrist;
    // Extract from Eigen and publish
    Eigen::Quaterniond torso_to_right_wrist_rotation(torso_to_right_wrist.rotation());
    geometry_msgs::PoseStamped right_arm_pose;
    right_arm_pose.header.frame_id = "/torso_lift_link";
    right_arm_pose.pose.position.x = torso_to_right_wrist.translation().x();
    right_arm_pose.pose.position.y = torso_to_right_wrist.translation().y();
    right_arm_pose.pose.position.z = torso_to_right_wrist.translation().z();
    right_arm_pose.pose.orientation.x = torso_to_right_wrist_rotation.x();
    right_arm_pose.pose.orientation.y = torso_to_right_wrist_rotation.y();
    right_arm_pose.pose.orientation.z = torso_to_right_wrist_rotation.z();
    right_arm_pose.pose.orientation.w = torso_to_right_wrist_rotation.w();
    g_right_arm_pose_pub.publish(right_arm_pose);
}

void MocapMsgCB(lightweight_vicon_bridge::MocapState msg)
{
    bool got_base = false;
    bool got_left_arm = false;
    bool got_right_arm = false;
    geometry_msgs::Transform base_transform;
    geometry_msgs::Transform left_arm_transform;
    geometry_msgs::Transform right_arm_transform;
    for (size_t idx_o = 0; idx_o < msg.tracked_objects.size(); idx_o++)
    {
        lightweight_vicon_bridge::MocapObject current_object = msg.tracked_objects[idx_o];
        if (current_object.name == g_robot_object_name)
        {
            for (size_t idx_s = 0; idx_s < current_object.segments.size(); idx_s++)
            {
                lightweight_vicon_bridge::MocapSegment current_segment = current_object.segments[idx_s];
                if (current_segment.name == g_robot_segment_name)
                {
                    if (!current_segment.occluded)
                    {
                        base_transform = current_segment.transform;
                        got_base = true;
                    }
                    else
                    {
                        ROS_WARN("Tracking robot base, but base is occluded");
                    }
                }
            }
        }
        else if (current_object.name == g_left_arm_object)
        {
            for (size_t idx_s = 0; idx_s < current_object.segments.size(); idx_s++)
            {
                lightweight_vicon_bridge::MocapSegment current_segment = current_object.segments[idx_s];
                if (current_segment.name == g_left_arm_segment)
                {
                    if (!current_segment.occluded)
                    {
                        left_arm_transform = current_segment.transform;
                        got_left_arm = true;
                    }
                    else
                    {
                        ROS_WARN("Tracking left arm, but left arm is occluded");
                    }
                }
            }
        }
        else if (current_object.name == g_right_arm_object)
        {
            for (size_t idx_s = 0; idx_s < current_object.segments.size(); idx_s++)
            {
                lightweight_vicon_bridge::MocapSegment current_segment = current_object.segments[idx_s];
                if (current_segment.name == g_right_arm_segment)
                {
                    if (!current_segment.occluded)
                    {
                        right_arm_transform = current_segment.transform;
                        got_right_arm = true;
                    }
                    else
                    {
                        ROS_WARN("Tracking right arm, but right arm is occluded");
                    }
                }
            }
        }
    }
    // Query TF to get the transform from /torso_lift_link to /base_footprint
    try
    {
        tf::StampedTransform torso_transform;
        g_transform_listener->lookupTransform("/torso_lift_link", "/base_footprint", ros::Time(0), torso_transform);
        // Now that we have a transform, go to work
        if (got_base)
        {
            if (got_left_arm)
            {
                PublishLeftArmPose(torso_transform, base_transform, left_arm_transform);
            }
            else
            {
                ROS_WARN("Unable to publish left arm pose");
            }
            if (got_right_arm)
            {
                PublishRightArmPose(torso_transform, base_transform, right_arm_transform);
            }
            else
            {
                ROS_WARN("Unable to publish right arm pose");
            }
        }
        else
        {
            ROS_WARN("No base pose - unable to publish either left or right arm pose");
        }
    }
    catch (tf::TransformException ex)
    {
        ROS_WARN("Unable to lookup transform from /torso_lift_link to /base_footprint");
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "mocap_robot_arm_pub");
    ROS_INFO("Starting mocap robot arm pub...");
    ros::NodeHandle nh;
    ros::NodeHandle nhp("~");
    std::string tracker_topic;
    std::string left_arm_pose_topic;
    std::string right_arm_pose_topic;
    nhp.param(std::string("tracker_topic"), tracker_topic, std::string("mocap_tracking"));
    nhp.param(std::string("left_arm_pose_topic"), left_arm_pose_topic, std::string("/l_arm_pose_controller/pose"));
    nhp.param(std::string("right_arm_pose_topic"), right_arm_pose_topic, std::string("/r_arm_pose_controller/pose"));
    nhp.param(std::string("robot_object_name"), g_robot_object_name, std::string("ArchieBase"));
    nhp.param(std::string("robot_segment_name"), g_robot_segment_name, std::string("ArchieBase"));
    nhp.param(std::string("left_arm_object"), g_left_arm_object, std::string("ArchieLeftHand"));
    nhp.param(std::string("right_arm_object"), g_right_arm_object, std::string("ArchieRightHand"));
    nhp.param(std::string("left_arm_segment"), g_left_arm_segment, std::string("ArchieLeftHand"));
    nhp.param(std::string("right_arm_segment"), g_right_arm_segment, std::string("ArchieRightHand"));
    // Create the pose publishers
    g_left_arm_pose_pub = nh.advertise<geometry_msgs::PoseStamped>(left_arm_pose_topic, 1, false);
    g_right_arm_pose_pub = nh.advertise<geometry_msgs::PoseStamped>(right_arm_pose_topic, 1, false);
    // Create the TF listener
    g_transform_listener = std::unique_ptr<tf::TransformListener>(new tf::TransformListener(nh, ros::Duration(20.0)));
    ROS_INFO("Waiting for a few seconds to fill the TF cache...");
    ros::Duration(2.5).sleep();
    // Create the mocap data subscriber
    ros::Subscriber mocap_sub = nh.subscribe(tracker_topic, 1, MocapMsgCB);
    // Start streaming data
    ROS_INFO("Streaming data...");
    while (ros::ok())
    {
        // Handle ROS stuff
        ros::spinOnce();
    }
    return 0;
}


/*
 *
 * boundary.hpp
 * prototypes for boundary fns
 *
 */
#pragma once

#include <cstddef>
#include <cstdint>
#include <string>
#include <vector>
#include <algorithm>
#include <numeric>

#include <Eigen/Dense>

// Parallel sort
#include <boost/sort/sort.hpp>
typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    NumpyMatrix;
typedef std::tuple<std::vector<long>, std::vector<long>, std::vector<long>>
    network_coo;
typedef std::vector<std::tuple<long, long>> edge_tuple;

// https://stackoverflow.com/a/12399290
template <typename T>
std::vector<long> sort_indexes(const T &v, const uint32_t n_threads) {
  // initialize original index locations
  std::vector<long> idx(v.size());
  std::iota(idx.begin(), idx.end(), 0);

  boost::sort::parallel_stable_sort(
      idx.begin(), idx.end(), [&v](long i1, long i2) { return v[i1] < v[i2]; },
      n_threads);

  return idx;
}

Eigen::VectorXf assign_threshold(const NumpyMatrix &distMat, const int slope,
                                 const float x_max, const float y_max,
                                 unsigned int num_threads);

edge_tuple edge_iterate(const NumpyMatrix &distMat, const int slope,
                        const float x_max, const float y_max);

edge_tuple generate_tuples(const std::vector<int> &assignments,
                           const int within_label,
                           bool self,
                           const int num_ref,
                           const int int_offset);

edge_tuple generate_all_tuples(const int num_ref,
                                const int num_queries,
                                bool self,
                                const int int_offset);

network_coo threshold_iterate_1D(const NumpyMatrix &distMat,
                                 const std::vector<double> &offsets,
                                 const int slope, const float x0,
                                 const float y0, const float x1, const float y1,
                                 const int num_threads);

network_coo threshold_iterate_2D(const NumpyMatrix &distMat,
                                 const std::vector<float> &x_max,
                                 const float y_max);


#ifndef _ALGO_HPP_
#define _ALGO_HPP_

#include "../algorithms/util.h"
#include "../algorithms/algo_util.h"
#include "../algorithms/centeralizedMultiRoundSampling.h"
#include "../algorithms/succ_sampling.h"
#include "../algorithms/kmean_plus_plus.h"
#include "../algorithms/kmean_minus_minus.h"

#include "util.hpp"

#include <boost/mpi.hpp>
#include <boost/mpi/collectives.hpp>
#include <iostream>
#include <string>
#include <vector>
#include <random>
#include <map>
#include <exception>
#include <memory>
#include <cstdlib>
#include <boost/serialization/string.hpp>

namespace mpi = boost::mpi;
using namespace std;

// list of global constants
const int ROOT = 0;

class CoresetFunc {
public:
  virtual arma_vectors operator ()(const vector<arma::vec>& U, int k, int t) {};
};

arma_vectors bg_func(const vector<arma::vec>& U, int k, int t) {
  SuccSampling2 bg;
  //TODO: tune those parameters
  bg.setAlpha(1);
  bg.setBeta(0.3);
  bg.setGamma(5);
  ClusteringInput input;
  input.data = U;
  input.n_centers = k;
  input.n_outliers = t;
  ClusteringOutput output;
  bg.perform(input, output);
  auto& C = output.centers;
  auto& indices = output.centers_indices;
  for (int i : output.outliers_indices) {
    C.push_back(U[i]);
    indices.push_back(i);
  }
  return arma_vectors(C, indices);
}

arma_vectors bg_func(const arma_vectors& U, int k, int t) {
  SuccSampling2 bg;
  //TODO: tune those parameters
  bg.setAlpha(2);
  bg.setBeta(4.5);
  bg.setGamma(5);

  ClusteringInput input;
  input.data = to_vecs(U);
  input.n_centers = k;
  input.n_outliers = t;
  ClusteringOutput output;
  bg.perform(input, output);

  arma_vectors res;
  vector<double> ws;
  int i = 0;
  for (const int idx : output.centers_indices) {
    res.data.push_back(U.data[idx]);
    ws.push_back(output.weights[i]);
    ++i;
  }

  for (const int idx : output.outliers_indices) {
    res.data.push_back(U.data[idx]);
    ws.push_back(1);
  }
  res.weights = arma_vec(ws);
  return res;
}

arma_vectors kmmm_func(const arma_vectors& U, int k, int coreset_size) {
  KMeanMinusMinus algo;
  algo.setMaxIter(1000);
  algo.setThresh(0.01);
  ClusteringInput input;
  input.data = to_vecs(U);
  input.n_centers = k;
  input.n_outliers = coreset_size / 2;

  ClusteringOutput output;
  algo.perform(input, output);
  arma_vectors res;
  vector<double> ws;
  // first add outliers
  for (const int idx : output.outliers_indices) {
    res.data.push_back(U.data[idx]);
    ws.push_back(1);
  }

  vector<int> labels;
  vector<double> dists;
  util::compute_nearest(output.centers, input.data, labels, dists);

  auto&& extra_idx = util::sample_with_repl(U.data.size(), coreset_size - input.n_outliers - k);

  extra_idx.insert(extra_idx.end(), labels.begin(), labels.end());

  if (extra_idx.size() != coreset_size - res.data.size()) {
    throw runtime_error("!!!");
  }
  vector<arma::vec> extra_vecs;
  for (int i : extra_idx) {
    extra_vecs.push_back(input.data[i]);
  }

  util::remove_subset(input.data, output.outliers_indices);
  auto&& extra_ws = util::compute_weights(input.data, extra_vecs);

  for (size_t i = 0; i < extra_vecs.size(); ++i) {
    ws.push_back(extra_ws[i]);
    res.data.push_back(U.data[extra_idx[i]]);
  }

  res.weights = arma_vec(ws);
  if (res.data.size() != coreset_size) {
    cerr << "|res| = " << res.data.size() << " coreset_size = " << coreset_size << endl;
    throw runtime_error("kmmm_func: final size error");
  }
  return res;
}

arma_vectors km_func(const arma_vectors& U, const int coreset_size) {
  ClusteringInput input;
  input.data = to_vecs(U);
  input.n_centers = coreset_size;

  ClusteringOutput output;
  KMeanPP algo;
  algo.perform(input, output);

  arma_vectors res;
  vector<double> ws;
  int i = 0;
  for (const int idx : output.centers_indices) {
    res.data.push_back(U.data[idx]);
    ws.push_back(output.weights[i]);
    ++i;
  }
  res.weights = arma_vec(ws);
  return res;
}

arma_vectors rand_func(const arma_vectors& U, int n) {
  arma_vectors res;
  auto&& idx = util::sample_without_repl(U.data.size(), n);
  res.data = util::subset(U.data, idx);
  res.weights = arma_vec(compute_weights(to_vecs(U), to_vecs(res)));
  return res;
}

arma_vectors rand_func(const vector<arma::vec>& U, int k, int t) {
  return arma_vectors(util::subset(U,
				   util::sample_without_repl(U.size(),
							     k + t)));
}

// an even newer version, this version only compute weights
// locally. This version will also return the indices
class AlgoMPI3 {
public:
  void perform(const mpi::communicator& world,
	       const int root,
	       const arma_vectors& U,
	       arma_vectors& coreset,
	       const string& algo_name,
	       const int k,
	       const int t) {
    int coreset_size = 0;
    int outliers_size = 0;
    if (world.rank() != root) {
      coreset_size = k / world.size();
      outliers_size = t / world.size();
    } else {
      coreset_size = k - (world.size() - 1) * (k / world.size());
      outliers_size = t - (world.size() - 1) * (t / world.size());
    }
    if (coreset_size < 1) {
      coreset_size = 1;
    }
    // locally construct coreset
    arma_vectors local_set;
    if (algo_name == "rand") {
      local_set = rand_func(U, coreset_size);
    } else if (algo_name == "km") {
      local_set = km_func(U, coreset_size);
    } else if (algo_name == "kmean--") {
      local_set = kmmm_func(U, k, outliers_size);
    } else {
      //      local_set = bg_func(U, coreset_size, outliers_size);
      local_set = bg_func(U, k, outliers_size);
    }

    // compute local weights and send to root
    //    local_set.weights.data = compute_weights(to_vecs(U), to_vecs(local_set));

    vector<arma_vectors> all_sets;
    if (world.rank() == root) {
      mpi::gather(world, local_set, all_sets, root);
    } else {
      mpi::gather(world, local_set, root);
    }

    // aggregate local_set/weights in root
    if (world.rank() == root) {
      coreset.clear();
      vector<double>& ws = coreset.weights.data;
      for (const auto& v : all_sets) {
	coreset.data.insert(coreset.data.end(), v.data.begin(), v.data.end());
	ws.insert(ws.end(), v.weights.data.begin(), v.weights.data.end());
      }
    }
    if (coreset.weights.data.size() != coreset.data.size()) {
      runtime_error("AlgoMPI3: |weights| != |coreset|");
    }
  }
};

// this version computes weights
// locally. This version will also return the indices
class AlgoMPI4 {
public:
  void perform(const mpi::communicator& world,
	       const int root,
	       const arma_vectors& U,
	       arma_vectors& coreset,
	       const string& algo_name,
	       const int k,
	       const int t) {
    int coreset_size = 0;
    int outliers_size = 0;
    if (world.rank() != root) {
      coreset_size = k / world.size();
      outliers_size = t / world.size();
    } else {
      coreset_size = k - (world.size() - 1) * (k / world.size());
      outliers_size = t - (world.size() - 1) * (t / world.size());
    }
    // locally construct coreset
    arma_vectors local_set;
    if (algo_name == "rand") {
      local_set = rand_func(U, coreset_size);
    } else {
      local_set = bg_func(U, coreset_size, outliers_size);
    }

    // compute local weights and send to root
    //    local_set.weights.data = compute_weights(to_vecs(U), to_vecs(local_set));

    vector<arma_vectors> all_sets;
    if (world.rank() == root) {
      mpi::gather(world, local_set, all_sets, root);
    } else {
      mpi::gather(world, local_set, root);
    }

    mpi::broadcast(world, all_sets, root);

    // merge all points sampled in different sites
    coreset.clear();
    vector<double>& ws = coreset.weights.data;
    for (const auto& v : all_sets) {
      coreset.data.insert(coreset.data.end(), v.data.begin(), v.data.end());
      //      ws.insert(ws.end(), v.weights.data.begin(), v.weights.data.end());
    }

    // compute weights locally and send to root
    coreset.weights.data = compute_weights(to_vecs(U), to_vecs(coreset));
    if (coreset.weights.data.size() != coreset.data.size()) {
      runtime_error("AlgoMPI4: |weights| != |coreset|");
    }
    vector<arma_vec> all_weights;
    if (world.rank() == root) {
      mpi::gather(world, coreset.weights, all_weights, root);
    } else {
      mpi::gather(world, coreset.weights, root);
    }

    // aggregate weights in root
    if (world.rank() == root) {
      arma::vec&& ws = arma::vec(all_weights[0].data);
      for (int i = 1; i < all_weights.size(); ++i) {
	ws += arma::vec(all_weights[i].data);
      }
      coreset.weights.data = util::arma_vec_to_vector(ws);
    }
  }
};

// new version, this version only compute weights
// locally.
class AlgoMPI2 {
public:
  void perform(const mpi::communicator& world,
	       const int root,
	       const vector<arma::vec>& U,
	       vector<arma::vec>& coreset,
	       arma::vec& weights,
	       const string& algo_name,
	       const int k,
	       const int t) {
    int coreset_size = 0;
    int outliers_size = 0;
    if (world.rank() != root) {
      coreset_size = k / world.size();
      outliers_size = t / world.size();
    } else {
      coreset_size = k - (world.size() - 1) * (k / world.size());
      outliers_size = t - (world.size() - 1) * (t / world.size());
    }
    // locally construct coreset
    arma_vectors local_set;
    if (algo_name == "rand") {
      local_set = rand_func(U, coreset_size, outliers_size);
    } else {
      local_set = bg_func(U, coreset_size, outliers_size);
    }

    coreset = to_vecs(local_set);
    // compute local weights and send to root
    local_set.weights.data = compute_weights(U, coreset);

    vector<arma_vectors> all_sets;
    if (world.rank() == root) {
      mpi::gather(world, local_set, all_sets, root);
    } else {
      mpi::gather(world, local_set, root);
    }

    // aggregate local_set/weights in root
    if (world.rank() == root) {
      vector<double> ws;
      coreset.clear();
      for (const auto& v : all_sets) {
	auto&& ss = to_vecs(v);
	coreset.insert(coreset.end(), ss.begin(), ss.end());
	ws.insert(ws.end(), v.weights.data.begin(), v.weights.data.end());
      }
      weights = arma::vec(ws);
    }
    if (weights.size() != coreset.size()) {
      runtime_error("AlgoMPI2: |weights| != |coreset|");
    }
  }
};

class AlgoMPI {
public:
  void perform(const mpi::communicator& world,
	       const int root,
	       const vector<arma::vec>& U,
	       vector<arma::vec>& coreset,
	       arma::vec& weights,
	       const string& algo_name,
	       const int k,
	       const int t) {
    int coreset_size = 0;
    int outliers_size = 0;
    if (world.rank() != root) {
      coreset_size = k / world.size();
      outliers_size = t / world.size();
    } else {
      coreset_size = k - (world.size() - 1) * (k / world.size());
      outliers_size = t - (world.size() - 1) * (t / world.size());
    }
    // locally construct coreset
    arma_vectors local_set;
    if (algo_name == "rand") {
      local_set = rand_func(U, coreset_size, outliers_size);
    } else {
      local_set = bg_func(U, coreset_size, outliers_size);
    }

    vector<arma_vectors> all_sets;
    if (world.rank() == root) {
      mpi::gather(world, local_set, all_sets, root);
    } else {
      mpi::gather(world, local_set, root);
    }

    mpi::broadcast(world, all_sets, root);
    // merge all sampled points as the coreset, this happens in each
    // process
    coreset.clear();
    for (const auto& s : all_sets) {
      auto&& ss = to_vecs(s);
      coreset.insert(coreset.end(), ss.begin(), ss.end());
    }

    // compute local weights and send to root
    arma_vec ws;
    ws.data = compute_weights(U, coreset);
    vector<arma_vec> all_weights;
    if (world.rank() == root) {
      mpi::gather(world, ws, all_weights, root);
    } else {
      mpi::gather(world, ws, root);
    }

    // aggregate weights in root
    if (world.rank() == root) {
      weights = arma::vec(all_weights[0].data);
      for (int i = 1; i < all_weights.size(); ++i) {
	weights += arma::vec(all_weights[i].data);
      }
    }
  }

private:
  CoresetFunc func_;
};

void kmeanpp(const vector<arma::vec>& U,
             const arma::vec& weights,
             const int n_centers,
             vector<arma::vec>& coreset,
             arma::vec& ws) {
  Kmeanspp kmpp;
  ClusteringInput input;
  input.data = U;
  input.weights = weights;
  input.n_centers = n_centers;
  ClusteringOutput output;
  kmpp.perform(input, output);
  output.make_sure_labels_weights_outliers(input);
  coreset = output.centers;
  ws = output.weights;
}

void kmean_par(const mpi::communicator& world,
	       const int root,
	       const vector<arma::vec>& U,
	       vector<arma::vec>& coreset,
	       arma::vec& weights,
	       const int k,
	       int& msg_size) {
  msg_size = 0;
  const int n_rounds = 6;
  const double ell = k;
  arma_vectors C;  // centers
  C.data.push_back(arma_vec(U[0]));  // pick a initial center

  // initialize a uniform distribution
  std::random_device rd;  //Will be used to obtain a seed for the random number engine
  std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
  std::uniform_real_distribution<> dis(0, 1.0);

  for (int r = 0; r < n_rounds; ++r) {
    if (world.rank() != root) {
      C.data.clear();
    }

    mpi::broadcast(world, C, root);
    msg_size += C.data.size();
    const auto&& CC = to_vecs(C);

    // compute dists
    vector<double> dists;
    vector<int> labels;
    util::compute_nearest(U, CC, labels, dists);
    double phi_local = util::sum2(dists);
    double phi = -1;

    if (world.rank() == root) {
      vector<double> phis;
      mpi::gather(world, phi_local, phis, root);
      phi = util::sum(phis);
    } else {
      mpi::gather(world, phi_local, root);
    }
    mpi::broadcast(world, phi, root);

    // each processor has the phi, now will perform sampling
    arma_vectors new_centers;
    for (size_t i = 0; i < U.size(); ++i) {
      double rd = dis(gen);
      if (rd < ell * dists[i] * dists[i] / phi) {
        new_centers.data.push_back(arma_vec(U[i]));
      }
    }

    // gather all new centers
    if (world.rank() == root) {
      vector<arma_vectors> vec_new_centers;
      mpi::gather(world, new_centers, vec_new_centers, root);
      for (const auto& nc : vec_new_centers) {
        C.data.insert(C.data.end(), nc.data.begin(), nc.data.end());
      }
    } else {
      mpi::gather(world, new_centers, root);
    }
  }

  // broadcast the final coreset
  if (world.rank() != root) {
    C.data.clear();
  }
  mpi::broadcast(world, C, root);
  coreset = to_vecs(C);

  // compute local weights and send to root
  arma_vec ws;
  ws.data = compute_weights(U, coreset);
  vector<arma_vec> all_weights;
  if (world.rank() == root) {
    mpi::gather(world, ws, all_weights, root);
  } else {
    mpi::gather(world, ws, root);
  }

  // aggregate weights in root
  if (world.rank() == root) {
    weights = arma::vec(all_weights[0].data);
    for (int i = 1; i < all_weights.size(); ++i) {
      weights += arma::vec(all_weights[i].data);
    }
  }
}

void mpi_kmean(const mpi::communicator& world,
	       const int root,
	       const arma_vectors& U,
	       arma_vectors& coreset,
	       const int k,
	       int& msg_size) {
  msg_size = 0;
  const int n_rounds = 5;
  const double ell = k;
  arma_vectors C;  // centers
  C.data.push_back(U.data[0]);  // pick a initial center

  // initialize a uniform distribution
  std::random_device rd;  //Will be used to obtain a seed for the random number engine
  std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
  std::uniform_real_distribution<> dis(0, 1.0);

  auto&& UU = to_vecs(U);
  // compute dists
  vector<double> dists;
  vector<int> labels;
  util::compute_nearest(UU, to_vecs(C), labels, dists);

  arma_vectors new_centers = C;
  for (int r = 0; r < n_rounds; ++r) {
    if (world.rank() != root) {
      new_centers.clear();
    }

    mpi::broadcast(world, new_centers, root);
    msg_size += new_centers.data.size();
    const auto&& CC = to_vecs(new_centers);

    // update dists based on new_centers
    vector<double> new_dists;
    vector<int> new_labels;
    util::compute_nearest(UU, CC, new_labels, new_dists);
    for (size_t i = 0; i < UU.size(); ++i) {
      if (new_dists[i] < dists[i]) {
	dists[i] = new_dists[i];
      }
    }
    double phi_local = util::sum2(dists);
    double phi = -1;

    if (world.rank() == root) {
      vector<double> phis;
      mpi::gather(world, phi_local, phis, root);
      phi = util::sum(phis);
    } else {
      mpi::gather(world, phi_local, root);
    }
    mpi::broadcast(world, phi, root);

    // each processor has the phi, now will perform sampling
    //    arma_vectors new_centers;
    new_centers.clear();
    for (size_t i = 0; i < U.data.size(); ++i) {
      double rd = dis(gen);
      if (rd < ell * dists[i] * dists[i] / phi) {
        new_centers.data.push_back(U.data[i]);
      }
    }

    // gather all new centers
    if (world.rank() == root) {
      vector<arma_vectors> vec_new_centers;
      mpi::gather(world, new_centers, vec_new_centers, root);
      new_centers.clear();
      for (const auto& nc : vec_new_centers) {
        C.data.insert(C.data.end(), nc.data.begin(), nc.data.end());
	new_centers.data.insert(new_centers.data.end(), nc.data.begin(), nc.data.end());
      }
    } else {
      mpi::gather(world, new_centers, root);
    }
  }

  // broadcast the final coreset
  if (world.rank() != root) {
    C.data.clear();
  }
  mpi::broadcast(world, C, root);
  coreset = C;

  // compute local weights and send to root
  arma_vec ws;
  ws.data = compute_weights(to_vecs(U), to_vecs(coreset));
  vector<arma_vec> all_weights;
  if (world.rank() == root) {
    mpi::gather(world, ws, all_weights, root);
  } else {
    mpi::gather(world, ws, root);
  }

  // aggregate weights in root
  if (world.rank() == root) {
    auto&& weights = arma::vec(all_weights[0].data);
    for (int i = 1; i < all_weights.size(); ++i) {
      weights += arma::vec(all_weights[i].data);
    }
    coreset.weights = arma_vec(weights);
  }
}

#endif


#define BOOST_TEST_MODULE pcraster discr raster
#include <boost/test/unit_test.hpp>
#include "discr_raster.h"

BOOST_AUTO_TEST_CASE(constructor)
{
  using namespace discr;

  size_t nrRows = 3;
  size_t nrCols = 4;
  double cellSize = 1.5;
  double west = 1.0;
  double north = 0.0;

  {
    Raster raster(nrRows, nrCols);
    BOOST_CHECK(raster.nrRows() == nrRows);
    BOOST_CHECK(raster.nrCols() == nrCols);
    BOOST_CHECK(raster.nrCells() == nrRows * nrCols);
    BOOST_CHECK(raster.cellSize() == 1.0);
    BOOST_CHECK(raster.west() == 0.0);
    BOOST_CHECK(raster.north() == 0.0);
  }

  {
    Raster raster(nrRows, nrCols, cellSize, west, north);
    BOOST_CHECK(raster.nrRows() == nrRows);
    BOOST_CHECK(raster.nrCols() == nrCols);
    BOOST_CHECK(raster.nrCells() == nrRows * nrCols);
    BOOST_CHECK(raster.cellSize() == cellSize);
    BOOST_CHECK(raster.west() == west);
    BOOST_CHECK(raster.north() == north);
  }
}

BOOST_AUTO_TEST_CASE(equals)
{
  using namespace discr;

  Raster raster1(3, 4, 1.5, 1.0, 0.0);
  Raster raster2(3, 4, 1.5, 1.0, 1.0);
  BOOST_CHECK(raster1 == raster1);
  BOOST_CHECK(raster2 == raster2);
  BOOST_CHECK(raster1 != raster2);
  BOOST_CHECK(raster2 != raster1);
}


/**
*  @file
*  @copyright defined in xmax/LICENSE
*/
#pragma once
#include <pro/types/build.hpp>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>
namespace pro
{
	/**
	* typedef of string, using stl version
	*/
	using string = std::string;
	/**
	* typedef of uint8, using stl version
	*/
	using uint8 = std::uint8_t;
	/**
	* typedef of uint16, using stl version
	*/
	using uint16 = std::uint16_t;
	/**
	* typedef of uint32, using stl version
	*/
	using uint32 = std::uint32_t;
	/**
	* typedef of uint64, using stl version
	*/
	using uint64 = std::uint64_t;
	/**
	* typedef of int8, using stl version
	*/
	using int8 = std::int8_t;
	/**
	* typedef of int16, using stl version
	*/
	using int16 = std::int16_t;
	/**
	* typedef of int32, using stl version
	*/
	using int32 = std::int32_t;
	/**
	* typedef of int64, using stl version
	*/
	using int64 = std::int64_t;

	using uint128 = boost::multiprecision::uint128_t;

	using int128 = boost::multiprecision::int128_t;

	using int256 = boost::multiprecision::int256_t;

	using uint256 = boost::multiprecision::uint256_t;
}

#define MB_SIZE (1024 * 1024)

#define MB(v) (v * MB_SIZE)


/*
 [auto_generated]
 boost/numeric/odeint/iterator/adaptive_time_iterator.hpp

 [begin_description]
 Iterator for iterating throught the solution of an ODE with adaptive step size. The dereferenced types containes also the time.
 [end_description]

 Copyright 2012-2013 Karsten Ahnert
 Copyright 2012-2013 Mario Mulansky

 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 BOOST_NUMERIC_ODEINT_ITERATOR_ADAPTIVE_TIME_ITERATOR_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_ITERATOR_ADAPTIVE_TIME_ITERATOR_HPP_INCLUDED

#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/util/stepper_traits.hpp>
#include <boost/numeric/odeint/iterator/impl/adaptive_iterator_impl.hpp>

namespace boost {
namespace numeric {
namespace odeint {

    /* use the adaptive_iterator_impl with the right tags */
    template< class Stepper , class System , class State
#ifndef DOXYGEN_SKIP
        , class StepperTag = typename base_tag< typename traits::stepper_category< Stepper >::type >::type
#endif
    >
    class adaptive_time_iterator : public adaptive_iterator_impl<
            adaptive_time_iterator< Stepper , System , State , StepperTag > ,
            Stepper , System , State , detail::ode_state_time_iterator_tag , StepperTag
        >
    {
        typedef typename traits::time_type< Stepper >::type time_type;
        typedef adaptive_time_iterator< Stepper , System , State , StepperTag > iterator_type;

    public:
        adaptive_time_iterator( Stepper stepper , System sys , State &s , time_type t_start , time_type t_end , time_type dt )
            : adaptive_iterator_impl< iterator_type , Stepper , System , State , detail::ode_state_time_iterator_tag , StepperTag >( stepper , sys , s , t_start , t_end , dt )
        {}

        adaptive_time_iterator( Stepper stepper , System sys , State &s )
            : adaptive_iterator_impl< iterator_type , Stepper , System , State , detail::ode_state_time_iterator_tag , StepperTag >( stepper , sys , s )
        {}
    };

    template< class Stepper , class System , class State >
    adaptive_time_iterator< Stepper , System , State > make_adaptive_time_iterator_begin(
        Stepper stepper ,
        System system ,
        State &x ,
        typename traits::time_type< Stepper >::type t_start ,
        typename traits::time_type< Stepper >::type t_end ,
        typename traits::time_type< Stepper >::type dt )
    {
        return adaptive_time_iterator< Stepper , System , State >( stepper , system , x , t_start , t_end , dt );
    }

    template< class Stepper , class System , class State >
    adaptive_time_iterator< Stepper , System , State > make_adaptive_time_iterator_end(
        Stepper stepper ,
        System system ,
        State &x )
    {
        return adaptive_time_iterator< Stepper , System , State >( stepper , system , x );
    }

    template< class Stepper , class System , class State >
    std::pair< adaptive_time_iterator< Stepper , System , State > , adaptive_time_iterator< Stepper , System , State > >
    make_adaptive_time_range(
        Stepper stepper ,
        System system ,
        State &x ,
        typename traits::time_type< Stepper >::type t_start ,
        typename traits::time_type< Stepper >::type t_end ,
        typename traits::time_type< Stepper >::type dt )
    {
        return std::make_pair(
            adaptive_time_iterator< Stepper , System , State >( stepper , system , x , t_start , t_end , dt ) ,
            adaptive_time_iterator< Stepper , System , State >( stepper , system , x ) );
    }

    /**
     * \class adaptive_time_iterator
     *
     * \brief ODE Iterator with adaptive step size. The value type of this iterator is a std::pair containing state and time.
     *
     * Implements an iterator representing the solution of an ODE from t_start
     * to t_end evaluated at steps with an adaptive step size dt.
     * After each iteration the iterator dereferences to a pair containing state
     * and time at the next time point t+dt where dt is controlled by the stepper.
     * This iterator can be used with ControlledSteppers and
     * DenseOutputSteppers and it always makes use of the all the given steppers
     * capabilities. A for_each over such an iterator range behaves similar to
     * the integrate_adaptive routine.
     *
     * adaptive_iterator is a model of single-pass iterator.
     *
     * The value type of this iterator is a std::pair of  state and time of the stepper.
     *
     * \tparam Stepper The stepper type which should be used during the iteration.
     * \tparam System The type of the system function (ODE) which should be solved.
     * \tparam State The state type of the ODE.
     */

    /**
     * \fn make_adaptive_time_iterator_begin( Stepper stepper , System system , State &x ,
         typename traits::time_type< Stepper >::type t_start ,
         typename traits::time_type< Stepper >::type t_end ,
         typename traits::time_type< Stepper >::type dt )
     *
     * \brief Factory function for adaptive_time_iterator. Constructs a begin iterator.
     *
     * \param stepper The stepper to use during the iteration.
     * \param system The system function (ODE) to solve.
     * \param x The initial state. adaptive_time_iterator stores a reference of s and changes its value during the iteration.
     * \param t_start The initial time.
     * \param t_end The end time, at which the iteration should stop.
     * \param dt The initial time step.
     * \returns The adaptive time iterator.
     */

    /**
     * \fn make_adaptive_time_iterator_end( Stepper stepper , System system , State &x )
     * \brief Factory function for adaptive_time_iterator. Constructs a end iterator.
     *
     * \param stepper The stepper to use during the iteration.
     * \param system The system function (ODE) to solve.
     * \param x The initial state. adaptive_time_iterator stores a reference of s and changes its value during the iteration.
     * \returns The adaptive time iterator.
     */

    /**
     * \fn make_adaptive_time_range( Stepper stepper , System system , State &x ,
        typename traits::time_type< Stepper >::type t_start ,
        typename traits::time_type< Stepper >::type t_end ,
        typename traits::time_type< Stepper >::type dt )
     *
     * \brief Factory function to construct a single pass range of adaptive time iterators. A range is here a pair of adaptive_time_iterators.
     *
     * \param stepper The stepper to use during the iteration.
     * \param system The system function (ODE) to solve.
     * \param x The initial state. adaptive_time_iterator stores a reference of s and changes its value during the iteration.
     * \param t_start The initial time.
     * \param t_end The end time, at which the iteration should stop.
     * \param dt The initial time step.
     * \returns The adaptive time range.
     */

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_ITERATOR_ADAPTIVE_TIME_ITERATOR_HPP_INCLUDED


// C/C++ File

// Author:   Alexandre Tea <alexandre.qtea@gmail.com>
// File:     /Users/alexandretea/Work/ddti/srcs/utils/matrix.hpp
// Purpose:  matrix utilities
// Created:  2017-08-14 16:44:51
// Modified: 2017-08-23 23:19:40

#ifndef MATRIX_H
#define MATRIX_H

#include <armadillo>

namespace utils {
namespace matrix {

template <typename T>
arma::Mat<T>
load_arma_matrix(T* buffer, size_t nb_elems, size_t nb_entries,
                 bool column_major = true, bool free_buffer = true)
{
    arma::Mat<T>    matrix(buffer, nb_elems, nb_entries);

    if (not column_major) {
        // armadillo matrices are column-major and the matrix we
        // received is row-major, so we transpose it
        matrix = matrix.t();
    }
    if (free_buffer)
        delete buffer;
    return matrix;
}

}   // end of namespace matrix
}   // end of namespace utils

#endif /* end of include guard: MATRIX_H */


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

#include "../src/subsequences.hpp"

#include <vector>
#include <iterator>
#include <sstream>

using std::vector;
using std::back_inserter;
using std::ostringstream;

BOOST_AUTO_TEST_CASE(unite_test)
{
    subset_t a = 0b011;
    subset_t b = 0b101;
    subset_t c = 0b110;

    BOOST_CHECK_EQUAL(unite(a, b), unite(b, c));
}

BOOST_AUTO_TEST_CASE(print_test)
{
    subset_t a = 0b00101011;
    subset_t b = 0b10101010;
    subset_t c = 0b00000000;

    ostringstream oss;
    print_subset(a, oss);
    BOOST_CHECK_EQUAL(oss.str(), "[0, 1, 3, 5]");

    oss = ostringstream{};
    print_subset(b, oss);
    BOOST_CHECK_EQUAL(oss.str(), "[1, 3, 5, 7]");

    oss = ostringstream{};
    print_subset(c, oss);
    BOOST_CHECK_EQUAL(oss.str(), "[]");
}

BOOST_AUTO_TEST_CASE(two_subsequences_test)
{
    relevant_subsets_t<2> expected1 = {{0, 2, 1, 3}};

    relevant_subsets_t<2> got1 = relevant_subsets<2>();
    BOOST_CHECK(got1 == expected1);

    relevant_subsets_t<3> expected2 = {{0, 4, 2, 6, 1, 5, 3, 7}};

    relevant_subsets_t<3> got2 = relevant_subsets<3>();
    BOOST_CHECK(got2 == expected2);
}


/*!
 * @file     mppi_methods.cpp
 * @author   Giuseppe Rizzi
 * @date     11.06.2020
 * @version  1.0
 * @brief    description
 */

#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <array>
#include <chrono>
#include "mppi/policies/receding_horizon_spline.h"

using namespace Eigen;
using namespace mppi;

class RecedingHorizonSplineTest : public ::testing::Test {
 protected:
  void SetUp() override {
    config_.samples = 5;
    config_.step_size = 0.1;
    config_.sigma = 1.0;
    config_.min_value = -1.0;
    config_.max_value = 1.0;
    config_.dt = 0.01;
    config_.cp_dt = 0.1;
    config_.horizon = 1.0;
    config_.degree = 3;
    config_.apply_bounds = true;

    spline_ = std::make_unique<RecedingHorizonSpline>(config_);
  }

  BSplinePolicyConfig config_;
  std::unique_ptr<RecedingHorizonSpline> spline_;
};

TEST_F(RecedingHorizonSplineTest, VerboseRun) {
  int n_cpoints = spline_->c_points_.values_.size();
  std::vector<Eigen::ArrayXd> control_polygons;
  for (int i = 0; i < config_.samples; i++) {
    control_polygons.push_back(spline_->get_sample_control_polygon(i));
  }

  // shifting less then discretization dt should not change the control polygon
  double delta_time = 0.000001;
  spline_->shift(delta_time);
  for (int i = 0; i < config_.samples; i++) {
    ASSERT_TRUE(
        control_polygons[i].isApprox(spline_->get_sample_control_polygon(i)));
  }

  // shifting more should shift c-polygon of one point only
  spline_->shift(config_.cp_dt + delta_time);
  for (int i = 0; i < config_.samples; i++) {
    ASSERT_TRUE(control_polygons[i]
                    .tail(n_cpoints - 1)
                    .isApprox(spline_->get_sample_control_polygon(i).head(
                        n_cpoints - 1)));
  }
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}