// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Time/TimeSteppers/AdamsBashforthN.hpp"

#include <algorithm>
#include <boost/iterator/transform_iterator.hpp>
#include <iterator>
#include <limits>
#include <map>
#include <ostream>
#include <pup.h>
#include <tuple>

#include "NumericalAlgorithms/Interpolation/LagrangePolynomial.hpp"
#include "Time/BoundaryHistory.hpp"
#include "Time/EvolutionOrdering.hpp"
#include "Time/History.hpp"
#include "Time/SelfStart.hpp"
#include "Time/Time.hpp"
#include "Time/TimeStepId.hpp"
#include "Utilities/CachedFunction.hpp"
#include "Utilities/EqualWithinRoundoff.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/ErrorHandling/Error.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/Math.hpp"
#include "Utilities/Overloader.hpp"

namespace TimeSteppers {

namespace {
// TimeDelta-like interface to a double used for dense output
struct ApproximateTimeDelta {
  double delta = std::numeric_limits<double>::signaling_NaN();
  double value() const { return delta; }
  bool is_positive() const { return delta > 0.; }

  // Only the operators that are actually used are defined.
  friend bool operator<(const ApproximateTimeDelta& a,
                        const ApproximateTimeDelta& b) {
    return a.value() < b.value();
  }
};

// Time-like interface to a double used for dense output
struct ApproximateTime {
  double time = std::numeric_limits<double>::signaling_NaN();
  double value() const { return time; }

  // Only the operators that are actually used are defined.
  friend ApproximateTimeDelta operator-(const ApproximateTime& a,
                                        const Time& b) {
    return {a.value() - b.value()};
  }

  friend bool operator<(const Time& a, const ApproximateTime& b) {
    return a.value() < b.value();
  }

  friend bool operator<(const ApproximateTime& a, const Time& b) {
    return a.value() < b.value();
  }

  friend std::ostream& operator<<(std::ostream& s, const ApproximateTime& t) {
    return s << t.value();
  }
};
}  // namespace

AdamsBashforthN::AdamsBashforthN(const size_t order) : order_(order) {
  if (order_ < 1 or order_ > maximum_order) {
    ERROR("The order for Adams-Bashforth Nth order must be 1 <= order <= "
          << maximum_order);
  }
}

size_t AdamsBashforthN::order() const { return order_; }

size_t AdamsBashforthN::error_estimate_order() const { return order_ - 1; }

size_t AdamsBashforthN::number_of_past_steps() const { return order_ - 1; }

double AdamsBashforthN::stable_step() const {
  if (order_ == 1) {
    return 1.;
  }

  // This is the condition that the characteristic polynomial of the
  // recurrence relation defined by the method has the correct sign at
  // -1.  It is not clear whether this is actually sufficient.
  const auto& coefficients = constant_coefficients(order_);
  double invstep = 0.;
  double sign = 1.;
  for (const auto coef : coefficients) {
    invstep += sign * coef;
    sign = -sign;
  }
  return 1. / invstep;
}

TimeStepId AdamsBashforthN::next_time_id(const TimeStepId& current_id,
                                         const TimeDelta& time_step) const {
  ASSERT(current_id.substep() == 0, "Adams-Bashforth should not have substeps");
  return {current_id.time_runs_forward(), current_id.slab_number(),
          current_id.step_time() + time_step};
}

std::vector<double> AdamsBashforthN::get_coefficients_impl(
    const std::vector<double>& steps) {
  const size_t order = steps.size();
  ASSERT(order >= 1 and order <= maximum_order, "Bad order" << order);
  if (std::all_of(steps.begin(), steps.end(), [&steps](const double s) {
        return equal_within_roundoff(
            s, steps[0], 10.0 * std::numeric_limits<double>::epsilon(), 0.0);
      })) {
    return constant_coefficients(order);
  }

  return variable_coefficients(steps);
}

std::vector<double> AdamsBashforthN::variable_coefficients(
    const std::vector<double>& steps) {
  const size_t order = steps.size();  // "k" in below equations
  std::vector<double> result;
  result.reserve(order);

  // The `steps` vector contains the step sizes:
  //   steps = {dt_{n-k+1}, ..., dt_n}
  // Our goal is to calculate, for each j, the coefficient given by
  //   \int_0^1 dt ell_j(t dt_n; dt_n, dt_n + dt_{n-1}, ...,
  //                             dt_n + ... + dt_{n-k+1})
  // (Where the ell_j are the Lagrange interpolating polynomials.)

  std::vector<double> poly(order);
  double step_sum_j = 0.0;
  for (size_t j = 0; j < order; ++j) {
    // Calculate coefficients of the Lagrange interpolating polynomials,
    // in the standard a_0 + a_1 t + a_2 t^2 + ... form.
    std::fill(poly.begin(), poly.end(), 0.0);

    step_sum_j += steps[order - j - 1];
    poly[0] = 1.0;

    double step_sum_m = 0.0;
    for (size_t m = 0; m < order; ++m) {
      step_sum_m += steps[order - m - 1];
      if (m == j) {
        continue;
      }
      const double denom = 1.0 / (step_sum_j - step_sum_m);
      for (size_t i = m < j ? m + 1 : m; i > 0; --i) {
        poly[i] = (poly[i - 1] - poly[i] * step_sum_m) * denom;
      }
      poly[0] *= -step_sum_m * denom;
    }

    // Integrate p(t dt_n), term by term.
    for (size_t m = 0; m < order; ++m) {
      poly[m] /= m + 1.0;
    }
    result.push_back(evaluate_polynomial(poly, steps.back()));
  }
  return result;
}

std::vector<double> AdamsBashforthN::constant_coefficients(const size_t order) {
  switch (order) {
    case 1: return {1.};
    case 2: return {1.5, -0.5};
    case 3: return {23.0 / 12.0, -4.0 / 3.0, 5.0 / 12.0};
    case 4: return {55.0 / 24.0, -59.0 / 24.0, 37.0 / 24.0, -3.0 / 8.0};
    case 5: return {1901.0 / 720.0, -1387.0 / 360.0, 109.0 / 30.0,
          -637.0 / 360.0, 251.0 / 720.0};
    case 6: return {4277.0 / 1440.0, -2641.0 / 480.0, 4991.0 / 720.0,
          -3649.0 / 720.0, 959.0 / 480.0, -95.0 / 288.0};
    case 7: return {198721.0 / 60480.0, -18637.0 / 2520.0, 235183.0 / 20160.0,
          -10754.0 / 945.0, 135713.0 / 20160.0, -5603.0 / 2520.0,
          19087.0 / 60480.0};
    case 8: return {16083.0 / 4480.0, -1152169.0 / 120960.0, 242653.0 / 13440.0,
          -296053.0 / 13440.0, 2102243.0 / 120960.0, -115747.0 / 13440.0,
          32863.0 / 13440.0, -5257.0 / 17280.0};
    default:
      ERROR("Bad order: " << order);
  }
}

void AdamsBashforthN::pup(PUP::er& p) {
  LtsTimeStepper::pup(p);
  p | order_;
}

template <typename T>
void AdamsBashforthN::update_u_impl(
    const gsl::not_null<T*> u, const gsl::not_null<UntypedHistory<T>*> history,
    const TimeDelta& time_step) const {
  ASSERT(history->size() >= history->integration_order(),
         "Insufficient data to take an order-" << history->integration_order()
         << " step.  Have " << history->size() << " times, need "
         << history->integration_order());
  history->mark_unneeded(
      history->end() -
      static_cast<typename decltype(history->end())::difference_type>(
          history->integration_order()));
  update_u_common(u, *history, time_step, history->integration_order());
}

template <typename T>
bool AdamsBashforthN::update_u_impl(
    const gsl::not_null<T*> u, const gsl::not_null<T*> u_error,
    const gsl::not_null<UntypedHistory<T>*> history,
    const TimeDelta& time_step) const {
  ASSERT(history->size() >= history->integration_order(),
         "Insufficient data to take an order-" << history->integration_order()
         << " step.  Have " << history->size() << " times, need "
         << history->integration_order());
  history->mark_unneeded(
      history->end() -
      static_cast<typename decltype(history->end())::difference_type>(
          history->integration_order()));
  update_u_common(u, *history, time_step, history->integration_order());
  // the error estimate is only useful once the history has enough elements to
  // do more than one order of step
  update_u_common(u_error, *history, time_step,
                  history->integration_order() - 1);
  *u_error = *u - *u_error;
  return true;
}

template <typename T>
bool AdamsBashforthN::dense_update_u_impl(const gsl::not_null<T*> u,
                                          const UntypedHistory<T>& history,
                                          const double time) const {
  const ApproximateTimeDelta time_step{time - history.back().value()};
  update_u_common(make_not_null(&*make_math_wrapper(u)), history, time_step,
                  history.integration_order());
  return true;
}

template <typename T, typename Delta>
void AdamsBashforthN::update_u_common(const gsl::not_null<T*> u,
                                      const UntypedHistory<T>& history,
                                      const Delta& time_step,
                                      const size_t order) const {
  ASSERT(
      history.size() > 0,
      "Cannot meaningfully update the evolved variables with an empty history");
  ASSERT(order <= order_,
         "Requested integration order higher than integrator order");

  const auto history_start =
      history.end() -
      static_cast<typename UntypedHistory<T>::difference_type>(order);
  const auto coefficients =
      get_coefficients(history_start, history.end(), time_step);

  *u = *history.untyped_most_recent_value();
  auto coefficient = coefficients.rbegin();
  for (auto history_entry = history_start;
       history_entry != history.end();
       ++history_entry, ++coefficient) {
    *u += time_step.value() * *coefficient * *history_entry.derivative();
  }
}

template <typename T>
bool AdamsBashforthN::can_change_step_size_impl(
    const TimeStepId& time_id, const UntypedHistory<T>& history) const {
  // We need to forbid local time-stepping before initialization is
  // complete.  The self-start procedure itself should never consider
  // changing the step size, but we need to wait during the main
  // evolution until the self-start history has been replaced with
  // "real" values.
  const evolution_less<Time> less{time_id.time_runs_forward()};
  return not ::SelfStart::is_self_starting(time_id) and
         (history.size() == 0 or
          (less(history.back(), time_id.step_time()) and
           std::is_sorted(history.begin(), history.end(), less)));
}

template <typename T>
void AdamsBashforthN::add_boundary_delta_impl(
    const gsl::not_null<T*> result,
    const TimeSteppers::BoundaryHistoryEvaluator<T>& coupling,
    const TimeSteppers::BoundaryHistoryCleaner& cleaner,
    const TimeDelta& time_step) const {
  const auto signed_order =
      static_cast<typename decltype(cleaner.local_end())::difference_type>(
          cleaner.integration_order());

  ASSERT(cleaner.local_size() >= cleaner.integration_order(),
         "Insufficient data to take an order-" << cleaner.integration_order()
         << " step.  Have " << cleaner.local_size() << " times, need "
         << cleaner.integration_order());
  cleaner.local_mark_unneeded(cleaner.local_end() - signed_order);

  if (std::equal(cleaner.local_begin(), cleaner.local_end(),
                 cleaner.remote_end() - signed_order)) {
    // GTS
    ASSERT(cleaner.remote_size() >= cleaner.integration_order(),
           "Insufficient data to take an order-" << cleaner.integration_order()
           << " step.  Have " << cleaner.remote_size() << " times, need "
           << cleaner.integration_order());
    cleaner.remote_mark_unneeded(cleaner.remote_end() - signed_order);
  } else {
    const auto remote_step_for_step_start =
        std::upper_bound(cleaner.remote_begin(), cleaner.remote_end(),
                         *(cleaner.local_end() - 1),
                         evolution_less<Time>{time_step.is_positive()});
    ASSERT(remote_step_for_step_start - cleaner.remote_begin() >= signed_order,
           "Insufficient data to take an order-" << cleaner.integration_order()
           << " step.  Have "
           << remote_step_for_step_start - cleaner.remote_begin()
           << " times before the step, need " << cleaner.integration_order());
    cleaner.remote_mark_unneeded(remote_step_for_step_start - signed_order);
  }

  boundary_impl(result, coupling, *(cleaner.local_end() - 1) + time_step);
}

template <typename T>
void AdamsBashforthN::boundary_dense_output_impl(
    const gsl::not_null<T*> result,
    const TimeSteppers::BoundaryHistoryEvaluator<T>& coupling,
    const double time) const {
  return boundary_impl(result, coupling, ApproximateTime{time});
}

template <typename T, typename TimeType>
void AdamsBashforthN::boundary_impl(const gsl::not_null<T*> result,
                                    const BoundaryHistoryEvaluator<T>& coupling,
                                    const TimeType& end_time) const {
  // Might be different from order_ during self-start.
  const auto current_order = coupling.integration_order();

  ASSERT(current_order <= order_,
         "Local history is too long for target order (" << current_order
         << " should not exceed " << order_ << ")");
  ASSERT(coupling.remote_size() >= current_order,
         "Remote history is too short (" << coupling.remote_size()
         << " should be at least " << current_order << ")");

  // Avoid billions of casts
  const auto order_s = static_cast<
      typename BoundaryHistoryEvaluator<T>::iterator::difference_type>(
      current_order);

  // Start and end of the step we are trying to take
  const Time start_time = *(coupling.local_end() - 1);
  const auto time_step = end_time - start_time;

  // We define the local_begin and remote_begin variables as the start
  // of the part of the history relevant to this calculation.
  // Boundary history cleanup happens immediately before the step, but
  // boundary dense output happens before that, so there may be data
  // left over that was needed for the previous step and has not been
  // cleaned out yet.
  const auto local_begin = coupling.local_end() - order_s;

  if (std::equal(local_begin, coupling.local_end(),
                 coupling.remote_end() - order_s)) {
    // No local time-stepping going on.
    const auto coefficients =
        get_coefficients(local_begin, coupling.local_end(), time_step);

    auto local_it = local_begin;
    auto remote_it = coupling.remote_end() - order_s;
    for (auto coefficients_it = coefficients.rbegin();
         coefficients_it != coefficients.rend();
         ++coefficients_it, ++local_it, ++remote_it) {
      *result +=
          time_step.value() * *coefficients_it * *coupling(local_it, remote_it);
    }
    return;
  }

  ASSERT(current_order == order_,
         "Cannot perform local time-stepping while self-starting.");

  const evolution_less<> less{time_step.is_positive()};
  const auto remote_begin =
      std::upper_bound(coupling.remote_begin(), coupling.remote_end(),
                       start_time, less) -
      order_s;

  ASSERT(std::is_sorted(local_begin, coupling.local_end(), less),
         "Local history not in order");
  ASSERT(std::is_sorted(remote_begin, coupling.remote_end(), less),
         "Remote history not in order");
  ASSERT(not less(start_time, *(remote_begin + (order_s - 1))),
         "Remote history does not extend far enough back");
  ASSERT(less(*(coupling.remote_end() - 1), end_time),
         "Please supply only older data: " << *(coupling.remote_end() - 1)
         << " is not before " << end_time);

  // Union of times of all step boundaries on any side.
  const auto union_times = [&coupling, &local_begin, &remote_begin, &less]() {
    std::vector<Time> ret;
    ret.reserve(coupling.local_size() + coupling.remote_size());
    std::set_union(local_begin, coupling.local_end(), remote_begin,
                   coupling.remote_end(), std::back_inserter(ret), less);
    return ret;
  }();

  using UnionIter = typename decltype(union_times)::const_iterator;

  // Find the union times iterator for a given time.
  const auto union_step = [&union_times, &less](const Time& t) {
    return std::lower_bound(union_times.cbegin(), union_times.cend(), t, less);
  };

  // The union time index for the step start.
  const auto union_step_start = union_step(start_time);

  // min(union_times.end(), it + order_s) except being careful not
  // to create out-of-range iterators.
  const auto advance_within_step = [order_s,
                                    &union_times](const UnionIter& it) {
    return union_times.end() - it >
                   static_cast<typename decltype(union_times)::difference_type>(
                       order_s)
               ? it + static_cast<typename decltype(
                          union_times)::difference_type>(order_s)
               : union_times.end();
  };

  // Calculating the Adams-Bashforth coefficients is somewhat
  // expensive, so we cache them.  ab_coefs(it, step) returns the
  // coefficients used to step from *it to *it + step.
  auto ab_coefs = make_overloader(
      make_cached_function<std::tuple<UnionIter, TimeDelta>, std::map>(
          [order_s](const std::tuple<UnionIter, TimeDelta>& args) {
            return get_coefficients(
                std::get<0>(args) -
                    static_cast<typename UnionIter::difference_type>(order_s -
                                                                     1),
                std::get<0>(args) + 1, std::get<1>(args));
          }),
      make_cached_function<std::tuple<UnionIter, ApproximateTimeDelta>,
                           std::map>(
          [order_s](const std::tuple<UnionIter, ApproximateTimeDelta>& args) {
            return get_coefficients(
                std::get<0>(args) -
                    static_cast<typename UnionIter::difference_type>(order_s -
                                                                     1),
                std::get<0>(args) + 1, std::get<1>(args));
          }));

  // The value of the coefficient of `evaluation_step` when doing
  // a standard Adams-Bashforth integration over the union times
  // from `step` to `step + 1`.
  const auto base_summand = [&ab_coefs, &end_time, &union_times](
                                const UnionIter& step,
                                const UnionIter& evaluation_step) {
    if (step + 1 != union_times.end()) {
      const TimeDelta step_size = *(step + 1) - *step;
      return step_size.value() *
             ab_coefs(std::make_tuple(
                 step, step_size))[static_cast<size_t>(step - evaluation_step)];
    } else {
      const auto step_size = end_time - *step;
      return step_size.value() *
             ab_coefs(std::make_tuple(
                 step, step_size))[static_cast<size_t>(step - evaluation_step)];
    }
  };

  for (auto local_evaluation_step = local_begin;
       local_evaluation_step != coupling.local_end();
       ++local_evaluation_step) {
    const auto union_local_evaluation_step = union_step(*local_evaluation_step);
    for (auto remote_evaluation_step = remote_begin;
         remote_evaluation_step != coupling.remote_end();
         ++remote_evaluation_step) {
      double deriv_coef = 0.;

      if (*local_evaluation_step == *remote_evaluation_step) {
        // The two elements stepped at the same time.  This gives a
        // standard Adams-Bashforth contribution to each segment
        // making up the current step.
        const auto union_step_upper_bound =
            advance_within_step(union_local_evaluation_step);
        for (auto step = union_step_start;
             step < union_step_upper_bound;
             ++step) {
          deriv_coef += base_summand(step, union_local_evaluation_step);
        }
      } else {
        // In this block we consider a coupling evaluation that is not
        // performed at equal times on the two sides of the mortar.

        // Makes an iterator with a map to give time as a double.
        const auto make_lagrange_iterator = [](const auto& it) {
          return boost::make_transform_iterator(
              it, [](const Time& t) { return t.value(); });
        };

        const auto union_remote_evaluation_step =
            union_step(*remote_evaluation_step);
        const auto union_step_lower_bound =
            std::max(union_step_start, union_remote_evaluation_step);

        // Compute the contribution to an interpolation over the local
        // times to `remote_evaluation_step->value()`, which we will
        // use as the coupling value for that time.  If there is an
        // actual evaluation at that time then skip this because the
        // Lagrange polynomial will be zero.
        if (not std::binary_search(local_begin, coupling.local_end(),
                                   *remote_evaluation_step, less)) {
          const auto union_step_upper_bound =
              advance_within_step(union_remote_evaluation_step);
          for (auto step = union_step_lower_bound;
               step < union_step_upper_bound;
               ++step) {
            deriv_coef += base_summand(step, union_remote_evaluation_step);
          }
          deriv_coef *=
              lagrange_polynomial(make_lagrange_iterator(local_evaluation_step),
                                  remote_evaluation_step->value(),
                                  make_lagrange_iterator(local_begin),
                                  make_lagrange_iterator(coupling.local_end()));
        }

        // Same qualitative calculation as the previous block, but
        // interpolating over the remote times.  This case is somewhat
        // more complicated because the latest remote time that can be
        // used varies for the different segments making up the step.
        if (not std::binary_search(remote_begin, coupling.remote_end(),
                                   *local_evaluation_step, less)) {
          auto union_step_upper_bound =
              advance_within_step(union_local_evaluation_step);
          if (coupling.remote_end() - remote_evaluation_step > order_s) {
            union_step_upper_bound = std::min(
                union_step_upper_bound,
                union_step(*(remote_evaluation_step + order_s)));
          }

          auto control_points = make_lagrange_iterator(
              remote_evaluation_step - remote_begin >= order_s
                  ? remote_evaluation_step - (order_s - 1)
                  : remote_begin);
          for (auto step = union_step_lower_bound;
               step < union_step_upper_bound;
               ++step, ++control_points) {
            deriv_coef +=
                base_summand(step, union_local_evaluation_step) *
                lagrange_polynomial(
                    make_lagrange_iterator(remote_evaluation_step),
                    local_evaluation_step->value(), control_points,
                    control_points +
                        static_cast<typename decltype(
                            control_points)::difference_type>(order_s));
          }
        }
      }

      if (deriv_coef != 0.) {
        // Skip the (potentially expensive) coupling calculation if
        // the coefficient is zero.
        *result += deriv_coef *
                   *coupling(local_evaluation_step, remote_evaluation_step);
      }
    }  // for remote_evaluation_step
  }  // for local_evaluation_step
}

template <typename Iterator, typename Delta>
std::vector<double> AdamsBashforthN::get_coefficients(
    const Iterator& times_begin, const Iterator& times_end, const Delta& step) {
  if (times_begin == times_end) {
    return {};
  }
  std::vector<double> steps;
  // This may be slightly more space than we need, but we can't get
  // the exact amount without iterating through the iterators, which
  // is not necessarily cheap depending on the iterator type.
  steps.reserve(maximum_order);
  for (auto t = times_begin; std::next(t) != times_end; ++t) {
    steps.push_back((*std::next(t) - *t).value());
  }
  steps.push_back(step.value());
  return get_coefficients_impl(steps);
}

bool operator==(const AdamsBashforthN& lhs, const AdamsBashforthN& rhs) {
  return lhs.order_ == rhs.order_;
}

bool operator!=(const AdamsBashforthN& lhs, const AdamsBashforthN& rhs) {
  return not(lhs == rhs);
}

TIME_STEPPER_DEFINE_OVERLOADS(AdamsBashforthN)
LTS_TIME_STEPPER_DEFINE_OVERLOADS(AdamsBashforthN)
}  // namespace TimeSteppers

PUP::able::PUP_ID TimeSteppers::AdamsBashforthN::my_PUP_ID =  // NOLINT
    0;


//=======================================================================
// Copyright (c) 2014 Piotr Smulewicz
//               2013 Piotr Wygocki
//
// 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)
//=======================================================================
/**
 * @file steiner_tree_greedy.hpp
 * @brief
 * @author Piotr Wygocki, Piotr Smulewicz
 * @version 1.0
 * @date 2013-11-27
 */
#ifndef PAAL_STEINER_TREE_GREEDY_HPP
#define PAAL_STEINER_TREE_GREEDY_HPP

#include "paal/utils/accumulate_functors.hpp"
#include "paal/utils/functors.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/two_bit_color_map.hpp>

#include <boost/property_map/property_map.hpp>

#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/fill.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/range/algorithm/unique.hpp>
#include <boost/range/as_array.hpp>
#include <boost/range/numeric.hpp>

#include <algorithm>
#include <utility>

/// enum for edge base property
enum edge_base_t { edge_base };

namespace boost {
/// macro create edge base property
BOOST_INSTALL_PROPERTY(edge, base);
}

namespace paal {

/**
 * @brief enum indicates if given color represents terminal or NONTERMINAL.
 */
enum Terminals { NONTERMINAL, TERMINAL };

namespace detail {
template <typename NearestMap, typename LastEdgeMap, typename Tag>
class nearest_recorder
    : boost::base_visitor<nearest_recorder<NearestMap, LastEdgeMap, Tag>> {
  public:
    using event_filter = Tag;
    nearest_recorder(NearestMap &nearest_map, LastEdgeMap &vpred)
        : m_nearest_map(nearest_map), m_vpred(vpred) {};
    template <typename Edge, typename Graph>
    void operator()(Edge const e, Graph const &g) {
        m_nearest_map[target(e, g)] = m_nearest_map[source(e, g)];
        m_vpred[target(e, g)] = e;
    }

  private:
    NearestMap &m_nearest_map;
    LastEdgeMap &m_vpred;
};

template <typename NearestMap, typename LastEdgeMap, typename Tag>
nearest_recorder<NearestMap, LastEdgeMap, Tag>
make_nearest_recorder(NearestMap &nearest_map, LastEdgeMap &vpred, Tag) {
    return nearest_recorder<NearestMap, LastEdgeMap, Tag>{ nearest_map, vpred };
}
}
/**
 * @brief non-named version of  steiner_tree_greedy
 *
 * @tparam Graph
 * @tparam OutputIterator
 * @tparam EdgeWeightMap
 * @tparam ColorMap
 * @param g - given graph
 * @param out - edge output iterator
 * @param edge_weight
 * @param color_map
 */
template <typename Graph, typename OutputIterator, typename EdgeWeightMap,
          typename ColorMap>
auto steiner_tree_greedy(const Graph &g, OutputIterator out,
                         EdgeWeightMap edge_weight, ColorMap color_map)
    -> typename std::pair<
          typename boost::property_traits<EdgeWeightMap>::value_type,
          typename boost::property_traits<EdgeWeightMap>::value_type> {
    using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
    using Edge = typename boost::graph_traits<Graph>::edge_descriptor;
    using Base = typename boost::property<edge_base_t, Edge>;
    using Weight = typename boost::property_traits<EdgeWeightMap>::value_type;
    using WeightProperty =
        typename boost::property<boost::edge_weight_t, Weight, Base>;
    using TerminalGraph =
        boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                              boost::no_property, WeightProperty>;
    using EdgeTerminal =
        typename boost::graph_traits<TerminalGraph>::edge_descriptor;

    auto N = num_vertices(g);

    // distance array used in the dijkstra runs
    std::vector<Weight> distance(N);

    // computing terminals
    std::vector<int> terminals;
    auto terminals_nr = accumulate_functor(
        vertices(g), 0, [=](Vertex v) { return get(color_map, v); });
    terminals.reserve(terminals_nr);
    for (auto v : boost::as_array(vertices(g))) {
        if (get(color_map, v) == Terminals::TERMINAL) {
            terminals.push_back(v);
        }
    }
    if (terminals.empty()) {
        return std::make_pair(Weight{}, Weight{});
    }
    std::vector<Vertex> nearest_terminal(num_vertices(g));
    auto index = get(boost::vertex_index, g);
    auto nearest_terminal_map = boost::make_iterator_property_map(
        nearest_terminal.begin(), get(boost::vertex_index, g));
    for (auto terminal : terminals) {
        nearest_terminal_map[terminal] = terminal;
    }

    // compute voronoi diagram each vertex get nearest terminal and last edge on
    // path to nearest terminal
    auto distance_map = make_iterator_property_map(distance.begin(), index);
    std::vector<Edge> vpred(N);
    auto last_edge = boost::make_iterator_property_map(
        vpred.begin(), get(boost::vertex_index, g));
    boost::dijkstra_shortest_paths(
        g, terminals.begin(), terminals.end(), boost::dummy_property_map(),
        distance_map, edge_weight, index, utils::less(),
        boost::closed_plus<Weight>(), std::numeric_limits<Weight>::max(), 0,
        boost::make_dijkstra_visitor(detail::make_nearest_recorder(
            nearest_terminal_map, last_edge, boost::on_edge_relaxed{})));

    // computing distances between terminals
    // creating terminal_graph
    TerminalGraph terminal_graph(N);
    for (auto w : boost::as_array(edges(g))) {
        auto const &nearest_to_source = nearest_terminal_map[source(w, g)];
        auto const &nearest_to_target = nearest_terminal_map[target(w, g)];
        if (nearest_to_source != nearest_to_target) {
            add_edge(nearest_to_source, nearest_to_target,
                     WeightProperty(distance[source(w, g)] +
                                        distance[target(w, g)] + edge_weight[w],
                                    Base(w)),
                     terminal_graph);
        }
    }
    // computing spanning tree on terminal_graph
    std::vector<Edge> terminal_edge;
    boost::kruskal_minimum_spanning_tree(terminal_graph,
                                         std::back_inserter(terminal_edge));

    // computing result
    std::vector<Edge> tree_edges;
    tree_edges.reserve(terminals_nr);
    for (auto edge : terminal_edge) {
        auto base = get(edge_base, terminal_graph, edge);
        tree_edges.push_back(base);
        for (auto pom : { source(base, g), target(base, g) }) {
            while (nearest_terminal_map[pom] != pom) {
                tree_edges.push_back(vpred[pom]);
                pom = source(vpred[pom], g);
            }
        }
    }

    // because in each voronoi region we have unique patch to all vertex from
    // terminal, result graph contain no cycle
    // and all leaf are terminal

    boost::sort(tree_edges);
    auto get_weight=[&](Edge edge){return edge_weight[edge];};
    auto lower_bound=accumulate_functor(tree_edges, Weight{}, get_weight);
    auto unique_edges = boost::unique(tree_edges);
    auto cost_solution=accumulate_functor(unique_edges, Weight{}, get_weight);
    boost::copy(unique_edges, out);
    return std::make_pair(cost_solution, lower_bound / 2.);
}

/**
 * @brief named version of  steiner_tree_greedy
 *
 * @tparam Graph
 * @tparam OutputIterator
 * @tparam P
 * @tparam T
 * @tparam R
 * @param g - given graph
 * @param out - edge output iterator
 * @param params
 */
template <typename Graph, typename OutputIterator, typename P, typename T,
          typename R>
auto steiner_tree_greedy(const Graph &g, OutputIterator out,
                         const boost::bgl_named_params<P, T, R> &params) {
    return steiner_tree_greedy(
        g, out, choose_const_pmap(get_param(params, boost::edge_weight), g,
                                  boost::edge_weight),
        choose_const_pmap(get_param(params, boost::vertex_color), g,
                          boost::vertex_color));
}

/**
 * @brief version of  steiner_tree_greedy with all default parameters
 *
 * @tparam Graph
 * @tparam OutputIterator
 * @param g - given graph
 * @param out - edge output iterator
 */
template <typename Graph, typename OutputIterator>
auto steiner_tree_greedy(const Graph &g, OutputIterator out) {
    return steiner_tree_greedy(g, out, boost::no_named_parameters());
}

} // paal

#endif // PAAL_STEINER_TREE_GREEDY_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_INIT_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_INIT_HPP

#include <cstdlib>
#include <string>
#include <vector>

#include <boost/algorithm/string.hpp>
#include <boost/range.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/condition.hpp>

#include <boost/geometry/srs/projections/impl/dms_parser.hpp>
#include <boost/geometry/srs/projections/impl/pj_datum_set.hpp>
#include <boost/geometry/srs/projections/impl/pj_datums.hpp>
#include <boost/geometry/srs/projections/impl/pj_ell_set.hpp>
#include <boost/geometry/srs/projections/impl/pj_param.hpp>
#include <boost/geometry/srs/projections/impl/pj_units.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/proj4.hpp>

namespace boost { namespace geometry { namespace projections
{

namespace detail
{

template <typename BGParams, typename T>
inline void pj_push_defaults(BGParams const& /*bg_params*/, parameters<T>& pin)
{
    pin.params.push_back(pj_mkparam<T>("ellps", "WGS84"));

    if (pin.name == "aea")
    {
        pin.params.push_back(pj_mkparam<T>("lat_1", "29.5"));
        pin.params.push_back(pj_mkparam<T>("lat_2", "45.5 "));
    }
    else if (pin.name == "lcc")
    {
        pin.params.push_back(pj_mkparam<T>("lat_1", "33"));
        pin.params.push_back(pj_mkparam<T>("lat_2", "45"));
    }
    else if (pin.name == "lagrng")
    {
        pin.params.push_back(pj_mkparam<T>("W", "2"));
    }
}

template <BOOST_GEOMETRY_PROJECTIONS_DETAIL_TYPENAME_PX, typename T>
inline void pj_push_defaults(srs::static_proj4<BOOST_GEOMETRY_PROJECTIONS_DETAIL_PX> const& /*bg_params*/,
                             parameters<T>& pin)
{
    typedef srs::static_proj4<BOOST_GEOMETRY_PROJECTIONS_DETAIL_PX> static_parameters_type;
    typedef typename srs::par4::detail::pick_proj_tag
        <
            static_parameters_type
        >::type proj_tag;

    // statically defaulting to WGS84
    //pin.params.push_back(pj_mkparam("ellps", "WGS84"));

    if (BOOST_GEOMETRY_CONDITION((boost::is_same<proj_tag, srs::par4::aea>::value)))
    {
        pin.params.push_back(pj_mkparam<T>("lat_1", "29.5"));
        pin.params.push_back(pj_mkparam<T>("lat_2", "45.5 "));
    }
    else if (BOOST_GEOMETRY_CONDITION((boost::is_same<proj_tag, srs::par4::lcc>::value)))
    {
        pin.params.push_back(pj_mkparam<T>("lat_1", "33"));
        pin.params.push_back(pj_mkparam<T>("lat_2", "45"));
    }
    else if (BOOST_GEOMETRY_CONDITION((boost::is_same<proj_tag, srs::par4::lagrng>::value)))
    {
        pin.params.push_back(pj_mkparam<T>("W", "2"));
    }
}

template <typename T>
inline void pj_init_units(std::vector<pvalue<T> > const& params,
                          std::string const& sunits,
                          std::string const& sto_meter,
                          T & to_meter,
                          T & fr_meter,
                          T const& default_to_meter,
                          T const& default_fr_meter)
{
    std::string s;
    std::string units = pj_get_param_s(params, sunits);
    if (! units.empty())
    {
        const int n = sizeof(pj_units) / sizeof(pj_units[0]);
        int index = -1;
        for (int i = 0; i < n && index == -1; i++)
        {
            if(pj_units[i].id == units)
            {
                index = i;
            }
        }

        if (index == -1) {
            BOOST_THROW_EXCEPTION( projection_exception(error_unknow_unit_id) );
        }
        s = pj_units[index].to_meter;
    }

    if (s.empty())
    {
        s = pj_get_param_s(params, sto_meter);
    }

    if (! s.empty())
    {
        std::size_t const pos = s.find('/');
        if (pos == std::string::npos)
        {
            to_meter = geometry::str_cast<T>(s);
        }
        else
        {
            T const numerator = geometry::str_cast<T>(s.substr(0, pos));
            T const denominator = geometry::str_cast<T>(s.substr(pos + 1));
            if (numerator == 0.0 || denominator == 0.0)
            {
                BOOST_THROW_EXCEPTION( projection_exception(error_unit_factor_less_than_0) );
            }
            to_meter = numerator / denominator;
        }
        if (to_meter == 0.0)
        {
            BOOST_THROW_EXCEPTION( projection_exception(error_unit_factor_less_than_0) );
        }
        fr_meter = 1. / to_meter;
    }
    else
    {
        to_meter = default_to_meter;
        fr_meter = default_fr_meter;
    }
}

/************************************************************************/
/*                              pj_init()                               */
/*                                                                      */
/*      Main entry point for initialing a PJ projections                */
/*      definition.  Note that the projection specific function is      */
/*      called to do the initial allocation so it can be created        */
/*      large enough to hold projection specific parameters.            */
/************************************************************************/
template <typename T, typename BGParams, typename R>
inline parameters<T> pj_init(BGParams const& bg_params, R const& arguments, bool use_defaults = true)
{
    parameters<T> pin;
    for (std::vector<std::string>::const_iterator it = boost::begin(arguments);
        it != boost::end(arguments); it++)
    {
        pin.params.push_back(pj_mkparam<T>(*it));
    }

    // maybe TODO: handle "init" parameter
    /* check if +init present */
    //std::string sinit;
    //if (pj_param_s(pin.params, "init", sinit))
    //{
    //    //if (!(curr = get_init(&arguments, curr, sinit)))
    //}

    // find projection -> implemented in projection factory
    pin.name = pj_get_param_s(pin.params, "proj");
    // exception thrown in projection<>
    // TODO: consider throwing here both projection_unknown_id_exception and
    // projection_not_named_exception in order to throw before other exceptions
    //if (pin.name.empty())
    //{ BOOST_THROW_EXCEPTION( projection_not_named_exception() ); }

    // set defaults, unless inhibited
    // GL-Addition, if use_defaults is false then defaults are ignored
    if (use_defaults && ! pj_get_param_b(pin.params, "no_defs"))
    {
        // proj4 gets defaults from "proj_def.dat", file of 94/02/23 with a few defaults.
        // Here manually
        pj_push_defaults(bg_params, pin);
        //curr = get_defaults(&arguments, curr, name);
    }

    /* allocate projection structure */
    // done by BGParams constructor:
    // pin.is_latlong = 0;
    // pin.is_geocent = 0;
    // pin.long_wrap_center = 0.0;
    // pin.long_wrap_center = 0.0;
    pin.is_long_wrap_set = false;

    /* set datum parameters */
    pj_datum_set(bg_params, pin.params, pin);

    /* set ellipsoid/sphere parameters */
    pj_ell_set(bg_params, pin.params, pin.a, pin.es);

    pin.a_orig = pin.a;
    pin.es_orig = pin.es;

    pin.e = sqrt(pin.es);
    pin.ra = 1. / pin.a;
    pin.one_es = 1. - pin.es;
    if (pin.one_es == 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(error_eccentricity_is_one) );
    }
    pin.rone_es = 1./pin.one_es;

    /* Now that we have ellipse information check for WGS84 datum */
    if( pin.datum_type == datum_3param
        && pin.datum_params[0] == 0.0
        && pin.datum_params[1] == 0.0
        && pin.datum_params[2] == 0.0
        && pin.a == 6378137.0
        && geometry::math::abs(pin.es - 0.006694379990) < 0.000000000050 )/*WGS84/GRS80*/
    {
        pin.datum_type = datum_wgs84;
    }

    /* set pin.geoc coordinate system */
    pin.geoc = (pin.es && pj_get_param_b(pin.params, "geoc"));

    /* over-ranging flag */
    pin.over = pj_get_param_b(pin.params, "over");

    /* longitude center for wrapping */
    pin.is_long_wrap_set = pj_param_r(pin.params, "lon_wrap", pin.long_wrap_center);

    /* central meridian */
    pin.lam0 = pj_get_param_r(pin.params, "lon_0");

    /* central latitude */
    pin.phi0 = pj_get_param_r(pin.params, "lat_0");

    /* false easting and northing */
    pin.x0 = pj_get_param_f(pin.params, "x_0");
    pin.y0 = pj_get_param_f(pin.params, "y_0");

    /* general scaling factor */
    if (pj_param_f(pin.params, "k_0", pin.k0)) {
        /* empty */
    } else if (pj_param_f(pin.params, "k", pin.k0)) {
        /* empty */
    } else
        pin.k0 = 1.;
    if (pin.k0 <= 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(error_k_less_than_zero) );
    }

    /* set units */
    pj_init_units(pin.params, "units", "to_meter",
                  pin.to_meter, pin.fr_meter, 1., 1.);
    pj_init_units(pin.params, "vunits", "vto_meter",
                  pin.vto_meter, pin.vfr_meter, pin.to_meter, pin.fr_meter);

    /* prime meridian */
    std::string pm = pj_get_param_s(pin.params, "pm");
    if (! pm.empty())
    {
        std::string value;

        int n = sizeof(pj_prime_meridians) / sizeof(pj_prime_meridians[0]);
        for (int i = 0; i < n ; i++)
        {
            if(pj_prime_meridians[i].id == pm)
            {
                value = pj_prime_meridians[i].defn;
                break;
            }
        }

        dms_parser<T, true> parser;

        // TODO: Is this try-catch needed?
        // In other cases the bad_str_cast exception is simply thrown
        BOOST_TRY
        {
            if (value.empty()) {
                pin.from_greenwich = parser.apply(pm).angle();
            } else {
                pin.from_greenwich = parser.apply(value).angle();
            }
        }
        BOOST_CATCH(geometry::bad_str_cast const&)
        {
            BOOST_THROW_EXCEPTION( projection_exception(error_unknown_prime_meridian) );
        }
        BOOST_CATCH_END
    }
    else
    {
        pin.from_greenwich = 0.0;
    }

    return pin;
}

/************************************************************************/
/*                            pj_init_plus()                            */
/*                                                                      */
/*      Same as pj_init() except it takes one argument string with      */
/*      individual arguments preceeded by '+', such as "+proj=utm       */
/*      +zone=11 +ellps=WGS84".                                         */
/************************************************************************/
template <typename T, typename BGParams>
inline parameters<T> pj_init_plus(BGParams const& bg_params, std::string const& definition, bool use_defaults = true)
{
    const char* sep = " +";

    /* split into arguments based on '+' and trim white space */

    // boost::split splits on one character, here it should be on " +", so implementation below
    // todo: put in different routine or sort out
    std::vector<std::string> arguments;
    std::string def = boost::trim_copy(definition);
    boost::trim_left_if(def, boost::is_any_of(sep));

    std::string::size_type loc = def.find(sep);
    while (loc != std::string::npos)
    {
        std::string par = def.substr(0, loc);
        boost::trim(par);
        if (! par.empty())
        {
            arguments.push_back(par);
        }

        def.erase(0, loc);
        boost::trim_left_if(def, boost::is_any_of(sep));
        loc = def.find(sep);
    }

    if (! def.empty())
    {
        arguments.push_back(def);
    }

    /*boost::split(arguments, definition, boost::is_any_of("+"));
    for (std::vector<std::string>::iterator it = arguments.begin(); it != arguments.end(); it++)
    {
        boost::trim(*it);
    }*/
    return pj_init<T>(bg_params, arguments, use_defaults);
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_INIT_HPP


/*!
@file
Internal header to break cyclic dependencies.

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

#ifndef BOOST_HANA_ORDERABLE_DETAIL_ORDERABLE_FWD_HPP
#define BOOST_HANA_ORDERABLE_DETAIL_ORDERABLE_FWD_HPP

#include <boost/hana/core/typeclass.hpp>
#include <boost/hana/detail/constexpr.hpp>

namespace boost { namespace hana {
    namespace orderable_detail { namespace operators { struct enable { }; }}

    //! @ingroup group-typeclasses
    //! The `Orderable` type class is used for data types defining a
    //! [strict weak ordering](http://en.wikipedia.org/wiki/Strict_weak_ordering).
    //!
    //! @anchor strict_weak_ordering
    //! ### Laws
    //! `less` must define a strict weak ordering. Formally, let
    //!
    //! @code
    //!     x ~ y if and only if !(x < y) && !(y < x)
    //! @endcode
    //!
    //! be the incomparability relation. Then, for all `a`, `b`, `c` of an
    //! orderable data type,
    //!
    //! @code
    //!     !(a < a)                     // Irreflexivity
    //!     if a < b then !(b < a)       // Asymmetry
    //!     if a < b && b < c then a < c // Transitivity
    //!     if a ~ b && b ~ c then a ~ c // Transitivity of incomparability
    //! @endcode
    struct Orderable {
        BOOST_HANA_BINARY_TYPECLASS(Orderable);
        struct less_mcd;
        struct laws;
        using operators = orderable_detail::operators::enable;
    };

    //! Returns a `Logical` representing whether `x` is less than `y`.
    //! @relates Orderable
    //!
    //! ### Example
    //! @snippet example/orderable/less.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto less = [](auto x, auto y) {
        return Orderable::instance<
            datatype_t<decltype(x)>, datatype_t<decltype(y)>
        >::less_impl(x, y);
    };

    //! Returns a `Logical` representing whether `x` is less than or
    //! equal to `y`.
    //! @relates Orderable
    //!
    //! ### Example
    //! @snippet example/orderable/less_equal.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto less_equal = [](auto x, auto y) {
        return Orderable::instance<
            datatype_t<decltype(x)>, datatype_t<decltype(y)>
        >::less_equal_impl(x, y);
    };

    //! Returns a `Logical` representing whether `x` is greater than `y`.
    //! @relates Orderable
    //!
    //! ### Example
    //! @snippet example/orderable/greater.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto greater = [](auto x, auto y) {
        return Orderable::instance<
            datatype_t<decltype(x)>, datatype_t<decltype(y)>
        >::greater_impl(x, y);
    };

    //! Returns a `Logical` representing whether `x` is greater than or
    //! equal to `y`.
    //! @relates Orderable
    //!
    //! ### Example
    //! @snippet example/orderable/greater_equal.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto greater_equal = [](auto x, auto y) {
        return Orderable::instance<
            datatype_t<decltype(x)>, datatype_t<decltype(y)>
        >::greater_equal_impl(x, y);
    };

    //! Returns the smallest of its arguments according to the `less` ordering.
    //! @relates Orderable
    //!
    //! ### Example
    //! @snippet example/orderable/min.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto min = [](auto x, auto y) {
        return Orderable::instance<
            datatype_t<decltype(x)>, datatype_t<decltype(y)>
        >::min_impl(x, y);
    };

    //! Returns the greatest of its arguments according to the `less` ordering.
    //! @relates Orderable
    //!
    //! ### Example
    //! @snippet example/orderable/max.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto max = [](auto x, auto y) {
        return Orderable::instance<
            datatype_t<decltype(x)>, datatype_t<decltype(y)>
        >::max_impl(x, y);
    };

    //! Returns a function performing `less` after applying a transformation
    //! to both arguments.
    //! @relates Orderable
    //!
    //! This is not a method of the `Orderable` type class, but just a
    //! convenience function provided with it. Also note that
    //! @code
    //!     ordering(f) == less ^on^ f
    //! @endcode
    //!
    //! ### Example
    //! @snippet example/orderable/ordering.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto ordering = [](auto f) {
        return [=](auto x, auto y) {
            return less(f(x), f(y));
        };
    };

    namespace orderable_detail { namespace operators {
        //! Equivalent to `less`.
        //! @relates boost::hana::Orderable
        template <typename T, typename U>
        constexpr auto operator<(T t, U u)
        { return less(t, u); }

        //! Equivalent to `less_equal`.
        //! @relates boost::hana::Orderable
        template <typename T, typename U>
        constexpr auto operator<=(T t, U u)
        { return less_equal(t, u); }

        //! Equivalent to `greater`.
        //! @relates boost::hana::Orderable
        template <typename T, typename U>
        constexpr auto operator>(T t, U u)
        { return greater(t, u); }

        //! Equivalent to `greater_equal`.
        //! @relates boost::hana::Orderable
        template <typename T, typename U>
        constexpr auto operator>=(T t, U u)
        { return greater_equal(t, u); }
    }}
}} // end namespace boost::hana

#endif // !BOOST_HANA_ORDERABLE_DETAIL_ORDERABLE_FWD_HPP


#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/scope_exit.hpp>

#include <amgcl/io/binary.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/mpi/make_solver.hpp>
#include <amgcl/mpi/cpr.hpp>
#include <amgcl/mpi/amg.hpp>
#include <amgcl/mpi/coarsening/runtime.hpp>
#include <amgcl/mpi/relaxation/runtime.hpp>
#include <amgcl/mpi/relaxation/as_preconditioner.hpp>
#include <amgcl/mpi/direct_solver/runtime.hpp>
#include <amgcl/mpi/partition/runtime.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/profiler.hpp>

namespace amgcl {
    profiler<> prof;
}

using amgcl::prof;
using amgcl::precondition;

//---------------------------------------------------------------------------
ptrdiff_t read_matrix_market(
        amgcl::mpi::communicator comm,
        const std::string &A_file, const std::string &rhs_file, int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<double>    &val,
        std::vector<double>    &rhs)
{
    amgcl::io::mm_reader A_mm(A_file);
    ptrdiff_t n = A_mm.rows();

    ptrdiff_t chunk = (n + comm.size - 1) / comm.size;
    if (chunk % block_size != 0) {
        chunk += block_size - chunk % block_size;
    }

    ptrdiff_t row_beg = std::min(n, chunk * comm.rank);
    ptrdiff_t row_end = std::min(n, row_beg + chunk);

    chunk = row_end - row_beg;

    A_mm(ptr, col, val, row_beg, row_end);

    if (rhs_file.empty()) {
        rhs.resize(chunk);
        std::fill(rhs.begin(), rhs.end(), 1.0);
    } else {
        amgcl::io::mm_reader rhs_mm(rhs_file);
        rhs_mm(rhs, row_beg, row_end);
    }

    return chunk;
}

//---------------------------------------------------------------------------
ptrdiff_t read_binary(
        amgcl::mpi::communicator comm,
        const std::string &A_file, const std::string &rhs_file, int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<double>    &val,
        std::vector<double>    &rhs)
{
    ptrdiff_t n = amgcl::io::crs_size<ptrdiff_t>(A_file);

    ptrdiff_t chunk = (n + comm.size - 1) / comm.size;
    if (chunk % block_size != 0) {
        chunk += block_size - chunk % block_size;
    }

    ptrdiff_t row_beg = std::min(n, chunk * comm.rank);
    ptrdiff_t row_end = std::min(n, row_beg + chunk);

    chunk = row_end - row_beg;

    amgcl::io::read_crs(A_file, n, ptr, col, val, row_beg, row_end);

    if (rhs_file.empty()) {
        rhs.resize(chunk);
        std::fill(rhs.begin(), rhs.end(), 1.0);
    } else {
        ptrdiff_t rows, cols;
        amgcl::io::read_dense(rhs_file, rows, cols, rhs, row_beg, row_end);
    }

    return chunk;
}

//---------------------------------------------------------------------------
template <class Backend, class Matrix>
std::shared_ptr< amgcl::mpi::distributed_matrix<Backend> >
partition(amgcl::mpi::communicator comm, const Matrix &Astrip,
        std::vector<double> &rhs, const typename Backend::params &bprm,
        amgcl::runtime::mpi::partition::type ptype, int block_size = 1)
{
    typedef amgcl::mpi::distributed_matrix<Backend> DMatrix;

    using amgcl::prof;

    auto A = std::make_shared<DMatrix>(comm, Astrip);

    if (comm.size == 1 || ptype == amgcl::runtime::mpi::partition::merge)
        return A;

    prof.tic("partition");
    boost::property_tree::ptree prm;
    prm.put("type", ptype);
    prm.put("shrink_ratio", 1);
    amgcl::runtime::mpi::partition::wrapper<Backend> part(prm);

    auto I = part(*A, block_size);
    auto J = transpose(*I);
    A = product(*J, *product(*A, *I));

    std::vector<double> new_rhs(J->loc_rows());

    J->move_to_backend(bprm);

    amgcl::backend::spmv(1, *J, rhs, 0, new_rhs);
    rhs.swap(new_rhs);
    prof.toc("partition");

    return A;
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
    BOOST_SCOPE_EXIT(void) {
        MPI_Finalize();
    } BOOST_SCOPE_EXIT_END

    amgcl::mpi::communicator comm(MPI_COMM_WORLD);

    if (comm.rank == 0)
        std::cout << "World size: " << comm.size << std::endl;

    using amgcl::prof;

    // Read configuration from command line
    namespace po = boost::program_options;
    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        ("matrix,A",
         po::value<std::string>(),
         "System matrix in the MatrixMarket format. "
         "When not specified, a Poisson problem in 3D unit cube is assembled. "
        )
        (
         "rhs,f",
         po::value<std::string>()->default_value(""),
         "The RHS vector in the MatrixMarket format. "
         "When omitted, a vector of ones is used by default. "
         "Should only be provided together with a system matrix. "
        )
        (
         "binary,B",
         po::bool_switch()->default_value(false),
         "When specified, treat input files as binary instead of as MatrixMarket. "
         "It is assumed the files were converted to binary format with mm2bin utility. "
        )
        (
         "block-size,b",
         po::value<int>()->default_value(1),
         "The block size of the system matrix. "
        )
        (
         "partitioner,r",
         po::value<amgcl::runtime::mpi::partition::type>()->default_value(
#if defined(AMGCL_HAVE_SCOTCH)
             amgcl::runtime::mpi::partition::ptscotch
#elif defined(AMGCL_HAVE_PASTIX)
             amgcl::runtime::mpi::partition::parmetis
#else
             amgcl::runtime::mpi::partition::merge
#endif
             ),
         "Repartition the system matrix"
        )
        ("prm-file,P",
         po::value<std::string>(),
         "Parameter file in json format. "
        )
        (
         "prm,p",
         po::value< std::vector<std::string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        if (comm.rank == 0) std::cout << desc << std::endl;
        return 0;
    }

    boost::property_tree::ptree prm;
    if (vm.count("prm-file")) {
        read_json(vm["prm-file"].as<std::string>(), prm);
    }

    if (vm.count("prm")) {
        for(const std::string &v : vm["prm"].as<std::vector<std::string> >()) {
            amgcl::put(prm, v);
        }
    }

    ptrdiff_t n;
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<double>    val;
    std::vector<double>    rhs;

    int block_size = vm["block-size"].as<int>();
    prm.put("precond.block_size", block_size);

    prof.tic("read");
    if (vm["binary"].as<bool>()) {
        n = read_binary(comm,
                vm["matrix"].as<std::string>(),
                vm["rhs"].as<std::string>(),
                block_size, ptr, col, val, rhs);
    } else {
        n = read_matrix_market(comm,
                vm["matrix"].as<std::string>(),
                vm["rhs"].as<std::string>(),
                block_size, ptr, col, val, rhs);
    }
    prof.toc("read");

    typedef amgcl::backend::builtin<double> Backend;

    auto A = partition<Backend>(comm,
            std::tie(n, ptr, col, val), rhs, Backend::params(),
            vm["partitioner"].as<amgcl::runtime::mpi::partition::type>(),
            block_size);

    prof.tic("setup");
    typedef
        amgcl::mpi::make_solver<
            amgcl::mpi::cpr<
                amgcl::mpi::amg<
                    Backend,
                    amgcl::runtime::mpi::coarsening::wrapper<Backend>,
                    amgcl::runtime::mpi::relaxation::wrapper<Backend>,
                    amgcl::runtime::mpi::direct::solver<double>,
                    amgcl::runtime::mpi::partition::wrapper<Backend>
                    >,
                amgcl::mpi::relaxation::as_preconditioner<
                    amgcl::runtime::mpi::relaxation::wrapper<Backend>
                    >
                >,
            amgcl::runtime::solver::wrapper
            >
        Solver;

    Solver solve(comm, A, prm);
    prof.toc("setup");

    if (comm.rank == 0)
        std::cout << solve << std::endl;

    std::vector<double> x(rhs.size(), 0.0);

    prof.tic("solve");
    size_t iters;
    double error;
    std::tie(iters, error) = solve(rhs, x);
    prof.toc("solve");

    if (comm.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << error << std::endl
            << prof << std::endl;
    }
}


// Modified from boost/math/tools/rational.hpp (as of Boost 1.61)
//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are 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 NAM_EVALUATE_HPP
#define NAM_EVALUATE_HPP

#include <boost/mpl/int.hpp>
namespace mpl = boost::mpl;
#include "polynomial_horner2_20.hpp"

//
// Forward declaration to keep two phase lookup happy:
//
template <class T, class U>
constexpr U evaluate_polynomial(const T* poly, U const& z, std::size_t count);

namespace detail {
    // Fallback if none of the unrolled ones (up to size 20) suffice
    template <class T, class V, class Tag>
    constexpr V evaluate_polynomial_c_imp(const T* a, const V& val, const Tag*) {
       return evaluate_polynomial(a, val, Tag::value);
    }
} // namespace detail

//
// Polynomial evaluation with runtime size.
// This requires a for-loop which may be more expensive than
// the loop expanded versions above:
//
template <class T, class U>
constexpr U evaluate_polynomial(const T* poly, U const& z, std::size_t count) {
   assert(count > 0);
   U sum = static_cast<U>(poly[count - 1]);
   for(int i = static_cast<int>(count) - 2; i >= 0; --i) {
      sum *= z;
      sum += static_cast<U>(poly[i]);
   }
   return sum;
}

//
// Compile time sized polynomials, just inline forwarders to the
// implementations above:
//
template <std::size_t N, class T, class V>
constexpr V evaluate_polynomial(const T(&a)[N], const V& val) {
   typedef mpl::int_<N> tag_type;
   return detail::evaluate_polynomial_c_imp(static_cast<const T*>(a), val, static_cast<tag_type const*>(0));
}

#endif // NAM_EVALUATE_HPP


/*
 *  Jan Totz <jantotz@itp.tu-berlin.de>
 */

// C++11 & Boost libraries, OpenMP
#include <boost/program_options.hpp>			// command line parsing
#include <boost/property_tree/ptree.hpp>		// support structure for ini file writing
#include <boost/property_tree/ini_parser.hpp>	// read/write ini file
#include <iostream>								// cout, cerr
#include <fstream>								// ofstream, ifstream
#include <chrono>								// chrono::high_resolution_clock::now()
#include <random>								// default_random_engine, normal_distribution<double>, uniform_real_distribution
#include <stdlib.h>								// system(), NULL, EXIT_FAILURE
#include <cstdlib>								// getenv()
#include <unistd.h>								// readlink()
#include <omp.h>								// omp_get_max_threads(), omp_set_num_threads()

#include "SWC_CPU_solver.hpp"					// struct params
#include "safe.hpp"								// saving
#include "safe_coupling.hpp"					// save phi coupling
#include "models.hpp"							// reaction models
#include "coupling.hpp"							// coupling schemes

// namespaces
namespace po = boost::program_options;
namespace pt = boost::property_tree;
using namespace std;

inline double posi_fmod(double i, double n){ return fmod((fmod(i,n)+n),n); }
inline int posi_imod(int i, int n){ return (i % n + n) % n; }

// custom parser for arrays in inifile, works for arrays of any kind: string, int, float, double...
template<typename T>
std::vector<T> to_array(const std::string& s){
	std::vector<T> result;
	std::stringstream ss(s);
	std::string item;
	while(std::getline(ss, item, ',')) result.push_back(boost::lexical_cast<T>(item));
	return result;
}

std::string get_executable_path(){

	char buff[1024];
	ssize_t len = ::readlink("/proc/self/exe", buff, sizeof(buff)-1);
	string pthexe;

	// get pthfnexe
	if(len != -1){				// success
		buff[len] = '\0';
		pthexe = std::string(buff);
	}else{							// error
		printf("Error! Could not find path to executable!"); exit(EXIT_FAILURE);
	}

	// remove filename from pathexe
	const size_t last_slash_idx = pthexe.rfind('/');
	if (std::string::npos != last_slash_idx){
		pthexe = pthexe.substr(0, last_slash_idx+1);
	}

	return pthexe;
}

std::string replaceEnvironmentVariables(std::string input){

	size_t pos1 = input.find("$", 0);
	if(pos1 != std::string::npos){										// "$" was found in string
		size_t pos2 = input.find("/", pos1);
		string envVarString = input.substr(pos1+1,pos1+pos2-1).c_str();
		string envVarValue;
		if( getenv(envVarString.c_str()) != NULL){
			envVarValue = getenv(envVarString.c_str());
		}
		return input.replace(pos1,pos1+pos2,envVarValue);
	}else{
		return input;
	}
}

std::string RealArrayToString(Real *array, int n){

	std::string result="";
	std::ostringstream strs;

	for(int i=0; i<n-1; i++) strs << array[i] << ",";
	strs << array[n-1];

	return strs.str();

}

std::string RealVectorToString(vector<Real> vec){

	std::string result="";
	std::ostringstream strs;

	for(int i=0; i<(int)vec.size()-1; i++) strs << vec[i] << ",";
	strs << vec[vec.size()-1];

	return strs.str();
}

// read command line parameters with boost
void main_cmdln_params(int &ac, char *av[], string &pthexe, string &pthini, string &ini, params &p){

	int errorflag=0;			// no error: 0, error: 1
	string spatial_settings1_string;

	try{
		po::options_description desc("Allowed options");
		desc.add_options()
			("help", "produce help message")
			("pthini", po::value<string>(&pthini)->default_value(pthexe+"../ini"), "path for ini")
			("pthout", po::value<string>(&p.pthout)->default_value("-1"), "path for output")
			("ini", po::value<string>(&ini)->default_value("chimera_spiral_zbke2k.ini"), "name of inifile")
			("spatial_settings1", po::value<string>(&spatial_settings1_string)->default_value("-1"), "excitable array, no whitespace!")
			("coupling_coeffs", po::value<string>(&p.coupling_coeffs_string)->default_value("-1"), "coupling/diffusion coefficents, no whitespace!")
			("uSeed", po::value<int>(&p.uSeed)->default_value(-1), "uSeed")
			("pSeed", po::value<int>(&p.pSeed)->default_value(-1), "seed for parameter heterogeneity")
			("nx", po::value<int>(&p.nx)->default_value(-1), "nx")
			("ny", po::value<int>(&p.ny)->default_value(-1), "ny")
			("diffusionChoice", po::value<int>(&p.diffusionChoice)->default_value(-1), "diffusionChoice")
			("hehoflag", po::value<int>(&p.hehoflag)->default_value(-1), "hehoflag")
			("reactionModel", po::value<int>(&p.reactionModel)->default_value(-1), "reactionModel")
			("ChimeraCutOffRange", po::value<int>(&p.ChimeraCutOffRange)->default_value(-1), "ChimeraCutOffRange")
			("stepsEnd", po::value<size_t>(&p.stepsEnd)->default_value(0), "stepsEnd")
			("stepsSaveState", po::value<size_t>(&p.stepsSaveState)->default_value(0), "stepsSaveState")
			("stepsSaveStateOffset", po::value<size_t>(&p.stepsSaveStateOffset)->default_value(0), "stepsSaveStateOffset")
			("delayTimeMax", po::value<float>(&p.delayTimeMax)->default_value(-1.0f), "delayTimeMax")
			("ChimeraK", po::value<Real>(&p.ChimeraK)->default_value(-1.0), "ChimeraK")
			("ChimeraKappa", po::value<Real>(&p.ChimeraKappa)->default_value(-1.0), "ChimeraKappa")
			("dt", po::value<Real>(&p.dt)->default_value(-1.0), "dt")
			("dx", po::value<Real>(&p.dx)->default_value(-1.0), "dx")
			("dy", po::value<Real>(&p.dy)->default_value(-1.0), "dy")
			;

		po::variables_map vm;
		po::store(po::parse_command_line(ac, av, desc), vm);
		po::notify(vm);

		if(vm.count("help")){ cout << desc << "\n"; exit(EXIT_SUCCESS); }
	}
	catch(exception& e){
		cerr << "error: " << e.what() << "\n";
		errorflag=1;
	}
	catch(...){
		cerr << "Exception of unknown type!\n";
		errorflag=1;
	}

	if(errorflag){
		cout<<"--- program shutdown due to error at cmdln_params ---"<<endl;
		exit(EXIT_FAILURE);
	}

	// convert string to vector
	p.spatial_settings1 = to_array<Real>(spatial_settings1_string);

}

// resize phase inside function
void get_limitCycleData(vector<Real>& phases, int nc, string pthfn_lc){

	ifstream fs;
	fs.open(pthfn_lc);
	string temp_line;
	int ni=0;
	while(getline(fs,temp_line)){
		if(!temp_line.empty()) ni++;
	}
	fs.clear();
	fs.seekg(fs.beg);
	//~ cout << "ni: " << ni << endl;		// DEBUG
	phases.resize(ni*nc);
	for(int i=0; i<ni; i++){
	for(int c=0; c<nc; c++){
		fs>>phases[c+i*nc];
	}}
	fs.close();
}

// preliminary for all components, needs extra functions
void initialCondition(Real *cfield, params &p, modelparams &m){

	int nc = p.ncomponents;

	if(p.ic=="uniform_noise"){
		printf("ic: %s\n",p.ic.c_str());

		// random numbers from uniform distribution
		default_random_engine generator(p.uSeed);
		uniform_real_distribution<Real> udistribution(p.uMin,p.uMax);

		for(int c=0; c<p.ncomponents; c++){
		for(int i=0; i<p.n; i++){
			cfield[i+c*p.n]=udistribution(generator);
		}}

	}else if(p.ic=="homo"){										// homogeneous background concentration
		printf("ic: %s\n",p.ic.c_str());

		for(int c=0; c<p.ncomponents; c++){
		for(int i=0; i<p.n; i++){
			cfield[i+c*p.n]=m.model_phases[2*nc+c];
		}}

	}else if(p.ic=="phase_proto_spiral"){
		switch(p.spaceDim){
			case 2:  printf("ic: phase proto spiral wave\n"); break;
			default: printf("Error: spaceDim=%d is insufficient for phase proto spiral ic, use 2 instead!\n",p.spaceDim); exit(EXIT_FAILURE); break;
		}

		// spatial_settings1 overloaded for options: spatial_settings1 = {phasex0,phasey0,nArms,phi0,chirality}
		auto result1 = std::minmax_element(p.spatial_settings1.begin(), p.spatial_settings1.end());
		float minmaxdiff=*result1.second - *result1.first;
		if(minmaxdiff<1e-5){ // default settings, otherwise: ini settings
			p.spatial_settings1={0.5,0.5,M_PI,1};
		}

		// pattern parameters
		float phasex0 = p.spatial_settings1[0]; 			// x-coords of phase singularity = spiral center
		float phasey0 = p.spatial_settings1[1];				// y-coords of phase singularity = spiral center
		float phi0 = p.spatial_settings1[2];				// initial phase offset
		int chi = p.spatial_settings1[3];					// chirality

		vector<Real> phases;				// initialize empty vector
		get_limitCycleData(phases,nc,p.pthfn_lc);
		int ni=phases.size()/nc;

		for(int y = 0; y < p.ny; ++y){
		for(int x = 0; x < p.nx; ++x){
			int index = posi_fmod( atan2(y-phasey0*p.ny,chi*(x-phasex0*p.nx)) - phi0, 2.0*M_PI )/(2.0*M_PI)*(ni-1);
			for(int c=0; c<nc; c++) cfield[x+y*p.nx+c*p.n]=phases[c+index*nc];
		}}

	}else{
		cout<<"Error: ic \""<<p.ic<<"\" does not exist!" <<endl; exit(EXIT_FAILURE);
	}

}

// save params as binary float
void saveParamDistro(Real *k, params &p){

	char spbuff[100];
	sprintf(spbuff,"/paramDistro.bin");
	ofstream dataout;
	dataout.open(p.pthout+spbuff,ios::binary);

	int size=sizeof(Real);

	// save header (2 ints): nx,ny,nz
	dataout.write((char*) &p.nx, sizeof(int));
	dataout.write((char*) &p.ny, sizeof(int));

	// save data
	for(int n=0; n<p.n; n++) dataout.write((char*) &k[n], size);

	dataout.close();
}

void main_parameterDistribution(Real *hetArray, params &p, modelparams &m){

	// random numbers from gaussian distribution
	default_random_engine generator(p.pSeed);
	normal_distribution<double> p_normal_distribution(p.het1,p.pSigma);
	uniform_real_distribution<double> p_uniform_distribution(p.het1-p.pSigma,p.het1+p.pSigma);

	switch(p.hehoflag){
		case 0:			// homogeneous distribution
			cout << "main_parameterDistribution (" << p.hehoflag << "): homogeneous" << endl;
			for(int i=0; i<p.n; i++) hetArray[i]=p.het1;
			break;

		case 1:			// heterogeneous, normal distribution
			cout << "main_parameterDistribution (" << p.hehoflag << "): bounded normal distribution" << endl;
			for(int i=0; i<p.n; i++){
				float prnd=0.;
				while(prnd<p.het1-p.pSigma or prnd>p.het1+p.pSigma) prnd = p_normal_distribution(generator);			// limits for parameter p
				hetArray[i]=prnd;
			}
			break;

		case 7:				// heterogeneous, uniform distribution
			cout << "main_parameterDistribution (" << p.hehoflag << "): uniform distribution" << endl;
			for(int i=0; i<p.n; i++) hetArray[i]=p_uniform_distribution(generator);
			break;

		default:
			cout << "chosen value (" << p.hehoflag << ") for hehoflag is not implemented!" << endl; exit(1);
			break;
	}
}

// only linux!, OS-independent option: boost
void main_housekeeper(string pthout, string pthexe){

	#ifdef _WIN32

	#elif __linux__
	// remove potential old files in directories
	string cmd0 = "rm -r "+pthout+"/* 2> /dev/null";
	int systemRet = system(const_cast<char*>(cmd0.c_str()));
	if(systemRet == -1){cout<<"rm system command failed"<<endl;}

	// create directory for output
	cmd0 = "mkdir -p "+pthout+" "+pthout+"/states";
	systemRet = system(const_cast<char*>(cmd0.c_str()));
	if(systemRet == -1){cout<<"mkdir system command failed"<<endl;}

	// copy current source code into output directory
	cmd0 = "mkdir -p "+pthout+" "+pthout+"/source_code";
	cout << cmd0 << endl;
	systemRet = system(const_cast<char*>(cmd0.c_str()));
	if(systemRet == -1){cout<<"mkdir system command failed"<<endl;}
	// cp .{hpp,cpp} is not supported in sh
	cmd0 = "cd "+pthexe+" && cp *.cpp *.hpp "+pthout+"/source_code/";
	systemRet = system(const_cast<char*>(cmd0.c_str()));
	if(systemRet == -1){cout<<"cp system command failed"<<endl;}
	#endif

}

// strip string of unwanted chars
string stripString(string &str){

	// escaped quotation mark
	char badchars[] = "\"";

	for (unsigned int i = 0; i < strlen(badchars); ++i){
		str.erase (std::remove(str.begin(), str.end(), badchars[i]), str.end());
	}

	return str;
}

void setReactionCouplingParams(params &ip, pt::ptree pt, modelparams &m, params &pExtra){

	switch(ip.reactionModel){
		case 24:												// zbke2k
		case 2401:												// zbke2k qhet
			m.ncomponents=2;
			m.nparams=9;
			m.model_params = new Real[9] {1.0/0.11,1.7e-5,1.6e-3,0.1,1.7e-5,1.2,2.4e-4,0.7,5.25e-4};		// ooeps1, eps2, eps3, alpha, beta, gamma, mu, q, phi0
			m.model_phases = new Real[6] {0.1,0.01,0.0001,0.3,0.0,0.0};
			m.coupling_coeffs = new Real[2] {1.0/0.11,2.0};
			break;

		case 25:												// fhn
			m.ncomponents=2;
			m.nparams=2;
			m.model_params = new Real[2] {1.0/0.05,1.1};							// ooeps, a
			m.model_phases = new Real[6] {1.5,0.0,0.0,1.5,-1.1,-0.656333};			// bg = stable fixed point
			m.coupling_coeffs = new Real[2] {1.0,0.0};
			break;

		default:												// default case
			printf("Error: reactionModel not implemented. (readInifile @ line: %d)\n",__LINE__); exit(EXIT_FAILURE);
			break;
	}

	// a1) read model data from inifile
	std::vector<Real> modelParameters0 = to_array<Real>(pt.get<std::string>("model_parameters.modelParams","0"));
	std::vector<Real> coupling_coeffs0 = to_array<Real>(pt.get<std::string>("model_parameters.coupling_coeffs","0"));
	std::vector<Real> phases0 = to_array<Real>(pt.get<std::string>("model_parameters.phases","0"));

	// a2) read model.coupling_coeffs from commandline
	if(pExtra.coupling_coeffs_string!="-1") coupling_coeffs0 = to_array<Real>(pExtra.coupling_coeffs_string);

	// b) apply it; if not set, then write default values into resulting ini file
	if(modelParameters0.size()!=1 or modelParameters0[0]!=0){
		m.model_params = (Real *) malloc(modelParameters0.size()*sizeof(Real));
		for(size_t i=0; i<modelParameters0.size(); i++) m.model_params[i]=modelParameters0[i];
	}else{
		std::string s=RealArrayToString(m.model_params,m.nparams);
		pt.put<std::string>("model_parameters.modelParams",s);
	}

	if(coupling_coeffs0.size()!=1 or coupling_coeffs0[0]!=0){
		m.coupling_coeffs = (Real *) malloc(coupling_coeffs0.size()*sizeof(Real));
		for(size_t i=0; i<coupling_coeffs0.size(); i++) m.coupling_coeffs[i]=coupling_coeffs0[i];
	}else{
		std::string s=RealArrayToString(m.coupling_coeffs,m.ncomponents);
		pt.put<std::string>("model_parameters.coupling_coeffs",s);
	}

	if(phases0.size()!=1 or phases0[0]!=0){
		m.model_phases = (Real *) malloc(phases0.size()*sizeof(Real));
		for(size_t i=0; i<phases0.size(); i++) m.model_phases[i]=phases0[i];
	}else{
		std::string s=RealArrayToString(m.model_phases,m.ncomponents*3);
		pt.put<std::string>("model_parameters.phases",s);
	}
}

// read parameters from text file
void main_readInifile(string pthini, string pthexe, string &ini, params &ip, pt::ptree pt, modelparams &m, params &pExtra){

	// initialize variables
	string pthfnini=pthini+"/"+ini;

	// read ini
	pt::ini_parser::read_ini(pthfnini, pt);
	cout<<"\n############# reading ini file ###################"<<endl;
	cout<<pthfnini<<endl;
	ip.nx = max(pt.get<int>("numerics_space.nx",100),1);		// max: in case user input is 0 instead of 1
	ip.ny = max(pt.get<int>("numerics_space.ny",100),1);
	ip.dx = pt.get<Real>("numerics_space.dx",1.0);
	ip.dy = pt.get<Real>("numerics_space.dy",1.0);
	ip.bc = pt.get<string>("numerics_space.boundary_condition","periodic");			// periodic or neumann
	ip.diffusionChoice = pt.get<int>("numerics_space.diffusionChoice",0);
	ip.dt = pt.get<Real>("numerics_time.dt",0.01);
	ip.stepsSaveState = pt.get<size_t>("numerics_time.stepsSaveState",10000);
	ip.stepsSaveStateOffset = pt.get<size_t>("numerics_time.stepsSaveStateOffset",0);
	ip.stepsEnd = pt.get<size_t>("numerics_time.stepsEnd",100000);
	ip.couplingStartTime = pt.get<float>("numerics_time.couplingStartTime",0);
	ip.delayTimeMax = pt.get<float>("numerics_time.delayTimeMax",0);
	ip.delayStartTime = pt.get<float>("numerics_time.delayStartTime",1);
	ip.nCpuThresh = pt.get<int>("device.cpu_nCoreThresh",100);
	ip.ic = pt.get<string>("initial_condition.ic","uniform_noise");
	ip.pthfn_lc = pt.get<string>("initial_condition.pthfn_lc",pthexe+"/../lc_zbke2k_phi0_1.6e-4.dat");
	ip.uSeed = pt.get<unsigned int>("initial_condition.uSeed",1);
	ip.uMin = pt.get<Real>("initial_condition.uMin",0.);
	ip.uMax = pt.get<Real>("initial_condition.uMax",1.);
	ip.reactionModel = pt.get<int>("model_parameters.reactionModel",10);
	ip.hehoflag = pt.get<int>("model_parameters.hehoflag",0);
	ip.pSeed = pt.get<unsigned int>("model_parameters.pSeed",1);
	ip.pSigma = pt.get<float>("model_parameters.pSigma",0.01);
	ip.het1 = pt.get<Real>("model_parameters.het1",0.01);									// heterogeneity parameter
	ip.ChimeraK = pt.get<Real>("chimera.ChimeraK",0.0001);
	ip.ChimeraKappa = pt.get<Real>("chimera.ChimeraKappa",3.0);
	ip.ChimeraCutOffRange = pt.get<int>("chimera.ChimeraCutoffRange",3);
	ip.pthout = pt.get<string>("dir_management.pthout",pthexe+"/../../Simulations/test");
	ip.saveSingleFlag = pt.get<int>("dir_management.saveSingleFlag",0);
	ip.spatial_settings1 = to_array<Real>(pt.get<std::string>("initial_condition.spatial_settings1","0"));

	// assign additional cmd line values to params
	if(pExtra.uSeed>=0){ ip.uSeed = pExtra.uSeed; pt.put<int>("initial_condition.uSeed",ip.uSeed); }
	if(pExtra.pSeed>=0){ ip.pSeed = pExtra.pSeed; pt.put<int>("model_parameters.pSeed",ip.pSeed); }
	if(pExtra.diffusionChoice>=0){ ip.diffusionChoice = pExtra.diffusionChoice; pt.put<int>("numerics_space.diffusionChoice",ip.diffusionChoice); }
	if(pExtra.nx>=0){ ip.nx = pExtra.nx; pt.put<int>("numerics_space.nx",ip.nx); }
	if(pExtra.ny>=0){ ip.ny = pExtra.ny; pt.put<int>("numerics_space.ny",ip.ny); }
	if(pExtra.dx>=0){ ip.dx = pExtra.dx; pt.put<Real>("numerics_space.dx",ip.dx); }
	if(pExtra.dy>=0){ ip.dy = pExtra.dy; pt.put<Real>("numerics_space.dy",ip.dy); }
	if(pExtra.dt>=0){ ip.dt = pExtra.dt; pt.put<Real>("numerics_time.dt",ip.dt); }
	if(pExtra.ChimeraCutOffRange>=0){ ip.ChimeraCutOffRange = pExtra.ChimeraCutOffRange; pt.put<int>("chimera.ChimeraCutOffRange",ip.ChimeraCutOffRange); }
	if(pExtra.ChimeraKappa>=0){ ip.ChimeraKappa = pExtra.ChimeraKappa; pt.put<Real>("chimera.ChimeraKappa",ip.ChimeraKappa); }
	if(pExtra.ChimeraK>=0){ ip.ChimeraK = pExtra.ChimeraK; pt.put<Real>("chimera.ChimeraK",ip.ChimeraK); }
	if(pExtra.hehoflag>=0){ ip.hehoflag = pExtra.hehoflag; pt.put<int>("model_parameters.hehoflag",ip.hehoflag); }
	if(pExtra.reactionModel>=0){ ip.reactionModel = pExtra.reactionModel; pt.put<int>("model_parameters.reactionModel",ip.reactionModel); }
	if(pExtra.stepsEnd>0){ ip.stepsEnd = pExtra.stepsEnd; pt.put<size_t>("numerics_time.stepsEnd",ip.stepsEnd); }
	if(pExtra.stepsSaveState>0){ ip.stepsSaveState = pExtra.stepsSaveState; pt.put<size_t>("numerics_time.stepsSaveState",ip.stepsSaveState); }
	if(pExtra.stepsSaveStateOffset>0){ ip.stepsSaveStateOffset = pExtra.stepsSaveStateOffset; pt.put<size_t>("numerics_time.stepsSaveStateOffset",ip.stepsSaveStateOffset); }
	if(pExtra.delayTimeMax>=0){ ip.delayTimeMax = pExtra.delayTimeMax; pt.put<float>("numerics_time.delayTime",ip.delayTimeMax); }
	if(pExtra.spatial_settings1[0]>=0){ ip.spatial_settings1 = pExtra.spatial_settings1; pt.put<std::string>("initial_condition.spatial_settings1",RealVectorToString(ip.spatial_settings1)); }
	if(pExtra.pthout!="-1"){ ip.pthout = pExtra.pthout; pt.put<std::string>("dir_management.pthout",ip.pthout); }

	// interpret environment variables in pthout string
	ip.pthout = replaceEnvironmentVariables(ip.pthout);
	ip.pthfn_lc = replaceEnvironmentVariables(ip.pthfn_lc);

	// recalculate total simulation time, so that no superfluous, unsaved simulation steps are performed
	ip.stepsEnd = (ip.stepsEnd/ip.stepsSaveState)*ip.stepsSaveState;
	pt.put<size_t>("numerics_time.stepsEnd",ip.stepsEnd);

	// check correctness of delay settings, if inappropriate switch to easiest case
	if(ip.diffusionChoice==6){ ip.delayFlag=1; }
	else{ ip.delayFlag=0; }

	if(ip.delayFlag==1 and ip.delayTimeMax==0.0){
		printf("\nWarning: delayTimeMax of 0.0 is not supported for diffusionChoice=6! Switching to diffusionChoice=5 (readInifile @ line: %d)\n",__LINE__);
		ip.diffusionChoice=5;
		ip.delayFlag=0;
	}
	pt.put<int>("numerics_space.diffusionChoice",ip.diffusionChoice);

	// input checks
	if(ip.delayFlag){
		if(ip.delayStartTime<ip.delayTimeMax and ip.couplingStartTime<ip.delayTimeMax){
			cout << "\nWarning: delayTimeMax must be <= delayStartTime or couplingStartTime! Setting delayStartTime = delayTimeMin! Correcting...\n" << endl;
			ip.delayStartTime = ip.delayTimeMax;
		}
	}

	// find the space dimension automatically
	ip.spaceDim=2;
	if(ip.nx==1) ip.spaceDim--;
	if(ip.ny==1) ip.spaceDim--;

	#ifdef DOUBLE
		cout << "dataytpe mode: DOUBLE" << endl;
	#else
		cout << "dataytpe mode: FLOAT" << endl;
	#endif

	setReactionCouplingParams(ip,pt,m,pExtra);
	ip.ncomponents=m.ncomponents;

	ip.delayStepsMax=ip.delayTimeMax/ip.dt*!!ip.delayFlag;
	ip.delayStartSteps=ip.delayStartTime/ip.dt*!!ip.delayFlag;
	ip.stepsCouplingStart=ip.couplingStartTime/ip.dt;
	ip.pthout=stripString(ip.pthout)+"_cpu";
	pt.put<std::string>("dir_management.pthout",ip.pthout);
	main_housekeeper(ip.pthout,pthexe);
	ip.bc=stripString(ip.bc);
	ip.ic=stripString(ip.ic);
	ip.pthfn_lc=stripString(ip.pthfn_lc);
	ip.n=ip.nx*ip.ny;					// total number of cells
	pt.put<int>("numerics_space.n",ip.n);

	string iniOutString=ip.pthout+"/used0.ini";
	cout<<"output inifile: "<<iniOutString<<endl;
	pt::ini_parser::write_ini(iniOutString,pt);

	// terminal output
	cout<<"spaceDim: "<<ip.spaceDim<<endl;
	cout<<"boundary condition: "<<ip.bc<<endl;
	printf("nx: %d, ny: %d = lx: %.2f, ly: %.2f\n",ip.nx,ip.ny,ip.nx*ip.dx,ip.ny*ip.dy);
	printf("dx: %.2f, dy: %.2f\n",ip.dx,ip.dy);
	cout<<"dt: "<<ip.dt<<endl;
	cout<<"reaction model: "<<ip.reactionModel<<endl;
	cout<<"stepsSaveState: "<<ip.stepsSaveState<<" steps = "<<ip.stepsSaveState*ip.dt<<" time units"<<endl;
	cout<<"stepsEnd: "<<ip.stepsEnd<<" steps = "<<ip.stepsEnd*ip.dt<<" time units"<<endl;
	cout<<"pthout: "<<ip.pthout<<endl;
	cout<<"##############################################\n"<<endl;

}

// Courant-Friedrichs-Levy (CFL) criterion
void calcCFL(params &p, Real *k, modelparams &m){

	if(p.diffusionChoice==0 || p.diffusionChoice==1 || p.diffusionChoice==2){
		Real kmax = *max_element(k,k+p.n);
		Real diffmax = *max_element(m.coupling_coeffs,m.coupling_coeffs+m.ncomponents);
		if(p.hehoflag) diffmax=kmax;
		Real cfl=2.0*p.spaceDim*p.dt*diffmax/(p.dx*p.dx);

		cout<<"CFL: "<< cfl << endl;
		if(cfl>0.9) { cout<<"CFL violated! "<<cfl<<">0.9 (conservative)."<<endl; exit(1); }
	}
}

Real kernelfunction(int i, int j, int i0, int j0, params &p){

	Real value=0.0;

	// exponential decay
	switch(p.spaceDim){
		case 2:
			value=p.dx*p.dy*exp(-sqrt((i-i0)*(i-i0)*p.dx*p.dx+(j-j0)*(j-j0)*p.dy*p.dy)/p.ChimeraKappa);
			break;
	}

	return value;
}

void create_kernel_and_rescale(params &p, Real* &kernel, modelparams &m){

	switch(p.diffusionChoice){
		// nonlocal chimera coupling: Exp(-d/kappa), d = euclidean distance
		case 5:
		case 6:
			{
				int i=0, j=0, i0=0, j0=0;
				p.kdia=2*p.ChimeraCutOffRange+1;
				p.kradius = p.ChimeraCutOffRange;
				p.ksum=0.0;
				switch(p.spaceDim){

					case 2:
						p.klen=pow(p.kdia,2);
						i0=p.ChimeraCutOffRange; j0=i0;
						kernel = new Real[p.klen];
						for(i=0; i<p.kdia; i++){
						for(j=0; j<p.kdia; j++){
							kernel[i+j*p.kdia] = kernelfunction(i,j,i0,j0,p);
						}}
						for(i=0; i<p.klen; i++) kernel[i] *= p.ChimeraK*p.dt;
						break;

				}
				// save kernel in binary data format for later
				std::ofstream dataout;
				dataout.open(p.pthout+"/coupling_kernel.bin",std::ios::binary);
				for(int i=0; i<p.klen; i++) dataout.write((char*) &(kernel[i]), sizeof(Real));
				dataout.close();
				for(int i=0; i<p.klen; i++) p.ksum += kernel[i];

			}
			break;

		// rescaling is accounted for in coupling
		case 7:			// no coupling
			break;
		default: printf("Unknown value for diffusionChoice (create_kernel_and_rescale()).\n"); exit(EXIT_FAILURE); break;
	}

}

// c,cnew pointers are passed by reference, because they are changed in the function!
void dynamics(Real *&c, Real *&cnew, Real *c0, Real *kernel, Real *k, params &p, modelparams &m, size_t step, Real* feedback){

	switch(p.delayFlag){
		case 0:											// no delay
			reaction(c,cnew,k,p);												// reaction part, see models.cpp
			coupling(c,cnew,c0,k,p,m,kernel,step,feedback);						// spatial coupling, see coupling.cpp
			swap(c,cnew);														// update position values = pointer swap
			break;

		case 1:											// with delay
			reaction(c,cnew,k,p);												// reaction part, see models.cpp
			coupling(c,cnew,c0,k,p,m,kernel,step,feedback);						// spatial coupling, see coupling.cpp

			// pointer position iteration as update
			c = c0 + ((step+1) % (p.delayStepsMax+1))*p.n*p.ncomponents;
			cnew = c0 + ((step+2) % (p.delayStepsMax+1))*p.n*p.ncomponents;
			break;

		default: printf("Unknown value for delayFlag (rd_dynamics).\n"); exit(EXIT_FAILURE); break;
	}
}

void solverCPU_2d(Real *c, Real *cnew, Real *k, params &p, modelparams &m, Real* kernel, Real* c0, Real* feedback){

	if(p.delayFlag){
		for(int i=0; i<p.n*p.ncomponents; i++) c0[i] = c[i];					// set initial conditions
		// init array pointers for first step
		c = c0;
		cnew = c0+p.n*p.ncomponents;
	}

	// init containers for analysis; can not be done conditionally ;(
	int nSaveStates=100;
	Safe safe(p,nSaveStates);
	int useSafeCouplingQ = 0;
	if(p.reactionModel==24 or p.reactionModel==2401 or p.reactionModel==25) useSafeCouplingQ = 1;
	Safe_coupling safe_coupling(p,nSaveStates,useSafeCouplingQ);

	// save initial condition
	safe.save(c,0);

	// time loop
	for(size_t step=0; step<p.stepsEnd; step++){

		// dynamics
		dynamics(c,cnew,c0,kernel,k,p,m,step,feedback);

		if(c[0]!=c[0]){ printf("step: %zu, u[0]=%f. Abort!\n",step,c[0]);  exit(EXIT_FAILURE); }

		// step counter output
		if(step>0){

			// output: save state
			if(!(step%p.stepsSaveState)){
				safe.save(c,step);																							// save concentrations
				if(p.reactionModel==24 or p.reactionModel==2401 or p.reactionModel==25) safe_coupling.save(feedback,step);		// save feedback
			}
		}
	}
}

// adapt number of processors based on system size
void set_OpenMP_Cores(params &p){

	int maxCPUs = omp_get_max_threads();
	int nOMPs = min(p.n/p.nCpuThresh+1,maxCPUs);
	omp_set_num_threads(nOMPs);
	printf("# of used processors: %d/%d\n",nOMPs,maxCPUs);

}

int main(int ac, char* av[]){

// message at program start
cout << "############ Solver is starting ############ " << endl;

// 0. parameter declaration
struct params inifile_params, inifile_params_cmdline;
struct modelparams model_params;
auto t1=chrono::high_resolution_clock::now(), t2=chrono::high_resolution_clock::now();
string pthini, ini;
string pthexe = get_executable_path();

// 1. read pth of inifile from commandline (default path)
main_cmdln_params(ac,av,pthexe,pthini,ini,inifile_params_cmdline);

// 2a. read data from inifile
pt::ptree iniPropTree;
main_readInifile(pthini,pthexe,ini,inifile_params,iniPropTree,model_params,inifile_params_cmdline);

// 3. prepare system
set_OpenMP_Cores(inifile_params);
int array_size=inifile_params.n*inifile_params.ncomponents;
int history_array_size=1;

if(inifile_params.delayFlag){
	history_array_size = array_size*(inifile_params.delayStepsMax+1);
	printf("c0 size: %.3f MB = states: %zu\n",history_array_size*sizeof(Real)/(1024.*1024.), inifile_params.delayStepsMax+1);
}											// set number of used cpu cores
Real *c0 = new Real[history_array_size]();		// holds all concentration values for time delay (history array)
Real *cfield = new Real[array_size];			// dynamic variables = concentrations
Real *cnew = new Real[array_size]();			// all dynamic variables {u,v,w,} (more are not implemented), init to zero
Real *feedback = new Real[inifile_params.n]();	// save scalar feedback in case of ZBKE model
Real *kfield = new Real[inifile_params.n];									// parameter k for every cell									// parameter k for every cell
Real *kernel = NULL;														// kernel for convolution operations
initialCondition(cfield,inifile_params,model_params);						// set initial condition
main_parameterDistribution(kfield,inifile_params,model_params);				// set spatially heterogeneous parameter distribution
saveParamDistro(kfield,inifile_params);
create_kernel_and_rescale(inifile_params,kernel,model_params);				// create kernel and rescale coupling coefficents

// 3b. CFL check
calcCFL(inifile_params,kfield,model_params);

// 4. simulation
t1 = std::chrono::high_resolution_clock::now();											// timer start
solverCPU_2d(cfield,cnew,kfield,inifile_params,model_params,kernel,c0,feedback);		// simulation
t2 = std::chrono::high_resolution_clock::now();											// timer end
int runtime = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count();

// write report
ofstream dataout;
char spbuff[100];
sprintf(spbuff,"execution time: %d ms = %d h, %d min, %d s.",runtime,runtime/1000/3600,runtime/1000/60%60,int(floor(runtime/1000.+0.5))%60);
dataout.open(inifile_params.pthout+"/report.txt", ios::out);
dataout<<spbuff<<endl;
dataout.close();
cout<<"\n"<<spbuff<<endl;

// deallocate memory
delete[] cfield;
delete[] kfield;
delete[] kernel;
delete[] model_params.coupling_coeffs;
delete[] model_params.model_params;
delete[] model_params.model_phases;
delete[] cnew;
delete[] c0;
delete[] feedback;

return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file DSFVector.cpp
 * @date Jun 25, 2010
 * @author Kai Ni
 * @brief a faster implementation for DSF, which uses vector rather than btree.
 */

#include <gtsam/base/DSFVector.h>
#include <boost/make_shared.hpp>
#include <algorithm>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
DSFBase::DSFBase(const size_t numNodes) {
  v_ = std::make_shared < V > (numNodes);
  int index = 0;
  for (V::iterator it = v_->begin(); it != v_->end(); it++, index++)
    *it = index;
}

/* ************************************************************************* */
DSFBase::DSFBase(const std::shared_ptr<V>& v_in) {
  v_ = v_in;
  int index = 0;
  for (V::iterator it = v_->begin(); it != v_->end(); it++, index++)
    *it = index;
}

/* ************************************************************************* */
size_t DSFBase::find(size_t key) const {
  // follow parent pointers until we reach set representative
  size_t parent = (*v_)[key];
  if (parent != key)
    parent = find(parent); // recursive call
  (*v_)[key] = parent; // path compression
  return parent;
}

/* ************************************************************************* */
void DSFBase::merge(const size_t& i1, const size_t& i2) {
  (*v_)[find(i2)] = find(i1);
}

/* ************************************************************************* */
DSFVector::DSFVector(const size_t numNodes) :
    DSFBase(numNodes) {
  keys_.reserve(numNodes);
  for (size_t index = 0; index < numNodes; index++)
    keys_.push_back(index);
}

/* ************************************************************************* */
DSFVector::DSFVector(const std::vector<size_t>& keys) :
    DSFBase(1 + *std::max_element(keys.begin(), keys.end())), keys_(keys) {
}

/* ************************************************************************* */
DSFVector::DSFVector(const std::shared_ptr<V>& v_in,
    const std::vector<size_t>& keys) :
    DSFBase(v_in), keys_(keys) {
  assert(*(std::max_element(keys.begin(), keys.end()))<v_in->size());
}

/* ************************************************************************* */
bool DSFVector::isSingleton(const size_t& label) const {
  bool result = false;
  for(size_t key: keys_) {
    if (find(key) == label) {
      if (!result) // find the first occurrence
        result = true;
      else
        return false;
    }
  }
  return result;
}

/* ************************************************************************* */
std::set<size_t> DSFVector::set(const size_t& label) const {
  std::set < size_t > set;
  for(size_t key: keys_)
    if (find(key) == label)
      set.insert(key);
  return set;
}

/* ************************************************************************* */
std::map<size_t, std::set<size_t> > DSFVector::sets() const {
  std::map<size_t, std::set<size_t> > sets;
  for(size_t key: keys_)
    sets[find(key)].insert(key);
  return sets;
}

/* ************************************************************************* */
std::map<size_t, std::vector<size_t> > DSFVector::arrays() const {
  std::map<size_t, std::vector<size_t> > arrays;
  for(size_t key: keys_)
    arrays[find(key)].push_back(key);
  return arrays;
}

} // namespace  gtsam