#include <iostream>
#include <fstream>

#include <boost/filesystem.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/fpfh.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>

//Struct to read pcd files from http://rgbd-dataset.cs.washington.edu/dataset/rgbd-dataset_pcd_ascii/
struct PointXYZRGBIM
{
  union
  {
    struct
    {
      float x;
      float y;
      float z;
      float rgb;
      float imX;
      float imY;
    };
    float data[6];
  };
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRGBIM,
                                    (float, x, x)
                                    (float, y, y)
                                    (float, z, z)
                                    (float, rgb, rgb)
                                    (float, imX, imX)
                                    (float, imY, imY)
)

//variables used to keep track over how many files were converted and what is the total size.
// If total_size > 200 Giga stop the program.
static int files_converted = 0;
static uintmax_t total_size = 0;
const uintmax_t MAX_SIZE = 200000000000;

/*
This function iterates through all pcd files in directory
*/
void iterate_over_folder(const std::string& source_path, const std::string& target_path)
{
    boost::filesystem::path target_dir(target_path);
    boost::filesystem::path source_dir(source_path);
    boost::filesystem::directory_iterator it(source_dir), eod;

    BOOST_FOREACH(boost::filesystem::path const &file_path, std::make_pair(it, eod))
    {
        if(is_regular_file(file_path))
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
            pcl::PointCloud<PointXYZRGBIM>::Ptr cloud_im (new pcl::PointCloud<PointXYZRGBIM>);
            if (pcl::io::loadPCDFile<pcl::PointXYZ> (file_path.string(), *cloud) == -1 ||
            pcl::io::loadPCDFile<PointXYZRGBIM> (file_path.string(), *cloud_im) == -1) //* load the file
            {
                PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
            }

            //Estimate normals
            pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());
            pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
            pcl::search::KdTree<pcl::PointXYZ>::Ptr tree0(new pcl::search::KdTree<pcl::PointXYZ>);

            tree0->setInputCloud(cloud);
            n.setInputCloud(cloud);
            n.setSearchMethod(tree0);
            n.setKSearch(20);
            n.compute(*normals);

            // Create the FPFH estimation class, and pass the input dataset+normals to it
            pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
            fpfh.setInputCloud (cloud);
            fpfh.setInputNormals (normals);

            // Create an empty kdtree representation, and pass it to the FPFH estimation object.
            // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
            pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

            fpfh.setSearchMethod (tree);

            // Output datasets
            pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ());

            // Use all neighbors in a sphere of radius 5cm
            // IMPORTANT: the radius used here has to be larger than the radius used to estimate the surface normals!!!
            fpfh.setRadiusSearch (0.05);

            // Compute the features
            fpfh.compute (*fpfhs);

            //Save features
            std::string fpfh_file_name =  file_path.stem().string() + "_fpfh.csv";
            boost::filesystem::path fpfh_folder_path = target_dir / source_dir.stem();
            boost::filesystem::create_directory(fpfh_folder_path);
            boost::filesystem::path fpfh_file_path = fpfh_folder_path / fpfh_file_name;
            const char * fpfh_file_path_c = fpfh_file_path.string().c_str();

            std::ofstream myfile;
            myfile.open(fpfh_file_path_c);

            std::cout << "Loaded "
                << cloud->width * cloud->height
                << " data points from test_pcd.pcd with the following fields: "
                << std::endl;

            if (myfile.is_open())
            {
                for (int i = 0; i < fpfhs->points.size(); ++i){

                    uint32_t rgb = *reinterpret_cast<int*>(&cloud_im->points[i].rgb);
                    uint8_t r = (rgb >> 16) & 0x0000ff;
                    uint8_t g = (rgb >> 8)  & 0x0000ff;
                    uint8_t b = (rgb)       & 0x0000ff;

                    for (int j = 0; j < 33; ++j){
                        myfile << fpfhs->points[i].histogram[j] << " " ;
                    }

                    myfile << (int)r << " " << (int)g << " " << (int)b << " " <<
                        cloud_im->points[i].imX << " " << cloud_im->points[i].imY << " \n";
                }
                myfile.close();
                std::cout << "saved file: " << fpfh_file_path.string() << "\n";
                files_converted ++;
                total_size += boost::filesystem::file_size(fpfh_file_path);
                if (total_size >= MAX_SIZE)
                {
                    std::cout << "program converted " << files_converted << " files and exceeded maximal size set to: "
                    << MAX_SIZE << " bytes." << std::endl;
                }
                else
                {
                    std::cout << files_converted << std::endl;
                }
            }
            else std::cout << "Unable to open file";
        }
    }

}

int main()
{
    const char * homedir = getenv("HOME");
    std::string objects[] = {"coffee_mug", "greens", "soda_can", "ball", "comb", "hand_towel", "sponge", "dry_battery", "instant_noodles", "peach", "stapler", "flashlight",
"keyboard", "binder", "food_bag", "kleenex", "pitcher", "toothbrush", "bowl", "food_box", "plate", "toothpaste", "calculator", "food_can", "lightbulb", "pliers", "water_bottle"
"camera", "food_cup", "cap", "food_jar", "marker", "rubber_eraser", "cell_phone", "garlic", "scissors", "cereal_box", "glue_stick", "notebook" };

    std::string datasets_dir_string = std::string(homedir) + "/datasets/rgbd-dataset/";
    std::string target_dir = "/mnt/raid/dnn/fpfh";

    //std::string datasets_dir_string = "/Users/smartMac/Downloads/rgbd-dataset";
    //std::string target_dir = "/Users/smartMac/lab_rot_1/pcl_proj/fpfh";

    boost::filesystem::path datasets_dir(datasets_dir_string);
    boost::filesystem::directory_iterator it(datasets_dir_string), eod;

    for(int i = 0; i < 13; ++i)
    {

        boost::filesystem::path object_path(datasets_dir_string + objects[i]);
        if(boost::filesystem::is_directory(object_path))
        {
            boost::filesystem::directory_iterator it2(object_path), eod2;
            BOOST_FOREACH(boost::filesystem::path const &instance_path, std::make_pair(it2, eod2))
            {
                if(boost::filesystem::is_directory(instance_path))
                {
                    iterate_over_folder(instance_path.string(), target_dir);
                }
            }
        }
        else
        {
            std::cout << object_path.string() << " not a directory" << std::endl;
        }

    }
    return 0;
}


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Daichi Yoshikawa
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Daichi Yoshikawa nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Daichi Yoshikawa
 *
 *********************************************************************/

#ifndef __AHL_ROBOT_SAMPLES_YOUBOT_PARAM_HPP
#define __AHL_ROBOT_SAMPLES_YOUBOT_PARAM_HPP

#include <memory>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include "ahl_robot_samples/YouBotParamConfig.h"

namespace ahl_sample
{

  class YouBotParam
  {
  public:
    explicit YouBotParam()
    {
      const uint32_t dof = 8;

      show_target = true;
      sin_x = false;
      sin_y = false;
      sin_z = false;
      x_arm = Eigen::Vector3d::Zero();
      R_arm = Eigen::Matrix3d::Zero();
      x_base = Eigen::Vector3d::Zero();
      R_base = Eigen::Matrix3d::Zero();
      q = Eigen::VectorXd::Zero(dof);

      ros::NodeHandle local_nh("~/youbot/");
      f_ = boost::bind(&YouBotParam::update, this, _1, _2);
      server_ = std::make_shared<YouBotParamConfigServer>(local_nh);
      server_->setCallback(f_);
    }

    bool show_target;
    bool sin_x;
    bool sin_y;
    bool sin_z;
    Eigen::Vector3d x_arm;
    Eigen::Matrix3d R_arm;
    Eigen::Vector3d x_base;
    Eigen::Matrix3d R_base;
    Eigen::VectorXd q;
  private:
    void update(ahl_robot_samples::YouBotParamConfig& config, uint32_t level)
    {
      show_target = config.show_target;

      sin_x = config.sin_x;
      sin_y = config.sin_y;
      sin_z = config.sin_z;

      x_arm[0] = config.x_arm;
      x_arm[1] = config.y_arm;
      x_arm[2] = config.z_arm;

      R_arm = Eigen::AngleAxisd(config.roll_arm,  Eigen::Vector3d::UnitX())
            * Eigen::AngleAxisd(config.pitch_arm, Eigen::Vector3d::UnitY())
            * Eigen::AngleAxisd(config.yaw_arm,   Eigen::Vector3d::UnitZ());

      x_base[0] = config.x_base;
      x_base[1] = config.y_base;
      x_base[2] = config.z_base;

      R_base = Eigen::AngleAxisd(config.roll_base,  Eigen::Vector3d::UnitX())
             * Eigen::AngleAxisd(config.pitch_base, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(config.yaw_base,   Eigen::Vector3d::UnitZ());

      q[0] = config.q_base1;
      q[1] = config.q_base2;
      q[2] = config.q_base3;
      q[3] = config.q1;
      q[4] = config.q2;
      q[5] = config.q3;
      q[6] = config.q4;
      q[7] = config.q5;
    }

    using YouBotParamConfigServer = dynamic_reconfigure::Server<ahl_robot_samples::YouBotParamConfig>;
    using YouBotParamConfigServerPtr = std::shared_ptr<YouBotParamConfigServer>;

    YouBotParamConfigServerPtr server_;
    dynamic_reconfigure::Server<ahl_robot_samples::YouBotParamConfig>::CallbackType f_;
  };

  using YouBotParamPtr = std::shared_ptr<YouBotParam>;

} // namespace ahl_sample

#endif // __AHL_ROBOT_SAMPLES_YOUBOT_PARAM_HPP


#include "teca_component_area_filter.h"

#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_cartesian_mesh.h"
#include "teca_string_util.h"

#include <iostream>
#include <set>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

namespace {

// this constructs a map from input label id to the output label id, the list
// of ids that survive and their respective areas. if the area of the label is
// outside of the range it will be replaced in the output.
template <typename label_t, typename area_t, typename container_t>
void build_label_map(const label_t *comp_ids, const area_t *areas,
    size_t n, double low_area_threshold, double high_area_threshold,
    label_t mask_value, container_t &label_map,
    std::vector<label_t> &ids_out, std::vector<area_t> &areas_out)
{
    for (size_t i = 0; i < n; ++i)
    {
        if ((areas[i] < low_area_threshold) || (areas[i] > high_area_threshold))
        {
            // outside the range, mask this label
            label_map[comp_ids[i]] = mask_value;
        }
        else
        {
            // inside the range, pass it through
            label_map[comp_ids[i]] = comp_ids[i];
            ids_out.push_back(comp_ids[i]);
            areas_out.push_back(areas[i]);
        }
    }
}

// visit every point in the data, apply the map. The map is such that labels
// ouside of the specified range are replaced
template <typename label_t, typename container_t>
void apply_label_map(label_t *labels, const label_t *labels_in,
    container_t &label_map, size_t n)
{
    for (unsigned long i = 0; i < n; ++i)
        labels[i] = label_map[labels_in[i]];
}

}

// --------------------------------------------------------------------------
teca_component_area_filter::teca_component_area_filter() :
    component_variable(""), number_of_components_key("number_of_components"),
    component_ids_key("component_ids"), component_area_key("component_area"),
    mask_value(-1), low_area_threshold(std::numeric_limits<double>::lowest()),
    high_area_threshold(std::numeric_limits<double>::max()),
    variable_post_fix(""), contiguous_component_ids(0)
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_component_area_filter::~teca_component_area_filter()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_component_area_filter::get_properties_description(
    const std::string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_component_area_filter":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::string, prefix, component_variable,
            "name of the varibale containing connected component labeling")
        TECA_POPTS_GET(std::string, prefix, number_of_components_key,
            "name of the key that contains the number of components")
        TECA_POPTS_GET(std::string, prefix, component_ids_key,
            "name of the key that contains the list of component ids")
        TECA_POPTS_GET(std::string, prefix, component_area_key,
            "name of the key that contains the list of component areas")
        TECA_POPTS_GET(int, prefix, mask_value,
            "components with area outside of the range will be replaced "
            "by this label value")
        TECA_POPTS_GET(double, prefix, low_area_threshold,
            "set the lower end of the range of areas to pass through. "
            "components smaller than this are masked out.")
        TECA_POPTS_GET(double, prefix, high_area_threshold,
            "set the higher end of the range of areas to pass through. "
            "components larger than this are masked out.")
        TECA_POPTS_GET(std::string, prefix, variable_post_fix,
            "set a string that will be appended to variable names and "
            "metadata keys in the filter's output")
        TECA_POPTS_GET(int, prefix, contiguous_component_ids,
            "when the region label ids start at 0 and are consecutive "
            "this flag enables use of an optimization")
        ;

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_component_area_filter::set_properties(const std::string &prefix,
    variables_map &opts)
{
    this->teca_algorithm::set_properties(prefix, opts);

    TECA_POPTS_SET(opts, std::string, prefix, component_variable)
    TECA_POPTS_SET(opts, std::string, prefix, number_of_components_key)
    TECA_POPTS_SET(opts, std::string, prefix, component_ids_key)
    TECA_POPTS_SET(opts, std::string, prefix, component_area_key)
    TECA_POPTS_SET(opts, int, prefix, mask_value)
    TECA_POPTS_SET(opts, double, prefix, low_area_threshold)
    TECA_POPTS_SET(opts, double, prefix, high_area_threshold)
    TECA_POPTS_SET(opts, std::string, prefix, variable_post_fix)
}
#endif

// --------------------------------------------------------------------------
teca_metadata teca_component_area_filter::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_component_area_filter::get_output_metadata" << endl;
#endif
    (void) port;

    // add in the array we will generate
    teca_metadata out_md(input_md[0]);

    const std::string &var_post_fix = this->variable_post_fix;
    if (!var_post_fix.empty())
    {
        std::string component_var = this->component_variable;
        out_md.append("variables", component_var + var_post_fix);
    }

    return out_md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_component_area_filter::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_component_area_filter::get_upstream_request" << endl;
#endif
    (void) port;
    (void) input_md;

    std::vector<teca_metadata> up_reqs;

    // get the name of the array to request
    if (this->component_variable.empty())
    {
        TECA_FATAL_ERROR("The component variable was not specified")
        return up_reqs;
    }

    // pass the incoming request upstream, and
    // add in what we need
    teca_metadata req(request);
    std::set<std::string> arrays;
    if (req.has("arrays"))
        req.get("arrays", arrays);
    arrays.insert(this->component_variable);

    // remove the arrays we produce if the post-fix is set,
    // and replace it with the actual requested array.
    const std::string &var_post_fix = this->variable_post_fix;
    if (!var_post_fix.empty())
    {
        teca_string_util::remove_post_fix(arrays, var_post_fix);
    }

    req.set("arrays", arrays);

    // send up
    up_reqs.push_back(req);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_component_area_filter::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_component_area_filter::execute" << endl;
#endif
    (void)port;

    // get the input
    const_p_teca_cartesian_mesh in_mesh =
        std::dynamic_pointer_cast<const teca_cartesian_mesh>(
            input_data[0]);
    if (!in_mesh)
    {
        TECA_FATAL_ERROR("empty input, or not a cartesian_mesh")
        return nullptr;
    }

    // create output and copy metadata, coordinates, etc
    p_teca_cartesian_mesh out_mesh = teca_cartesian_mesh::New();

    out_mesh->shallow_copy(
        std::const_pointer_cast<teca_cartesian_mesh>(in_mesh));

    // get the input array
    if (this->component_variable.empty())
    {
        TECA_FATAL_ERROR("The component variable was not specified")
        return nullptr;
    }

    const_p_teca_variant_array labels_in
        = out_mesh->get_point_arrays()->get(this->component_variable);

    if (!labels_in)
    {
        TECA_FATAL_ERROR("labels variable \"" << this->component_variable
            << "\" is not in the input")
        return nullptr;
    }

    // get the list of component ids, and their corresponding areas
    teca_metadata &in_metadata =
        const_cast<teca_metadata&>(in_mesh->get_metadata());

    const_p_teca_variant_array ids_in
        = in_metadata.get(this->component_ids_key);

    if (!ids_in)
    {
        TECA_FATAL_ERROR("Metadata missing component ids")
        return nullptr;
    }

    size_t n_ids_in = ids_in->size();

    const_p_teca_variant_array areas_in
        = in_metadata.get(this->component_area_key);

    if (!areas_in)
    {
        TECA_FATAL_ERROR("Metadata missing component areas")
        return nullptr;
    }

    // get threshold values
    double low_val = this->low_area_threshold;
    if (low_val == std::numeric_limits<double>::lowest()
        && request.has("low_area_threshold"))
        request.get("low_area_threshold", low_val);

    double high_val = this->high_area_threshold;
    if (high_val == std::numeric_limits<double>::max()
        && request.has("high_area_threshold"))
        request.get("high_area_threshold", high_val);

    // allocate the array to store the output with labels outside the requested
    // range removed.
    size_t n_elem = labels_in->size();
    p_teca_variant_array labels_out = labels_in->new_instance(n_elem);

    // pass to the output
    std::string labels_var_post_fix = this->component_variable + this->variable_post_fix;
    out_mesh->get_point_arrays()->set(labels_var_post_fix, labels_out);

    // get the output metadata to add results to after the filter is applied
    teca_metadata &out_metadata = out_mesh->get_metadata();

    long mask_value = this->mask_value;
    if (this->mask_value == -1)
    {
        if (in_metadata.get("background_id", mask_value))
        {
            TECA_FATAL_ERROR("Metadata is missing the key \"background_id\". "
                "One should specify it via the \"mask_value\" algorithm "
                "property")
            return nullptr;
        }
    }

    // apply the filter
    NESTED_TEMPLATE_DISPATCH_I(teca_variant_array_impl,
        labels_out.get(),
        _LABEL,

        // pointer to input/output labels
        const NT_LABEL *p_labels_in =
            static_cast<const TT_LABEL*>(labels_in.get())->get();

        NT_LABEL *p_labels_out =
            static_cast<TT_LABEL*>(labels_out.get())->get();

        // pointer to input ids and a container to hold ids which remain
        // after the filtering operation
        const NT_LABEL *p_ids_in =
            static_cast<const TT_LABEL*>(ids_in.get())->get();

        std::vector<NT_LABEL> ids_out;

        NESTED_TEMPLATE_DISPATCH_FP(const teca_variant_array_impl,
            areas_in.get(),
            _AREA,

            // pointer to the areas in and a container to hold areas which
            // remain after the filtering operation
            const NT_AREA *p_areas = static_cast<TT_AREA*>(areas_in.get())->get();

            std::vector<NT_AREA> areas_out;

            // if we labels with small values we can speed the calculation by
            // using a contiguous buffer to hold the map. otherwise we need to
            // use an associative container
            if (this->contiguous_component_ids)
            {
                // find the max label id, used to size the map buffer
                NT_LABEL max_id = std::numeric_limits<NT_LABEL>::lowest();
                for (unsigned int i = 0; i < n_ids_in; ++i)
                    max_id = std::max(max_id, p_ids_in[i]);

                // allocate the map
                std::vector<NT_LABEL> label_map(max_id+1, NT_LABEL(mask_value));

                // construct the map from input label to output label.
                // removing a lable from the output ammounts to applying
                // the mask value to the labels
                ::build_label_map(p_ids_in, p_areas, n_ids_in,
                        low_val, high_val, NT_LABEL(mask_value),
                        label_map, ids_out, areas_out);

                // use the map to mask out removed labels
                ::apply_label_map(p_labels_out, p_labels_in, label_map, n_elem);
            }
            else
            {
                decltype(std::map<NT_LABEL, NT_LABEL>()) label_map;

                // construct the map from input label to output label.
                // removing a lable from the output ammounts to applying
                // the mask value to the labels
                ::build_label_map(p_ids_in, p_areas, n_ids_in,
                        low_val, high_val, NT_LABEL(mask_value),
                        label_map, ids_out, areas_out);

                // use the map to mask out removed labels
                ::apply_label_map(p_labels_out, p_labels_in, label_map, n_elem);
            }

            // pass the updated set of component ids and their coresponding areas
            // to the output
            out_metadata.set(this->number_of_components_key + this->variable_post_fix, ids_out.size());
            out_metadata.set(this->component_ids_key + this->variable_post_fix, ids_out);
            out_metadata.set(this->component_area_key + this->variable_post_fix, areas_out);
            out_metadata.set("background_id" + this->variable_post_fix, mask_value);

            // pass the threshold values used
            out_metadata.set("low_area_threshold_km", low_val);
            out_metadata.set("high_area_threshold_km", high_val);
            )
        )

    return out_mesh;
}


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

#ifndef BOOST_ESX_ALTERNATIVE_ROUTING_HPP
#define BOOST_ESX_ALTERNATIVE_ROUTING_HPP

#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>

#include <arlib/terminators.hpp>
#include <arlib/type_traits.hpp>

#include <arlib/details/esx_impl.hpp>

#include <queue>
#include <unordered_set>
#include <vector>

/**
 * An Alternative-Routing library for Boost.Graph
 */
namespace arlib {
/**
 * An implementation of ESX k-shortest path with limited overlap for
 * Boost::Graph.
 *
 * This implementation refers to the following publication:
 *
 * Theodoros Chondrogiannis, Panagiotis Bouros, Johann Gamper and Ulf Leser,
 * Exact and Approximate Algorithms for Finding k-Shortest Paths with Limited
 * Overlap , In Proc. of the 20th Int. Conf. on Extending Database Technology
 * (EDBT) (2017)
 *
 * @tparam Graph A Boost::VertexAndEdgeListGraph
 * @tparam WeightMap The weight or "length" of each edge in the graph. The
 *         weights must all be non-negative, and the algorithm will throw a
 *         negative_edge exception is one of the edges is negative. The type
 *         WeightMap must be a model of Readable Property Map. The edge
 *         descriptor type of the graph needs to be usable as the key type for
 *         the weight map. The value type for this map must be the same as the
 *         value type of the distance map.
 * @tparam MultiPredecessorMap The multi predecessor map records the edges in
 *         the alternative paths from @p s to @p t. Upon completion of the
 *         algorithm, for any vertex `v` on any alternative path `p`, multi
 *         predecessor map stores the predecessor of node `v` on path `p`. If no
 *         predecessor for a node `v'` is reported, then `v'` is not part of any
 *         alternative path or it is the source node @p s. The type of
 *         `MultiPredecessorMap` must be a model of `Read Property Map`. The
 *         vertex descriptor of the input graph @p G must be usable as a key
 *         type for the multi predecessor map. Whereas the value type must
 *         satisfy the `UnorderedAssociativeContainer` concept, where its key is
 *         an `int`, the index/number of alternative path for which it exists a
 *         predecessor of `v`, and where the value type is a vertex descriptor
 *         of input graph @p G.
 * @tparam Vertex The vertex descriptor.
 * @param G The input graph.
 * @param weight The weight map of @p G.
 * @param predecessors The multi predecessor map of @p G.
 * @param s The source node.
 * @param t The target node.
 * @param k The number of alternative paths to compute.
 * @param theta The similarity threshold.
 * @param algorithm The routing kernel to employ. The default is
 *        routing_kernels::astar
 *
 */
template <typename Graph, typename WeightMap, typename MultiPredecessorMap,
          typename Terminator = arlib::always_continue,
          typename Vertex = vertex_of_t<Graph>,
          typename = std::enable_if_t<std::is_same_v<
              typename boost::property_traits<MultiPredecessorMap>::key_type,
              Vertex>>>
void esx(const Graph &G, WeightMap const &weight,
         MultiPredecessorMap &predecessors, Vertex s, Vertex t, int k,
         double theta, routing_kernels algorithm = routing_kernels::astar,
         Terminator &&terminator = Terminator{}) {
  details::esx_dispatch(G, weight, predecessors, s, t, k, theta, algorithm,
                        std::forward<Terminator>(terminator));
}

template <typename Graph, typename WeightMap, typename MultiPredecessorMap,
          typename EdgeCentralityMap,
          typename Terminator = arlib::always_continue,
          typename Vertex = vertex_of_t<Graph>,
          typename = std::enable_if_t<std::is_same_v<
              typename boost::property_traits<MultiPredecessorMap>::key_type,
              Vertex>>>
void esx(const Graph &G, WeightMap const &weight,
         MultiPredecessorMap &predecessors,
         EdgeCentralityMap const &edge_centrality, Vertex s, Vertex t, int k,
         double theta, routing_kernels algorithm = routing_kernels::astar,
         Terminator &&terminator = Terminator{}) {
  details::esx_dispatch(G, weight, predecessors, edge_centrality, s, t, k,
                        theta, algorithm, std::forward<Terminator>(terminator));
}

/**
 * An implementation of `ESX` k-shortest path with limited overlap for
 * `Boost::Graph`.
 *
 * This overload takes an input graph modeling `PropertyGraph` concept having at
 * least one edge property with tag `boost::edge_weight_t`. Moreover it does not
 * require an explicit `WeightMap` parameter, because it is directly gathered
 * from the `PropertyGraph`.
 *
 * @see esx(const Graph &G, WeightMap const &weight,
 *          MultiPredecessorMap &predecessors, Vertex s, Vertex t, int k,
 *          double theta,
 *          routing_kernels algorithm)
 *
 * @tparam PropertyGraph A Boost::PropertyGraph having at least one edge
 *         property with tag boost::edge_weight_t.
 *
 */
template <typename PropertyGraph, typename MultiPredecessorMap,
          typename Terminator = arlib::always_continue,
          typename Vertex = vertex_of_t<PropertyGraph>,
          typename = std::enable_if_t<std::is_same_v<
              typename boost::property_traits<MultiPredecessorMap>::key_type,
              Vertex>>>
void esx(const PropertyGraph &G, MultiPredecessorMap &predecessors, Vertex s,
         Vertex t, int k, double theta,
         routing_kernels algorithm = routing_kernels::astar,
         Terminator &&terminator = Terminator{}) {
  using namespace boost;
  using Edge = typename graph_traits<PropertyGraph>::edge_descriptor;

  BOOST_CONCEPT_ASSERT(
      (PropertyGraphConcept<PropertyGraph, Edge, edge_weight_t>));

  auto weight = get(edge_weight, G);
  details::esx_dispatch(G, weight, predecessors, s, t, k, theta, algorithm,
                        std::forward<Terminator>(terminator));
}

template <typename PropertyGraph, typename MultiPredecessorMap,
          typename EdgeCentralityMap,
          typename Terminator = arlib::always_continue,
          typename Vertex = vertex_of_t<PropertyGraph>,
          typename = std::enable_if_t<std::is_same_v<
              typename boost::property_traits<MultiPredecessorMap>::key_type,
              Vertex>>>
void esx(const PropertyGraph &G, MultiPredecessorMap &predecessors,
         EdgeCentralityMap const &edge_centrality, Vertex s, Vertex t, int k,
         double theta, routing_kernels algorithm = routing_kernels::astar,
         Terminator &&terminator = Terminator{}) {
  using namespace boost;
  using Edge = typename graph_traits<PropertyGraph>::edge_descriptor;

  BOOST_CONCEPT_ASSERT(
      (PropertyGraphConcept<PropertyGraph, Edge, edge_weight_t>));

  auto weight = get(edge_weight, G);
  esx(G, weight, predecessors, edge_centrality, s, t, k, theta, algorithm,
      std::forward<Terminator>(terminator));
}
} // namespace arlib

#endif


#ifndef ISOCHRONE_HPP
#define ISOCHRONE_HPP

#include "engine/api/isochrone_parameters.hpp"
#include "engine/plugins/plugin_base.hpp"
#include "osrm/json_container.hpp"
#include "util/binary_heap.hpp"
#include "util/static_graph.hpp"

#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/function.hpp>
#include <tbb/parallel_sort.h>

#include <algorithm>
#include <cstdlib>
#include <utility>
#include <vector>

namespace osrm
{
namespace engine
{
namespace plugins
{
struct HeapData
{
    NodeID parent;
    HeapData(NodeID p) : parent(p) {}
};
struct SimpleEdgeData
{
    SimpleEdgeData() : weight(INVALID_EDGE_WEIGHT), real(false) {}
    SimpleEdgeData(unsigned weight_, bool real_) : weight(weight_), real(real_) {}
    unsigned weight;
    bool real;
};

struct IsochroneNode
{
    IsochroneNode(){};
    IsochroneNode(osrm::extractor::QueryNode node,
                  osrm::extractor::QueryNode predecessor,
                  double distance,
                  int duration)
        : node(node), predecessor(predecessor), distance(distance), duration(duration)
    {
    }

    osrm::extractor::QueryNode node;
    osrm::extractor::QueryNode predecessor;
    double distance;
    int duration;

    bool operator==(const IsochroneNode &n) const
    {
        if (n.node.node_id == node.node_id)
            return true;
        else
            return false;
    }
};

using SimpleGraph = util::StaticGraph<SimpleEdgeData>;
using SimpleEdge = SimpleGraph::InputEdge;
using QueryHeap = osrm::util::
    BinaryHeap<NodeID, NodeID, int, HeapData, osrm::util::UnorderedMapStorage<NodeID, int>>;
typedef std::vector<IsochroneNode> IsochroneVector;

class IsochronePlugin final : public BasePlugin
{
  private:
    boost::filesystem::path base;
    std::shared_ptr<engine::plugins::SimpleGraph> graph;
    std::vector<osrm::extractor::QueryNode> coordinate_list;
    std::vector<engine::plugins::SimpleEdge> graph_edge_list;
    std::size_t number_of_nodes;

    std::size_t loadGraph(const std::string &path,
                          std::vector<extractor::QueryNode> &coordinate_list,
                          std::vector<SimpleEdge> &graph_edge_list);

    void dijkstraByDuration(IsochroneVector &set, NodeID &source, int duration);
    void dijkstraByDistance(IsochroneVector &isochroneSet, NodeID &source, double distance);
    void update(IsochroneVector &s, IsochroneNode node);

  public:
    explicit IsochronePlugin(const std::string base);

    Status HandleRequest(const std::shared_ptr<datafacade::BaseDataFacade> facade,
                         const api::IsochroneParameters &params,
                         util::json::Object &json_result);
};
}
}
}

#endif // ISOCHRONE_HPP


//
// drd-estimator.hpp
//
//  Created by Peter Gusev on 07 June 2016.
//  Copyright 2013-2016 Regents of the University of California
//

#ifndef __drd_estimator_h__
#define __drd_estimator_h__

#include <vector>
#include <boost/thread/mutex.hpp>

#include "estimators.hpp"

namespace ndnrtc
{
class IDrdEstimatorObserver;

/**
 * DRD Estimator class runs estimations for DRD (Data Retrieval Delay) values
 * using sliding average estimators.
 * Estimator runs two estimations - one for original data (answered by previously
 * issued Interest) and one for data coming from cache.
 * @see SlotSegment::isOriginal()
 */
class DrdEstimator
{
  public:
    DrdEstimator(unsigned int initialEstimationMs = 150, unsigned int windowMs = 200);

    void setInitialEstimation(double initial) { initialEstimation_ = initial; }
    void newValue(double drd, bool isOriginal, double dGen);
    double getCachedEstimation() const;
    double getOriginalEstimation() const;
    void reset();

    const estimators::Average &getCachedAverage() const { return cachedDrd_; }
    const estimators::Average &getOriginalAverage() const { return originalDrd_; }
    const estimators::Average &getLatestUpdatedAverage() const { return *latest_; }
    const estimators::Average &getGenerationDelayAverage() const { return generationDelay_; }

    void attach(IDrdEstimatorObserver *o);
    void detach(IDrdEstimatorObserver *o);

  private:
    boost::mutex mutex_;
    std::vector<IDrdEstimatorObserver *> observers_;
    unsigned int windowSize_, initialEstimation_;
    estimators::Average cachedDrd_, originalDrd_;
    estimators::Average generationDelay_;
    estimators::Average *latest_;
};

class IDrdEstimatorObserver
{
  public:
    virtual void onDrdUpdate() = 0;
    virtual void onCachedDrdUpdate(double, double) = 0;
    virtual void onOriginalDrdUpdate(double, double) = 0;
};
}

#endif


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

#include "extend_truth_table.hpp"

#include <boost/range/iterator_range.hpp>

#include "../io/io_utils_p.hpp"

namespace cirkit
{

  template<typename Iterator>
  Iterator in_cube_to_full_cubes( binary_truth_table::in_const_iterator first, binary_truth_table::in_const_iterator last, Iterator result )
  {
    // Example
    // 0--11
    // base = 00011 = 3
    // dc_positions = 1,2
    // numbers = 3, 7, 11, 15

    binary_truth_table::cube_type cube( first, last );

    std::vector<unsigned> dc_positions;
    unsigned pos = 0;

    while ( first != last )
    {
      if ( !*first ) // if DC
      {
        dc_positions += pos;
      }

      ++pos;
      ++first;
    }

    for ( unsigned i = 0; i < ( 1u << dc_positions.size() ); ++i )
    {
      for ( unsigned j = 0; j < dc_positions.size(); ++j )
      {
        unsigned local_bit = i & ( 1u << ( dc_positions.size() - j - 1 ) ) ? 1 : 0;
        cube.at( dc_positions.at( j ) ) = local_bit;
      }
      *result++ = cube;
    }

    return result;
  }

  void extend_truth_table( binary_truth_table& spec )
  {

    typedef std::map<std::vector<binary_truth_table::cube_type>, binary_truth_table::cube_type> cube_map;
    cube_map new_cubes;

    for ( binary_truth_table::const_iterator it = spec.begin(); it != spec.end(); ++it )
    {
      // fully specified outputs
      assert( std::find( it->second.first, it->second.second, constant() ) == it->second.second );

      std::vector<binary_truth_table::cube_type> in_cube_values;
      in_cube_to_full_cubes( it->first.first, it->first.second, std::back_inserter( in_cube_values ) );

      binary_truth_table::cube_type output( it->second.first, it->second.second );

      new_cubes.insert( std::make_pair( in_cube_values, output ) );
    }

    spec.clear();

    for ( cube_map::const_iterator it = new_cubes.begin(); it != new_cubes.end(); ++it )
    {
      for ( std::vector<binary_truth_table::cube_type>::const_iterator itCube = it->first.begin(); itCube != it->first.end(); ++itCube )
      {
        spec.add_entry( *itCube, it->second );
      }
    }

    // fill the outputs (truth table is ordered)
    binary_truth_table::cube_type out_cube( spec.num_outputs(), false );
    unsigned current_pos = 0;
    for ( binary_truth_table::const_iterator it = spec.begin(); ; ++it )
    {
      unsigned pos = 0;
      unsigned i = spec.num_inputs();

      // vector to number
      if ( it == spec.end() ) // after the end
      {
        pos = 1u << spec.num_inputs();
      }
      else
      {
        for ( const auto& in_bit : boost::make_iterator_range( it->first ) )
        {
          pos |= *in_bit << --i;
        }
      }

      for ( i = current_pos; i < pos; ++i )
      {
        spec.add_entry( number_to_truth_table_cube( i, spec.num_inputs() ), out_cube );
      }

      current_pos = pos;

      // there is no <= comparison possible
      if ( it == spec.end() )
      {
        break;
      }
    }

  }

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

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

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_BOX_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_BOX_HPP

#include <cstddef>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/views/detail/indexed_point_view.hpp>

#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/algorithms/detail/normalize.hpp>
#include <boost/geometry/algorithms/detail/envelope/transform_units.hpp>

#include <boost/geometry/algorithms/dispatch/envelope.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace envelope
{

template
<
    std::size_t Index,
    std::size_t Dimension,
    std::size_t DimensionCount
>
struct envelope_indexed_box
{
    template <typename BoxIn, typename BoxOut>
    static inline void apply(BoxIn const& box_in, BoxOut& mbr)
    {
        detail::indexed_point_view<BoxIn const, Index> box_in_corner(box_in);
        detail::indexed_point_view<BoxOut, Index> mbr_corner(mbr);

        detail::conversion::point_to_point
            <
                detail::indexed_point_view<BoxIn const, Index>,
                detail::indexed_point_view<BoxOut, Index>,
                Dimension,
                DimensionCount
            >::apply(box_in_corner, mbr_corner);
    }
};

template
<
    std::size_t Index,
    std::size_t DimensionCount
>
struct envelope_indexed_box_on_spheroid
{
    template <typename BoxIn, typename BoxOut>
    static inline void apply(BoxIn const& box_in, BoxOut& mbr)
    {
        // transform() does not work with boxes of dimension higher
        // than 2; to account for such boxes we transform the min/max
        // points of the boxes using the indexed_point_view
        detail::indexed_point_view<BoxIn const, Index> box_in_corner(box_in);
        detail::indexed_point_view<BoxOut, Index> mbr_corner(mbr);

        // first transform the units
        transform_units(box_in_corner, mbr_corner);

        // now transform the remaining coordinates
        detail::conversion::point_to_point
            <
                detail::indexed_point_view<BoxIn const, Index>,
                detail::indexed_point_view<BoxOut, Index>,
                2,
                DimensionCount
            >::apply(box_in_corner, mbr_corner);
    }
};

struct envelope_box
{
    template<typename BoxIn, typename BoxOut, typename Strategy>
    static inline void apply(BoxIn const& box_in,
                             BoxOut& mbr,
                             Strategy const&)
    {
        envelope_indexed_box
            <
                min_corner, 0, dimension<BoxIn>::value
            >::apply(box_in, mbr);

        envelope_indexed_box
            <
                max_corner, 0, dimension<BoxIn>::value
            >::apply(box_in, mbr);
    }
};

struct envelope_box_on_spheroid
{
    template <typename BoxIn, typename BoxOut, typename Strategy>
    static inline void apply(BoxIn const& box_in,
                             BoxOut& mbr,
                             Strategy const&)
    {
        BoxIn box_in_normalized = detail::return_normalized<BoxIn>(box_in);

        envelope_indexed_box_on_spheroid
            <
                min_corner, dimension<BoxIn>::value
            >::apply(box_in_normalized, mbr);

        envelope_indexed_box_on_spheroid
            <
                max_corner, dimension<BoxIn>::value
            >::apply(box_in_normalized, mbr);
    }
};

}} // namespace detail::envelope
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Box>
struct envelope<Box, box_tag, cartesian_tag>
    : detail::envelope::envelope_box
{};

template <typename Box>
struct envelope<Box, box_tag, spherical_polar_tag>
    : detail::envelope::envelope_box_on_spheroid
{};

template <typename Box>
struct envelope<Box, box_tag, spherical_equatorial_tag>
    : detail::envelope::envelope_box_on_spheroid
{};

template <typename Box>
struct envelope<Box, box_tag, geographic_tag>
    : detail::envelope::envelope_box_on_spheroid
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_BOX_HPP


#ifndef CHEMICALTRACKINGMODIFIER_HPP_
#define CHEMICALTRACKINGMODIFIER_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include <boost/shared_ptr.hpp>

#include "AbstractCellBasedSimulationModifier.hpp"
#include "ChemicalSrnModel.hpp"
#include "AbstractCellPopulation.hpp"
#include "AbstractCellProperty.hpp"
#include "TransportCellProperty.hpp"
#include "MembraneCellProperty.hpp"

// class to handle the different methods of altering the chemical concentrations (state variable) within each cell
// in a cell based simulation coupled to a domain based pde system. The class utlises and update cell properties and
// sums all the contributions at the end of a simulation timestep, chekcing the cocnentrations against a zeroing threshold.

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
class ChemicalTrackingModifier : public AbstractCellBasedSimulationModifier<ELEMENT_DIM,SPACE_DIM>
{
private:
    double mDelta_error;

    bool mIsCheckConcentration;

public:

    ChemicalTrackingModifier();

    virtual ~ChemicalTrackingModifier();

    virtual void UpdateAtEndOfTimeStep(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation);

    virtual void SetupSolve(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation, std::string outputDirectory);

    void SetupSRNFromCellData(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation);

    void UpdateCellData(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation);

    void OutputSimulationModifierParameters(out_stream& rParamsFile);

    void SetDeltaError(double);

    double GetDeltaError();

    void CheckConcentration(const std::vector<double>&);

    void CheckConcentration(const double&);

    void SetIsCheckConcentration(bool);

    bool GetIsCheckConcentration();
};

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::ChemicalTrackingModifier()
    : AbstractCellBasedSimulationModifier<SPACE_DIM>()
{
    SetIsCheckConcentration(true);
    SetDeltaError(1e-6);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::~ChemicalTrackingModifier()
{
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void ChemicalTrackingModifier<ELEMENT_DIM, SPACE_DIM>::UpdateAtEndOfTimeStep(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation)
{
    //std::cout<<"ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::UpdateAtEndOfTimeStep - start"<<std::endl;
    UpdateCellData(rCellPopulation);
    //std::cout<<"ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::UpdateAtEndOfTimeStep - end"<<std::endl;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::SetupSolve(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation, std::string outputDirectory)
{
    /*
     * We must update CellData in SetupSolve(), otherwise it will not have been
     * fully initialised by the time we enter the main time loop.
     */
    SetupSRNFromCellData(rCellPopulation);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::SetupSRNFromCellData(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation)
{
    //std::cout<<"ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::SetupSRNFromCellData - start"<<std::endl;
    rCellPopulation.Update();

    for (typename AbstractCellPopulation<SPACE_DIM>::Iterator cell_iter = rCellPopulation.Begin();
         cell_iter != rCellPopulation.End();
         ++cell_iter)
    {
        // update cells ODE states from cellData
        ChemicalSrnModel* p_model = static_cast<ChemicalSrnModel*>(cell_iter->GetSrnModel());

        AbstractChemistry* this_cell_chemistry = p_model->GetCellChemistry();

        unsigned numberOfChemicals = this_cell_chemistry->GetNumberChemicals();

        std::vector<double> this_SRN_concentration_vector(numberOfChemicals,0.0);
        std::string this_chemical_name="";

        for(unsigned i=0; i<numberOfChemicals;i++)
        {
            this_chemical_name = this_cell_chemistry -> GetChemicalNamesByIndex(i);
            this_SRN_concentration_vector[i] = cell_iter->GetCellData()->GetItem(this_chemical_name);
        }

        p_model->GetOdeSystem()->SetStateVariables(this_SRN_concentration_vector);

    }
    //std::cout<<"ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::SetupSRNFromCellData - end"<<std::endl;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::UpdateCellData(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation)
{
    //std::cout<<"ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::UpdateCellData( - start"<<std::endl;
    // Make sure the cell population is updated, i.e cell cycle, cell state
    rCellPopulation.Update();

    unsigned count=0;
    /// run through each cell in the population and update the internal chemical concentrations in cellData
    for (typename AbstractCellPopulation<SPACE_DIM>::Iterator cell_iter = rCellPopulation.Begin();
         cell_iter != rCellPopulation.End();
         ++cell_iter)
    {

        ChemicalSrnModel* p_model = static_cast<ChemicalSrnModel*>(cell_iter->GetSrnModel());
        AbstractChemistry* this_cell_srn_chemistry = p_model->GetCellChemistry();
        unsigned numberOfSrnChemicals = this_cell_srn_chemistry->GetNumberChemicals();

        // retrieve the cell data values for the SRN model
        double this_concentration=0;
        std::string this_name="";
        for(unsigned i=0; i<numberOfSrnChemicals;i++)
        {
            this_name = this_cell_srn_chemistry -> GetChemicalNamesByIndex(i);

            this_concentration = p_model-> GetStateValueByName(this_name);

            CheckConcentration(this_concentration);
            cell_iter->GetCellData()->SetItem(this_name, this_concentration);
        }

        // update the cell data based on any transport properties
        if (cell_iter-> template HasCellProperty<TransportCellProperty>())
        {
            boost::shared_ptr<TransportCellProperty> transport_cell_property = boost::static_pointer_cast<TransportCellProperty>(cell_iter-> template rGetCellPropertyCollection(). template GetPropertiesType<TransportCellProperty>().GetProperty());

            AbstractChemistry* this_cell_transport_chemistry = transport_cell_property -> GetTransportReactionSystem() -> GetCellChemistry();
            unsigned numberOfTransportChemicals = this_cell_transport_chemistry->GetNumberChemicals();

            for(unsigned i=0; i<numberOfTransportChemicals; i++)
            {

                this_name = this_cell_transport_chemistry -> GetChemicalNamesByIndex(i);

                this_concentration = cell_iter->GetCellData()->GetItem(this_name);

                this_concentration += transport_cell_property->GetReactionCalls() *transport_cell_property-> GetChangeInternalCellBoundaryConcentrationByName(this_name);
                CheckConcentration(this_concentration);
                cell_iter->GetCellData()->SetItem(this_name, this_concentration);
            }

        }

        // update the cell data based on any membrane properties
        if (cell_iter-> template HasCellProperty<MembraneCellProperty>())
        {
            boost::shared_ptr<MembraneCellProperty> membrane_cell_property = boost::static_pointer_cast<MembraneCellProperty>(cell_iter-> template rGetCellPropertyCollection(). template GetPropertiesType<MembraneCellProperty>().GetProperty());

            AbstractChemistry* this_cell_membrane_chemistry = membrane_cell_property -> GetMembraneReactionSystem() -> GetCellChemistry();
            unsigned numberOfMembraneChemicals = this_cell_membrane_chemistry->GetNumberChemicals();

            for(unsigned i=0; i<numberOfMembraneChemicals; i++)
            {

                this_name = this_cell_membrane_chemistry -> GetChemicalNamesByIndex(i);

                this_concentration = cell_iter->GetCellData()->GetItem(this_name);

                this_concentration += membrane_cell_property->GetReactionCalls() *membrane_cell_property-> GetChangeInternalCellBoundaryConcentrationByName(this_name);
                CheckConcentration(this_concentration);
                cell_iter->GetCellData()->SetItem(this_name, this_concentration);

            }

        }

        // update the internal cell states for the properties
        if (cell_iter-> template HasCellProperty<TransportCellProperty>())
        {
            boost::shared_ptr<TransportCellProperty> transport_cell_property = boost::static_pointer_cast<TransportCellProperty>(cell_iter-> template rGetCellPropertyCollection(). template GetPropertiesType<TransportCellProperty>().GetProperty());

            AbstractChemistry* this_cell_transport_chemistry = transport_cell_property -> GetTransportReactionSystem() -> GetCellChemistry();
            unsigned numberOfTransportChemicals = this_cell_transport_chemistry->GetNumberChemicals();

            std::vector<double> transportStateVector(numberOfTransportChemicals,0.0);

            std::string this_state="";
            for(unsigned i=0; i<numberOfTransportChemicals; i++)
            {
                this_state = this_cell_transport_chemistry->GetChemicalNamesByIndex(i);

                transportStateVector[i] = cell_iter->GetCellData()->GetItem(this_state);
            }

            transport_cell_property -> UpdateCellConcentrationVector(transportStateVector);
        }

        if (cell_iter-> template HasCellProperty<MembraneCellProperty>())
        {
            boost::shared_ptr<MembraneCellProperty> membrane_cell_property = boost::static_pointer_cast<MembraneCellProperty>(cell_iter-> template rGetCellPropertyCollection(). template GetPropertiesType<MembraneCellProperty>().GetProperty());

            AbstractChemistry* this_cell_membrane_chemistry = membrane_cell_property -> GetMembraneReactionSystem() -> GetCellChemistry();
            unsigned numberOfMembraneChemicals = this_cell_membrane_chemistry->GetNumberChemicals();
            std::vector<double> membraneStateVector(numberOfMembraneChemicals,0.0);

            std::string this_state="";
            for(unsigned i=0; i<numberOfMembraneChemicals; i++)
            {
                this_state = this_cell_membrane_chemistry->GetChemicalNamesByIndex(i);

                membraneStateVector[i] = cell_iter->GetCellData()->GetItem(this_state);
            }

            membrane_cell_property -> UpdateCellConcentrationVector(membraneStateVector);
        }

        // finally update the SRN state variables to the new post transport values
        p_model -> UpdateOdeStatesFromCellData();

        count++;
    }

    //std::cout<<"ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::UpdateCellData( - end"<<std::endl;

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::OutputSimulationModifierParameters(out_stream& rParamsFile)
{
    // No parameters to output, so just call method on direct parent class
    AbstractCellBasedSimulationModifier<SPACE_DIM>::OutputSimulationModifierParameters(rParamsFile);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::SetDeltaError(double delta_error)
{
    mDelta_error = delta_error;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
double ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::GetDeltaError()
{
    return mDelta_error;
}

// overload between a vector and a double value
template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::CheckConcentration(const std::vector<double>& rY)
{
    // if chemical concentration gets too low then nan can occur, concentration must be +ve

    for(unsigned i=0; i<rY.size(); i++)
    {
        // due to the discrete nature occasionally rY can evaluate to <0
        // ensure rY >= 0
        if(rY[i]<mDelta_error)
        {
            const_cast<double&>(rY[i]) = 0;
        }
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::CheckConcentration(const double& rY)
{
    // if chemical concentration gets too low then nan can occur, concentration must be +ve

    // due to the discrete nature occasionally rY can evaluate to <0
    // ensure rY >= 0
    if(rY<mDelta_error)
    {
        const_cast<double&>(rY) = 0;
    }

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
void ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::SetIsCheckConcentration(bool IsCheckConcentration)
{
    mIsCheckConcentration = IsCheckConcentration;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM>
bool ChemicalTrackingModifier<ELEMENT_DIM,SPACE_DIM>::GetIsCheckConcentration()
{
    return mIsCheckConcentration;
}

#endif


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      International Earth Rotation and Reference System Service, Conventions 2010 (IERS2010),
 *        http://www.iers.org/IERS/EN/Publications/TechnicalNotes/tn36.html.
 *      JPL, NASA. Astrodynamic Constants, http://ssd.jpl.nasa.gov/?constants,
 *        last updated: 13 Dec, 2012, last accessed: 19th March, 2013.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/celestialBodyConstants.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_celestial_body_constants )

//! Test if the correct gravitational parameters are returned.
BOOST_AUTO_TEST_CASE( test_GravitationalParameters )
{
    // Test if Sun's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as stated in (JPL, 2012).
        const double expectedGravitationalParameter = 1.32712440018e20;

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER,
                    expectedGravitationalParameter, 1.0e-15 );
    }

    // Test if Mercury's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as derived from mass ratio given by
        // (JPL, 2012).
        const double expectedGravitationalParameter =
                celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER
                / 6023600.0;

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::MERCURY_GRAVITATIONAL_PARAMETER,
                    expectedGravitationalParameter, 1.0e-15 );
    }

    // Test if Venus's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as derived from mass ratio given by
        // (JPL, 2012).
        const double expectedGravitationalParameter =
                celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER
                / 408523.71;

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::VENUS_GRAVITATIONAL_PARAMETER,
                    expectedGravitationalParameter, 1.0e-15 );
    }

    // Test if Earth's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as stated in (IERS, 2010).
        const double exptectedGravitationalParameter = 3.986004418e14;

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::EARTH_GRAVITATIONAL_PARAMETER,
                    exptectedGravitationalParameter, 1.0e-15 );
    }

    // Test if Moon's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as derived from mass ratio given by
        // (JPL, 2012).
        const double expectedGravitationalParameter =
                celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER
                / ( 328900.56 * ( 1.0 + 81.30059 ) );

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::MOON_GRAVITATIONAL_PARAMETER,
                    expectedGravitationalParameter, 1.0e-15 );
    }

    // Test if Mars's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as derived from mass ratio given by
        // (JPL, 2012).
        const double expectedGravitationalParameter =
                celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER
                / 3098708.0;

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::MARS_GRAVITATIONAL_PARAMETER,
                    expectedGravitationalParameter, 1.0e-15 );
    }

    // Test if Jupiter's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as derived from mass ratio given by
        // (JPL, 2012).
        const double expectedGravitationalParameter =
                celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER
                / 1047.3486;

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::JUPITER_GRAVITATIONAL_PARAMETER,
                    expectedGravitationalParameter, 1.0e-15 );
    }

    // Test if Saturn's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as derived from mass ratio given by
        // (JPL, 2012).
        const double expectedGravitationalParameter =
                celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER
                / 3497.898;

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::SATURN_GRAVITATIONAL_PARAMETER,
                    expectedGravitationalParameter, 1.0e-15 );
    }

    // Test if Uranus's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as derived from mass ratio given by
        // (JPL, 2012).
        const double expectedGravitationalParameter =
                celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER
                / 22902.98;

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::URANUS_GRAVITATIONAL_PARAMETER,
                    expectedGravitationalParameter, 1.0e-15 );
    }

    // Test if Neptune's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as derived from mass ratio given by
        // (JPL, 2012).
        const double expectedGravitationalParameter =
                celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER
                / 19412.24;

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::NEPTUNE_GRAVITATIONAL_PARAMETER,
                    expectedGravitationalParameter, 1.0e-15 );
    }

    // Test if Pluto's gravitational parameter is defined correctly.
    {
        // Defined expected gravitational parameter, as derived from mass ratio given by
        // (JPL, 2012).
        const double expectedGravitationalParameter =
                celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER
                / 1.35e8;

        // Test that gravitational parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::PLUTO_GRAVITATIONAL_PARAMETER,
                    expectedGravitationalParameter, 1.0e-15 );
    }
}

//! Test if the correct spherical harmonics coefficients are returned.
BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsCoefficients )
{
    // Test if Earth's J2 parameter is defined correctly.
    {
        // Defined expected J2 parameter, as derived from mass ratio given by (JPL, 2012).
        const double expectedJ2 = -0.484165143790815E-03;

        // Test that J2 parameter is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::EARTH_GEODESY_NORMALIZED_J2,
                    expectedJ2, 1.0e-15 );

    }
}

//! Test if the correct equatorial radii are returned.
BOOST_AUTO_TEST_CASE( test_Radii )
{
    // Test if Earth's equatorial radius is defined correctly.
    {
        // Defined expected radius, as stated in (IERS, 2010).
        const double exptectedEquatorialRadius = 6378136.6;

        // Test that radius is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::EARTH_EQUATORIAL_RADIUS,
                    exptectedEquatorialRadius, 1.0e-15 );
    }
}

//! Test if the correct flattening parameters are returned.
BOOST_AUTO_TEST_CASE( test_FlatteningFactors )
{
    // Test if Earth's flattening factor is defined correctly.
    {
        // Defined expected flattening factor, as stated in (IERS, 2010).
        const double exptectedFlatteningFactor = 298.25642;

        // Test that flattening factor is defined correctly.
        BOOST_CHECK_CLOSE_FRACTION(
                    celestial_body_constants::EARTH_FLATTENING_FACTOR,
                    exptectedFlatteningFactor, 1.0e-15 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <iostream>
#include <boost/compute/core.hpp>
#include <vector>
#include <boost/concept_check.hpp>
#include "../pointsets/Pointset.hpp"
#include "../io/fileIO.hpp"
#include "../io/histogramIO.hpp"

#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/algorithm/sort.hpp>
#include <boost/compute/algorithm/max_element.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/functional/math.hpp>
#include <boost/compute/algorithm/count_if.hpp>
#include <boost/compute/function.hpp>

namespace compute = boost::compute;

const char source2[] = BOOST_COMPUTE_STRINGIZE_SOURCE(__kernel void computeScoreMinDist(__global const unsigned int* permut_lsb,
																				__global const unsigned int* permut_msb,
																				__global const unsigned int* pattern,
																				__global const float* pcf,
																				__global float* min_dist,
                                                                                __global float* score
																				//__global unsigned int* debug
                                                                              )
													{
														uint gid = get_global_id(0);

														unsigned int permut_x_lsb = permut_lsb[2*gid];
														unsigned int permut_y_lsb = permut_lsb[2*gid+1];
														unsigned int permut_x_msb = permut_msb[2*gid];
														unsigned int permut_y_msb = permut_msb[2*gid+1];

														unsigned int permutN_lsb[2];
														unsigned int permutN_msb[2];
														permutN_lsb[0] = permut_x_lsb;
														permutN_lsb[1] = permut_y_lsb;
														permutN_msb[0] = permut_x_msb;
														permutN_msb[1] = permut_y_msb;

														unsigned int permutM_lsb[2];
														unsigned int permutM_msb[2];
														permutM_lsb[0] = 0;
														permutM_lsb[1] = 0;
														permutM_msb[0] = 0;
														permutM_msb[1] = 0;

														uint tilesize = 64;
														uint LOGK=6;
														uint D=2;

														uint x[5][2];
														for(uint d=0; d<D; d++)
														{
															int nb_flags = 2;
															int orig_lvl = tilesize-2 -1;
															for(uint i=1; i<LOGK; i++)
															{
																x[i-1][d] = orig_lvl - (pattern[d]/(tilesize/nb_flags));
																orig_lvl -= nb_flags;
																nb_flags*=2;
															}
														}

														uint xcount[2];
														for(uint d=0; d<D; d++)
															xcount[d] = tilesize-LOGK-2; // bitset indexing: 0==least significant bit

														for(uint i=1; i<tilesize-1; i++) { // start form the second i because xpermuts15bits[0].x = xpermuts15bits[0].y = 0;
															uint ind = tilesize-2-i;

															for(uint d=0; d<D; d++)
															{
																bool isFlag=true;
																for(uint i=0; i<(uint)(LOGK-1); i++)
																	isFlag &= (ind != x[i][d]);

																if( isFlag )
																{
																	if(xcount[d] < 32)
																	{
																		int digit_value = ((permutN_lsb[d] >> xcount[d]) & 1);
																		if(ind < 32)
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << ind);
																			if(digit_value)
																				permutM_lsb[d] |= mask;
																			else
																				permutM_lsb[d] &= (unsigned int)~mask;
																		}
																		else
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << (ind-32));
																			if(digit_value)
																				permutM_msb[d] |= mask;
																			else
																				permutM_msb[d] &= (unsigned int)~mask;
																		}
																	}
																	else
																	{
																		int digit_value = ((permutN_msb[d] >> (xcount[d]-32)) & 1);
																		if(ind < 32)
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << ind);
																			if(digit_value)
																				permutM_lsb[d] |= mask;
																			else
																				permutM_lsb[d] &= (unsigned int)~mask;
																		}
																		else
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << (ind-32));
																			if(digit_value)
																				permutM_msb[d] |= mask;
																			else
																				permutM_msb[d] &= (unsigned int)~mask;
																		}
																	}
																	xcount[d]--;
																}
															}
														}

														permut_x_lsb = permutM_lsb[0];
														permut_y_lsb = permutM_lsb[1];
														permut_x_msb = permutM_msb[0];
														permut_y_msb = permutM_msb[1];

														unsigned int permutted_pattern[128];

														int ind;
														unsigned int xpermuts_lsb[2];
														xpermuts_lsb[0] = permut_x_lsb;
														xpermuts_lsb[1] = permut_y_lsb;

														unsigned int xpermuts_msb[2];
														xpermuts_msb[0] = permut_x_msb;
														xpermuts_msb[1] = permut_y_msb;

														uint NBITS = 6;
														uint PERMUTS_NBITS = tilesize-1;

														for(uint ipt=0; ipt<tilesize; ipt++)
														{
															unsigned int xresdigits[2];
															unsigned int thisSobolPt[2];
															for(uint d=0; d<2; d++)
																thisSobolPt[d] = pattern[2*ipt+d];
															xresdigits[0] = 0;
															xresdigits[1] = 0;

															for(uint ilevel=0; ilevel<NBITS; ilevel++) {

																for(uint d=0; d<2; d++)
																{
																	int power=(1 << ilevel);
																	ind = (pattern[2*ipt+d] >> (NBITS-ilevel)) + (power-1);
																	int digit_value_1 = 0;
																	if(PERMUTS_NBITS-ind-1 < 32)
																		digit_value_1 =  (xpermuts_lsb[d] >> (PERMUTS_NBITS-ind-1)) & 1;
																	else
																		digit_value_1 =  (xpermuts_msb[d] >> (PERMUTS_NBITS-ind-1-32)) & 1;
																	int digit_value_2 =  (thisSobolPt[d] >> (NBITS-ilevel-1)) & 1;
																	int digit_value = (digit_value_1 ^ digit_value_2);
																	int digit_pos = NBITS-ilevel-1;
																	unsigned int mask = (unsigned int)((unsigned int)1 << digit_pos);
																	if(digit_value)
																		xresdigits[d] |= mask;
																	else
																		xresdigits[d] &= (unsigned int)~mask;
																}
															}

															for(uint d=0; d<2; d++)
																permutted_pattern[2*ipt+d] = xresdigits[d];
														}

														float mindist = 10000.0;
														for (uint ipts = 0; ipts < tilesize; ipts++) {
															for (uint ipts_to_compare = ipts+1; ipts_to_compare < tilesize; ipts_to_compare++) {

																float dist_sq = 0.0;
																float dx = (float)permutted_pattern[2*ipts]/(float)tilesize - (float)permutted_pattern[2*ipts_to_compare]/(float)tilesize;
																dist_sq += dx*dx;
																float dy = (float)permutted_pattern[2*ipts+1]/(float)tilesize - (float)permutted_pattern[2*ipts_to_compare+1]/(float)tilesize;
																dist_sq += dy*dy;

																if (dist_sq < mindist) mindist = dist_sq;
															}
														}
														min_dist[gid] = sqrt(mindist);
														score[gid]= min_dist[gid];
													}
                                                    );

const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(__kernel void computeScorePCF(__global const unsigned int* permut_lsb,
																				__global const unsigned int* permut_msb,
																				__global const unsigned int* pattern,
																				__global const float* pcf,
																				__global float* min_dist,
                                                                                __global float* score
																				//__global unsigned int* debug
                                                                              )
													{
														uint gid = get_global_id(0);

														unsigned int permut_x_lsb = permut_lsb[2*gid];
														unsigned int permut_y_lsb = permut_lsb[2*gid+1];
														unsigned int permut_x_msb = permut_msb[2*gid];
														unsigned int permut_y_msb = permut_msb[2*gid+1];

														unsigned int permutN_lsb[2];
														unsigned int permutN_msb[2];
														permutN_lsb[0] = permut_x_lsb;
														permutN_lsb[1] = permut_y_lsb;
														permutN_msb[0] = permut_x_msb;
														permutN_msb[1] = permut_y_msb;

														unsigned int permutM_lsb[2];
														unsigned int permutM_msb[2];
														permutM_lsb[0] = 0;
														permutM_lsb[1] = 0;
														permutM_msb[0] = 0;
														permutM_msb[1] = 0;

														uint tilesize = 64;

														uint LOGK=6;
														uint D=2;

														uint x[5][2];
														for(uint d=0; d<D; d++)
														{
															int nb_flags = 2;
															int orig_lvl = tilesize-2 -1;
															for(uint i=1; i<LOGK; i++)
															{
																x[i-1][d] = orig_lvl - (pattern[d]/(tilesize/nb_flags));
																orig_lvl -= nb_flags;
																nb_flags*=2;
															}
														}

														uint xcount[2];
														for(uint d=0; d<D; d++)
															xcount[d] = tilesize-LOGK-2; // bitset indexing: 0==least significant bit

														for(uint i=1; i<tilesize-1; i++) { // start form the second i because xpermuts15bits[0].x = xpermuts15bits[0].y = 0;
															uint ind = tilesize-2-i;

															for(uint d=0; d<D; d++)
															{
																bool isFlag=true;
																for(uint i=0; i<(uint)(LOGK-1); i++)
																	isFlag &= (ind != x[i][d]);

																if( isFlag )
																{
																	if(xcount[d] < 32)
																	{
																		int digit_value = ((permutN_lsb[d] >> xcount[d]) & 1);
																		if(ind < 32)
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << ind);
																			if(digit_value)
																				permutM_lsb[d] |= mask;
																			else
																				permutM_lsb[d] &= (unsigned int)~mask;
																		}
																		else
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << (ind-32));
																			if(digit_value)
																				permutM_msb[d] |= mask;
																			else
																				permutM_msb[d] &= (unsigned int)~mask;
																		}
																	}
																	else
																	{
																		int digit_value = ((permutN_msb[d] >> (xcount[d]-32)) & 1);
																		if(ind < 32)
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << ind);
																			if(digit_value)
																				permutM_lsb[d] |= mask;
																			else
																				permutM_lsb[d] &= (unsigned int)~mask;
																		}
																		else
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << (ind-32));
																			if(digit_value)
																				permutM_msb[d] |= mask;
																			else
																				permutM_msb[d] &= (unsigned int)~mask;
																		}
																	}
																	xcount[d]--;
																}
															}
														}

														permut_x_lsb = permutM_lsb[0];
														permut_y_lsb = permutM_lsb[1];
														permut_x_msb = permutM_msb[0];
														permut_y_msb = permutM_msb[1];

														unsigned int permutted_pattern[128];

														int ind;
														unsigned int xpermuts_lsb[2];
														xpermuts_lsb[0] = permut_x_lsb;
														xpermuts_lsb[1] = permut_y_lsb;

														unsigned int xpermuts_msb[2];
														xpermuts_msb[0] = permut_x_msb;
														xpermuts_msb[1] = permut_y_msb;

														uint NBITS = 6;
														uint PERMUTS_NBITS = tilesize-1;

														for(uint ipt=0; ipt<tilesize; ipt++)
														{
															unsigned int xresdigits[2];
															unsigned int thisSobolPt[2];
															for(uint d=0; d<2; d++)
																thisSobolPt[d] = pattern[2*ipt+d];
															xresdigits[0] = 0;
															xresdigits[1] = 0;

															for(uint ilevel=0; ilevel<NBITS; ilevel++) {

																for(uint d=0; d<2; d++)
																{
																	int power=(1 << ilevel);
																	ind = (pattern[2*ipt+d] >> (NBITS-ilevel)) + (power-1);
																	int digit_value_1 = 0;
																	if(PERMUTS_NBITS-ind-1 < 32)
																		digit_value_1 =  (xpermuts_lsb[d] >> (PERMUTS_NBITS-ind-1)) & 1;
																	else
																		digit_value_1 =  (xpermuts_msb[d] >> (PERMUTS_NBITS-ind-1-32)) & 1;
																	int digit_value_2 =  (thisSobolPt[d] >> (NBITS-ilevel-1)) & 1;
																	int digit_value = (digit_value_1 ^ digit_value_2);
																	int digit_pos = NBITS-ilevel-1;
																	unsigned int mask = (unsigned int)((unsigned int)1 << digit_pos);
																	if(digit_value)
																		xresdigits[d] |= mask;
																	else
																		xresdigits[d] &= (unsigned int)~mask;
																}
															}

															for(uint d=0; d<2; d++)
																permutted_pattern[2*ipt+d] = xresdigits[d];
														}

														float mindist = 10000.0;
														for (uint ipts = 0; ipts < tilesize; ipts++) {
															for (uint ipts_to_compare = ipts+1; ipts_to_compare < tilesize; ipts_to_compare++) {

																float dist_sq = 0.0;
																float dx = (float)permutted_pattern[2*ipts]/(float)tilesize - (float)permutted_pattern[2*ipts_to_compare]/(float)tilesize;
																dist_sq += dx*dx;
																float dy = (float)permutted_pattern[2*ipts+1]/(float)tilesize - (float)permutted_pattern[2*ipts_to_compare+1]/(float)tilesize;
																dist_sq += dy*dy;

																if (dist_sq < mindist) mindist = dist_sq;
															}
														}
														min_dist[gid] = sqrt(mindist);

														if(min_dist[gid] < 0.07)
														{
															score[gid] = 0.01;
														}
														else
														{
															float norm_dist = 2.0 * sqrt( 1.0 / (2.0*sqrt(3.0)*tilesize) );
															float smoothing = 0.1 * norm_dist;
															float ra = 1.5*smoothing;
															float rb = 2.5*norm_dist;

															float ani_smoothing = 0.01;
															float aa = 0;
															float ab = 2*M_PI;

															uint nbbins = 20;
															float pcf_pattern[20];
															float anisotropy_pattern[20];
															for(uint pcfid = 0; pcfid < nbbins; ++pcfid)
															{
																pcf_pattern[pcfid] = 0;
																anisotropy_pattern[pcfid] = 0;
															}

															float bin_size = (rb-ra)/(float)nbbins;
															uint covering = ((2.0*smoothing)/bin_size)+1;
															if(covering > nbbins)
																covering = nbbins;

															float ani_bin_size = (ab-aa)/(float)nbbins;
															uint ani_covering = ((2.0*ani_smoothing)/ani_bin_size)+1;
															if(ani_covering > nbbins)
																ani_covering = nbbins;

															for(uint i=0; i<tilesize; i++)
															{
																for(uint j=(i+1); j<tilesize; j++)
																{
																	float dist_sq = 0;
																	float dx = (float)permutted_pattern[2*i]/(float)tilesize - (float)permutted_pattern[2*j]/(float)tilesize;
																	dx*=dx;
																	dist_sq += dx;
																	float dy = (float)permutted_pattern[2*i+1]/(float)tilesize - (float)permutted_pattern[2*j+1]/(float)tilesize;
																	dy*=dy;
																	dist_sq += dy;
																	float dxy = dist_sq;
																	dx=sqrt(dx);
																	dy=sqrt(dy);

																	float vector_angle = atan(dy/dx);
																	int ani_init_bin = vector_angle/bin_size;
																	anisotropy_pattern[ani_init_bin] += 1;

																	int init_bin = sqrt(dxy)/bin_size;
																	int ida = init_bin - covering;
																	int idb = init_bin + covering;
																	if(ida < 0) ida = 0;
																	if(idb > nbbins) idb = nbbins;
																	for(uint pcfid = ida; pcfid < idb; ++pcfid)
																	{
																		float r = ra + pcfid*(rb - ra) / (float) nbbins;
																		float dist = r - sqrt(dxy);
																		float val = ( 1.0/(sqrt(2*M_PI)*smoothing) ) * exp( -0.5*(dist*dist)/(smoothing*smoothing) );
																		pcf_pattern[pcfid] += 2*val;
																	}
																}
															}

															for(uint pcfid = 0; pcfid < nbbins; ++pcfid)
															{
																float r = ra + pcfid*(rb - ra) / (float) nbbins;

																float cov = 1.0 - ((2.0*r)/M_PI)*2.0 + ((r*r)/M_PI);
																float factor = 2.0*M_PI*r*cov;
																factor = 1.0/factor;

																pcf_pattern[pcfid] *= factor;
																pcf_pattern[pcfid] /= (tilesize*(tilesize-1));
																anisotropy_pattern[pcfid] /= 20.0;
															}

															float linf = 0;
															float ani_linf = 0;
															for(uint i=0; i<nbbins; i++)
															{
																if(pcf_pattern[i] != 0 && fabs(pcf[i] - pcf_pattern[i]) > linf )
																	linf = fabs(pcf[i] - pcf_pattern[i]);

																if(anisotropy_pattern[i] != 0 && fabs(1 - anisotropy_pattern[i]) > ani_linf )
																	ani_linf = fabs(1 - anisotropy_pattern[i]);
															}

															score[gid] = 10 -linf -ani_linf;
														}
													}
                                                    );

bool permutFromNbitsToMbits(uint N, uint M, uint fixed_point[2], unsigned long long int permutN_x, unsigned long long int permutN_y,
							unsigned long long int& permutM_x, unsigned long long int& permutM_y)
{
	int tilesize = M+1;
	int LOGK=log2(tilesize);
	uint D=2;

	std::vector< std::array<uint, 2> > x;
	x.resize(LOGK-1);
	for(uint d=0; d<D; d++)
	{
		int nb_flags = 2;
		int orig_lvl = tilesize-2 -1;
		for(int i=1; i<LOGK; i++)
		{
			x[i-1][d] = orig_lvl - (fixed_point[d]/(tilesize/nb_flags));
			orig_lvl -= nb_flags;
			nb_flags*=2;
		}
	}

	std::bitset<64> xpermutsNbits[2];
	xpermutsNbits[0] = permutN_x;
	xpermutsNbits[1] = permutN_y;

	std::bitset<64> xpermutsMbits[2];
	int xcount[2];
	for(uint d=0; d<D; d++)
		xcount[d] = tilesize-LOGK-2; // bitset indexing: 0==least significant bit
	for(uint i=1; i<(uint)(tilesize-1); i++) { // start form the second i because xpermuts15bits[0].x = xpermuts15bits[0].y = 0;
		uint ind = tilesize-2-i;

		for(uint d=0; d<D; d++)
		{
			bool isFlag=true;
			for(int i=0; i<LOGK-1; i++)
				isFlag &= (ind != x[i][d]);

			if( isFlag ) {
				xpermutsMbits[d][ind] = xpermutsNbits[d][xcount[d]];
				xcount[d]--;
			}
		}
	}

	permutM_x = xpermutsMbits[0].to_ullong();
	permutM_y = xpermutsMbits[1].to_ullong();

	return true;
}

/*void computeScoreCPU(const unsigned int* permut_lsb,
						const unsigned int* permut_msb,
						const unsigned int* pattern,
						const float* pcf,
						float* min_dist,
                        float* score, uint gid)
{
	//Copy OpenCL Kernel Here
}*/

bool optimizeGPU(utk::Pointset<2, unsigned int, utk::Point<2, unsigned int> > pts, utk::Histogram1dd pcf_data, utk::Vector<2, unsigned long long int>& resulting_permut)
{
	assert(pts.size() == 64);

	srand48(time(NULL));
	float max_score= 0;
	float max_dist = 0;
	int max_id=0;

	int t = time(NULL);

	//get default device and setup context
	compute::device device = compute::system::default_device();
	compute::context context(device);
	compute::command_queue queue(context, device);

	int nbbins = 20;
	assert(pcf_data.size() == (uint)nbbins);
	std::vector<float> local_pcf;
	local_pcf.resize(nbbins);
	for(int i=0; i<nbbins; i++)
		local_pcf[i] = pcf_data[i].second;

	std::cout << "Local PCF Vector OK" << std::endl;

	//Send PCF to the device
	compute::vector<float> device_pcf(local_pcf.size(), context);
	compute::copy(local_pcf.begin(), local_pcf.end(), device_pcf.begin(), queue);
	std::cout << "Device PCF Vector OK" << std::endl;

	std::vector<unsigned int> local_pattern(pts.size()*2, 0);
	for(uint i=0; i<pts.size(); i++)
	{
		local_pattern[2*i] = pts[i].pos()[0];
		local_pattern[2*i+1] = pts[i].pos()[1];
	}
	std::cout << "Local Pattern Vector OK" << std::endl;

	//Send pattern to the device
	compute::vector<unsigned int> device_pattern(pts.size()*2, context);
	compute::copy(local_pattern.begin(), local_pattern.end(), device_pattern.begin(), queue);
	std::cout << "Device Pattern Vector OK" << std::endl;

	uint tilesize=64;
	uint N, M;
	M = tilesize-1;
	N = M - log2(tilesize);
	uint zero_pt[2]; zero_pt[0] = local_pattern[0]; zero_pt[1] = local_pattern[1];
	int nbfreedigits = 11;
	int npermut = (1 << nbfreedigits);

	int nbbatches=2048;
	int batch_size = (npermut*npermut)/nbbatches;

	//Create result vector
	compute::vector<float> device_mindist(batch_size, context);
	std::fill(device_mindist.begin(), device_mindist.end(), 0);
	std::cout << "Device MinDist Vector OK" << std::endl;

	compute::vector<float> device_score(batch_size, context);
	std::fill(device_score.begin(), device_score.end(), 0);
	std::cout << "Device Result Vector OK" << std::endl;

	compute::vector<unsigned int> device_permutations_lsb(batch_size*2, context);
	compute::vector<unsigned int> device_permutations_msb(batch_size*2, context);
	std::vector<float> local_mindist(batch_size);
	std::vector<float> local_score(batch_size);

	std::cout << "Building Kernel ..." << std::endl;

	//Create and build the kernels

	//Create and build the kernels
	compute::program m_program;
	m_program = compute::program::create_with_source(source, context);
	try
	{
		m_program.build();
	}
	catch(boost::compute::opencl_error &e)
	{
		std::cout << m_program.build_log() << std::endl;
	}
	compute::kernel score_kernel;
	score_kernel = m_program.create_kernel("computeScorePCF");
	score_kernel.set_arg(0, device_permutations_lsb.get_buffer());
	score_kernel.set_arg(1, device_permutations_msb.get_buffer());
	score_kernel.set_arg(2, device_pattern.get_buffer());
	score_kernel.set_arg(3, device_pcf.get_buffer());
	score_kernel.set_arg(4, device_mindist.get_buffer());
	score_kernel.set_arg(5, device_score.get_buffer());
	std::cout << "OK" << std::endl;

	std::cout << "Optimizing first " << nbfreedigits << " digits ..." << std::endl;

	std::vector<unsigned long long int> local_permutations_full(npermut*npermut*2, 0);
	for(int i=0; i<npermut; i++)
	{
		for(int j=0; j<npermut; j++)
		{
			local_permutations_full[2*(i*npermut+j)] = (unsigned long long int)((unsigned long long int)i << (unsigned long long int)(N-nbfreedigits));
			local_permutations_full[2*(i*npermut+j)+1] = (unsigned long long int)((unsigned long long int)j << (unsigned long long int)(N-nbfreedigits));
		}
	}
	std::vector<unsigned int> local_permutations_lsb(npermut*npermut*2, 0);
	std::vector<unsigned int> local_permutations_msb(npermut*npermut*2, 0);
	for(int i=0; i<npermut; i++)
	for(int j=0; j<npermut; j++)
	{
		local_permutations_lsb[2*(i*npermut+j)] = (unsigned long long int)((local_permutations_full[2*(i*npermut+j)]) & (unsigned long long int)0xffffff);
		local_permutations_lsb[2*(i*npermut+j)+1] = (unsigned long long int)((local_permutations_full[2*(i*npermut+j)+1]) & (unsigned long long int)0xffffff);

		local_permutations_msb[2*(i*npermut+j)] = ((local_permutations_full[2*(i*npermut+j)]) >> 32);
		local_permutations_msb[2*(i*npermut+j)+1] = ((local_permutations_full[2*(i*npermut+j)+1]) >> 32);
	}
	std::cout << "Local Permut Vector OK" << std::endl;

	for(int batch=0; batch<nbbatches; batch++)
	{
		compute::copy(local_permutations_lsb.begin()+(2*batch*batch_size), local_permutations_lsb.begin()+(2*(batch+1)*batch_size), device_permutations_lsb.begin(), queue);
		compute::copy(local_permutations_msb.begin()+(2*batch*batch_size), local_permutations_msb.begin()+(2*(batch+1)*batch_size), device_permutations_msb.begin(), queue);

		//Go Go go !
		//std::cout << batch << "/" << nbbatches << "\r" << std::flush;
		queue.enqueue_1d_range_kernel(score_kernel, 0, batch_size, 0);

		compute::copy(device_mindist.begin(), device_mindist.end(), local_mindist.begin(), queue);
		compute::copy(device_score.begin(), device_score.end(), local_score.begin(), queue);

		for(int i=0; i<batch_size; i++)
		{
			if( max_score < local_score[i])
			{
				max_dist = local_mindist[i];
				max_id = batch*batch_size+i;
				max_score = local_score[i];
			}
		}
	}

	/*unsigned long long int final_perm_x, final_perm_y;
	permutFromNbitsToMbits(N, M, zero_pt, local_permutations_full[2*max_id], local_permutations_full[2*max_id+1], final_perm_x, final_perm_y);
	std::cout << "Done [" << time(NULL) - t << "s]" << std::endl;
	std::cout<< "[DEVICE] Current Best Permut " << final_perm_x << "," << final_perm_y << " (score " << max_score << ")" << "(dist " << max_dist << ")" << std::endl;*/

	std::cout << "Optimizing last " << (N-nbfreedigits) << " digits ..." << std::endl;
	for(int i=0; i<npermut; i++)
	{
		for(int j=0; j<npermut; j++)
		{
			unsigned long long int randval = (unsigned long long int)(drand48()*((unsigned long long int)1 << (unsigned long long int)(N-nbfreedigits)));
			local_permutations_full[2*(i*npermut+j)] = local_permutations_full[2*max_id] + randval;
			randval = drand48()*(1 << (N-nbfreedigits));
			local_permutations_full[2*(i*npermut+j)+1] = local_permutations_full[2*max_id+1] + randval;
		}
	}
	for(int i=0; i<npermut; i++)
	for(int j=0; j<npermut; j++)
	{
		local_permutations_lsb[2*(i*npermut+j)] = (unsigned long long int)((local_permutations_full[2*(i*npermut+j)]) & (unsigned long long int)0xffffff);
		local_permutations_lsb[2*(i*npermut+j)+1] = (unsigned long long int)((local_permutations_full[2*(i*npermut+j)+1]) & (unsigned long long int)0xffffff);

		local_permutations_msb[2*(i*npermut+j)] = ((local_permutations_full[2*(i*npermut+j)]) >> 32);
		local_permutations_msb[2*(i*npermut+j)+1] = ((local_permutations_full[2*(i*npermut+j)+1]) >> 32);
	}
	std::cout << "Local Permut Vector OK" << std::endl;

	for(int batch=0; batch<nbbatches; batch++)
	{
		compute::copy(local_permutations_lsb.begin()+(2*batch*batch_size), local_permutations_lsb.begin()+(2*(batch+1)*batch_size), device_permutations_lsb.begin(), queue);
		compute::copy(local_permutations_msb.begin()+(2*batch*batch_size), local_permutations_msb.begin()+(2*(batch+1)*batch_size), device_permutations_msb.begin(), queue);

		//Go Go go !
		//std::cout << batch << "/" << nbbatches << "\r" << std::flush;
		queue.enqueue_1d_range_kernel(score_kernel, 0, batch_size, 0);
		compute::copy(device_mindist.begin(), device_mindist.end(), local_mindist.begin(), queue);
		compute::copy(device_score.begin(), device_score.end(), local_score.begin(), queue);

		for(int i=0; i<batch_size; i++)
		{
			//computeScoreCPU(&local_permutations_lsb[2*batch*batch_size], &local_permutations_msb[2*batch*batch_size], &local_pattern[0], &local_pcf[0], &local_mindist[0], &local_score[0], i);
			//exit(1);
			if( max_score < local_score[i])
			{
				max_dist = local_mindist[i];
				max_id = batch*batch_size+i;
				max_score = local_score[i];
			}
		}
	}

	resulting_permut[0] = 0; resulting_permut[1] = 0;
	permutFromNbitsToMbits(N, M, zero_pt, local_permutations_full[2*max_id], local_permutations_full[2*max_id+1], resulting_permut[0], resulting_permut[1]);
	std::cout << "Done [" << time(NULL) - t << "s]" << std::endl;
	std::cout<< "[DEVICE] Best Permut " << resulting_permut[0] << "," << resulting_permut[1] << " (score " << max_score << ")" << "(dist " << max_dist << ")" << std::endl;

	return 0;
}