#pragma once
//===----------------------------------------------------------------------===//
#include "binary_tree_structure.hpp"
#include "plain_bitmap.hpp"
#include "rank1.hpp"
#include "rank1_logic_surf.hpp"

#include <dtl/dtl.hpp>

#include <boost/dynamic_bitset.hpp>

#include <cstring>
#include <iomanip>
#include <immintrin.h>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// Represents a bitmap as a binary tree. During construction, the bitmap tree
/// is compressed. An instance can be seen as an intermediate representation of
/// a bitmap from which a TEB is constructed.
template<i32 optimization_level_ = 3, u1 fast_path = true>
class bitmap_tree : public binary_tree_structure {
  using tree_t = dtl::binary_tree_structure;
  using bitmap_t = boost::dynamic_bitset<$u32>;

public: // TODO make protected
  /// The labels of the tree nodes.
  plain_bitmap<$u64> labels_;
protected:

  /// Represents an integer range [begin, end).
  struct range_t {
    std::size_t begin = 0;
    std::size_t end = 0;
    /// Returns true if the range is empty, false otherwise.
    inline u1 is_empty() const { return begin >= end; };
    /// Returns true if the given values is within the range.
    inline u1 contains(std::size_t i) const { return (i >= begin) && (i < end); };
    /// Returns length of the range.
    inline std::size_t length() const { return end - begin; };
  };

  //===--------------------------------------------------------------------===//
  // The following counters are required to compute the size of the resulting
  // tree-encoded bitmap. These counters are updated when the tree is modified.
  //
  // During the construction phase, the size is required to find the tree
  // instance with the lowest memory consumption. Recall that the number of
  // tree nodes does not directly correspond to the TEB size. A fully pruned
  // tree instance may result in a larger TEB than a partially pruned tree
  // instance.
  //===--------------------------------------------------------------------===//
  /// The number of inner nodes. As the tree is a full binary, the number of
  /// leaf nodes is the number of inner nodes + 1.
  std::size_t inner_node_cnt_ = 0;
  /// The number inner nodes until the first leaf occurs (in level order).
  std::size_t leading_inner_node_cnt_ = 0;
  /// The number leaf nodes after the last inner node (in level order).
  std::size_t trailing_leaf_node_cnt_ = 0;
  /// The number of 0-labels until the first 1-label occurs.
  std::size_t leading_0label_cnt_ = 0;
  /// The number of 0-labels after the last 1-label.
  std::size_t trailing_0label_cnt_ = 0;

  //===--------------------------------------------------------------------===//
  // The ranges below are used to cache the node indexes of the first and last
  // explicit node and label. This is required to incrementally update the
  // counters from above. Unlike to the counters from above, these ranges are
  // not required to estimate the size of a tree-encoded bitmap.
  //===--------------------------------------------------------------------===//
  /// The node indexes that need to be stored explicitly.
  range_t explicit_node_idxs_;
  /// The indexes of nodes with explicit labels.
  //  range_t explicit_label_idxs_; // TODO use range rather than the two variables below
  /// The first leaf node index that carries a 1-label.
  std::size_t first_node_idx_with_1label_ = 0;
  /// The last leaf node index that carries a 1-label.
  std::size_t last_node_idx_with_1label_ = 0;

  //===--------------------------------------------------------------------===//
  // The following two members cache the positions of the first and the last
  // 1-bit within the original bitmap. This allows for faster initialization
  // of the leading/trailing 0-label counters.
  //===--------------------------------------------------------------------===//
  /// The index of the first set bit in the original bitmap. The value is set
  /// to 'n' when the bits in the original bitmap are all 0.
  std::size_t first_bit_idx_ = 0;
  /// The index of the last set bit in the original bitmap. The value is set
  /// to 'n' when the bits in the original bitmap are all 0.
  std::size_t last_bit_idx_ = 0;

  /// Used to keep track of whether the counters are up to date or invalidated.
  $u1 counters_are_valid = false;

  std::size_t uncompressed_size = 0;

  /// The actual size of the bitmap. Note, internally we round up to the
  /// next power of two.
  std::size_t n_actual_;

public:
  /// C'tor
  explicit bitmap_tree(const bitmap_t& bitmap)
      : binary_tree_structure(dtl::next_power_of_two(bitmap.size())),
        labels_(max_node_cnt_ + offset),
        inner_node_cnt_(0),
        leading_inner_node_cnt_(0),
        trailing_leaf_node_cnt_(0),
        leading_0label_cnt_(0),
        trailing_0label_cnt_(0),
        first_node_idx_with_1label_(0),
        last_node_idx_with_1label_(0),
        first_bit_idx_(0), // first and last bit idx will be initialized in init_tree()
        last_bit_idx_(0),
        n_actual_(bitmap.size()) {
    // Init the binary tree.
    init_tree(bitmap);
    uncompressed_size = estimate_encoded_size_in_bytes();

    if (!fast_path) {
      // Fused code path.
      compress();
    }
    else {
      // Classic code path.
      init_labels();
      prune_tree();

      // Run space optimizations.
      if (optimization_level_ > 1) {
        init_counters();
        run_optimize();
      }
    }
  }

protected:
  /// C'tor
  explicit bitmap_tree(const std::size_t n, u1 init = true)
      : binary_tree_structure(dtl::next_power_of_two(n), init),
        labels_(max_node_cnt_ + offset),
        inner_node_cnt_(0),
        leading_inner_node_cnt_(0),
        trailing_leaf_node_cnt_(0),
        leading_0label_cnt_(0),
        trailing_0label_cnt_(0),
        first_node_idx_with_1label_(0),
        last_node_idx_with_1label_(0),
        first_bit_idx_(0), // first and last bit idx will be initialized in init_tree()
        last_bit_idx_(0),
        n_actual_(n) {
  }

public:
  bitmap_tree(const bitmap_tree& other) = default;
  bitmap_tree(bitmap_tree&& other) noexcept = default;
  bitmap_tree& operator=(const bitmap_tree& other) = default;
  bitmap_tree& operator=(bitmap_tree&& other) noexcept = default;
  ~bitmap_tree() override = default;

  /// Initialize a perfect binary tree on top of the given bitmap, where
  ///  - all the inner nodes have two children
  ///  - all the leaf nodes are on the same level
  ///  - the leaf nodes are labelled with the given bitmap
  void __attribute__((noinline, hot, flatten))
  init_tree(const boost::dynamic_bitset<$u32>& bitmap) {
    u64 length = max_node_cnt_;
    u64 height = height_;
    // Copy the input bitmap to the last level of the tree.
    {
      auto i = bitmap.find_first();
      first_bit_idx_ = (i != boost::dynamic_bitset<$u32>::npos) ? i : n_;
      last_bit_idx_ = first_bit_idx_;
      while (i != boost::dynamic_bitset<$u32>::npos) {
        last_bit_idx_ = i;
        labels_.set(length / 2 + i + offset);
        i = bitmap.find_next(i);
      }
    }

    // Init the counters that are required to estimate the TEB size.
    init_counters_perfect_binary_tree();
  }

  /// Propagate the label bits along the tree (bottom-up).  The labels of an
  /// internal node is the bitwise OR of the labels of both child nodes.
  /// Pre-computing the labels of ALL tree nodes helps to avoid ad hoc
  /// computations of the labels during collapsing (or expanding) the tree
  /// structure.
  void __attribute__((noinline))
  init_labels() {
    for (auto level = last_level(); level > 0; --level) {
      const auto src_node_idx_begin = first_node_idx_at_level(level);
      const auto src_node_idx_end = first_node_idx_at_level(level + 1);
      const auto dst_node_idx_begin = first_node_idx_at_level(level - 1);
      const auto dst_node_idx_end = first_node_idx_at_level(level);
      auto src_node_idx = src_node_idx_begin;
      auto dst_node_idx = dst_node_idx_begin;
      while (src_node_idx < src_node_idx_end) {
        const auto remaining = src_node_idx_end - src_node_idx;
        assert(remaining >= 2);
        if (remaining >= 64) {
          // Process 64 nodes at a time.  For each loaded 64-bit word we write
          // a 32-bit word. For this to be efficient, we want proper alignment
          // which is why we have the +1 offset in the label and tree bitmaps.
          const auto src_label_idx = label_idx_of_node(src_node_idx);
          const auto dst_label_idx = label_idx_of_node(dst_node_idx);
          assert(src_label_idx % 64 == 0);
          assert(src_label_idx % 32 == 0);

          auto* raw_ptr = labels_.data();
          $u64* src_ptr = &raw_ptr[src_label_idx / 64];
          $u32* dst_ptr = &reinterpret_cast<$u32*>(raw_ptr)[dst_label_idx / 32];

          auto src = *src_ptr;
          src |= src >> 1;
          auto dst = _pext_u64(src, 0x5555555555555555); // FIXME non-portable code
          *dst_ptr = static_cast<$u32>(dst);

          src_node_idx += 64;
          dst_node_idx += 32;
        }
        else {
          // Process two nodes (siblings) at a time.
          u1 label_0 = label_of_node(src_node_idx);
          u1 label_1 = label_of_node(src_node_idx + 1);
          u1 parent_label = label_0 | label_1;
          const auto parent_label_idx = label_idx_of_node(dst_node_idx);
          labels_.set(parent_label_idx, parent_label);
          src_node_idx += 2;
          dst_node_idx += 1;
        }
      }
    }
  }

  /// Bottom-up pruning (loss-less).  Eliminate all sibling leaf nodes which
  /// have the same label. The algorithm terminates when all pairs of sibling
  /// leaf nodes have different labels.
  /// Note: The counters are thereby invalidated.
  void __attribute__((noinline))
  prune_tree() {
    D(init_counters();)
    for (auto level = last_level(); level > 0; --level) {
      $u64 collapse_cnt = 0;
      const auto src_node_idx_begin = first_node_idx_at_level(level);
      const auto src_node_idx_end = first_node_idx_at_level(level + 1);
      const auto dst_node_idx_begin = first_node_idx_at_level(level - 1);
      const auto dst_node_idx_end = first_node_idx_at_level(level);
      auto src_node_idx = src_node_idx_begin;
      auto dst_node_idx = dst_node_idx_begin;
      while (src_node_idx < src_node_idx_end) {
        const auto remaining = src_node_idx_end - src_node_idx;
        assert(remaining >= 2);
        if (remaining < 64) {
          u1 left_bit = labels_[src_node_idx + offset];
          u1 right_bit = labels_[src_node_idx + 1 + offset];
          u1 prune_causes_false_positives = left_bit ^ right_bit;
          u1 both_nodes_are_leaves =
              !is_inner_node(src_node_idx)
              & !is_inner_node(src_node_idx + 1);
          u1 prune = both_nodes_are_leaves & !prune_causes_false_positives;
          if (prune) {
            binary_tree_structure::set_leaf(dst_node_idx); // FIXME inefficient
            ++collapse_cnt;
          }
          src_node_idx += 2;
          dst_node_idx += 1;
        }
        else {
          // Process 64 nodes at a time.
          const auto src_idx = label_idx_of_node(src_node_idx);
          const auto dst_idx = label_idx_of_node(dst_node_idx);
          assert(src_idx % 64 == 0);
          assert(src_idx % 32 == 0);

          const auto src_word_idx = src_idx / 64;

          $u64* raw_label_ptr = labels_.data();
          u64 src_labels = raw_label_ptr[src_word_idx];
          u32 collapse_causes_false_positives = static_cast<$u32>(
              _pext_u64(src_labels ^ (src_labels >> 1), 0x5555555555555555)); // FIXME non-portable code

          $u64* raw_node_ptr = is_inner_node_.data();
          u64 src_nodes = raw_node_ptr[src_word_idx];
          u32 both_nodes_are_leaves = static_cast<$u32>(
              _pext_u64(~src_nodes & (~src_nodes >> 1), 0x5555555555555555)); // FIXME non-portable code

          u32 collapse = both_nodes_are_leaves & ~collapse_causes_false_positives;
          collapse_cnt += dtl::bits::pop_count(collapse);

          $u32* dst_nodes_ptr = &reinterpret_cast<$u32*>(raw_node_ptr)[dst_idx / 32];
          *dst_nodes_ptr = (*dst_nodes_ptr) ^ collapse;

          // Set the pruned nodes inactive.
          $u64* raw_active_node_ptr = is_active_node_.data();
          auto src_active_nodes = raw_active_node_ptr[src_word_idx];
          $u64 a = _pdep_u64(~collapse, 0x5555555555555555);
          a = a | (a << 1);
          raw_active_node_ptr[src_word_idx] = a;

          src_node_idx += 64;
          dst_node_idx += 32;
        }
      }
      D(init_counters();)

      {
        auto node_cnt_in_next_higher_level = 1ull << (level - 1);
        auto leaf_node_cnt_in_next_higher_level = collapse_cnt;
        auto inner_node_cnt_in_next_higher_level =
            node_cnt_in_next_higher_level - collapse_cnt;
        if (leaf_node_cnt_in_next_higher_level < inner_node_cnt_in_next_higher_level/2) {
          break;
        }
      }
    }
#ifndef NDEBUG
    // Validate active nodes.
    assert(is_active_node(0));
    for (std::size_t i = 1; i < max_node_cnt_; ++i) {
      if (is_inner_node(i)) {
        assert(is_active_node(i));
      }
      else {
        assert(is_active_node(i) == is_inner_node(parent_of(i)));
      }
    }
#endif
    // Invalidate the counters.
    counters_are_valid = false;
  }

  /// Determine the total number of tree nodes and which of these nodes need
  /// to be stored explicitly.
  void __attribute__((noinline))
  init_counters() {
    inner_node_cnt_ = 0;
    leading_inner_node_cnt_ = 0;
    trailing_leaf_node_cnt_ = 0;
    explicit_node_idxs_.begin = 0;
    explicit_node_idxs_.end = 0;

    leading_0label_cnt_ = 0;
    trailing_0label_cnt_ = 0;
    first_node_idx_with_1label_ = max_node_cnt_;
    last_node_idx_with_1label_ = max_node_cnt_;

    auto active_leaf_nodes = is_active_node_.and_not(is_inner_node_);

    {
      auto active_leaf_nodes_with_1_labels = active_leaf_nodes & labels_;
      first_node_idx_with_1label_ =
          active_leaf_nodes_with_1_labels.find_first() - offset;
      last_node_idx_with_1label_ =
          active_leaf_nodes_with_1_labels.find_last() - offset;

      auto active_leaf_nodes_with_0_labels = active_leaf_nodes.and_not(labels_);
      leading_0label_cnt_ =
          active_leaf_nodes_with_0_labels.count(offset, first_node_idx_with_1label_ + offset);
      trailing_0label_cnt_ =
          active_leaf_nodes_with_0_labels.count(last_node_idx_with_1label_ + offset, max_node_cnt_ + offset);
    }

    {
      auto active_inner_nodes = is_active_node_ & is_inner_node_;
      inner_node_cnt_ = active_inner_nodes.count(offset, max_node_cnt_ + offset);
      explicit_node_idxs_.end = (active_inner_nodes.find_last() - offset) + 1;
    }

    {
      explicit_node_idxs_.begin =
          active_leaf_nodes.find_first(offset, max_node_cnt_ + offset) - offset;
      trailing_leaf_node_cnt_ =
          active_leaf_nodes.count(explicit_node_idxs_.end + offset, max_node_cnt_ + offset);
    }
    leading_inner_node_cnt_ = explicit_node_idxs_.begin;
    counters_are_valid = true;
#ifndef NDEBUG
    validate_counters();
#endif
  }

  /// Determine the total number of tree nodes and which of these nodes need
  /// to be stored explicitly.
  void __attribute__((noinline))
  init_counters_naive() {
    $u1 found_leaf_node = false;
    std::size_t node_cnt = 0;
    inner_node_cnt_ = 0;
    leading_inner_node_cnt_ = 0;
    trailing_leaf_node_cnt_ = 0;
    explicit_node_idxs_.begin = 0;
    explicit_node_idxs_.end = 0;

    $u1 found_leaf_node_with_1label = false;
    leading_0label_cnt_ = 0;
    trailing_0label_cnt_ = 0;
    first_node_idx_with_1label_ = max_node_cnt_;
    last_node_idx_with_1label_ = max_node_cnt_;

    const auto it_end = breadth_first_end();
    for (auto it = breadth_first_begin(); it != it_end; ++it) {
      u64 idx = (*it);
      u64 level = level_of(idx);
      u1 is_inner = is_inner_node(idx);
      u1 is_leaf = !is_inner;

      ++node_cnt;
      inner_node_cnt_ += is_inner;

      // Count the leading inner nodes.
      if (!found_leaf_node && is_inner) {
        ++leading_inner_node_cnt_;
      }
      if (!found_leaf_node && !is_inner) {
        found_leaf_node = true;
      }

      // Count the trailing leaf nodes.
      if (!is_inner) {
        ++trailing_leaf_node_cnt_;
      }
      else {
        trailing_leaf_node_cnt_ = 0;
        explicit_node_idxs_.end = idx + 1;
      }

      if (!found_leaf_node_with_1label && is_leaf) {
        if (label_of_node(idx) == false) {
          ++leading_0label_cnt_;
        }
        else {
          found_leaf_node_with_1label = true;
          first_node_idx_with_1label_ = idx;
        }
      }

      // Count the trailing 0-labels nodes.
      if (is_leaf) {
        if (label_of_node(idx) == false) {
          ++trailing_0label_cnt_;
        }
        else {
          trailing_0label_cnt_ = 0;
          last_node_idx_with_1label_ = idx;
        }
      }
    }
    explicit_node_idxs_.begin = leading_inner_node_cnt_;
    if (explicit_node_idxs_.end < explicit_node_idxs_.begin) {
      explicit_node_idxs_.end = explicit_node_idxs_.begin;
    }

    if (!found_leaf_node_with_1label) {
      // Handle the special case where the entire bitmap is 0.
      trailing_0label_cnt_ = 0;
    }
    counters_are_valid = true;
  }

  /// Initialize the counters if necessary.
  void
  ensure_counters_are_valid() noexcept {
    if (!counters_are_valid) {
      init_counters();
    }
  }

  /// Initialize the counters. This simplified version only works with unpruned
  /// trees.
  void
  init_counters_perfect_binary_tree() {
    inner_node_cnt_ = n_ - 1;
    leading_inner_node_cnt_ = inner_node_cnt_;
    trailing_leaf_node_cnt_ = inner_node_cnt_ + 1;
    explicit_node_idxs_.begin = inner_node_cnt_;
    explicit_node_idxs_.end = inner_node_cnt_;

    first_node_idx_with_1label_ = first_bit_idx_ + n_ - 1;
    last_node_idx_with_1label_ = last_bit_idx_ + n_ - 1;
    leading_0label_cnt_ = first_bit_idx_;
    trailing_0label_cnt_ = (first_bit_idx_ != n_)
        ? n_ - last_bit_idx_ - 1
        : 0;
    counters_are_valid = true;
  }

  /// A naive implementation of the tree-based compression algorithm.
  /// Prerequisites: The tree needs to be initialized, see init_tree(), and the
  /// tree MUST NOT be pruned.
  void __attribute__((noinline))
  compress() {
    assert(explicit_node_idxs_.begin == n_ - 1);
    assert(explicit_node_idxs_.end == n_ - 1);

    if (last_level() == 0) {
      // Nothing to compress.
      return;
    }

    // TODO remove special case
    if (leading_0label_cnt_ == inner_node_cnt_ + 1
        && trailing_0label_cnt_ == inner_node_cnt_ + 1) {
      trailing_0label_cnt_ = 0;
    }

    // Identifies the tree instance with the minimum size.
    $u64 min_size_tree_idx = first_node_idx_at_level(last_level()) - 1;
    // The estimated TEB size of the smallest tree instance found so far.
    $u64 min_size = estimate_encoded_size_in_bytes();

    // The tree-based compression works bottom-up in reverse level-order.
    for (auto level = last_level() - 1; level > 0; --level) {
      // The node indexes at the current level.
      u64 node_idx_begin = first_node_idx_at_level(level);
      u64 node_idx_end = first_node_idx_at_level(level + 1);

      // Count the number of nodes that were collapsed within the current level.
      $u64 collapse_cnt = 0;

      for ($u64 idx = node_idx_end - 1; idx != node_idx_begin - 1; --idx) {
        assert(is_inner_node(idx));
        u64 left_child = left_child_of(idx);
        u64 right_child = right_child_of(idx);

        if (is_leaf_node(left_child) && is_leaf_node(right_child)) {
          u1 left_label = label_of_node(left_child);
          u1 right_label = label_of_node(right_child);
          if (left_label == right_label) {
            const auto label = left_label;
            //===----------------------------------------------------------===//
            // Prune the two child nodes, i.e. collapse the current node.
            binary_tree_structure::set_leaf(idx );
            labels_.set(idx + offset, label);
            ++collapse_cnt;
            //===----------------------------------------------------------===//
            // Update counters.
            --inner_node_cnt_;

            assert(idx < explicit_node_idxs_.begin);
            explicit_node_idxs_.begin = idx;
            leading_inner_node_cnt_ = idx;
            if (idx + 1 == explicit_node_idxs_.end) {
              // The current node was the last explicit inner node.  Due to the
              // fact that pruning happens in reverse level-order, all
              // preceding nodes are inner nodes. Thus we can simply decrement
              // end of the range.
              --explicit_node_idxs_.end;
              --trailing_leaf_node_cnt_; // -2 + 1
            }
            else if (left_child >= explicit_node_idxs_.end) {
              // The two pruned nodes were trailing implicit nodes.
              trailing_leaf_node_cnt_ -= 2;
            }

            if (label == false) {
              // 'Moving' a 0-label one level up.  The label of the current node
              // is the very first label (in level order).
              ++leading_0label_cnt_;
              if (right_child < first_node_idx_with_1label_) {
                leading_0label_cnt_ -= 2;
              }
              if (left_child > last_node_idx_with_1label_) {
                trailing_0label_cnt_ -= 2;
              }
            }
            else {
              // 'Moving' a 1-label one level up.  The label of the current node
              // is the very first label (in level order).
              leading_0label_cnt_ = 0;
              first_node_idx_with_1label_ = idx;
              if (right_child == last_node_idx_with_1label_) {
                // The last node with a 1-label has been pruned. Thus, the new
                // last node with a 1-label is in the range [idx, left_child).
                for ($u64 j = 0; j < (left_child - idx); ++j) {
                  u64 i = left_child - 1 - j;
                  if (is_leaf_node(i)) {
                    if (label_of_node(i) == true) {
                      last_node_idx_with_1label_ = i;
                      break;
                    }
                    else {
                      ++trailing_0label_cnt_;
                    }
                  }
                }
              }
            }
            //===----------------------------------------------------------===//
            // Compute the size and memorize the smallest instance.
            const auto estimated_size = estimate_encoded_size_in_bytes();
            if (estimated_size < min_size) {
              min_size_tree_idx = idx;
              min_size = estimated_size;
            }
            //===----------------------------------------------------------===//
          }
        }
      }

      if (collapse_cnt < 2) {
        // Terminate pruning. As there will be nothing to prune in the next
        // higher level. Recall, in requires at least two (sibling) nodes.
        break;
      }
      if (level < level_of(min_size_tree_idx)) {
        break;
      }
    }
#ifndef NDEBUG
    validate_counters();
#endif

    if (optimization_level_ <= 1) return;

    // Restore the tree instance with the minimum size.
    expand_until(min_size_tree_idx);

#ifndef NDEBUG
    validate_counters();
#endif
  }

  /// Expands the tree nodes which have an index less than the given node index.
  void __attribute__((noinline))
  expand_until(u64 node_idx) {
    const auto it_end = breadth_first_end();
    for (auto it = breadth_first_begin(); it != it_end; ++it) {
      u64 idx = (*it);
      if (is_inner_node(idx)) continue;
      if (right_child_of(idx) >= max_node_cnt_) break;

      if (idx >= node_idx) break;
      // Expand the leaf node to an inner node.
      // DANGER: The tree structure is modified during iterating.
      set_inner<false>(idx);
    }
  }

  // For debugging purposes.
  void __forceinline__
  validate_counters() {
#ifndef NDEBUG
    //===------------------------------------------------------------------===//
    // Validation
    //===------------------------------------------------------------------===//
    // Back up the counters.
    const std::size_t _inner_node_cnt = inner_node_cnt_;
    const std::size_t _leading_inner_node_cnt = leading_inner_node_cnt_;
    const std::size_t _trailing_leaf_node_cnt = trailing_leaf_node_cnt_;
    const std::size_t _leading_0label_cnt = leading_0label_cnt_;
    const std::size_t _trailing_0label_cnt = trailing_0label_cnt_;
    // Back up the ranges with implicit nodes and labels.
    const std::size_t _explicit_node_idxs_begin = explicit_node_idxs_.begin;
    const std::size_t _explicit_node_idxs_end = explicit_node_idxs_.end;
    const std::size_t _first_node_idx_with_1label = first_node_idx_with_1label_;
    const std::size_t _last_node_idx_with_1label = last_node_idx_with_1label_;

    init_counters_naive();
    // TODO remove special case
    if (leading_0label_cnt_ == inner_node_cnt_ + 1
        && trailing_0label_cnt_ == inner_node_cnt_ + 1) {
      trailing_0label_cnt_ = 0;
    }
    assert(_inner_node_cnt == inner_node_cnt_);
    assert(_leading_inner_node_cnt == leading_inner_node_cnt_);
    assert(_trailing_leaf_node_cnt == trailing_leaf_node_cnt_);
    assert(_explicit_node_idxs_begin == explicit_node_idxs_.begin);
    assert(_explicit_node_idxs_end == explicit_node_idxs_.end);
    assert(_leading_0label_cnt == leading_0label_cnt_);
    assert(_trailing_0label_cnt == trailing_0label_cnt_);
    assert(_first_node_idx_with_1label == first_node_idx_with_1label_);
    assert(_last_node_idx_with_1label == last_node_idx_with_1label_);
#endif
  }

  /// Estimates the size in bytes, when the bitmap tree is succinctly encoded.
  /// This function basically resembles the size_in_bytes() function of TEBs.
  std::size_t __forceinline__
  estimate_encoded_size_in_bytes() {
    u64 explicit_tree_node_cnt = inner_node_cnt_ + inner_node_cnt_ + 1
        - leading_inner_node_cnt_ - trailing_leaf_node_cnt_;

    u64 explicit_label_cnt = optimization_level_ > 2
        ? inner_node_cnt_ + 1 - leading_0label_cnt_ - trailing_0label_cnt_
        : inner_node_cnt_ + 1;
    assert(leading_0label_cnt_ <= inner_node_cnt_ + 1);
    assert(trailing_0label_cnt_ <= inner_node_cnt_ + 1);

    u64 perfect_level_cnt = dtl::log_2(leading_inner_node_cnt_ + 1) + 1;

    return estimate_encoded_size_in_bytes_v2(
        explicit_tree_node_cnt, explicit_label_cnt, perfect_level_cnt);
  }

  std::size_t __forceinline__
  estimate_encoded_size_in_bytes_v2(
      std::size_t explicit_tree_node_cnt,
      std::size_t explicit_label_cnt,
      std::size_t perfect_level_cnt) {
    constexpr u64 block_bitlength = 64; // TODO remove magic number - refer to teb storage typse
    constexpr u64 block_size = block_bitlength / 8;
    $u64 bytes = 0;

    // Bit-length of the original bitmap.
    bytes += sizeof(n_);

    // The stored length of the tree structure.
    bytes += 4;
    // The number of implicit inner nodes.
    bytes += optimization_level_ > 0 ? 4 : 0;
    // The number of implicit leaf nodes can then be computed as
    //  2n-1 - # implicit nodes - length of the tree structure bit sequence
    // The offset to the beginning of T can also be computed.
    // The height of the encoded tree (after pruning).
    bytes += 1; // actually 5 bits

    // The stored length of L.
    bytes += 4;
    // The number of implicit labels.
    bytes += optimization_level_ > 2 ? 4 : 0;
    // The offset to the beginning of L can also be computed based on the
    // size of the header, T and R.

    // Level offsets for T and L, which are required by the tree scan algorithm.
    const auto encoded_tree_height = dtl::log_2(n_) + 1; // FIXME could be lower, but its unlikely
    assert(encoded_tree_height >= perfect_level_cnt);

    if (explicit_tree_node_cnt > 1024) {
      bytes += (4 + 4) * (encoded_tree_height - perfect_level_cnt);
    }

    // Padding. We want T to be 8-byte aligned.
    bytes += 8 - (bytes % 8);

    // Tree structure
    if (explicit_tree_node_cnt > 1) {
      bytes += ((explicit_tree_node_cnt + block_bitlength - 1) / block_bitlength)
          * block_size;
    }

    // Rank helper structure
    if (explicit_tree_node_cnt > 1024) {
      bytes += dtl::rank1_logic_surf<u64>::estimate_size_in_bytes(
          explicit_tree_node_cnt);
    }

    // Labels
    bytes += ((explicit_label_cnt + block_bitlength - 1) / block_bitlength)
        * block_size;

    return bytes;
  }

  /// Returns the maximum number of tree nodes. Which is 2n-1 for perfect
  /// full binary trees.
  inline std::size_t
  max_node_cnt() const {
    return max_node_cnt_;
  }

  /// Returns the label index of the given node.
  inline auto
  label_idx_of_node(u64 node_idx) const {
    return node_idx + offset;
  }

  /// Returns the label of the given node.
  inline u1
  label_of_node(u64 node_idx) const {
    return labels_[node_idx + offset];
  }

  /// Returns the number of nodes in the tree.
  inline u32
  get_node_cnt() const noexcept {
    return get_inner_node_cnt() + get_leaf_node_cnt();
  }

  /// Returns the number of inner nodes in the tree.
  inline u32
  get_inner_node_cnt() const noexcept {
    return inner_node_cnt_;
  }

  /// Returns the number of leaf nodes in the tree.
  inline u32
  get_leaf_node_cnt() const noexcept {
    return inner_node_cnt_ + 1;
  }

  /// Returns the number of perfect levels in the tree.
  inline u32
  get_perfect_level_cnt() {
    return dtl::log_2(leading_inner_node_cnt_ + 1) + 1;
  }

  /// Returns the height of the encoded tree.
  inline u32
  get_encoded_tree_height() {
    return dtl::log_2(n_) + 1; // FIXME could be lower, but its unlikely
  }

  /// Returns the number of leading inner nodes (in level order).
  inline u32
  get_leading_inner_node_cnt() const noexcept {
    assert(explicit_node_idxs_.begin == leading_inner_node_cnt_);
    return leading_inner_node_cnt_;
  }

  /// Returns the number of trailing leaf nodes (in level order).
  inline u32
  get_trailing_leaf_node_cnt() const noexcept {
    return trailing_leaf_node_cnt_;
  }

  /// Returns the node index of the first non-implicit node. - Note that the
  /// node index refers to the index within a perfect binary tree.
  inline u32
  get_first_explicit_node_idx() const noexcept {
    return explicit_node_idxs_.begin;
  }

  /// Returns the node index of the last non-implicit node. - Note that the
  /// node index refers to the index within a perfect binary tree.
  inline u32
  get_last_explicit_node_idx() const noexcept {
    return explicit_node_idxs_.end;
  }

  /// Returns the number of leading 0-labels (in level order).
  inline u32
  get_leading_0label_cnt() const noexcept {
    return leading_0label_cnt_;
  }

  /// Returns the number of trailing leaf nodes (in level order).
  inline u32
  get_trailing_0label_cnt() const noexcept {
    return trailing_0label_cnt_;
  }

  /// Returns the index of the first leaf node that carries a 1-label.
  inline u32
  get_first_node_idx_with_1label() const noexcept {
    return first_node_idx_with_1label_;
  }

  /// Returns the index of the last leaf node that carries a 1-label.
  inline u32
  get_last_node_idx_with_1label() const noexcept {
    return last_node_idx_with_1label_;
  }

  /// Returns the actual size of the bitmap.
  inline u32
  get_n() const noexcept {
    return n_actual_;
  }

  void
  print(std::ostream& os) const noexcept {
    std::stringstream l_os;
    for (std::size_t level = 0; level <= height_; ++level) {
      std::cout << std::setw(4) << level << ": ";
      u64 node_idx_from = (1ull << level) - 1;
      u64 node_idx_to = (1ull << (level + 1)) - 1;
      for ($u64 node_idx = node_idx_from; node_idx < node_idx_to; ++node_idx) {
        const auto spaces = (level < height_)
            ? (1u << (height_ - level)) - 1
            : 0;
        if (node_idx == root()) {
          std::cout << (is_leaf_node(node_idx) ? "0" : "1");
          if (is_leaf_node(node_idx)) {
            l_os << (label_of_node(node_idx) ? "1" : "0");
          }
        }
        else {
          std::cout << (is_leaf_node(node_idx)
                  ? is_leaf_node(parent_of(node_idx)) ? " " : "0"
                  : "1");
          if (is_leaf_node(node_idx) && !is_leaf_node(parent_of(node_idx))) {
            l_os << (label_of_node(node_idx) ? "1" : "0");
          }
        }
        for (std::size_t i = 0; i < spaces; ++i) {
          std::cout << " ";
        }
      }
      std::cout << std::endl;
    }
    std::cout
        << std::setw(4) << "L" << ": "
        << l_os.str() << std::endl;
  }

private:
  /// Expands the VERY FIRST explicit node. // TODO generalize to expand arbitrary nodes
  template<u1 fast>
  void
  set_inner(u64 idx) {
    assert(is_leaf_node(idx));
    if (fast) {
      // This invalidates the active node bitmap.
      binary_tree_structure::set_inner_fast(idx);
    }
    else {
      binary_tree_structure::set_inner(idx);
    }

    const auto left_child_idx = left_child_of(idx);
    const auto right_child_idx = right_child_of(idx);

    // Update the counters.
    ++inner_node_cnt_;
    // The number of leading inner nodes increases at least by one.
    ++leading_inner_node_cnt_;
    // Check if the newly created inner node is followed by more inner nodes.
    for (auto i = idx + 1; i < max_node_cnt_ && is_inner_node(i); ++i) { // TODO maybe implement a find_next_zero()
      ++leading_inner_node_cnt_;
    }
    // Update the index of the first explicit node.
    explicit_node_idxs_.begin = leading_inner_node_cnt_;
    // Update the number of trailing leaf nodes.
    if (unlikely(explicit_node_idxs_.begin >= explicit_node_idxs_.end)) {
      // The entire tree became implicit.
      leading_inner_node_cnt_ = inner_node_cnt_;
      trailing_leaf_node_cnt_ = inner_node_cnt_ + 1;
      explicit_node_idxs_.end = explicit_node_idxs_.begin;
    }
    else {
      if (left_child_idx >= explicit_node_idxs_.end) {
        // The newly created leaf is implicit.
        ++trailing_leaf_node_cnt_;
      }
      if (right_child_idx >= explicit_node_idxs_.end) {
        // The newly created leaf is implicit.
        ++trailing_leaf_node_cnt_;
      }
    }

    // Update the number of implicit 0-labels.
    if (idx < first_node_idx_with_1label_) {
      // The current node, that has been expanded had an implicit 0-label.
      --leading_0label_cnt_;

      if (left_child_idx < first_node_idx_with_1label_) {
        // The label of the newly created leaf is implicit.
        ++leading_0label_cnt_;
      }
      if (left_child_idx > last_node_idx_with_1label_) {
        // The label of the newly created leaf is implicit.
        ++trailing_0label_cnt_;
      }
      if (left_child_idx < first_node_idx_with_1label_) {
        // The label of the newly created leaf is implicit.
        ++leading_0label_cnt_;
      }
      if (right_child_idx > last_node_idx_with_1label_) {
        // The label of the newly created leaf is implicit.
        ++trailing_0label_cnt_;
      }
    }
    else if (idx == first_node_idx_with_1label_) {
      assert(label_of_node(idx) == true);
      // The current node is the first node with a 1-label.
      // Now that this node has been expanded, the node idx with the first
      // 1-label is somewhere in (idx, left-child(idx)].
      for (std::size_t i = idx + 1; i <= left_child_idx; ++i) {
        if (is_leaf_node(i)) {
          if (label_of_node(i) == true) {
            first_node_idx_with_1label_ = i;
            break;
          }
          else {
            ++leading_0label_cnt_;
            if (i > last_node_idx_with_1label_) {
              --trailing_0label_cnt_;
            }
          }
        }
      }
      assert(first_node_idx_with_1label_ != idx);

      // The node idx with the last 1-label is now the
      // max(last_node_idx_with_1label_, right-child(idx)).
      last_node_idx_with_1label_ =
          std::max(last_node_idx_with_1label_, right_child_idx);
      if (last_node_idx_with_1label_ == right_child_idx) {
        trailing_0label_cnt_ = 0;
      }
    }
    else {
      // The current node, that has been expanded had an implicit 0-label.
      // The node idx is greater than the last node idx with a 1-label,
      // therefore, the child also have larger indexes. Thus, only the trailing
      // 0-label cnt needs to be incremented.
      ++trailing_0label_cnt_;
    }
  }

  /// Restore the initial tree state.
  void __attribute__((noinline))
  uncompress() {
    is_inner_node_.set(0, first_node_idx_at_level(last_level()) + offset);
    is_active_node_.set(0, first_node_idx_at_level(last_level() + 1) + offset);
    init_counters_perfect_binary_tree();
#ifndef NDEBUG
    validate_counters();
#endif
  }

  void __attribute__((noinline))
  run_optimize() {
    // Optimization level 2.
    if (optimization_level_ > 1) {
      // Gradual decompression.
      // Find the tree instance with the minimum size.
      auto min_idx = explicit_node_idxs_.begin;
      auto min_level = level_of(min_idx);
      auto min_size = estimate_encoded_size_in_bytes();
      const std::size_t threshold = std::max(1024ul, uncompressed_size / 10);
      auto cpy = *this; // Work with a copy.

      {
        u64 start_idx = cpy.explicit_node_idxs_.begin;
        $u64 idx = cpy.explicit_node_idxs_.begin;
        $u64 prev_idx = 0;
        $u64 idx_level = cpy.level_of(idx);
        u64 last_pruning_level = last_level();
        $u64 old_level = idx_level;
        auto prev_level_size = cpy.estimate_encoded_size_in_bytes();
        auto prev_level_min_size = min_size;
        while (idx_level < last_pruning_level
            && !(min_level + 2 < idx_level)) {
          assert(cpy.is_leaf_node(idx));

          if (idx_level > old_level) {
            {
              auto now_size = cpy.estimate_encoded_size_in_bytes();
              if (now_size > prev_level_size && old_level != min_level) {
                goto expand_done;
              }
              else {
                prev_level_size = now_size;
              }
            }
            old_level = idx_level;
            if (idx_level < cpy.last_level()) {
              auto last_explicit_node = cpy.explicit_node_idxs_.end - 1;
              auto level_of_last_explicit_node = cpy.level_of(last_explicit_node);
              if (level_of_last_explicit_node == idx_level + 1) {
                auto parent_of_last_explicit_node =
                    cpy.parent_of(level_of_last_explicit_node);
                auto x_b = cpy.first_node_idx_at_level(idx_level);
                auto x_e = parent_of_last_explicit_node;
                auto y_b = parent_of_last_explicit_node + 1;
                auto y_e = cpy.first_node_idx_at_level(idx_level + 1);
                i64 x_ones = cpy.is_inner_node_.count(x_b + offset, x_e + offset);
                i64 x_zeros = (x_e - x_b) - x_ones;
                i64 y_ones = cpy.is_inner_node_.count(y_b + offset, y_e + offset);
                i64 y_zeros = (y_e - y_b) - y_ones;
                auto r = -(x_zeros + y_zeros) + 2 * x_zeros - (x_ones + y_ones);
                if (r > 0) {
                  goto expand_done;
                }
              }
              else {
                auto b = cpy.first_node_idx_at_level(idx_level);
                auto e = cpy.first_node_idx_at_level(idx_level + 1);
                auto ones = cpy.is_inner_node_.count(
                    b + offset,
                    e + offset);
                auto zeros = (e - b) - ones;
                if (zeros/2 > ones) {
                  goto expand_done;
                }
              }
            }
          }

          cpy.template set_inner<true>(idx);
          const auto compressed_size = cpy.estimate_encoded_size_in_bytes();
          // Estimates the size of a TEB.
          if (compressed_size <= min_size) { // less than or EQUAL because we prefer more balanced trees.
            min_idx = idx;
            min_level = cpy.level_of(idx);
            min_size = compressed_size;
          }
          if (compressed_size > min_size + threshold) {
            goto expand_done;
          }
          // Note:
          // If explicit_node_idxs_.begin overtakes parent_of(first_node_idx_with_1label_)
          // L starts to increase in size. And the next time where L could
          // decrease in size is when expanding first_node_idx_with_1label_.
          // All leaf nodes in [parent_of(first_node_idx_with_1label_) + 1, first_node_idx_with_1label_)
          // carry a 0-label. Thus, the L sequence starts with a 1 and until
          // first_node_idx_with_1label_ is reached, 0's are inserted after the
          // leading 1 in L.
          // Within that range, T can increase and decrease in size.
          //  with every expand, 1-bit is added to T, however there may be
          //  following inner nodes which then become implicit.
          if (cpy.explicit_node_idxs_.begin > cpy.explicit_node_idxs_.end) {
            goto tree_implicit;
          }

          // Next
          prev_idx = idx;
          idx = cpy.explicit_node_idxs_.begin;
          idx_level = cpy.level_of(idx);
        }

        // Check if decompression stopped at the minimum sized tree.
        if (min_idx == prev_idx) {
          // Take the working copy as is.
          *this = cpy;
          // Fix active node map.
          is_active_node_.set(start_idx + offset,
              std::min(max_node_cnt_, right_child_of(idx) + 1) + offset);
          D(validate_active_nodes();)
          return;
        }

        goto expand_done;

tree_implicit:
        // Take the working copy as is.
        *this = cpy;
        // Fix active node map.
        is_active_node_.set(start_idx + offset,
            std::min(max_node_cnt_, right_child_of(idx) + 1) + offset);
        D(validate_active_nodes();)
        return;

expand_done:

        // Check whether the bitmap can be compressed.
        if (min_size > uncompressed_size) {
          // Restore the initial tree state.
          uncompress();
          return;
        }
      }

      // Restore the tree instance with the minimum size. Expand the tree down
      // to the node 'min_idx' which identifies the smallest tree instance found
      // in the previous step.
      {
        const auto start_idx = explicit_node_idxs_.begin;
        const std::size_t expand_until_idx =
            std::min(first_node_idx_at_level(height_) - 1, min_idx);
        while (explicit_node_idxs_.begin <= expand_until_idx) {
          assert(is_leaf_node(explicit_node_idxs_.begin));
          set_inner<true>(explicit_node_idxs_.begin);
        }
        // Update the active node map.
        is_active_node_.set(start_idx + offset,
            std::min(max_node_cnt_, right_child_of(expand_until_idx) + 1) + offset);
        D(validate_active_nodes();)
      }
    }
  }
};
//===----------------------------------------------------------------------===//
} // namespace dtl


#include "FastqIterator.hpp"
#include "DagAligner.hpp"
#include "SvgPlot.hpp"
#include "Kernel.hpp"

using overlap_analysis::FastqElement;
using overlap_analysis::FastqIterator;

#include <algorithm>
#include <cmath>
#include <chrono>
#include <string>
#include <vector>
#include <iostream>

using std::max_element;
using std::string;
using std::pow;
using std::vector;
using std::cerr;

#include "boost/program_options.hpp"
#include "boost/bimap.hpp"
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>

using boost::program_options::options_description;
using boost::program_options::variables_map;
using boost::program_options::bool_switch;
using boost::program_options::value;
using boost::geometry::model::point;
using boost::geometry::model::box;
using boost::geometry::cs::cartesian;
using boost::geometry::index::rtree;
using boost::geometry::index::quadratic;
using boost::geometry::index::intersects;

#include "edlib.h"
#include "mummer/sparseSA.hpp"

using mummer::mummer::sparseSA;
using mummer::mummer::match_t;

void plot_mummer_matches(const vector<match_t>& matches, SvgPlot& plot, size_t line_width){
    string type = "rect";
    string color = "#8115A3";

    for (const auto& m: matches){
        for (size_t i=0; i<size_t(m.len); i+=50) {
            plot.add_point(
                    m.ref + i,
                    m.query + i,
                    type,
                    line_width,
                    color);
        }
    }
}

void align(const FastqElement& s, const FastqElement& s2, path output_directory){
    const string& ref = s.sequence;
    const string& query = s2.sequence;

    // Construct suffix array
    auto matcher = sparseSA::create_auto(ref.c_str(), ref.size(), 0, true);
    vector <pair <coord_t, size_t> > matches;

    // Search suffix array for exact matches
    matcher.findMEM_each(query, 10, false, [&](const match_t& m){
        matches.emplace_back(make_pair(make_pair(m.ref, m.query), m.len));
    });

    Dag dag(matches, ref.size(), query.size(), 100);

    // Set up plot
//    path plot_path = output_directory / (s.name + "_vs_" + s2.name + ".svg");
//    dag.write_to_svg(plot_path);

    dag.compute_alignment();

//    path plot_path2 = output_directory / (s.name + "_vs_" + s2.name + "_aligned.svg");
//    dag.write_to_svg(plot_path2);

    cerr << s.name << ' ' << s2.name << ' ' << matches.size() << '\n';

}

size_t align_all_vs_all(vector <FastqElement>& sequences){
    size_t n_comparisons = 0;

    path output_directory = "plots/";

    if (not exists(output_directory)){
        create_directories(absolute(output_directory));
    }

    for (const auto& s: sequences) {
        for (const auto& s2: sequences) {
            if (s.name == s2.name) {
                continue;
            }

            align(s, s2, output_directory);

            n_comparisons++;
        }
    }

    return n_comparisons;
}

void test(path fastq_path){
    // Pre-load all the sequences into memory using a vector
    vector <FastqElement> sequences;
    FastqElement element;

    FastqIterator fastq_iterator(fastq_path);

    cerr << "Loading sequences..." << '\n';

    while (fastq_iterator.next_element(element)){
        sequences.emplace_back(element);
        cerr << "Read '" << element.name << "' = " << element.sequence.size() << " bp " <<'\n';
    }

    size_t n_trials = 10;
    size_t total_comparisons = 0;

    auto t1 = std::chrono::high_resolution_clock::now();

    for (size_t i=0; i<n_trials; i++) {
        total_comparisons += align_all_vs_all(sequences);
    }

    auto t2 = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
    cerr << "Total alignments: " << total_comparisons << '\n';
    cerr << "Alignment completed in: " << elapsed.count() << " milliseconds" << '\n';

}

int main(int argc, char* argv[]){
    path fastq_path;
    path output_directory;

    options_description options("Arguments:");

    options.add_options()
            ("fastq,i",
             value<path>(&fastq_path)
                     ->required(),
             "File path of PAF file containing alignments to some reference")
            ;

    variables_map vm;
    store(parse_command_line(argc, argv, options), vm);

    // If help was specified, or no arguments given, provide help
    if (vm.count("help") || argc == 1) {
        cerr << options << "\n";
        return 0;
    }
    notify(vm);

    test(fastq_path);

    return 0;
}


//
// $Id: tabapprox.cpp 1195 2009-08-14 22:12:04Z chambm $
//
//
// Robert Burke <robert.burke@cshs.org>
//
// Copyright 2009 Spielberg Family Center for Applied Proteomics
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// 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.
//

#include <boost/program_options.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>
#include <string>

#include "FitFromPepXML.hpp"
#include "util/auto_vector.h"
#include "pepxml/Peptide.hpp"

#include "utils.hpp"

namespace ublas = boost::numeric::ublas;

using namespace std;
using namespace pwiz::pepxml;
using namespace pwiz::rtapprox;

int hasJunk = 0;
int hasProb = 1;

// Hack of a program used to read in a tab delimited file of peptides,
// retention times, and - perhaps - confidence values (aka p values).
int main(int argc, char** argv)
{
    *argv++;
    argc--;

    // Read in the input file
    if (argc < 1)
    {
        cout << "test <input file>" << endl;
        return 1;
    }

    // Create a list of Peptide objects from the input tab file.
    vector<Peptide> peptides;

    ifstream in(*argv);

    string line;
    while(getline(in, line))
    {
        istringstream ss(line);
        string junk;
        string peptide;
        string rt;
        string prob;

        if (hasJunk)
            ss >> junk;
        ss >> peptide;
        ss >> rt;
        if (hasProb)
            ss >> prob;

        Peptide p;
        p.setPeptide(peptide);
        p.setRetentionTime(rt);
        if (hasProb)
            p.setPValue(prob);
        peptides.push_back(p);
    }

    // Create dummy fit
    auto_ptr<FitFromPepXML> fit = FitFromPepXML::create();

    // Fit the read in peptides to some system.
    fit->createSystem(peptides);

    ublas::vector<double> result = fit->fit();

    for (int i=0; i<FitFromPepXML::NUMENTRIES; i++)
    {
        cout << FitFromPepXML::AMINOACIDS[i] << "\t";
    }

    cout << "\nresult size: " << result.size() << "\n";
    for (ublas::vector<double>::iterator i=result.begin(); i!=result.end(); i++)
    {
        cout << *i << "\t";
    }

    cout << "\npeptide\testimated\terror\n";

    // Check results
    double rms = 0;
    for (vector<Peptide>::iterator i=peptides.begin();i!=peptides.end(); i++)
    {
        double actualRt = atof((*i).getRetentionTime());
        ublas::vector<double> v = fit->countPeptide((*i).getPeptide());

        // This can be replaced with boost's inner_prod, but keeping
        // it manual for now.
        double rt = 0;
        for (ublas::vector<double>::size_type j=0; j<v.size()-1; j++)
            rt += v(j) * result(j);

        double error = (actualRt - rt);
        rms += error * error;
        cout << (*i).getPeptide() << "\t" << rt << "\t" << error << "\n";
    }

    rms = sqrt(rms / peptides.size() );
    cout << "\n\nrms = " << rms <<endl;

    return 0;
}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2018.
// Modifications copyright (c) 2018 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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_EXTENSIONS_ALGEBRA_CORE_COORDINATE_DIMENSION_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_COORDINATE_DIMENSION_HPP

#include <boost/geometry/core/coordinate_dimension.hpp>

#include <boost/geometry/extensions/algebra/core/tags.hpp>

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

#include <boost/mpl/assert.hpp>

namespace boost { namespace geometry {

namespace traits {

template <typename Geometry, std::size_t Index>
struct indexed_dimension
{
     BOOST_MPL_ASSERT_MSG(false,
                          NOT_IMPLEMENTED_FOR_THIS_GEOMETRY_OR_INDEX,
                          (Geometry, boost::integral_constant<std::size_t, Index>));
};

} // namespace traits

#ifndef DOXYGEN_NO_DISPATCH
namespace core_dispatch {

template <typename V>
struct dimension<vector_tag, V>
    : traits::dimension<typename geometry::util::bare_type<V>::type>
{};

template <typename G>
struct dimension<quaternion_tag, G>
    : traits::dimension<typename geometry::util::bare_type<G>::type>
{};

template <typename T, typename G, std::size_t Index>
struct indexed_dimension
{
    BOOST_MPL_ASSERT_MSG(false,
                         NOT_IMPLEMENTED_FOR_THIS_GEOMETRY_OR_INDEX,
                         (G, boost::integral_constant<std::size_t, Index>));
};

template <typename G, std::size_t Index>
struct indexed_dimension<matrix_tag, G, Index>
    : traits::indexed_dimension<typename geometry::util::bare_type<G>::type, Index>
{};

template <typename G>
struct dimension<rotation_quaternion_tag, G>
    : traits::dimension<typename geometry::util::bare_type<G>::type>
{};

template <typename G>
struct dimension<rotation_matrix_tag, G>
    : traits::dimension<typename geometry::util::bare_type<G>::type>
{};

} // namespace core_dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_COORDINATE_DIMENSION_HPP


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// 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.

#include "netbuilder/NetConstructionTraits.h"

#include <sstream>
#include <boost/algorithm/string/erase.hpp>

namespace NetBuilder {

    const std::string NetConstructionTraits<NetConstruction::EXPLICIT>::name = "Explicit";

    typedef typename NetConstructionTraits<NetConstruction::EXPLICIT>::GenValue GenValue;

    typedef NetConstructionTraits<NetConstruction::EXPLICIT>::SizeParameter SizeParameter;

    bool NetConstructionTraits<NetConstruction::EXPLICIT>::checkGenValue(const GenValue& genValue, const SizeParameter& sizeParameter)
    {
        return genValue.nRows() == nRows(sizeParameter) && genValue.nCols() == nCols(sizeParameter);
    }

    unsigned int NetConstructionTraits<NetConstruction::EXPLICIT>::nRows(const SizeParameter& sizeParameter) {return (unsigned int) sizeParameter.first; }

    unsigned int NetConstructionTraits<NetConstruction::EXPLICIT>::nCols(const SizeParameter& sizeParameter) {return (unsigned int) sizeParameter.second; }

    GeneratingMatrix*  NetConstructionTraits<NetConstruction::EXPLICIT>::createGeneratingMatrix(const GenValue& genValue, const SizeParameter& sizeParameter, const Dimension& dimension_j, const unsigned int nRows)
    {
        unsigned int finalnRows = (nRows == 0)? NetConstructionTraits<NetConstruction::EXPLICIT>::nRows(sizeParameter) : nRows;
        GeneratingMatrix* genMat = new GeneratingMatrix(genValue);
        genMat->resize(finalnRows, nCols(sizeParameter));
        return genMat;
    }

    std::vector<GenValue> NetConstructionTraits<NetConstruction::EXPLICIT>::genValueSpaceCoord(Dimension coord, const SizeParameter& sizeParameter)
    {
        throw std::logic_error("The space of all matrices is far too big to be exhautively explored.");
        return std::vector<GenValue>{};
    }

    std::vector<std::vector<GenValue>> NetConstructionTraits<NetConstruction::EXPLICIT>::genValueSpace(Dimension dimension, const SizeParameter& sizeParameter)
    {
        throw std::logic_error("The space of all matrices is far too big to be exhautively explored.");
        return std::vector<std::vector<GenValue>>{};
    }

    std::string NetConstructionTraits<NetConstruction::EXPLICIT>::format(const std::vector<std::shared_ptr<GeneratingMatrix>>& genMatrices, const std::vector<std::shared_ptr<GenValue>>& genVals, const SizeParameter& sizeParameter, OutputStyle outputStyle, unsigned int interlacingFactor)
    {
        std::ostringstream stream;

        if (outputStyle == OutputStyle::TERMINAL){
            stream << "Explicit Digital Net - Matrix size = " << sizeParameter.first << "x" << sizeParameter.second << std::endl;
            for(Dimension dim = 0; dim < genMatrices.size(); ++dim)
            {
                stream << "Coordinate " << dim << std::endl;
                stream << *genMatrices[dim] << std::endl;
            }
        }
        return stream.str();
    }
}


#pragma once

#ifdef USE_FIBONACCI_HEAP
#include <boost/heap/fibonacci_heap.hpp>
#endif

#include <boost/functional/hash.hpp>
#include <boost/heap/d_ary_heap.hpp>

#include <unordered_map>
#include <unordered_set>

// #include "neighbor.hpp"
// #include "planresult.hpp"
#include "a_star.hpp"

namespace libMultiRobotPlanning {

/*!
  \example sipp.cpp Simple example using a 2D grid world and
  up/down/left/right
  actions
*/

/*! \brief SIPP Algorithm to find the shortest path with dynamic obstacles

This class implements the SIPP algorithm. SIPP is an informed search algorithm
that finds the shortest path for a given map and dynamic a-priori known
obstacles.
It can use a heuristic that needs to be admissible.

Details of the algorithm can be found in the following paper:\n
Mike Phillips and Maxim Likhachev:\n
"SIPP:  Safe  Interval  Path  Planning  for  Dynamic  Environments". IEEE
International Conference on Robotics and Automation (ICRA), 2011\n
https://doi.org/10.1109/ICRA.2011.5980306

This class can either use a fibonacci heap, or a d-ary heap. The latter is the
default. Define "USE_FIBONACCI_HEAP" to use the fibonacci heap instead.

\tparam State Custom state for the search. Needs to be copy'able
\tparam Location Custom location type for the search. Needs to be copy'able
\tparam Cost Custom Cost type (integer or floating point types)
\tparam Environment This class needs to provide the custom A* logic. In
    particular, it needs to support the following functions:
  - `Cost admissibleHeuristic(const State& s)`\n
    This function can return 0 if no suitable heuristic is available.

  - `bool isSolution(const State& s)`\n
    Return true if the given state is a goal state.

  - `void getNeighbors(const State& s, std::vector<Neighbor<State, Action,
   int> >& neighbors)`\n
    Fill the list of neighboring state for the given state s.

  - `void onExpandNode(const State& s, int fScore, int gScore)`\n
    This function is called on every expansion and can be used for statistical
purposes.

  - `void onDiscover(const State& s, int fScore, int gScore)`\n
    This function is called on every node discovery and can be used for
   statistical purposes.
*/
template <typename State, typename Location, typename Action, typename Cost,
          typename Environment>
class SIPP {
 public:
  struct interval {
    interval(Cost start, Cost end) : start(start), end(end) {}

    Cost start;
    Cost end;

    friend bool operator<(const interval& a, const interval& b) {
      return a.start < b.start;
    }
  };

 public:
   struct edgeCollision{
 	  edgeCollision(Cost t, Action action) : t(t), action(action){}

 	Cost t;
 	Action action;
 	friend bool operator==(const edgeCollision& a, const edgeCollision& b){
 		return (a.t == b.t) && (a.action == b.action);
 	}
  };

 public:
  SIPP(Environment& environment) : m_env(environment), m_astar(m_env) {}

  void setCollisionIntervals(const Location& location,
                             const std::vector<interval>& intervals) {
    m_env.setCollisionIntervals(location, intervals);
  }

  void setEdgeCollisions(const Location& location,
                             const std::vector<edgeCollision>& ec) {
    m_env.setEdgeCollisions(location, ec);
  }

  bool mightHaveSolution(const State& goal) {
    return m_env.mightHaveSolution(goal);
  }

  bool search(const State& startState, const Action& waitAction,
              PlanResult<State, Action, Cost>& solution, Cost startTime = 0) {
    PlanResult<SIPPState, SIPPAction, Cost> astarsolution;
    solution.cost = 0;
    solution.fmin = 0;
    solution.actions.clear();
    solution.states.clear();
    size_t interval;
    if (!m_env.findSafeInterval(startState, startTime, interval)) {
      return false;
    }
    bool success = m_astar.search(SIPPState(startState, interval),
                                  astarsolution, startTime);
    solution.cost = astarsolution.cost - startTime;
    solution.fmin = astarsolution.fmin;
    for (size_t i = 0; i < astarsolution.actions.size(); ++i) {
      Cost waitTime =
          astarsolution.actions[i].second - astarsolution.actions[i].first.time;
      if (waitTime == 0) {
        solution.states.push_back(
            std::make_pair<>(astarsolution.states[i].first.state,
                             astarsolution.states[i].second));
        solution.actions.push_back(
            std::make_pair<>(astarsolution.actions[i].first.action,
                             astarsolution.actions[i].second));
      } else {
        // additional wait action before
        solution.states.push_back(
            std::make_pair<>(astarsolution.states[i].first.state,
                             astarsolution.states[i].second));
        solution.actions.push_back(std::make_pair<>(waitAction, waitTime));
        solution.states.push_back(
            std::make_pair<>(astarsolution.states[i].first.state,
                             astarsolution.states[i].second + waitTime));
        solution.actions.push_back(
            std::make_pair<>(astarsolution.actions[i].first.action,
                             astarsolution.actions[i].first.time));
      }
    }
    solution.states.push_back(
        std::make_pair<>(astarsolution.states.back().first.state,
                         astarsolution.states.back().second));

    return success;
  }

 private:
  // public:
  struct SIPPState {
    SIPPState(const State& state, size_t interval)
        : state(state), interval(interval) {}
    SIPPState(const State& state, size_t interval, unsigned int dir)
        : state(state), interval(interval), dir(dir) {}

    bool operator==(const SIPPState& other) const {
      return std::tie(state, interval) == std::tie(other.state, other.interval);
    }

    friend std::ostream& operator<<(std::ostream& os, const SIPPState& s) {
      return os << "(" << s.state << "," << s.interval << ")";
    }

    State state;
    unsigned int dir;
    size_t interval;
  };

  struct SIPPStateHasher {
    size_t operator()(const SIPPState& s) const {
      size_t seed = 0;
      boost::hash_combine(seed, std::hash<State>()(s.state));
      boost::hash_combine(seed, s.interval);
      return seed;
    }
  };

  struct SIPPAction {
    SIPPAction(const Action& action, Cost time) : action(action), time(time) {}

    Action action;
    Cost time;
  };

  // private:
  struct SIPPEnvironment {
    SIPPEnvironment(Environment& env) : m_env(env) {}

    Cost admissibleHeuristic(const SIPPState& s) {
      return m_env.admissibleHeuristic(s.state);
    }

    bool mightHaveSolution(const State& goal) {
      const auto& si = safeIntervals(m_env.getLocation(goal));
      return m_env.isSolution(goal) && !si.empty() &&
             si.back().end == std::numeric_limits<Cost>::max();
    }

    bool isSolution(const SIPPState& s) {
      return m_env.isSolution(s.state) &&
             safeIntervals(m_env.getLocation(s.state)).at(s.interval).end ==
                 std::numeric_limits<Cost>::max();
    }

    void getNeighbors(
        const SIPPState& s,
        std::vector<Neighbor<SIPPState, SIPPAction, Cost> >& neighbors) {
      std::vector<Neighbor<State, Action, Cost> > motions;
// 	  std::cout << "Current state ---------------------------------------GScore " << m_lastGScore << " " << s.state.x << " " << s.state.y << " dir " << s.dir << " \n";

      m_env.getNeighbors(s.state, motions);
      for (const auto& m : motions) {
    	  m_env.num_generation++;
        // std::cout << "gN " << m.state << std::endl;
        Cost m_time = m.cost;
        // std::cout << m_lastGScore;
        Cost start_t = m_lastGScore + m_time;
        Cost end_t =
            safeIntervals(m_env.getLocation(s.state)).at(s.interval).end;

        const auto& sis = safeIntervals(m_env.getLocation(m.state));
        for (size_t i = 0; i < sis.size(); ++i) {
          const interval& si = sis[i];
//          std::cout << m.state.x << " " << m.state.y << "  i " << i << ": " << si.start << " , " << si.end << " " << start_t << " " << end_t<< " \n";
          // std::endl;
          if (si.start - m_time > end_t || si.end < start_t) {
            continue;
          }
//          std::cout << " --------------\n";
          int t;
          unsigned int dir_1 = 0x00;
          Action a_temp;
          if(m.action == Action::Left) {a_temp = Action::Right;dir_1 = 0x01;}
          else if(m.action == Action::Right) {a_temp = Action::Left; dir_1 = 0x02;}
          else if(m.action == Action::Up) {a_temp = Action::Down;dir_1 = 0x04;}
          else if(m.action == Action::Down){ a_temp = Action::Up; dir_1 = 0x08;}

          if (m_env.isCommandValid(s.state, m.state, m.action, m_lastGScore,
                                   end_t, si.start, si.end, t)
        		  && !IsEdgeCollisions(m.state, edgeCollision(t - 1, a_temp))) {
            // std::cout << "  gN: " << m.state << "," << i << "," << t << ","
            // << m_lastGScore << std::endl;
            neighbors.emplace_back(Neighbor<SIPPState, SIPPAction, Cost>(
                SIPPState(m.state, i, dir_1), SIPPAction(m.action, m.cost),
                t - m_lastGScore));
//               std::cout << "Successor : " << m.state.x << " " << m.state.y <<" Cost " << m.cost  << " dir " << dir_1 << " Gscore " << t << " \n";
          }
        }
      }
    }

    void onExpandNode(const SIPPState& s, Cost fScore, Cost gScore) {
      // const auto& interval =
      // safeIntervals(m_env.getLocation(s.state)).at(s.interval);
      // std::cout << "expand: " << s.state << "," << interval.start << " to "
      // << interval.end << "(g: " << gScore << " f: " << fScore << ")" <<
      // std::endl;
      // This is called before getNeighbors(). We use the callback to find the
      // current cost (=time) of the expanded node
      m_env.num_expansion++;
      m_lastGScore = gScore;
      m_env.onExpandNode(s.state, fScore, gScore);
    }

    void onDiscover(const SIPPState& s, Cost fScore, Cost gScore) {
      // const auto& interval =
      // safeIntervals(m_env.getLocation(s.state)).at(s.interval);
      // std::cout << "discover: " << s.state << "," << interval.start << " to "
      // << interval.end << std::endl;
      m_env.onDiscover(s.state, fScore, gScore);
    }

    void setCollisionIntervals(const Location& location,
                               const std::vector<interval>& intervals) {
      m_safeIntervals.erase(location);
      std::vector<interval> sortedIntervals(intervals);
      std::sort(sortedIntervals.begin(), sortedIntervals.end());

      // std::cout << location << ": " << std::endl;
      if (intervals.size() > 0) {
        m_safeIntervals[location];  // create empty safe interval
        int start = 0;
        int lastEnd = 0;
        for (const auto& interval : sortedIntervals) {
          // std::cout << "  ci: " << interval.start << " - " << interval.end <<
          // std::endl;
          assert(interval.start <= interval.end);
          assert(start <= interval.start);
          // if (start + 1 != interval.start - 1) {
          // std::cout << start << "," << interval.start << std::endl;
          // assert(start + 1 < interval.start - 1);
          if (start <= interval.start - 1) {
            m_safeIntervals[location].push_back({start, interval.start - 1});
          }
          // }
          start = interval.end + 1;
          lastEnd = interval.end;
        }
        if (lastEnd < std::numeric_limits<int>::max()) {
          // assert(start < std::numeric_limits<int>::max());
          m_safeIntervals[location].push_back(
              {start, std::numeric_limits<int>::max()});
        }
      }

      // auto iter = m_safeIntervals.find(location);
      // if (iter != m_safeIntervals.end()) {
      //   for (const auto& si : iter->second) {
      //     std::cout << "  si: " << si.start << " - " << si.end << std::endl;
      //   }
      // }
    }

    bool findSafeInterval(const State& state, Cost time, size_t& interval) {
      const auto& si = safeIntervals(m_env.getLocation(state));
      for (size_t idx = 0; idx < si.size(); ++idx) {
        if (si[idx].start <= time && si[idx].end >= time) {
          interval = idx;
          return true;
        }
      }
      return false;
    }

    void setEdgeCollisions(const Location& location,
  		  	  	  	  	const std::vector<edgeCollision>& edge_collision) {
  	  m_edgeCollision.erase(location);
  	  if(edge_collision.size() > 0){
  		  m_edgeCollision[location];
  		  for(const auto& ec : edge_collision){
  			  m_edgeCollision[location].push_back(ec);
  		  }
  	  }
    }

    bool IsEdgeCollisions(const Location& location, const edgeCollision& ec){
//    		std::cout << location.x << " " << location.y << " " << ec.t << " " << ec.action << std::endl;
//    		return false;
    		const auto iter = m_edgeCollision.find(location);
    		if(iter == m_edgeCollision.end()) return false;
    		if((iter->second).size() == 0) return false;
    		for (auto& cec : (iter->second)){
    			if( cec == ec ){
    				return true;
    			}
    		}
    		return false;
    }

   private:
    const std::vector<interval>& safeIntervals(const Location& location) {
      static std::vector<interval> defaultInterval(
          1, {0, std::numeric_limits<Cost>::max()});
      const auto iter = m_safeIntervals.find(location);
      if (iter == m_safeIntervals.end()) {
        return defaultInterval;
      }
      return iter->second;
    }

   private:
    Environment& m_env;
    Cost m_lastGScore;
    std::unordered_map<Location, std::vector<interval> > m_safeIntervals;
    std::unordered_map<Location, std::vector<edgeCollision>> m_edgeCollision;
  };

 private:
  SIPPEnvironment m_env;
  AStar<SIPPState, SIPPAction, Cost, SIPPEnvironment, SIPPStateHasher> m_astar;
};

}  // namespace libMultiRobotPlanning


#include <utility>

//
// Created by whymatter on 31.10.18.
//

#include <iostream>
#include <algorithm>
#include <boost/make_shared.hpp>

#include "environment_core.h"
#include "alb_msgs/point.h"

namespace alb {
 namespace static_environment {
  EnvironmentCore::EnvironmentCore(::alb::cam_transform::CamTransformService camTransformService)
          : camTransformService_{std::move(camTransformService)} {
      this->visual_tools_.reset(new rviz_visual_tools::RvizVisualTools("camera", "/static_environment/marker"));
  }

  std::vector<::alb::alb_msgs::CupObject>
  EnvironmentCore::CalculateEnvironment(const ::alb::alb_msgs::CamDetections &detections) {
      ROS_INFO_STREAM("Received " << detections.detections.size() << " detections");

      std::vector<::alb::alb_msgs::Point> imagePoints;
      std::vector<::alb::alb_msgs::CamDetection> filteredDetections;
      std::vector<::alb::alb_msgs::CupObject> result;

      for (auto &detection : detections.detections) {
          if (detection.propability > 0.5 && detection.propability <= 1.0) {
              auto x = detection.boundingBox.x + detection.boundingBox.w / 2.0;
              auto y = detection.boundingBox.y + detection.boundingBox.h;
              imagePoints.push_back(::alb::alb_msgs::Point(x, y, 0.0));
              filteredDetections.push_back(detection);
//              ROS_INFO_STREAM("image: " << detection.boundingBox.x << "," << detection.boundingBox.y << "," << detection.propability);
          }
      }

      auto objectPoints = this->camTransformService_.ProjectToWorld(imagePoints);

      ROS_INFO_STREAM(objectPoints.size() << " object points");
      this->visual_tools_->deleteAllMarkers();

      int i = 0;
      for (auto &objectPoint : objectPoints) {
          ::geometry_msgs::Pose p;
          p.position.x = objectPoint.x / 100.0;
          p.position.y = objectPoint.y / 100.0;
          p.position.z = objectPoint.z;

          alb_msgs::CamDetection &detection = filteredDetections[i];
          ::alb::alb_msgs::CupObject cup(objectPoint, detection.propability, detection.hue);
          result.push_back(cup);

          this->visual_tools_->publishCylinder(p, rviz_visual_tools::BLUE, .85, .35);
      }

      // Don't forget to trigger the publisher!
      this->visual_tools_->trigger();

      return result;
  }
 }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2015 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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_INVERSE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_INVERSE_HPP

#include <boost/geometry.hpp>

namespace boost { namespace geometry
{

// TODO: this is shared with sectionalize, move to somewhere else (assign?)
template <typename Box, typename Value>
inline void enlarge_box(Box& box, Value value)
{
    geometry::set<0, 0>(box, geometry::get<0, 0>(box) - value);
    geometry::set<0, 1>(box, geometry::get<0, 1>(box) - value);
    geometry::set<1, 0>(box, geometry::get<1, 0>(box) + value);
    geometry::set<1, 1>(box, geometry::get<1, 1>(box) + value);
}

// TODO: when this might be moved outside extensions it should of course
// input/output a Geometry, instead of a WKT
template <typename Geometry, typename Value>
inline std::string inverse(std::string const& wkt, Value margin)
{
    Geometry geometry;
    read_wkt(wkt, geometry);

    geometry::correct(geometry);

    geometry::model::box<typename point_type<Geometry>::type> env;
    geometry::envelope(geometry, env);

    // Make its envelope a bit larger
    enlarge_box(env, margin);

    Geometry env_as_polygon;
    geometry::convert(env, env_as_polygon);

    Geometry inversed_result;
    geometry::difference(env_as_polygon, geometry, inversed_result);

    std::ostringstream out;

    out << geometry::wkt(inversed_result);
    return out.str();
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_INVERSE_HPP


#ifndef _NESTER_H
#define _NESTER_H

#include <memory>
#include <vector>

#include <boost/geometry/strategies/transform/matrix_transformers.hpp>

#include "../libnfporb/libnfp.hpp"
#include "../XDxfGen/include/xdxfgen.h"

using namespace std;
using namespace libnfp;

namespace trans = bg::strategy::transform;

namespace nester {

	typedef int color_t;
	const color_t DXF_OUTER_CUT_COLOR = 1;
	const color_t DXF_INNER_CUT_COLOR = 2;
	const color_t DXF_DEBUG_COLOR = 3;

	typedef shared_ptr<polygon_t> polygon_p;

	typedef trans::matrix_transformer<LongDouble, 2, 2> transformer_t;
	extern transformer_t makeTransformation(LongDouble angle, LongDouble x, LongDouble y);

	struct BoundingBox {
		LongDouble minX, minY;
		LongDouble maxX, maxY;

		BoundingBox() :
			minX(std::numeric_limits<long double>::infinity()),
			minY(std::numeric_limits<long double>::infinity()),
			maxX(-std::numeric_limits<long double>::infinity()),
			maxY(-std::numeric_limits<long double>::infinity()) {}

		LongDouble width() const {
			return maxX - minX;
		}

		LongDouble height() const {
			return maxY - minY;
		}

		void join(BoundingBox other) {
			minX = min(minX, other.minX);
			maxX = max(maxX, other.maxX);
			minY = min(minY, other.minY);
			maxY = max(maxY, other.maxY);
		}
	};

    class FileWriter {
        public:
          virtual void line(point_t p1, point_t p2, int color = 0) = 0;
    };

	class NesterEdge {
	public:
		virtual void write(shared_ptr<FileWriter> writer, color_t color, transformer_t& transformer) const = 0;
		virtual BoundingBox getBoundingBox() const = 0;
	};

	typedef shared_ptr<NesterEdge> NesterEdge_p;

	class NesterNurbs : public NesterEdge {
		vector<point_t> controlPoints;
		vector<double> knots;
	public:
		void addControlPoint(double x, double y);
		void addKnots(vector<double> knobs);

		virtual void write(shared_ptr<FileWriter> writer, color_t color, transformer_t& transformer) const;
		virtual BoundingBox getBoundingBox() const;
	};

	class NesterLine : public NesterEdge {
		point_t start, end;
	public:
		void setStartPoint(point_t p);
		void setEndPoint(point_t p);

		virtual void write(shared_ptr<FileWriter> writer, color_t color, transformer_t& transformer) const;
		virtual BoundingBox getBoundingBox() const;
	};

	// A ring is a closed line (either a loop of segments, a circle or an ellipse)
	class NesterRing {
	public:
		virtual void write(shared_ptr<FileWriter> writer, color_t color, transformer_t& transformer) const = 0;
		virtual BoundingBox getBoundingBox() const = 0;
	};

	typedef shared_ptr<NesterRing> NesterRing_p;

	class NesterLoop : public NesterRing {
		vector<NesterEdge_p> edges;
	public:
		void addEdge(NesterEdge_p primitive);
		virtual void write(shared_ptr<FileWriter> writer, color_t color, transformer_t& transformer) const;
		virtual BoundingBox getBoundingBox() const;
	};

	// A part has an outer boundary and zero or more inner boundaries (holes)
	class NesterPart {
		NesterRing_p outer_ring;
		vector<NesterRing_p> inner_rings;
	public:
		void setOuterRing(NesterRing_p loop);
		void addInnerRing(NesterRing_p loop);

		polygon_p toPolygon() const;
		virtual void write(shared_ptr<FileWriter> writer, transformer_t& transformer) const;
		virtual BoundingBox getBoundingBox() const;
	};

	typedef shared_ptr<NesterPart> NesterPart_p;

	class Nester {
		vector<NesterPart_p> parts;
		shared_ptr<ostream> log;
	public:
		Nester();

		void addPart(NesterPart_p part);

		void run();

		void write(shared_ptr<FileWriter> writer) const;
	};

}

#endif