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

#include "vehicle_map_corrector.hpp"
#include "cmath"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/transform_listener.h"
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

bool VehicleMapCorrector::correct(
  const VectorMap & vector_map, const geometry_msgs::TransformStamped & transform_stamped,
  autoware_perception_msgs::Shape & shape_output, geometry_msgs::Pose & pose_output,
  bool & orientation_output)
{
  geometry_msgs::Point point;
  tf2::doTransform(pose_output.position, point, transform_stamped);
  double map_coord_vector_yaw;
  if (vector_map.getyaw(point.x, point.y, map_coord_vector_yaw)) {
    // calc yaw from vector map
    tf2::Quaternion tf_quaternion;
    double map_coord_tf_roll, map_coord_tf_pitch, map_coord_tf_yaw;
    tf2::fromMsg(transform_stamped.transform.rotation, tf_quaternion);
    tf2::Matrix3x3(tf_quaternion).getRPY(map_coord_tf_roll, map_coord_tf_pitch, map_coord_tf_yaw);
    const double vector_map_yaw = map_coord_vector_yaw - map_coord_tf_yaw;

    // calc yaw from currrent orientation
    double input_roll, input_pitch, input_yaw;
    tf2::Quaternion input_quaternion;
    tf2::fromMsg(pose_output.orientation, input_quaternion);
    tf2::Matrix3x3(input_quaternion).getRPY(input_roll, input_pitch, input_yaw);

    double corrected_yaw;

    if (shape_output.dimensions.x < shape_output.dimensions.y) {
      double input_yaw_90_rotated = input_yaw + M_PI_2;
      double input_yaw_270_rotated = input_yaw + M_PI_2 * 3.0;
      double diff_90(std::atan2(
        std::sin(vector_map_yaw - input_yaw_90_rotated),
        std::cos(vector_map_yaw - input_yaw_90_rotated)));
      double diff_270(std::atan2(
        std::sin(vector_map_yaw - input_yaw_270_rotated),
        std::cos(vector_map_yaw - input_yaw_270_rotated)));
      if (rad_threshold_ < std::min(std::fabs(diff_90), std::fabs(diff_270))) return false;
      if (std::fabs(diff_90) < std::fabs(diff_270)) {
        corrected_yaw = input_yaw_90_rotated;
      } else {
        corrected_yaw = input_yaw_270_rotated;
      }
      double temp = shape_output.dimensions.y;
      shape_output.dimensions.y = shape_output.dimensions.x;
      shape_output.dimensions.x = temp;
    } else {
      double input_yaw_0_rotated = input_yaw;
      double input_yaw_180_rotated = input_yaw + M_PI_2 * 2.0;
      double diff_0(std::atan2(
        std::sin(vector_map_yaw - input_yaw_0_rotated),
        std::cos(vector_map_yaw - input_yaw_0_rotated)));
      double diff_180(std::atan2(
        std::sin(vector_map_yaw - input_yaw_180_rotated),
        std::cos(vector_map_yaw - input_yaw_180_rotated)));
      if (rad_threshold_ < std::min(std::fabs(diff_0), std::fabs(diff_180))) return false;
      if (std::fabs(diff_0) < std::fabs(diff_180)) {
        corrected_yaw = input_yaw_0_rotated;
      } else {
        corrected_yaw = input_yaw_180_rotated;
      }
    }
    tf2::Quaternion output_quaternion;
    output_quaternion.setRPY(0.0, 0.0, corrected_yaw);
    pose_output.orientation = tf2::toMsg(output_quaternion);
    orientation_output = true;
  } else {
    // quaternion.setRPY(0.0, -M_PI / 2.0, 0.0);
    // pose_output.orientation = tf2::toMsg(quaternion);
  }

  return true;
}


#include <algorithm>
#include <cstdlib>
#include <limits>
#include <random>
#include <vector>
#include <iostream>
#include <cmath>
#include <string>
#include <limits>
#include <map>
#include <set>
#include <chrono>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <string>
#include <boost/serialization/map.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include "util.hpp"
#include "munkres.cpp"

namespace brainclustering {

    class Table {
    private:
        friend class boost::serialization::access;

        using value_type = double;
        using duipair = std::pair<value_type, size_t>;
        size_t n_cluster_main_;
        size_t n_cluster_secondary_;
        double background_value_;
        std::vector <std::map<value_type, duipair>> vec_map_;

        template<class Archive>
        void serialize(Archive &ar, const unsigned int /* file_version */) {
            ar & n_cluster_main_ & n_cluster_secondary_ & background_value_ & vec_map_;
        }

    public:
        Table(size_t const n_main, size_t const n_secondary, double background = 0.0) :
                n_cluster_main_(n_main),
                n_cluster_secondary_(n_secondary),
                background_value_(background) {
            vec_map_.resize(n_cluster_main_);
        };

        void add_entry(ndarray const &data_vector_seq1, std::vector <std::vector<size_t>> const &dendogram_seq1,
                       ndarray const &data_vector_seq2, std::vector <std::vector<size_t>> const &dendogram_seq2) {

            // Data from the first sequencing
            auto *data_seq1 = get_c_array<value_type>(data_vector_seq1);
            // Data from the second sequencing
            auto *data_seq2 = get_c_array<value_type>(data_vector_seq2);

            // Sanity check
            if (data_vector_seq1.shape(0) != data_vector_seq2.shape(0)) {
                std::cerr << "The two sequencing files should have the same number of elements. ";
                std::cerr << "Please convert them such that they do. " << std::endl;
                std::abort();
            }
            // Sanity check
            if (dendogram_seq1.size() != (n_cluster_main_ + n_cluster_secondary_) ||
                dendogram_seq2.size() != (n_cluster_main_ + n_cluster_secondary_)) {
                std::cerr
                        << "The size of the dendograms has to be equal to the number of main clusters + number of sub clusters ";
                std::abort();
            }

            size_t n_points = data_vector_seq1.shape(0);

            std::vector <std::vector<size_t>> cluster_indices_seq1(n_cluster_main_);
            std::vector <std::vector<size_t>> cluster_indices_seq2(n_cluster_main_);
            std::vector <size_t> cluster_val_small_seq1(n_points);
            std::vector <size_t> cluster_val_small_seq2(n_points);
            std::vector <std::vector<int>> cost_matrix(n_cluster_main_, std::vector<int>(n_cluster_main_, 0));
            std::vector <std::pair<int, int>> cost_pairs(n_points);

            // TODO: This won't work for more than 2 levels, but do we really need more than 2 levels?
            // Go through the main clusters, which always start at index n_cluster_secondary_
            for (size_t i = n_cluster_secondary_; i < n_cluster_secondary_ + n_cluster_main_; ++i) {
                // Sanity check
                if (dendogram_seq1[i][0] != 0 || dendogram_seq2[i][0] != 0) {
                    std::cout << "The mapping should be considering the first level of the dendogram, but it is not."
                              << std::endl;
                }
                // Go through the nodes of the bigger clusters to reach the leaves
                for (size_t j = 1; j < dendogram_seq1[i].size(); ++j) {
                    // Node that contains the secondary level of the cluster
                    size_t current_smaller_cluster = dendogram_seq1[i][j];
                    // That node can be found exactly at row current_smaller_cluster of the dendogram
                    for (size_t k = 1; k < dendogram_seq1[current_smaller_cluster].size(); ++k) {
                        // Go through the elements inside this cluster
                        size_t current_index = dendogram_seq1[current_smaller_cluster][k];
                        // We give this cluster a label,
                        // which is the current iteration starting from n_cluster_secondary_
                        cost_pairs[current_index].first = i - n_cluster_secondary_;
                        // Add this element to the list of indices for this cluster
                        cluster_indices_seq1[i - n_cluster_secondary_].push_back(current_index);
                        // We also save the index of the position in which the smaller cluster is (0 - n_cluster_secondary)
                        cluster_val_small_seq1[current_index] = j - 1;
                    }
                }

                // The same happens here, but for the second image
                for (size_t j = 1; j < dendogram_seq2[i].size(); ++j) {
                    size_t current_smaller_cluster = dendogram_seq2[i][j];
                    for (size_t k = 1; k < dendogram_seq2[current_smaller_cluster].size(); ++k) {
                        size_t current_index = dendogram_seq2[current_smaller_cluster][k];
                        cost_pairs[current_index].second = i - n_cluster_secondary_;
                        cluster_indices_seq2[i - n_cluster_secondary_].push_back(current_index);
                        cluster_val_small_seq2[current_index] = j - 1;
                    }
                }
            }
            for (auto &cost_pair : cost_pairs) {
                // Build contingengy matrix
                cost_matrix[cost_pair.first][cost_pair.second]++;
            }

            // Run Hungarian algorithm
            auto mapping_tuples = Hungarian::Solve(cost_matrix);
            if (mapping_tuples.size() != n_cluster_main_) {
                std::cerr << "The length of the tuples is not correct." << std::endl;
                std::abort();
            }

            for (size_t t = 0; t < mapping_tuples.size(); ++t) {
                size_t cluster_seq1 = t;
                size_t cluster_seq2 = mapping_tuples[t];
                //std::cout << "The tuple is (" << cluster_seq1 << ", " << cluster_seq2 << ")" << std::endl;
                size_t size_cluster = std::max(dendogram_seq1[n_cluster_secondary_ + cluster_seq1].size(),
                                               dendogram_seq2[n_cluster_secondary_ + cluster_seq2].size()) - 1;

                std::vector <std::vector<int>> smaller_cost_matrix(size_cluster,
                                                                   std::vector<int>(size_cluster, 0));

                std::sort(cluster_indices_seq1[cluster_seq1].begin(),
                          cluster_indices_seq1[cluster_seq1].end());
                std::sort(cluster_indices_seq2[cluster_seq2].begin(),
                          cluster_indices_seq2[cluster_seq2].end());
                std::vector<int> intersection(std::max(cluster_indices_seq1[cluster_seq1].size(),
                                                       cluster_indices_seq2[cluster_seq2].size()));

                // Find the intersection between the values of one vector and the values of the other vector
                // so we know which elements are in the same clusters, and we can use those for munkres
                // Using the intersection values, find in which cluster they are stored
                auto it = std::set_intersection(cluster_indices_seq1[cluster_seq1].begin(),
                                                cluster_indices_seq1[cluster_seq1].end(),
                                                cluster_indices_seq2[cluster_seq2].begin(),
                                                cluster_indices_seq2[cluster_seq2].end(),
                                                intersection.begin());
                intersection.resize(it - intersection.begin());

                // We check for the points that are in both of the large clusters
                for (auto id : intersection) {
                    //std::cout << id << std::endl;
                    // We saved the values of the index in which the smaller cluster is saved
                    size_t cluster_id1 = cluster_val_small_seq1[id];
                    size_t cluster_id2 = cluster_val_small_seq2[id];
                    //std::cout << cluster_id1 << ", " << cluster_id2 << std::endl;
                    // Build contingengy matrix
                    smaller_cost_matrix[cluster_id1][cluster_id2]++;
                }

                auto smaller_mapping_tuples = Hungarian::Solve(smaller_cost_matrix);

                // Iterate through all points that correspond to each small particular fraction and
                // compute the average of these points
                for (size_t tt = 0; tt < smaller_mapping_tuples.size(); ++tt) {
                    // Since we considered the maximum of the two sizes, we need to take into account the fact that
                    // some of the mapping might not be valid.
                    // It might be that cluster 7 (in seq1) is mapped to cluster 5 (in seq2), which doesn't exist.
                    // So we need to ignore these mappings
                    if (tt >= dendogram_seq1[n_cluster_secondary_ + cluster_seq1].size() - 1 ||
                        smaller_mapping_tuples[tt] >= dendogram_seq2[n_cluster_secondary_ + cluster_seq2].size() - 1) {
                        continue;
                    }

                    size_t smaller_cluster_seq1 =
                            dendogram_seq1[n_cluster_secondary_ + cluster_seq1][tt + 1];
                    size_t smaller_cluster_seq2 =
                            dendogram_seq2[n_cluster_secondary_ + cluster_seq2][smaller_mapping_tuples[tt] + 1];

                    // Sanity check
                    if (dendogram_seq1[smaller_cluster_seq1][0] != 1 || dendogram_seq2[smaller_cluster_seq2][0] != 1) {
                        std::cout
                                << "The mapping should be considering the second level of the dendogram, but it is not."
                                << std::endl;
                    }

                    if (dendogram_seq1[smaller_cluster_seq1].size() == 1 ||
                        dendogram_seq2[smaller_cluster_seq2].size() == 1) {
                        continue;
                    }
                    double x = 0.0;
                    size_t n_x = 0;
                    for (size_t j = 1; j < dendogram_seq1[smaller_cluster_seq1].size(); ++j) {
                        // Sum up the values only if they are not zero
                        if (data_seq1[dendogram_seq1[smaller_cluster_seq1][j]] > background_value_) {
                            n_x++;
                            x += data_seq1[dendogram_seq1[smaller_cluster_seq1][j]];
                        }
                    }
                    // If n_x is zero and would produce a /0, then set it to zero (it will not be considered)
                    x = (n_x > 0) ? x / (n_x * 1.0) : 0.0;
                    double y = 0.0;
                    size_t n_y = 0;
                    for (size_t j = 1; j < dendogram_seq2[smaller_cluster_seq2].size(); ++j) {
                        if (data_seq2[dendogram_seq2[smaller_cluster_seq2][j]] > background_value_) {
                            n_y++;
                            y += data_seq2[dendogram_seq2[smaller_cluster_seq2][j]];
                        }
                    }
                    // If n_y is zero and would produce a /0, then set it to zero (it will not be considered)
                    y = (n_y > 0) ? y / (n_y * 1.0) : 0.0;

                    auto found = vec_map_[t].find(x);
                    if (found == vec_map_[t].end()) {
                        vec_map_[t].insert({x, duipair(y, 1)});
                    } else {
                        found->second.second++;
                        found->second.first += std::abs(y - found->second.first) / found->second.second;
                    }
                }
            }

            /*for (size_t k = 0; k < vec_map_.size(); ++k) {
                std::cout << "Looking at the " << k << "th map" << std::endl;
                for (auto p : vec_map_[k]) {
                    std::cout << "x: " << p.first << ", y: " << p.second.y_ << ", " <<
                              p.second.count_ << " times." << std::endl;
                }
            }*/
        }

        void return_sequencing(ndarray const &data_vector_seq1, ndarray const &labels_vector_seq1,
                               ndarray const &new_data_vector_seq2) {
            // Data of source sequencing
            auto *data_seq1 = get_c_array<value_type>(data_vector_seq1);
            // Labels of source sequencing
            auto *labels_seq1 = get_c_array<value_type>(labels_vector_seq1);
            // Empty array for new sequencing
            auto *new_data_seq2 = get_c_array<value_type>(new_data_vector_seq2);

            /*for (size_t cluster = 0; cluster < n_cluster_main_; ++cluster) {
                std::cout << "Looking up the " << cluster + 1 << "th map" << std::endl;
                for (auto p : vec_map_[cluster]) {
                    std::cout << "x: " << p.first << ", y: " << p.second.first << ", " <<
                              p.second.second << " times." << std::endl;
                }
            }*/
            std::vector <std::vector<int>> cost_matrix(n_cluster_main_, std::vector<int>(n_cluster_main_, 0));

            for (int i = 0; i < labels_vector_seq1.shape(0); ++i) {
                // Skip background points
                if (data_seq1[i] == background_value_) {
                    continue;
                }
                size_t mapped_cluster = labels_seq1[i];

                // Try to find the point
                auto found = vec_map_[mapped_cluster].find(data_seq1[i]);
                // If the point does not exist
                if (found == vec_map_[mapped_cluster].end()) {
                    // Find the upper bound
                    auto upper = vec_map_[mapped_cluster].upper_bound(data_seq1[i]);
                    // Set the lower bound to the upperbound
                    auto lower = upper;
                    // If we are at the beginning of the table
                    if (upper == vec_map_[mapped_cluster].begin()) {
                        new_data_seq2[i] = (upper->second.first * data_seq1[i]) / upper->first;
                        // If we are at the end of the table
                    } else if (upper == vec_map_[mapped_cluster].end()) {
                        // Decrease upper
                        --upper;
                        new_data_seq2[i] = (upper->second.first * data_seq1[i]) / upper->first;
                    } else {
                        // Otherwise do interpolation
                        --lower;
                        new_data_seq2[i] =
                                (lower->second.first * (upper->first - data_seq1[i]) +
                                 upper->second.first * (data_seq1[i] - lower->first)) /
                                (upper->first - lower->first);
                    }
                    /*std::cout << "Point " << data_seq1[i] << ", cluster " << labels_seq1[i] <<
                              "; Lowerbound: " << lower->first <<
                              ", upperbound " << upper->first << std::endl;
                    std::cout << "New point " << new_data_seq2[i] << "; " <<
                              "Lowerbound: " << lower->second.first <<
                              ", upperbound " << upper->second.first << "" << std::endl;*/
                } else {
                    // If found, give the same value
                    new_data_seq2[i] = found->second.first;
                }
            }
        }

    };

    void save_mapping(const Table &t, const char *filename) {
        // Make an archive
        std::ofstream ofs(filename);
        boost::archive::text_oarchive oa(ofs);
        oa << t;
    }

    void restore_mapping(Table &t, const char *filename) {
        // Open the archive
        std::ifstream ifs(filename);
        boost::archive::text_iarchive ia(ifs);

        // Restore the table from the archive
        ia >> t;
    }

    BOOST_PYTHON_MODULE (mapping) {
            numpy::initialize();

            python::class_<Table, boost::noncopyable>("Table", python::init<size_t, size_t, double>())
            .def("add_entry", &Table::add_entry)
            .def("return_sequencing", &Table::return_sequencing);
            python::def("save_mapping", save_mapping);
            python::def("restore_mapping", restore_mapping);

    }
}


#include "globimap/counting_globimap.hpp"
#include "globimap_test_config.hpp"
#include <algorithm>
#include <chrono>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <limits>
#include <math.h>
#include <string>

#include <highfive/H5File.hpp>
#include <tqdm.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include "archive.h"
#include "loc.hpp"
#include "rasterizer.hpp"
#include "shapefile.hpp"

#include <H5Cpp.h>

namespace fs = std::filesystem;

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
namespace bgt = boost::geometry::strategy::transform;
typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
typedef bg::model::box<point_t> box_t;
typedef bg::model::polygon<point_t> polygon_t;

typedef std::vector<polygon_t> poly_collection_t;

#ifndef TUM1_ICAML_ORG
const std::string base_path = "/mnt/G/datasets/atlas/";
const std::string vector_base_path = "/mnt/G/datasets/vector/";
const std::string experiments_path =
    "/home/moritz/workspace/bgdm/globimap/experiments/";
#else
const std::string base_path = "/home/moritz/tf/pointclouds_2d/data/";
const std::string experiments_path = "/home/moritz/tf/globimap/experiments/";
const std::string vector_base_path = "/home/moritz/tf/vector/";
#endif

std::vector<std::string> datasets{"twitter_1mio_coords.h5",
                                  "twitter_10mio_coords.h5",
                                  "twitter_100mio_coords.h5"};

// std::vector<std::string> datasets{"twitter_200mio_coords.h5",
//                                   "asia_200mio_coords.h5"};
std::vector<std::string> polygon_sets{"tl_2017_us_zcta510",
                                      "Global_LSIB_Polygons_Detailed"};
template <typename T> std::string render_hist(std::vector<T> hist) {
  std::stringstream ss;
  ss << "[";
  for (auto i = 0; i < hist.size(); ++i) {
    ss << hist[i] << ((i < (hist.size() - 1)) ? ", " : "");
  }
  ss << "]";
  return ss.str();
}

template <typename T>
std::string render_stat(const std::string &name, std::vector<T> stat) {
  double stat_min = FLT_MAX;
  double stat_max = 0;
  double stat_mean = 0;
  double stat_std = 0;

  for (double v : stat) {
    stat_min = std::min(stat_min, v);
    stat_max = std::max(stat_max, v);
    stat_mean += v;
  }
  stat_mean /= stat.size();

  for (double v : stat) {
    stat_std += pow((stat_mean - v), 2);
  }
  stat_std /= stat.size();
  stat_std = sqrt(stat_std);

  std::stringstream ss;
  ss << "\"" << name << "\": {\n";
  ss << "\"min\": " << stat_min << ",\n";
  ss << "\"max\": " << stat_max << ",\n";
  ss << "\"mean\": " << stat_mean << ",\n";
  ss << "\"std\": " << stat_std << ",\n";
  ss << "\"hist\": " << render_hist(globimap::make_histogram(stat, 1000))
     << "\n";
  ss << "}";
  return ss.str();
}

std::string
test_polys_mask(globimap::CountingGloBiMap<> &g, size_t poly_size,
                std::function<std::vector<uint64_t>(size_t)> poly_gen) {
  std::vector<uint32_t> errors_mask;
  std::vector<uint32_t> errors_hash;
  std::vector<uint32_t> diff_mask_hash;
  std::vector<uint32_t> sizes;
  std::vector<uint32_t> sums_mask;
  std::vector<uint32_t> sums_hash;
  std::vector<uint32_t> sums;
  std::vector<double> errors_mask_pc;
  std::vector<double> errors_hash_pc;
  int n = 0;
  std::cout << "polygons: " << poly_size << " ..." << std::endl;
  auto P = tq::trange(poly_size);
  P.set_prefix("mask for polygons ");
  for (const auto &idx : P) {
    auto raster = poly_gen(idx);

    if (raster.size() > 0) {
      auto hsfn = g.to_hashfn(raster);

      auto mask_conf = globimap::FilterConfig{
          g.config.hash_k, {{1, g.config.layers[0].logsize}}};
      auto mask = globimap::CountingGloBiMap(mask_conf, false);
      mask.put_all(raster);

      auto res_raster = g.get_sum_raster_collected(raster);
      auto res_mask = g.get_sum_masked(mask);
      auto res_hashfn = g.get_sum_hashfn(raster);

      sums.push_back(res_raster);
      sums_mask.push_back(res_mask);
      sums_hash.push_back(res_hashfn);

      uint64_t err_mask = std::abs((int64_t)res_mask - (int64_t)res_raster);
      uint64_t err_hash = std::abs((int64_t)res_hashfn - (int64_t)res_raster);
      uint64_t hash_mask_diff =
          std::abs((int64_t)res_mask - (int64_t)res_hashfn);

      errors_mask.push_back(err_mask);
      errors_hash.push_back(err_hash);

      double div = (double)res_raster;
      double err_mask_pc = ((double)err_mask) / div;
      double err_hash_pc = ((double)err_hash) / div;
      if (div == 0) {
        err_mask_pc = (err_mask > 0 ? 1 : 0);
        err_hash_pc = (err_hash > 0 ? 1 : 0);
      }

      errors_mask_pc.push_back(err_mask_pc);
      errors_hash_pc.push_back(err_hash_pc);

      sizes.push_back(raster.size() / 2);

      n++;
    }
  }

  std::stringstream ss;
  ss << "{\n";
  ss << "\"summary\": " << g.summary() << ",\n";
  ss << "\"polygons\": " << n << ",\n";
  ss << render_stat("errors_mask_pc", errors_mask_pc) << ",\n";
  ss << render_stat("errors_hash_pc", errors_hash_pc) << ",\n";
  ss << render_stat("errors_mask", errors_mask) << ",\n";
  ss << render_stat("errors_hash", errors_hash) << ",\n";
  ss << render_stat("sums", sums) << ",\n";
  ss << render_stat("sums_mask", sums_mask) << ",\n";
  ss << render_stat("sums_hash", sums_hash) << ",\n";
  ss << render_stat("sizes", sizes) << "\n";

  ss << "\n}" << std::endl;
  // std::cout << ss.str();
  return ss.str();
}

static void encode_dataset(globimap::CountingGloBiMap<> &g,
                           const std::string &name, const std::string &ds,
                           uint width, uint height) {
  auto filename = base_path + ds;
  auto batch_size = 4096;
  std::cout << "Encode \"" << filename << "\" \nwith cfg: " << name
            << std::endl;
  using namespace HighFive;

  // we create a new hdf5 file
  auto file = File(filename, File::ReadWrite);
  std::vector<std::vector<double>> read_data;

  // we get the dataset
  DataSet dataset = file.getDataSet("coords");

  using std::chrono::duration;
  using std::chrono::duration_cast;
  using std::chrono::high_resolution_clock;
  using std::chrono::milliseconds;
  auto t1 = high_resolution_clock::now();

  std::vector<std::vector<double>> result;
  auto shape = dataset.getDimensions();
  int batches = std::floor(shape[0] / batch_size);

  auto R = tq::trange(batches);
  R.set_prefix("encoding batches ");
  for (auto i : R) {
    dataset.select({i * batch_size, 0}, {batch_size, 2}).read(result);

    for (auto p : result) {
      double x = (double)width * (((double)p[0] + 180.0) / 360.0);
      double y = (double)height * (((double)p[0] + 90.0) / 180.0);
      g.put({(uint64_t)x, (uint64_t)y});
    }
  }
  auto t2 = high_resolution_clock::now();
  duration<double, std::milli> insert_time = t2 - t1;
}

int main() {
  std::vector<std::vector<globimap::LayerConfig>> cfgs;
  get_configurations(cfgs, {16, 20, 24}, {8, 16, 32});

  {
    uint k = 8;
    auto x = 0;
    uint width = 2 * 8192, height = 2 * 8192;
    std::string exp_name = "test_polygons_mask1";
    save_configs(experiments_path + std::string("config_") + exp_name, cfgs);
    mkdir((experiments_path + exp_name).c_str(), 0777);
    for (auto c : cfgs) {
      globimap::FilterConfig fc{k, c};
      std::cout << "\n******************************************"
                << "\n******************************************" << std::endl;
      std::cout << x << " / " << cfgs.size() << " fc: " << fc.to_string()
                << std::endl;
      auto y = 0;
      for (auto shp : polygon_sets) {
        std::stringstream ss1;
        ss1 << shp << "-" << width << "x" << height;
        auto polyset_name = ss1.str();
        std::stringstream ss;
        ss << vector_base_path << polyset_name;
        auto poly_path = ss.str();

        int poly_count = 0;
        for (auto e : fs::directory_iterator(poly_path))
          poly_count += (e.is_regular_file() ? 1 : 0);

        for (auto ds : datasets) {
          std::stringstream fss;

          fss << experiments_path << exp_name << "/" << exp_name << ".w"
              << width << "h" << height << "." << fc.to_string() << ds << "."
              << polyset_name << ".json";
          if (file_exists(fss.str())) {
            std::cout << "file already exists: " << fss.str() << std::endl;
          } else {
            std::cout << "run: " << fss.str() << std::endl;
            std::ofstream out(fss.str());
            auto g = globimap::CountingGloBiMap(fc, true);
            encode_dataset(g, fc.to_string(), ds, width, height);
            // g.detect_errors(0, 0, width, height);
            std::cout << " COUNTER SIZE: " << g.counter.size() << std::endl;
            std::cout << "test: " << polyset_name << std::endl;
            out << test_polys_mask(g, poly_count, [&](int idx) {
              std::vector<uint64_t> raster;
              std::stringstream ss;
              ss << poly_path << "/" << std::setw(8) << std::setfill('0')
                 << idx;
              auto filename = ss.str();
              std::ifstream ifile(filename, std::ios::binary);
              if (!ifile.is_open()) {
                std::cout << "ERROR file doesn't exist: " << filename
                          << std::endl;
                return raster;
              }
              Archive<std::ifstream> a(ifile);
              a >> raster;
              ifile.close();
              return raster;
            });
            out.close();
            std::cout << "\n" << std::endl;
          }
        }
        y++;
      }
      x++;
    }
  }
};


//=======================================================================
// Copyright (c) 2014 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include "test_utils/sample_graph.hpp"

#include "paal/data_structures/facility_location/k_median_solution.hpp"
#include "paal/data_structures/voronoi/voronoi.hpp"
#include "paal/local_search/facility_location/facility_location_solution_adapter.hpp"

#include <boost/test/unit_test.hpp>

using namespace paal::data_structures;
using namespace paal::local_search;

BOOST_AUTO_TEST_CASE(k_median_solutionAdapterTest) {
    typedef sample_graphs_metrics SGM;
    auto gm = SGM::get_graph_metric_small();

    const int k = 2;
    typedef voronoi<decltype(gm)> voronoi;
    typedef paal::data_structures::voronoi_traits<voronoi> VT;
    typedef typename VT::GeneratorsSet GSet;
    typedef typename VT::VerticesSet VSet;
    voronoi vor(GSet{ SGM::A, SGM::B },
                VSet{ SGM::A, SGM::B, SGM::C, SGM::D, SGM::E }, gm);

    typedef paal::data_structures::k_median_solution<voronoi> Sol;

    Sol sol(std::move(vor), GSet{ SGM::C, SGM::D, SGM::E }, k);
    facility_location_solution_adapter<Sol> sa(sol);
    auto &realSol = sa.getfacility_location_solution();

    realSol.add_facility(SGM::C);
    realSol.rem_facility(SGM::A);
    realSol.add_facility(SGM::A);
    realSol.rem_facility(SGM::C);
    realSol.add_facility(SGM::D);
    realSol.rem_facility(SGM::A);
}


#include "nanocv/nanocv.h"
#include "nanocv/sampler.h"
#include "nanocv/tabulator.h"
#include "nanocv/measure.hpp"
#include "nanocv/thread/loopi.hpp"
#include "nanocv/tasks/task_synthetic_shapes.h"
#include <boost/program_options.hpp>

int main(int argc, char *argv[])
{
        ncv::init();

        using namespace ncv;

        // parse the command line
        boost::program_options::options_description po_desc("", 160);
        po_desc.add_options()("help,h", "benchmark sampling");
        po_desc.add_options()("samples,s",
                boost::program_options::value<size_t>()->default_value(8000),
                "number of samples to use [256, 100000]");

        boost::program_options::variables_map po_vm;
        boost::program_options::store(
                boost::program_options::command_line_parser(argc, argv).options(po_desc).run(),
                po_vm);
        boost::program_options::notify(po_vm);

        // check arguments and options
        if (	po_vm.empty() ||
                po_vm.count("help"))
        {
                std::cout << po_desc;
                return EXIT_FAILURE;
        }

        const size_t cmd_samples = math::clamp(po_vm["samples"].as<size_t>(), 256, 100 * 1000);

        const size_t cmd_rows = 28;
        const size_t cmd_cols = 28;
        const size_t cmd_outputs = 10;
        const color_mode cmd_color = color_mode::luma;

        const size_t cmd_min_samples = cmd_samples / 8;
        const size_t cmd_max_samples = cmd_min_samples * 8;

        const size_t cmd_min_nthreads = 1;
        const size_t cmd_max_nthreads = ncv::n_threads();

        // create synthetic task
        synthetic_shapes_task_t task(cmd_rows, cmd_cols, cmd_outputs, cmd_color, cmd_samples);
        task.load("");

        tensors_t inputs(cmd_max_samples);
        vectors_t targets(cmd_max_samples);

        // construct tables to compare sampling
        tabulator_t table("sampling\\threads");

        for (size_t nthreads = cmd_min_nthreads; nthreads <= cmd_max_nthreads; nthreads ++)
        {
                table.header() << (text::to_string(nthreads) + "xCPU [mu]");
        }

        // evaluate sampling
        for (size_t is = cmd_min_samples; is <= cmd_max_samples; is *= 2)
        {
                const string_t cmd_name = "sample size " + text::to_string(is);

                tabulator_t::row_t& row = table.append(cmd_name);

                log_info() << "<<< running test [" << cmd_name << "] ...";

                // select random samples
                sampler_t sampler(task);
                sampler.setup(sampler_t::stype::uniform, is);
                sampler.setup(sampler_t::atype::annotated);

                const samples_t samples = sampler.get();

                // process the samples
                for (size_t nthreads = cmd_min_nthreads; nthreads <= cmd_max_nthreads; nthreads ++)
                {
                        ncv::thread_pool_t pool(nthreads);

                        const auto micros = ncv::measure_robustly_usec([&]
                        {
                                ncv::thread_loopi(samples.size(), pool, [&] (size_t i)
                                {
                                        const sample_t& sample = samples[i];
                                        const image_t& image = task.image(sample.m_index);

                                        inputs[i] = image.to_tensor(sample.m_region);
                                        targets[i] = sample.m_target;
                                });
                        }, 1);

                        log_info() << "<<< processed [" << samples.size() << "] samples in " << micros << " us.";

                        row << micros;
                }

                log_info();
        }

        // print results
        table.print(std::cout);

        // OK
        log_info() << done;
        return EXIT_SUCCESS;
}


#include <boost/math/constants/constants.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <ompl/datastructures/BinaryHeap.h>
#include <ompl/tools/config/SelfConfig.h>

#include <ompl/datastructures/NearestNeighborsGNAT.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/geometric/planners/fmt/BFMT.h>

#include <fstream>
#include <ompl/base/spaces/RealVectorStateSpace.h>

namespace ompl
{
    namespace geometric
    {
        BFMT::BFMT(const base::SpaceInformationPtr &si)
          : base::Planner(si, "BFMT")
          , freeSpaceVolume_(si_->getStateSpace()->getMeasure())  // An upper bound on the free space volume is the
                                                                  // total space volume; the free fraction is estimated
                                                                  // in sampleFree
        {
            specs_.approximateSolutions = false;
            specs_.directed = false;

            ompl::base::Planner::declareParam<unsigned int>("num_samples", this, &BFMT::setNumSamples,
                                                            &BFMT::getNumSamples, "10:10:1000000");
            ompl::base::Planner::declareParam<double>("radius_multiplier", this, &BFMT::setRadiusMultiplier,
                                                      &BFMT::getRadiusMultiplier, "0.1:0.05:50.");
            ompl::base::Planner::declareParam<bool>("nearest_k", this, &BFMT::setNearestK, &BFMT::getNearestK, "0,1");
            ompl::base::Planner::declareParam<bool>("balanced", this, &BFMT::setExploration, &BFMT::getExploration,
                                                    "0,1");
            ompl::base::Planner::declareParam<bool>("optimality", this, &BFMT::setTermination, &BFMT::getTermination,
                                                    "0,1");
            ompl::base::Planner::declareParam<bool>("heuristics", this, &BFMT::setHeuristics, &BFMT::getHeuristics,
                                                    "0,1");
            ompl::base::Planner::declareParam<bool>("cache_cc", this, &BFMT::setCacheCC, &BFMT::getCacheCC, "0,1");
            ompl::base::Planner::declareParam<bool>("extended_fmt", this, &BFMT::setExtendedFMT, &BFMT::getExtendedFMT,
                                                    "0,1");
        }

        ompl::geometric::BFMT::~BFMT()
        {
            freeMemory();
        }

        void BFMT::setup()
        {
            if (pdef_)
            {
                /* Setup the optimization objective. If no optimization objective was
                specified, then default to optimizing path length as computed by the
                distance() function in the state space */
                if (pdef_->hasOptimizationObjective())
                    opt_ = pdef_->getOptimizationObjective();
                else
                {
                    OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.",
                                getName().c_str());
                    opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
                    // Store the new objective in the problem def'n
                    pdef_->setOptimizationObjective(opt_);
                }
                Open_[0].getComparisonOperator().opt_ = opt_.get();
                Open_[0].getComparisonOperator().heuristics_ = heuristics_;
                Open_[1].getComparisonOperator().opt_ = opt_.get();
                Open_[1].getComparisonOperator().heuristics_ = heuristics_;

                if (!nn_)
                    nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<BiDirMotion *>(this));
                nn_->setDistanceFunction([this](const BiDirMotion *a, const BiDirMotion *b)
                                         {
                                             return distanceFunction(a, b);
                                         });

                if (nearestK_ && !nn_->reportsSortedResults())
                {
                    OMPL_WARN("%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy "
                              "disabled.",
                              getName().c_str());
                    nearestK_ = false;
                }
            }
            else
            {
                OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
                setup_ = false;
            }
        }

        void BFMT::freeMemory()
        {
            if (nn_)
            {
                BiDirMotionPtrs motions;
                nn_->list(motions);
                for (auto &motion : motions)
                {
                    si_->freeState(motion->getState());
                    delete motion;
                }
            }
        }

        void BFMT::clear()
        {
            Planner::clear();
            sampler_.reset();
            freeMemory();
            if (nn_)
                nn_->clear();
            Open_[FWD].clear();
            Open_[REV].clear();
            Open_elements[FWD].clear();
            Open_elements[REV].clear();
            neighborhoods_.clear();
            collisionChecks_ = 0;
        }

        void BFMT::getPlannerData(base::PlannerData &data) const
        {
            base::Planner::getPlannerData(data);
            BiDirMotionPtrs motions;
            nn_->list(motions);

            int numStartNodes = 0;
            int numGoalNodes = 0;
            int numEdges = 0;
            int numFwdEdges = 0;
            int numRevEdges = 0;

            int fwd_tree_tag = 1;
            int rev_tree_tag = 2;

            for (auto motion : motions)
            {
                bool inFwdTree = (motion->currentSet_[FWD] != BiDirMotion::SET_UNVISITED);

                // For samples added to the fwd tree, add incoming edges (from fwd tree parent)
                if (inFwdTree)
                {
                    if (motion->parent_[FWD] == nullptr)
                    {
                        // Motion is a forward tree root node
                        ++numStartNodes;
                    }
                    else
                    {
                        bool success =
                            data.addEdge(base::PlannerDataVertex(motion->parent_[FWD]->getState(), fwd_tree_tag),
                                         base::PlannerDataVertex(motion->getState(), fwd_tree_tag));
                        if (success)
                        {
                            ++numFwdEdges;
                            ++numEdges;
                        }
                    }
                }
            }

            // The edges in the goal tree are reversed so that they are in the same direction as start tree
            for (auto motion : motions)
            {
                bool inRevTree = (motion->currentSet_[REV] != BiDirMotion::SET_UNVISITED);

                // For samples added to a tree, add incoming edges (from fwd tree parent)
                if (inRevTree)
                {
                    if (motion->parent_[REV] == nullptr)
                    {
                        // Motion is a reverse tree root node
                        ++numGoalNodes;
                    }
                    else
                    {
                        bool success =
                            data.addEdge(base::PlannerDataVertex(motion->getState(), rev_tree_tag),
                                         base::PlannerDataVertex(motion->parent_[REV]->getState(), rev_tree_tag));
                        if (success)
                        {
                            ++numRevEdges;
                            ++numEdges;
                        }
                    }
                }
            }
        }

        void BFMT::saveNeighborhood(BiDirMotion *m)
        {
            // Check if neighborhood has already been saved
            if (neighborhoods_.find(m) == neighborhoods_.end())
            {
                BiDirMotionPtrs neighborhood;
                if (nearestK_)
                    nn_->nearestK(m, NNk_, neighborhood);
                else
                    nn_->nearestR(m, NNr_, neighborhood);

                if (!neighborhood.empty())
                {
                    // Save the neighborhood but skip the first element (m)
                    neighborhoods_[m] = std::vector<BiDirMotion *>(neighborhood.size() - 1, nullptr);
                    std::copy(neighborhood.begin() + 1, neighborhood.end(), neighborhoods_[m].begin());
                }
                else
                {
                    // Save an empty neighborhood
                    neighborhoods_[m] = std::vector<BiDirMotion *>(0);
                }
            }
        }

        void BFMT::sampleFree(const std::shared_ptr<NearestNeighbors<BiDirMotion *>> &nn,
                              const base::PlannerTerminationCondition &ptc)
        {
            unsigned int nodeCount = 0;
            unsigned int sampleAttempts = 0;
            auto *motion = new BiDirMotion(si_, &tree_);

            // Sample numSamples_ number of nodes from the free configuration space
            while (nodeCount < numSamples_ && !ptc)
            {
                sampler_->sampleUniform(motion->getState());
                sampleAttempts++;
                if (si_->isValid(motion->getState()))
                {  // collision checking
                    ++nodeCount;
                    nn->add(motion);
                    motion = new BiDirMotion(si_, &tree_);
                }
            }
            si_->freeState(motion->getState());
            delete motion;

            // 95% confidence limit for an upper bound for the true free space volume
            freeSpaceVolume_ =
                boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) *
                si_->getStateSpace()->getMeasure();
        }

        double BFMT::calculateUnitBallVolume(const unsigned int dimension) const
        {
            if (dimension == 0)
                return 1.0;
            if (dimension == 1)
                return 2.0;
            return 2.0 * boost::math::constants::pi<double>() / dimension * calculateUnitBallVolume(dimension - 2);
        }

        double BFMT::calculateRadius(const unsigned int dimension, const unsigned int n) const
        {
            double a = 1.0 / (double)dimension;
            double unitBallVolume = calculateUnitBallVolume(dimension);

            return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) *
                   std::pow(log((double)n) / (double)n, a);
        }

        void BFMT::initializeProblem(base::GoalSampleableRegion *&goal_s)
        {
            checkValidity();
            if (!sampler_)
            {
                sampler_ = si_->allocStateSampler();
            }
            goal_s = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
        }

        base::PlannerStatus BFMT::solve(const base::PlannerTerminationCondition &ptc)
        {
            base::GoalSampleableRegion *goal_s;
            initializeProblem(goal_s);
            if (goal_s == nullptr)
            {
                OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
                return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
            }

            useFwdTree();

            // Add start states to Unvisitedfwd and Openfwd
            bool valid_initMotion = false;
            BiDirMotion *initMotion;
            while (const base::State *st = pis_.nextStart())
            {
                initMotion = new BiDirMotion(si_, &tree_);
                si_->copyState(initMotion->getState(), st);

                initMotion->currentSet_[REV] = BiDirMotion::SET_UNVISITED;
                nn_->add(initMotion);  // S <-- {x_init}
                if (si_->isValid(initMotion->getState()))
                {
                    // Take the first valid initial state as the forward tree root
                    Open_elements[FWD][initMotion] = Open_[FWD].insert(initMotion);
                    initMotion->currentSet_[FWD] = BiDirMotion::SET_OPEN;
                    initMotion->cost_[FWD] = opt_->initialCost(initMotion->getState());
                    valid_initMotion = true;
                    heurGoalState_[1] = initMotion->getState();
                }
            }

            if ((initMotion == nullptr) || !valid_initMotion)
            {
                OMPL_ERROR("Start state undefined or invalid.");
                return base::PlannerStatus::INVALID_START;
            }

            // Sample N free states in configuration state_
            sampleFree(nn_, ptc);  // S <-- SAMPLEFREE(N)
            OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
                        nn_->size());

            // Calculate the nearest neighbor search radius
            if (nearestK_)
            {
                NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
                                 (boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
                                 log((double)nn_->size()));
                OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
            }
            else
            {
                NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
                OMPL_DEBUG("Using radius of %f", NNr_);
            }

            // Add goal states to Unvisitedrev and Openrev
            bool valid_goalMotion = false;
            BiDirMotion *goalMotion;
            while (const base::State *st = pis_.nextGoal())
            {
                goalMotion = new BiDirMotion(si_, &tree_);
                si_->copyState(goalMotion->getState(), st);

                goalMotion->currentSet_[FWD] = BiDirMotion::SET_UNVISITED;
                nn_->add(goalMotion);  // S <-- {x_goal}
                if (si_->isValid(goalMotion->getState()))
                {
                    // Take the first valid goal state as the reverse tree root
                    Open_elements[REV][goalMotion] = Open_[REV].insert(goalMotion);
                    goalMotion->currentSet_[REV] = BiDirMotion::SET_OPEN;
                    goalMotion->cost_[REV] = opt_->terminalCost(goalMotion->getState());
                    valid_goalMotion = true;
                    heurGoalState_[0] = goalMotion->getState();
                }
            }

            if ((goalMotion == nullptr) || !valid_goalMotion)
            {
                OMPL_ERROR("Goal state undefined or invalid.");
                return base::PlannerStatus::INVALID_GOAL;
            }

            useRevTree();

            // Plan a path
            BiDirMotion *connection_point = nullptr;
            bool earlyFailure = true;

            if (initMotion != nullptr && goalMotion != nullptr)
            {
                earlyFailure = plan(initMotion, goalMotion, connection_point, ptc);
            }
            else
            {
                OMPL_ERROR("Initial/goal state(s) are undefined!");
            }

            if (earlyFailure)
            {
                return base::PlannerStatus(false, false);
            }

            // Save the best path (through z)
            if (!ptc)
            {
                base::Cost fwd_cost, rev_cost, connection_cost;

                // Construct the solution path
                useFwdTree();
                BiDirMotionPtrs path_fwd;
                tracePath(connection_point, path_fwd);
                fwd_cost = connection_point->getCost();

                useRevTree();
                BiDirMotionPtrs path_rev;
                tracePath(connection_point, path_rev);
                rev_cost = connection_point->getCost();

                // ASSUMES FROM THIS POINT THAT z = path_fwd[0] = path_rev[0]
                // Remove the first element, z, in the traced reverse path
                // (the same as the first element in the traced forward path)
                if (path_rev.size() > 1)
                {
                    connection_cost = base::Cost(rev_cost.value() - path_rev[1]->getCost().value());
                    path_rev.erase(path_rev.begin());
                }
                else if (path_fwd.size() > 1)
                {
                    connection_cost = base::Cost(fwd_cost.value() - path_fwd[1]->getCost().value());
                    path_fwd.erase(path_fwd.begin());
                }
                else
                {
                    OMPL_ERROR("Solution path traced incorrectly or otherwise constructed improperly \
                through forward/reverse trees (both paths are one node in length, each).");
                }

                // Adjust costs/parents in reverse tree nodes as cost/direction from forward tree root
                useFwdTree();
                path_rev[0]->setCost(base::Cost(path_fwd[0]->getCost().value() + connection_cost.value()));
                path_rev[0]->setParent(path_fwd[0]);
                for (unsigned int i = 1; i < path_rev.size(); ++i)
                {
                    path_rev[i]->setCost(
                        base::Cost(fwd_cost.value() + (rev_cost.value() - path_rev[i]->getCost().value())));
                    path_rev[i]->setParent(path_rev[i - 1]);
                }

                BiDirMotionPtrs mpath;
                std::reverse(path_rev.begin(), path_rev.end());
                mpath.reserve(path_fwd.size() + path_rev.size());  // preallocate memory
                mpath.insert(mpath.end(), path_rev.begin(), path_rev.end());
                mpath.insert(mpath.end(), path_fwd.begin(), path_fwd.end());

                // Set the solution path
                auto path(std::make_shared<PathGeometric>(si_));
                for (int i = mpath.size() - 1; i >= 0; --i)
                {
                    path->append(mpath[i]->getState());
                }

                static const bool approximate = false;
                static const double cost_difference_from_goal = 0.0;
                pdef_->addSolutionPath(path, approximate, cost_difference_from_goal, getName());

                OMPL_DEBUG("Total path cost: %f\n", fwd_cost.value() + rev_cost.value());
                return base::PlannerStatus(true, false);
            }

            // Planner terminated without accomplishing goal
            return {false, false};
        }

        void BFMT::expandTreeFromNode(BiDirMotion *&z, BiDirMotion *&connection_point)
        {
            // Define Opennew and set it to NULL
            BiDirMotionPtrs Open_new;

            // Define Znear as all unexplored nodes in the neighborhood around z
            BiDirMotionPtrs zNear;
            const BiDirMotionPtrs &zNeighborhood = neighborhoods_[z];

            for (auto i : zNeighborhood)
            {
                if (i->getCurrentSet() == BiDirMotion::SET_UNVISITED)
                {
                    zNear.push_back(i);
                }
            }

            // For each node x in Znear
            for (auto x : zNear)
            {
                if (!precomputeNN_)
                    saveNeighborhood(x);  // nearest neighbors

                // Define Xnear as all frontier nodes in the neighborhood around the unexplored node x
                BiDirMotionPtrs xNear;
                const BiDirMotionPtrs &xNeighborhood = neighborhoods_[x];
                for (auto j : xNeighborhood)
                {
                    if (j->getCurrentSet() == BiDirMotion::SET_OPEN)
                    {
                        xNear.push_back(j);
                    }
                }
                // Find the node in Xnear with minimum cost-to-come in the current tree
                BiDirMotion *xMin = nullptr;
                double cMin = std::numeric_limits<double>::infinity();
                for (auto &j : xNear)
                {
                    // check if node costs are smaller than minimum
                    double cNew = j->getCost().value() + distanceFunction(j, x);

                    if (cNew < cMin)
                    {
                        xMin = j;
                        cMin = cNew;
                    }
                }

                // xMin was found
                if (xMin != nullptr)
                {
                    bool collision_free = false;
                    if (cacheCC_)
                    {
                        if (!xMin->alreadyCC(x))
                        {
                            collision_free = si_->checkMotion(xMin->getState(), x->getState());
                            ++collisionChecks_;
                            // Due to FMT3* design, it is only necessary to save unsuccesful
                            // connection attemps because of collision
                            if (!collision_free)
                                xMin->addCC(x);
                        }
                    }
                    else
                    {
                        ++collisionChecks_;
                        collision_free = si_->checkMotion(xMin->getState(), x->getState());
                    }

                    if (collision_free)
                    {  // motion between yMin and x is obstacle free
                        // add edge from xMin to x
                        x->setParent(xMin);
                        x->setCost(base::Cost(cMin));
                        xMin->getChildren().push_back(x);

                        if (heuristics_)
                            x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), heurGoalState_[tree_]));

                        // check if new node x is in the other tree; if so, save result
                        if (x->getOtherSet() != BiDirMotion::SET_UNVISITED)
                        {
                            if (connection_point == nullptr)
                            {
                                connection_point = x;
                                if (termination_ == FEASIBILITY)
                                {
                                    break;
                                }
                            }
                            else
                            {
                                if ((connection_point->cost_[FWD].value() + connection_point->cost_[REV].value()) >
                                    (x->cost_[FWD].value() + x->cost_[REV].value()))
                                {
                                    connection_point = x;
                                }
                            }
                        }

                        Open_new.push_back(x);                      // add x to Open_new
                        x->setCurrentSet(BiDirMotion::SET_CLOSED);  // remove x from Unvisited
                    }
                }
            }  // End "for x in Znear"

            // Remove motion z from binary heap and map
            BiDirMotionBinHeap::Element *zElement = Open_elements[tree_][z];
            Open_[tree_].remove(zElement);
            Open_elements[tree_].erase(z);
            z->setCurrentSet(BiDirMotion::SET_CLOSED);

            // add nodes in Open_new to Open
            for (auto &i : Open_new)
            {
                if(Open_elements[tree_][i] == nullptr)
                {
                    Open_elements[tree_][i] = Open_[tree_].insert(i);
                    i->setCurrentSet(BiDirMotion::SET_OPEN);
                }
            }
        }

        bool BFMT::plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&connection_point,
                        const base::PlannerTerminationCondition &ptc)
        {
            // If pre-computation, find neighborhoods for all N sample nodes plus initial
            // and goal state(s).  Otherwise compute the neighborhoods of the initial and
            // goal states separately and compute the others as needed.
            BiDirMotionPtrs sampleNodes;
            nn_->list(sampleNodes);
            /// \todo This precomputation is useful only if the same planner is used many times.
            /// otherwise is probably a waste of time. Do a real precomputation before calling solve().
            if (precomputeNN_)
            {
                for (auto &sampleNode : sampleNodes)
                {
                    saveNeighborhood(sampleNode);  // nearest neighbors
                }
            }
            else
            {
                saveNeighborhood(x_init);  // nearest neighbors
                saveNeighborhood(x_goal);  // nearest neighbors
            }

            // Copy nodes in the sample set to Unvisitedfwd.  Overwrite the label of the initial
            // node with set Open for the forward tree, since it starts in set Openfwd.
            useFwdTree();
            for (auto &sampleNode : sampleNodes)
            {
                sampleNode->setCurrentSet(BiDirMotion::SET_UNVISITED);
            }
            x_init->setCurrentSet(BiDirMotion::SET_OPEN);

            // Copy nodes in the sample set to Unvisitedrev.  Overwrite the label of the goal
            // node with set Open for the reverse tree, since it starts in set Openrev.
            useRevTree();
            for (auto &sampleNode : sampleNodes)
            {
                sampleNode->setCurrentSet(BiDirMotion::SET_UNVISITED);
            }
            x_goal->setCurrentSet(BiDirMotion::SET_OPEN);

            // Expand the trees until reaching the termination condition
            bool earlyFailure = false;
            bool success = false;

            useFwdTree();
            BiDirMotion *z = x_init;

            while (!success)
            {
                expandTreeFromNode(z, connection_point);

                // Check if the algorithm should terminate.  Possibly redefines connection_point.
                if (termination(z, connection_point, ptc))
                    success = true;
                else
                {
                    if (Open_[tree_].empty())  // If this heap is empty...
                    {
                        if (!extendedFMT_)  // ... eFMT not enabled...
                        {
                            if (Open_[(tree_ + 1) % 2].empty())  // ... and this one, failure.
                            {
                                OMPL_INFORM("Both Open are empty before path was found --> no feasible path exists");
                                earlyFailure = true;
                                return earlyFailure;
                            }
                        }
                        else  // However, if eFMT is enabled, run it.
                            insertNewSampleInOpen(ptc);
                    }

                    // This function will be always reached with at least one state in one heap.
                    // However, if ptc terminates, we should skip this.
                    if (!ptc)
                        chooseTreeAndExpansionNode(z);
                    else
                        return true;
                }
            }
            earlyFailure = false;
            return earlyFailure;
        }

        void BFMT::insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
        {
            // Sample and connect samples to tree only if there is
            // a possibility to connect to unvisited nodes.
            std::vector<BiDirMotion *> nbh;
            std::vector<base::Cost> costs;
            std::vector<base::Cost> incCosts;
            std::vector<std::size_t> sortedCostIndices;

            // our functor for sorting nearest neighbors
            CostIndexCompare compareFn(costs, *opt_);

            auto *m = new BiDirMotion(si_, &tree_);
            while (!ptc && Open_[tree_].empty())  //&& oneSample)
            {
                // Get new sample and check whether it is valid.
                sampler_->sampleUniform(m->getState());
                if (!si_->isValid(m->getState()))
                    continue;

                // Get neighbours of the new sample.
                std::vector<BiDirMotion *> yNear;
                if (nearestK_)
                    nn_->nearestK(m, NNk_, nbh);
                else
                    nn_->nearestR(m, NNr_, nbh);

                yNear.reserve(nbh.size());
                for (auto &j : nbh)
                {
                    if (j->getCurrentSet() == BiDirMotion::SET_CLOSED)
                    {
                        if (nearestK_)
                        {
                            // Only include neighbors that are mutually k-nearest
                            // Relies on NN datastructure returning k-nearest in sorted order
                            const base::Cost connCost = opt_->motionCost(j->getState(), m->getState());
                            if(neighborhoods_[j].size() == 0){
                                printf("***** empty neighboorhood detected, skipping ****\n");
                                continue;
                            }
                            const base::Cost worstCost =
                                opt_->motionCost(neighborhoods_[j].back()->getState(), j->getState());

                            if (opt_->isCostBetterThan(worstCost, connCost))
                                continue;
                            yNear.push_back(j);
                        }
                        else
                            yNear.push_back(j);
                    }
                }

                // Sample again if the new sample does not connect to the tree.
                if (yNear.empty())
                    continue;

                // cache for distance computations
                //
                // Our cost caches only increase in size, so they're only
                // resized if they can't fit the current neighborhood
                if (costs.size() < yNear.size())
                {
                    costs.resize(yNear.size());
                    incCosts.resize(yNear.size());
                    sortedCostIndices.resize(yNear.size());
                }

                // Finding the nearest neighbor to connect to
                // By default, neighborhood states are sorted by cost, and collision checking
                // is performed in increasing order of cost
                //
                // calculate all costs and distances
                for (std::size_t i = 0; i < yNear.size(); ++i)
                {
                    incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
                    costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
                }

                // sort the nodes
                //
                // we're using index-value pairs so that we can get at
                // original, unsorted indices
                for (std::size_t i = 0; i < yNear.size(); ++i)
                    sortedCostIndices[i] = i;
                std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(), compareFn);

                // collision check until a valid motion is found
                for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
                     i != sortedCostIndices.begin() + yNear.size(); ++i)
                {
                    ++collisionChecks_;
                    if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
                    {
                        const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
                        m->setParent(yNear[*i]);
                        yNear[*i]->getChildren().push_back(m);
                        m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
                        m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), heurGoalState_[tree_]));
                        m->setCurrentSet(BiDirMotion::SET_OPEN);
                        Open_elements[tree_][m] = Open_[tree_].insert(m);

                        nn_->add(m);
                        saveNeighborhood(m);
                        updateNeighborhood(m, nbh);

                        break;
                    }
                }
            }  // While Open_[tree_] empty
        }

        bool BFMT::termination(BiDirMotion *&z, BiDirMotion *&connection_point,
                               const base::PlannerTerminationCondition &ptc)
        {
            bool terminate = false;
            switch (termination_)
            {
                case FEASIBILITY:
                    // Test if a connection point was found during tree expansion
                    return (connection_point != nullptr || ptc);
                    break;

                case OPTIMALITY:
                    // Test if z is in SET_CLOSED (interior) of other tree
                    if (ptc)
                        terminate = true;
                    else if (z->getOtherSet() == BiDirMotion::SET_CLOSED)
                        terminate = true;

                    break;
            };
            return terminate;
        }

        // Choose exploration tree and node z to expand
        void BFMT::chooseTreeAndExpansionNode(BiDirMotion *&z)
        {
            switch (exploration_)
            {
                case SWAP_EVERY_TIME:
                    if (Open_[(tree_ + 1) % 2].empty())
                        z = Open_[tree_].top()->data;  // Continue expanding the current tree (not empty by exit
                                                       // condition in plan())
                    else
                    {
                        z = Open_[(tree_ + 1) % 2].top()->data;  // Take top of opposite tree heap as new z
                        swapTrees();                             // Swap to the opposite tree
                    }
                    break;

                case CHOOSE_SMALLEST_Z:
                    BiDirMotion *z1, *z2;
                    if (Open_[(tree_ + 1) % 2].empty())
                        z = Open_[tree_].top()->data;  // Continue expanding the current tree (not empty by exit
                                                       // condition in plan())
                    else if (Open_[tree_].empty())
                    {
                        z = Open_[(tree_ + 1) % 2].top()->data;  // Take top of opposite tree heap as new z
                        swapTrees();                             // Swap to the opposite tree
                    }
                    else
                    {
                        z1 = Open_[tree_].top()->data;
                        z2 = Open_[(tree_ + 1) % 2].top()->data;

                        if (z1->getCost().value() < z2->getOtherCost().value())
                            z = z1;
                        else
                        {
                            z = z2;
                            swapTrees();
                        }
                    }
                    break;
            };
        }

        // Trace a path of nodes along a tree towards the root (forward or reverse)
        void BFMT::tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
        {
            BiDirMotion *solution = z;

            while (solution != nullptr)
            {
                path.push_back(solution);
                solution = solution->getParent();
            }
        }

        void BFMT::swapTrees()
        {
            tree_ = (TreeType)((((int)tree_) + 1) % 2);
        }

        void BFMT::updateNeighborhood(BiDirMotion *m, const std::vector<BiDirMotion *> nbh)
        {
            // Neighborhoods are only updated if the new motion is within bounds (k nearest or within r).
            for (auto i : nbh)
            {
                // If CLOSED, that neighborhood won't be used again.
                // Else, if neighhboorhod already exists, we have to insert the node in
                // the corresponding place of the neighborhood of the neighbor of m.
                if (i->getCurrentSet() == BiDirMotion::SET_CLOSED)
                    continue;

                auto it = neighborhoods_.find(i);
                if (it != neighborhoods_.end())
                {
                    if (it->second.empty())
                        continue;

                    const base::Cost connCost = opt_->motionCost(i->getState(), m->getState());
                    const base::Cost worstCost = opt_->motionCost(it->second.back()->getState(), i->getState());

                    if (opt_->isCostBetterThan(worstCost, connCost))
                        continue;

                    // insert the neighbor in the vector in the correct order
                    std::vector<BiDirMotion *> &nbhToUpdate = it->second;
                    for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
                    {
                        // If connection to the new state is better than the current neighbor tested, insert.
                        const base::Cost cost = opt_->motionCost(i->getState(), nbhToUpdate[j]->getState());
                        if (opt_->isCostBetterThan(connCost, cost))
                        {
                            nbhToUpdate.insert(nbhToUpdate.begin() + j, m);
                            break;
                        }
                    }
                }
            }
        }
    }  // End "geometric" namespace
}  // End "ompl" namespace


//
// boost/radix/static_ibitstream_lsb.hpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// 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_RADIX_STATICOBITSTREAMLSB_HPP
#define BOOST_RADIX_STATICOBITSTREAMLSB_HPP

#include <boost/radix/common.hpp>

#include <boost/radix/bitmask.hpp>
#include <boost/radix/detail/bits.hpp>

#include <boost/cstdint.hpp>

#ifdef BOOST_HAS_PRAGMA_ONCE
#  pragma once
#endif

namespace boost { namespace radix {

template <std::size_t Bits>
class static_obitstream_lsb {
 public:
  template <typename UnpackedSegment, typename OutputIterator>
  OutputIterator operator()(
      UnpackedSegment const& unpacked, OutputIterator out) const {
    using namespace bits;
    std::size_t const unpacked_size = to_unpacked_segment_size<Bits>::value;
    std::size_t const packed_size   = to_packed_segment_size<Bits>::value;

    BOOST_STATIC_ASSERT(packed_size <= sizeof(boost::uint64_t));

    boost::uint64_t packed = 0;
    for(std::size_t i = 0; i < unpacked_size; ++i) {
      packed |= boost::uint64_t(unpacked[i]) << (i * Bits);
    }

    for(std::size_t i = 0; i < packed_size; ++i) {
      *out++ = static_cast<bits_type>(packed & mask<8>::value);
      packed >>= 8;
    }

    return out;
  }
};

}} // namespace boost::radix

#endif // BOOST_RADIX_STATICOBITSTREAMLSB_HPP