/**
 * @file random.hpp
 *
 * Miscellaneous math random-related routines.
 *
 * This file is part of mlpack 1.0.12.
 *
 * mlpack is free software; you may redstribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef __MY_MLPACK_CORE_MATH_RANDOM_HPP
#define __MY_MLPACK_CORE_MATH_RANDOM_HPP

#include <stdlib.h>
#include <math.h>
#include <float.h>

#include <boost/random.hpp>

namespace mips {
namespace math /** Miscellaneous math routines. */ {

// Annoying Boost versioning issues.
#include <boost/version.hpp>

#if BOOST_VERSION >= 104700
  // Global random object.
  extern boost::random::mt19937 randGen;
  // Global uniform distribution.
  extern boost::random::uniform_01<> randUniformDist;
  // Global normal distribution.
  extern boost::random::normal_distribution<> randNormalDist;
#else
  // Global random object.
  extern boost::mt19937 randGen;

  #if BOOST_VERSION >= 103900
    // Global uniform distribution.
    extern boost::uniform_01<> randUniformDist;
  #else
    // Pre-1.39 Boost.Random did not give default template parameter values.
    extern boost::uniform_01<boost::mt19937, double> randUniformDist;
  #endif

  // Global normal distribution.
  extern boost::normal_distribution<> randNormalDist;
#endif

/**
 * Set the random seed used by the random functions (Random() and RandInt()).
 * The seed is casted to a 32-bit integer before being given to the random
 * number generator, but a size_t is taken as a parameter for API consistency.
 *
 * @param seed Seed for the random number generator.
 */
inline void RandomSeed(const size_t seed)
{
  randGen.seed((uint32_t) seed);
  srand((unsigned int) seed);
}

/**
 * Generates a uniform random number between 0 and 1.
 */
inline double Random()
{
#if BOOST_VERSION >= 103900
  return randUniformDist(randGen);
#else
  // Before Boost 1.39, we did not give the random object when we wanted a
  // random number; that gets given at construction time.
  return randUniformDist();
#endif
}

/**
 * Generates a uniform random number in the specified range.
 */
inline double Random(const double lo, const double hi)
{
#if BOOST_VERSION >= 103900
  return lo + (hi - lo) * randUniformDist(randGen);
#else
  // Before Boost 1.39, we did not give the random object when we wanted a
  // random number; that gets given at construction time.
  return lo + (hi - lo) * randUniformDist();
#endif
}

/**
 * Generates a uniform random integer.
 */
inline int RandInt(const int hiExclusive)
{
#if BOOST_VERSION >= 103900
  return (int) std::floor((double) hiExclusive * randUniformDist(randGen));
#else
  // Before Boost 1.39, we did not give the random object when we wanted a
  // random number; that gets given at construction time.
  return (int) std::floor((double) hiExclusive * randUniformDist());
#endif
}

/**
 * Generates a uniform random integer.
 */
inline int RandInt(const int lo, const int hiExclusive)
{
#if BOOST_VERSION >= 103900
  return lo + (int) std::floor((double) (hiExclusive - lo)
                               * randUniformDist(randGen));
#else
  // Before Boost 1.39, we did not give the random object when we wanted a
  // random number; that gets given at construction time.
  return lo + (int) std::floor((double) (hiExclusive - lo)
                               * randUniformDist());
#endif

}

/**
 * Generates a normally distributed random number with mean 0 and variance 1.
 */
inline double RandNormal()
{
  return randNormalDist(randGen);
}

/**
 * Generates a normally distributed random number with specified mean and
 * variance.
 *
 * @param mean Mean of distribution.
 * @param variance Variance of distribution.
 */
inline double RandNormal(const double mean, const double variance)
{
  return variance * randNormalDist(randGen) + mean;
}

}; // namespace math
}; // namespace mlpack

#endif // __MLPACK_CORE_MATH_MATH_LIB_HPP


// Copyright Nick Thompson, 2017
// 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)

// This implements the compactly supported cubic b spline algorithm described in
// Kress, Rainer. "Numerical analysis, volume 181 of Graduate Texts in Mathematics." (1998).
// Splines of compact support are faster to evaluate and are better conditioned than classical cubic splines.

// Let f be the function we are trying to interpolate, and s be the interpolating spline.
// The routine constructs the interpolant in O(N) time, and evaluating s at a point takes constant time.
// The order of accuracy depends on the regularity of the f, however, assuming f is
// four-times continuously differentiable, the error is of O(h^4).
// In addition, we can differentiate the spline and obtain a good interpolant for f'.
// The main restriction of this method is that the samples of f must be evenly spaced.
// Look for barycentric rational interpolation for non-evenly sampled data.
// Properties:
// - s(x_j) = f(x_j)
// - All cubic polynomials interpolated exactly

#ifndef BOOST_MATH_INTERPOLATORS_CUBIC_B_SPLINE_HPP
#define BOOST_MATH_INTERPOLATORS_CUBIC_B_SPLINE_HPP

#include <boost/math/interpolators/detail/cubic_b_spline_detail.hpp>

namespace boost{ namespace math{

template <class Real>
class cubic_b_spline
{
public:
    // If you don't know the value of the derivative at the endpoints, leave them as nans and the routine will estimate them.
    // f[0] = f(a), f[length -1] = b, step_size = (b - a)/(length -1).
    template <class BidiIterator>
    cubic_b_spline(const BidiIterator f, BidiIterator end_p, Real left_endpoint, Real step_size,
                   Real left_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN(),
                   Real right_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN());
    cubic_b_spline(const Real* const f, size_t length, Real left_endpoint, Real step_size,
       Real left_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN(),
       Real right_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN());

    cubic_b_spline() = default;
    Real operator()(Real x) const;

    Real prime(Real x) const;

private:
    std::shared_ptr<detail::cubic_b_spline_imp<Real>> m_imp;
};

template<class Real>
cubic_b_spline<Real>::cubic_b_spline(const Real* const f, size_t length, Real left_endpoint, Real step_size,
                                     Real left_endpoint_derivative, Real right_endpoint_derivative) : m_imp(std::make_shared<detail::cubic_b_spline_imp<Real>>(f, f + length, left_endpoint, step_size, left_endpoint_derivative, right_endpoint_derivative))
{
}

template <class Real>
template <class BidiIterator>
cubic_b_spline<Real>::cubic_b_spline(BidiIterator f, BidiIterator end_p, Real left_endpoint, Real step_size,
   Real left_endpoint_derivative, Real right_endpoint_derivative) : m_imp(std::make_shared<detail::cubic_b_spline_imp<Real>>(f, end_p, left_endpoint, step_size, left_endpoint_derivative, right_endpoint_derivative))
{
}

template<class Real>
Real cubic_b_spline<Real>::operator()(Real x) const
{
    return m_imp->operator()(x);
}

template<class Real>
Real cubic_b_spline<Real>::prime(Real x) const
{
    return m_imp->prime(x);
}

}}
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/vector/map_view.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/mtl/operation/real.hpp>
#include <boost/numeric/mtl/operation/imag.hpp>
#include <boost/numeric/mtl/operation/scale.hpp>

#if 0
#include <boost/numeric/mtl/operation/hermitian.hpp>
#endif

using std::cout;  using std::complex;

typedef complex<double> ct;

double value(double)
{
    return 7.0;
}

complex<double> value(complex<double>)
{
    return ct(7.0, 1.0);
}

// scaled value
double svalue(double)
{
    return 14.0;
}

ct svalue(ct)
{
    return ct(14.0, 2.0);
}

// conjugated value
double cvalue(double)
{
    return 7.0;
}

ct cvalue(ct)
{
    return ct(7.0, -1.0);
}

// complex scaled value
ct csvalue(double)
{
    return ct(0.0, 7.0);
}

ct csvalue(ct)
{
    return ct(-1.0, 7.0);
}

template <typename Vector>
void test(Vector& vector, const char* name)
{
    using mtl::real; using mtl::imag;

    set_to_zero(vector);
    typename Vector::value_type ref(0);

    vector[2]= value(ref);
    vector[4]= value(ref) + 1.0;
    vector[5]= value(ref) + 2.0;

#if 0 // When sparse vectors are used there should be an inserter class for vectors too
    {
	inserter<Vector>  ins(vector);
	ins(2) << value(ref);
	ins(4) << value(ref) + 1.0;
	ins(5) << value(ref) + 2.0;
    }
#endif

    cout << "\n\n" << name << "\n";
    cout << "Original vector:\n" << vector << "\n";

    mtl::vec::scaled_view<double, Vector>  scaled_vector(2.0, vector);
    cout << "vector  scaled with 2.0\n" << scaled_vector << "\n";
    MTL_THROW_IF(scaled_vector(2) != svalue(ref), mtl::runtime_error("scaling wrong"));

    mtl::vec::conj_view<Vector>  conj_vector(vector);
    cout << "conjugated vector\n" << conj_vector << "\n";
    MTL_THROW_IF(conj_vector(2) != cvalue(ref), mtl::runtime_error(" wrong"));

    mtl::vec::scaled_view<ct, Vector>  cscaled_vector(ct(0.0, 1.0), vector);
    cout << "vector scaled with i (complex(0, 1))\n" << cscaled_vector << "\n";
    MTL_THROW_IF(cscaled_vector(2) != csvalue(ref), mtl::runtime_error("complex scaling wrong"));

#if 0 // transposition of vector is not an issue (yet)
    mtl::vec::hermitian_view<Vector>  hermitian_vector(vector);
    cout << "Hermitian vector (conjugate transposed)\n" << hermitian_vector << "\n";
    if (hermitian_vector(3, 2) != cvalue(ref))
	throw "conjugate transposing  wrong";
#endif

    cout << "vector  scaled with 2.0 (free function)\n" << scale(2.0, vector) << "\n";
    MTL_THROW_IF(scale(2.0, vector)(2) != svalue(ref), mtl::runtime_error("scaling wrong"));

#if defined(__GNUC__) && __GNUC__ == 4 && (__GNUC_MINOR__ >= 3 && __GNUC_MINOR__ <= 6)
    cout << "conjugated vector (free function) \n" << mtl::conj(vector) << "\n";
    MTL_THROW_IF(mtl::conj(vector)[2] != cvalue(ref), mtl::runtime_error("conjugating wrong"));

    cout << "real vector (free function) \n" << mtl::vec::real(vector) << "\n";
    MTL_THROW_IF(mtl::vec::real(vector)[2] != real(value(ref)), mtl::runtime_error("real part wrong"));

    cout << "imag vector (free function) \n" << mtl::vec::imag(vector) << "\n";
    MTL_THROW_IF(mtl::vec::imag(vector)[2] != imag(value(ref)), mtl::runtime_error("imag part wrong"));
#else
    cout << "conjugated vector (free function) \n" << conj(vector) << "\n";
    MTL_THROW_IF(conj(vector)[2] != cvalue(ref), mtl::runtime_error("conjugating wrong"));

    cout << "real vector (free function) \n" << real(vector) << "\n";
    MTL_THROW_IF(real(vector)[2] != real(value(ref)), mtl::runtime_error("real part wrong"));

    cout << "imag vector (free function) \n" << imag(vector) << "\n";
    MTL_THROW_IF(imag(vector)[2] != imag(value(ref)), mtl::runtime_error("imag part wrong"));
#endif

    cout << "vector scaled with i (complex(0, 1)) (free function)\n" << scale(ct(0.0, 1.0), vector) << "\n";
    MTL_THROW_IF(scale(ct(0.0, 1.0), vector)(2) != csvalue(ref), mtl::runtime_error("complex scaling wrong"));

#if 0 // transposition of vector is not an issue (yet)
    cout << "Hermitian  vector (conjugate transposed) (free function)\n" << hermitian(vector) << "\n";
    if (hermitian(vector)(3, 2) != cvalue(ref))
	throw "conjugate transposing wrong";
#endif

}

int main(int argc, char* argv[])
{
    unsigned size= 7;
    if (argc > 1) size= atoi(argv[1]);

    mtl::dense_vector<double>                                 dv(size);
    mtl::dense_vector<complex<double> >                       drc(size);

    test(dv, "Dense double vector");
    test(drc, "Dense complex vector");

    return 0;
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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 SMOOTH__FEEDBACK__COLLOCATION__MESH_HPP_
#define SMOOTH__FEEDBACK__COLLOCATION__MESH_HPP_

/**
 * @file
 * @brief Refinable Legendre-Gauss-Radau mesh of time interval [0, 1]
 */

#include <Eigen/Core>
#include <Eigen/LU>
#include <smooth/internal/utils.hpp>
#include <smooth/polynomial/quadrature.hpp>

#include <ranges>
#include <span>
#include <vector>

#include "smooth/feedback/traits.hpp"
#include "smooth/feedback/utils/sparse.hpp"

namespace smooth::feedback {

using smooth::utils::zip;

using std::views::iota, std::views::drop, std::views::reverse, std::views::take,
  std::views::transform, std::views::join;

namespace detail {

/**
 * @brief Legendre-Gauss-Radau nodes including an extra node at +1.
 */
template<std::size_t K, std::size_t I = 8>
constexpr std::pair<std::array<double, K + 1>, std::array<double, K + 1>> lgr_plus_one()
{
  auto lgr_norm = ::smooth::lgr_nodes<K, I>();

  std::array<double, K + 1> ns, ws;
  for (auto i = 0u; i < K; ++i) {
    ns[i] = lgr_norm.first[i];
    ws[i] = lgr_norm.second[i];
  }
  ns[K] = 1;
  ws[K] = 0;
  return {ns, ws};
}

}  // namespace detail

/**
 * @brief Collocation mesh of interval [0, 1].
 * @tparam _Kmin minimal number of collocation points per interval
 * @tparam _Kmax maximal number of collocation points per interval
 *
 * [0, 1] is divided into non-overlapping intervals I_i, and each interval I_i has K_i LGR
 * collocation points.
 */
template<std::size_t _Kmin = 5, std::size_t _Kmax = 10>
  requires(_Kmin <= _Kmax)
class Mesh
{
  using MatMap = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>;

public:
  /// @brief Minimal number of collocation points per interval
  static constexpr auto Kmin = _Kmin;
  /// @brief Maximal number of collocation points per interval
  static constexpr auto Kmax = _Kmax;

  /**
   * @brief Create a mesh consisting of a single interval [0, 1].
   *
   * @param Kmin minimal polynomial degree in mesh
   * @param Kmax maximal polynomial degree in mesh
   *
   * @note It must hold that kKmin <= Kmin <= Kmax <= kKmax, where kKmin and kKmax are compile-time
   * constants that define which LGR nodes to pre-compute.
   *
   * @note Allocates heap memory.
   */
  inline Mesh() : intervals_(1, Interval{.K = Kmin, .tau0 = 0.}) {}

  /**
   * @brief Create a mesh consisting of a n intervals of equal size over [0, 1].
   *
   * @param n number of intervals. If n==0, only a single interval is created.
   * @param k polynomial degree for all intervals
   *
   * @note Allocates heap memory.
   */
  inline Mesh(const std::size_t n, const std::size_t k = Kmin)
  {
    assert(Kmin <= k && k <= Kmax + 1);

    if (n < 2) {
      intervals_.emplace_back(k, 0.);
    } else {
      const double dx = 1. / static_cast<double>(n);
      intervals_.reserve(n);
      for (std::size_t i = 0; i < n; ++i) {
        intervals_.emplace_back(k, static_cast<double>(i) * dx);
      }
    }
  }

  /**
   * @brief Number of intervals in mesh.
   */
  inline std::size_t N_ivals() const { return intervals_.size(); }

  /**
   * @brief Number of collocation points in mesh.
   */
  inline std::size_t N_colloc() const
  {
    return std::accumulate(
      intervals_.begin(), intervals_.end(), 0u, [](std::size_t curr, const auto & x) {
        return curr + x.K;
      });
  }

  /**
   * @brief Number of collocation points in interval i.
   *
   * @note This is also equal to the polynomial degree inside interval i, since the polynomial is
   * fitted with an "extra" point belonging to the subsequent interval.
   */
  inline std::size_t N_colloc_ival(std::size_t i) const
  {
    assert(i < intervals_.size());
    return intervals_[i].K;
  }

  /**
   * @brief Refine interval using the ph strategy.
   *
   * @param i index of interval to refine
   * @param D target number of collocation points in refined interval
   *
   * If D > Kmax, or current degree > Kmax    then the interval is divided into
   *                                          n = max(2, ceil(D / Kmin)) intervals with deg Kmin
   * If D < current degree,                   then nothing is done.
   * If D <= Kmax,                            then the polynomial degree is increased to D.
   *
   * @note May allocate heap memory due to vector resizing if the number of intervals is increased.
   */
  inline void refine_ph(std::size_t i, std::size_t D)
  {
    assert(i < intervals_.size());
    if (D > Kmax || intervals_[i].K > Kmax) {
      // refine by splitting interval into n intervals, each with degree Kmin_
      std::size_t n = std::max<std::size_t>(2u, (D + Kmin - 1) / Kmin);

      const double tau0 = intervals_[i].tau0;
      const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
      const double taum = (tauf - tau0) / n;

      while (n-- > 1) {
        intervals_.insert(intervals_.begin() + i + 1, Interval{.K = Kmin, .tau0 = tau0 + n * taum});
      }
    } else if (D < intervals_[i].K) {
      return;
    } else if (D <= Kmax) {
      // refine by increasing degree in interval
      intervals_[i].K = D;
    }
  }

  /**
   * @brief Refine intervals in mesh to satisfy a target error criterion.
   * @param errs relative errors for all intervals (@see mesh_dyn_error())
   * @param target_err target relative error
   */
  inline void refine_errors(std::ranges::sized_range auto && errs, double target_err)
  {
    const auto N = N_ivals();

    assert(N == std::size_t(std::ranges::size(errs)));

    for (const auto & [i, e] : zip(iota(0u, N) | reverse, errs | reverse)) {

      const auto Ki = N_colloc_ival(i);

      if (e > target_err) {
        const auto Ktarget = Ki + std::lround(std::log(e / target_err) / std::log(Ki) + 1);
        refine_ph(i, Ktarget);
      }
    }
  }

  /**
   * @brief Set the number of collocation points in interval i to K
   * @param i interval index
   * @param K number of collocation points s.t. (Kmin <= K <= Kmax + 1)
   */
  inline void set_N_colloc_ival(std::size_t i, std::size_t K)
  {
    assert(Kmin <= K);
    assert(K <= Kmax + 1);
    intervals_[i].K = K;
  }

  /**
   * @brief Interval nodes (as range of doubles).
   *
   * @note Includes extra point at 1, i.e. size of returned range is equal to N_colloc_ival()+1
   */
  inline auto interval_nodes(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;

    assert(Kmin <= k && k <= Kmax + 1);

    std::span<const double> sp;

    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();

        sp = std::span<const double>(nw_ext_s.first.data(), k + 1);
      }
    });

    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
    const double al   = (tauf - tau0) / 2;

    return transform(std::move(sp), [tau0, al](double d) -> double { return tau0 + al * (d + 1); });
  }

  /**
   * @brief Nodes (as range of doubles).
   *
   * @note Includes extra point at 1, i.e. size of returned range is equal to N_colloc()+1
   *
   * @note The result is an input range
   */
  inline auto all_nodes() const
  {
    const auto n_ivals = N_ivals();
    auto all_views     = iota(0u, n_ivals) | transform([this, n_ivals = n_ivals](auto i) {
                       const auto n_ival    = N_colloc_ival(i);
                       const int64_t n_take = i + 1 < n_ivals ? n_ival : n_ival + 1;
                       return interval_nodes(i) | take(n_take);
                     });

    return join(std::move(all_views));
  }

  /**
   * @brief Interval weights (as range of doubles)
   *
   * @note Includes zero weight at 1, i.e. size of returned range is equal to N_colloc_ival()+1
   */
  inline auto interval_weights(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;

    assert(Kmin <= k && k <= Kmax + 1);

    std::span<const double> sp;

    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();

        sp = std::span<const double>(nw_ext_s.second.data(), k + 1);
      }
    });

    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
    const double al   = (tauf - tau0) / 2;

    return transform(std::move(sp), [al](double d) -> double { return al * d; });
  }

  /**
   * @brief Weights (as range of doubles)
   *
   * @note Includes zero weight at 1, i.e. size of returned range is equal to N_colloc()+1
   *
   * @note The result is an input range
   */
  inline auto all_weights() const
  {
    const auto n_ivals = N_ivals();
    auto all_views     = iota(0u, n_ivals) | transform([this, n_ivals = n_ivals](auto i) {
                       const auto n_ival    = N_colloc_ival(i);
                       const int64_t n_take = i + 1 < n_ivals ? n_ival : n_ival + 1;
                       return interval_weights(i) | take(n_take);
                     });

    return join(std::move(all_views));
  }

  /**
   * @brief Interval differentiation matrix w.r.t. [0, 1] timescale.
   *
   * Returns a \f$ (K+1 \times K) \f$ matrix \f$ D \f$ s.t.
   * \f[
   *   \begin{bmatrix} y'(\tau_{i, 0}) & y'(\tau_{i, 1}) & \cdots & y'(\tau_{i, K-1}) \end{bmatrix}
   *  =
   *   \begin{bmatrix} y(\tau_{i, 0}) & y(\tau_{i, 1}) & \cdots & y(\tau_{i, K}) \end{bmatrix} D
   * \f],
   * where \f$ y(\cdot) \in \mathbb{R}^{d \times 1} \f$ is a Lagrange polynomial in interval i.
   *
   * @note Allocates heap memory for return value.
   */
  inline Eigen::MatrixXd interval_diffmat(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;

    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;

    Eigen::MatrixXd ret(k + 1, k);

    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
        static constexpr auto B_ext_s  = lagrange_basis<K>(nw_ext_s.first);
        static constexpr auto D_ext_s =
          polynomial_basis_derivatives<K, K + 1>(B_ext_s, nw_ext_s.first)
            .template block<K + 1, K>(0, 0);
        ret = MatMap(D_ext_s[0].data(), k + 1, k);
      }
    });

    ret *= 2. / (tauf - tau0);
    return ret;
  }

  /**
   * @brief Interval differentiation matrix (unscaled).
   *
   * Returns a Map D_us and a scalar alpha s.t. D = alpha * D_us
   *
   * @see interval_diffmat
   */
  inline std::pair<double, MatMap> interval_diffmat_unscaled(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;

    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;

    MatMap ret(nullptr, 0, 0);

    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
        static constexpr auto B_ext_s  = lagrange_basis<K>(nw_ext_s.first);
        static constexpr auto D_ext_s =
          polynomial_basis_derivatives<K, K + 1>(B_ext_s, nw_ext_s.first)
            .template block<K + 1, K>(0, 0);
        // Eigen maps don't have copy constructors so use placement new
        new (&ret) MatMap(D_ext_s[0].data(), k + 1, k);
      }
    });

    return {2. / (tauf - tau0), ret};
  }

  /**
   * @brief Interval integration matrix w.r.t. [0, 1] timescale.
   *
   * Returns a \f$ (K \times K) \f$ matrix \f$ I \f$ s.t.
   * \f[
   *   \begin{bmatrix}
   *      y(\tau_{i, 1}) & y(\tau_{i, 2}) & \cdots & y(\tau_{i, K})
   *   \end{bmatrix}
   *  = y(\tau_{i, 0}) \begin{bmatrix} 1 & \ldots & 1 \end{bmatrix}
   *    + \begin{bmatrix}
   *        \dot y(\tau_{i, 0}) & \dot y(\tau_{i, 1}) & \cdots & \dot y(\tau_{i, K-1})
   *      \end{bmatrix} I
   * \f],
   * where \f$ y(\cdot) \in \mathbb{R}^{d \times 1} \f$ is a Lagrange
   * polynomial in interval i.
   *
   * @note Allocates heap memory for return value.
   *
   * @note Performs a matrix inverse.
   */
  inline Eigen::MatrixXd interval_intmat(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;
    return interval_diffmat(i).block(1, 0, k, k).inverse();
  }

  /**
   * @brief Find interval index that contains t
   */
  inline std::size_t interval_find(double t) const
  {
    if (t < 0) { return 0; }
    if (t > 1) { return intervals_.size() - 1; }
    auto it = utils::binary_interval_search(
      intervals_, t, [](const auto & ival, double _t) { return ival.tau0 <=> _t; });
    if (it != intervals_.end()) { return std::distance(intervals_.begin(), it); }
    return 0;
  }

  /**
   * @brief Increase the degree of all intervals by one (up to maximal degree Kmax + 1)
   */
  void increase_degrees()
  {
    for (auto & ival : intervals_) { ival.K = std::min(ival.K + 1, Kmax + 1); }
  }

  /**
   * @brief Decrease the degree of all intervals by one (down to minimal degree Kmin)
   */
  void decrease_degrees()
  {
    for (auto & ival : intervals_) { ival.K = std::max(ival.K - 1, Kmin); }
  }

  /**
   * @brief Evaluate a function
   *
   * @tparam RetT return value type
   *
   * @param t time value in [0, 1]
   * @param r values for the collocation points (size N [extend=false] or N+1 [extend=true])
   * @param p derivative to evaluate
   * @param extend set to true if a value is provided for t=+1
   */
  template<smooth::traits::RnType RetT>
  RetT eval(double t, std::ranges::range auto && r, std::size_t p = 0, bool extend = true) const
  {
    const std::size_t ival = interval_find(t);
    const std::size_t k    = intervals_[ival].K;

    const double tau0 = intervals_[ival].tau0;
    const double tauf = ival + 1 < intervals_.size() ? intervals_[ival + 1].tau0 : 1.;

    const double u = 2 * (t - tau0) / (tauf - tau0) - 1;

    int64_t N_before = 0;
    for (auto i = 0u; i < ival; ++i) { N_before += intervals_[i].K; }

    // initialize output variable
    RetT ret = RetT::Zero(dof(*std::ranges::begin(r)));

    utils::static_for<Kmax + 2 - Kmin>([&](auto i) {
      static constexpr auto K = Kmin + i;
      if (K == k) {
        if (extend || ival + 1 < intervals_.size()) {
          static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
          static constexpr auto B_ext_s  = lagrange_basis<K>(nw_ext_s.first);  // K+1 x K+1
          const auto U                   = monomial_derivative<K>(u, p);       // 1 x K+1
          const auto W                   = U * B_ext_s;                        // 1 x K+1

          for (const auto & [w, v] : zip(std::span(W[0].data(), k + 1), r | drop(N_before))) {
            ret += w * v;
          }
        } else {
          static constexpr auto nw_s = lgr_nodes<K>();
          static constexpr auto B_s  = lagrange_basis<K - 1>(nw_s.first);  // K x K
          const auto U               = monomial_derivative<K - 1>(u, p);   // 1 x K
          const auto W               = U * B_s;                            // 1 x K

          for (const auto & [w, v] : zip(std::span(W[0].data(), k), r | drop(N_before))) {
            ret += w * v;
          }
        }
      }
    });

    return ret;
  }

private:
  struct Interval
  {
    /// @brief Polynomial degree in interval
    std::size_t K;
    /// @brief Start of interval on [0, 1] timescale
    double tau0;
  };

  /// @brief Mesh intervals
  std::vector<Interval> intervals_;
};

/// @brief MeshType is a specialization of Mesh
template<typename T>
concept MeshType = traits::is_specialization_of_sizet_v<std::decay_t<T>, Mesh>;

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__COLLOCATION__MESH_HPP_


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file popcount.hpp
 *
 * This header defines \c popcount algorithm, which counts the number of non-zero bits in an integer.
 */

#ifndef BOOST_BIT_OPS_COUNTING_POPCOUNT_HPP_INCLUDED_
#define BOOST_BIT_OPS_COUNTING_POPCOUNT_HPP_INCLUDED_

#include <boost/cstdint.hpp>
#include <boost/bit_ops/detail/config.hpp>
#include <boost/bit_ops/detail/int_sizes.hpp>
#include <boost/bit_ops/detail/type_traits/enable_if.hpp>
#include <boost/bit_ops/detail/type_traits/integral_constant.hpp>
#include <boost/bit_ops/detail/type_traits/is_integral.hpp>
#include <boost/bit_ops/detail/type_traits/is_unsigned.hpp>

#if defined(_MSC_VER)
#include <intrin.h>
#endif

namespace boost {
namespace bit_ops {

namespace detail {

#if defined(BOOST_BIT_OPS_DETAIL_HAS_BUILTIN_POPCOUNT)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N <= BOOST_BIT_OPS_DETAIL_SIZEOF_INT, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__builtin_popcount(value));
}

#if BOOST_BIT_OPS_DETAIL_SIZEOF_LONG > BOOST_BIT_OPS_DETAIL_SIZEOF_INT

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == BOOST_BIT_OPS_DETAIL_SIZEOF_LONG, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__builtin_popcountl(value));
}

#endif // BOOST_BIT_OPS_DETAIL_SIZEOF_LONG > BOOST_BIT_OPS_DETAIL_SIZEOF_INT

#if BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG > BOOST_BIT_OPS_DETAIL_SIZEOF_LONG

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__builtin_popcountll(value));
}

#endif // BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG > BOOST_BIT_OPS_DETAIL_SIZEOF_LONG

#elif defined(BOOST_BIT_OPS_DETAIL_HAS_POPCNT)

#if defined(BOOST_BIT_OPS_DETAIL_HAS_POPCNT16)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N <= 2u, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__popcnt16(value));
}

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == 4u, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__popcnt(value));
}

#else // defined(BOOST_BIT_OPS_DETAIL_HAS_POPCNT16)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N <= 4u, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__popcnt(value));
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_POPCNT16)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == 8u, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
#if defined(BOOST_BIT_OPS_DETAIL_HAS_POPCNT64)
    return static_cast< unsigned int >(__popcnt64(value));
#else
    return static_cast< unsigned int >(__popcnt(static_cast< unsigned int >(value)) + __popcnt(static_cast< unsigned int >(value >> 32u)));
#endif
}

#else

template< typename T >
inline unsigned int popcount(T value, bit_ops::detail::integral_constant< unsigned int, 1u >) BOOST_NOEXCEPT
{
    value = (value & 0x55u) + ((value >> 1u) & 0x55u);
    value = (value & 0x33u) + ((value >> 2u) & 0x33u);
    value = value + (value >> 4u);
    return static_cast< unsigned int >(value & 15u);
}

template< typename T >
inline unsigned int popcount(T value, bit_ops::detail::integral_constant< unsigned int, 2u >) BOOST_NOEXCEPT
{
    value = (value & 0x5555u) + ((value >> 1u) & 0x5555u);
    value = (value & 0x3333u) + ((value >> 2u) & 0x3333u);
    value = (value + (value >> 4u)) & 0x0F0Fu;
    value = value + (value >> 8u);
    return static_cast< unsigned int >(value & 31u);
}

template< typename T >
inline unsigned int popcount(T value, bit_ops::detail::integral_constant< unsigned int, 4u >) BOOST_NOEXCEPT
{
    value = (value & 0x55555555u) + ((value >> 1u) & 0x55555555u);
    value = (value & 0x33333333u) + ((value >> 2u) & 0x33333333u);
    value = (value + (value >> 4u)) & 0x0F0F0F0Fu;
    value = (value + (value >> 8u)) & 0x00FF00FFu;
    value = value + (value >> 16u);
    return static_cast< unsigned int >(value & 63u);
}

template< typename T >
inline unsigned int popcount(T value, bit_ops::detail::integral_constant< unsigned int, 8u >) BOOST_NOEXCEPT
{
    value = (value & UINT64_C(0x5555555555555555)) + ((value >> 1u) & UINT64_C(0x5555555555555555));
    value = (value & UINT64_C(0x3333333333333333)) + ((value >> 2u) & UINT64_C(0x3333333333333333));
    value = (value + (value >> 4u)) & UINT64_C(0x0F0F0F0F0F0F0F0F);
    value = (value + (value >> 8u)) & UINT64_C(0x00FF00FF00FF00FF);
    value = value + (value >> 16u);
    value = value + (value >> 32u);
    return static_cast< unsigned int >(value & 127u);
}

#endif

} // namespace detail

//! Returns the number of non-zero bits in \a value
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    unsigned int
>::type popcount(T value) BOOST_NOEXCEPT
{
    return bit_ops::detail::popcount(value, bit_ops::detail::integral_constant< unsigned int, sizeof(T) >());
}

} // namespace bit_ops
} // namespace boost

#endif // BOOST_BIT_OPS_COUNTING_POPCOUNT_HPP_INCLUDED_


/**
 *  The definition of an abstraction class.
 *
 *  Created by Yinan Li on Aug. 30, 2017.
 *
 *  Hybrid Systems Group, University of Waterloo.
 */

#ifndef _abstraction_h_
#define _abstraction_h_

#include <armadillo>
#include <boost/dynamic_bitset.hpp>
#include <functional>
#include <set>

#include "transition.hpp"
#include "system.hpp"

namespace rocs {

    // /**
    //  * Boolean function defining a set.
    //  */
    // typedef bool (*gset)(const ivec &x);

    /**
     * \brief Weighting function (of edges) of transitions.
     */
    typedef double (*WGT)(std::vector<double> &x0,
    			  std::vector<double> &x1,
    			  std::vector<double> &u0);

    /**
     * \brief An abstraction class of a dynamical system.
     *
     * This is a template class where the typename %S will be replaced by the actual dynamical system type.
     */
    template<typename S>
    class abstraction {

    public:
	grid _x;  /**< A grid of states */
	S *_ptrsys; /**< A pointer to the system object */
	std::vector<int> _labels; /**< A vector of labels for the state grid _x */
	WGT _wf;  /**< Weighting callback function */
	fts _ts;  /**< The finite transition system */

	/**
	 * \brief A constructor.
	 *
	 * Assign user defined system dynamics to an abstraction.
	 *
	 * @param[in] sys The pointer to a system object
	 */
	abstraction(S *sys) : _x(sys->_xdim), _ptrsys(sys), _wf(NULL) {}
	/**
	 * \brief A constructor.
	 *
	 * Assign user defined system dynamics as well as a weighting function to an abstraction.
	 *
	 * @param[in] sys The pointer to a system object
	 * @param[in] wf A weighting function
	 */
	abstraction(S *sys, WGT wf) : _x(sys->_xdim), _ptrsys(sys), _wf(wf) {}

	/**
	 * \brief Initialize the states in the abstraction.
	 *
	 * This includes
	 * - assigning a grid of states by the given bounds and grid size.
	 * - assigning labels
	 *
	 * @param[in] eta An array of grid size
	 * @param[in] xlb An array of lower bounds
	 * @param[in] xub An array of upper bounds
	 */
	void init_state(const double eta[], const double xlb[], const double xub[]) {
	    _x.gridding(eta, xlb, xub);
	    _labels.resize(_x._nv+1, 0); //the extra one is an out-of-domain node
	}

	/**
	 * \brief Initialize the finite transition system.
	 *
	 * Assign the number of predecessing (fts._npre) and successing (fts._npost) states.
	 */
	bool init_transitions() {
	    if (_x._nv > 0 && _ptrsys->_ugrid._nv > 0) {
		_ts.init(_x._nv+1, _ptrsys->_ugrid._nv);
	    } else {
		std::cout << "Transition initialization failed: gridding problem.\n";
		return false;
	    }
	    return true;
	}

	/**
	 * \brief Get state indicies of a given hyper-rectangle area.
	 *
	 * The area is given by its lower and upper bounds.
	 * It is allowed to be out of domain, but only the inside domain part is considered.
	 *
	 * @param xlb The lower bound of the area
	 * @param xub The upper bound of the area
	 * @return a list of indicies.
	 */
	std::vector<size_t> get_discrete_states(const double xlb[], const double xub[]) {
	    ivec states(_x._dim);
	    for (int i = 0; i < _x._dim; ++i) {
		states.setval(i, interval(xlb[i], xub[i]));
	    }

	    return _x.subset(states, false, false);  // allow area out of domain, and collect grids intersect the area
	}

	/**
	 * Get discrete state indicies of a given area:
	 * \f$\{x|g(x)=\text{True}\}\f$.
	 *
	 * @param[in] g The function that determines if x is inside the set
	 * @return a list of indicies.
	 */
	std::vector<size_t> get_discrete_states(std::function<bool(const ivec &)> g) {
	    std::vector<size_t> r;
	    if (_x._nv>0 && !_x._data.empty()) {
		ivec x(_x._dim);
		for (int i = 0; i < _x._nv; ++i) {
		    /* set interval x */
		    for (int j = 0; j < _x._dim; ++j) {
			x.setval(j, interval(_x._data[i][j]-_x._gw[j]/2, _x._data[i][j]+_x._gw[j]/2));
		    }
		    /* test */
		    if (g(x)) {
			r.push_back(i);
		    }
		}

	    } else {
		std::cout << "get_discrete_states: a grid of state space hasn't been iniitlized.\n";
	    }
	    return r;
	}

	/**
	 * \brief A template function that assigns labels to the state grid.
	 *
	 * @param[in] labeling A function that returns the label of a grid id
	 */
	template<typename F>
	void assign_labels(F labeling) {
	    for (size_t i = 0; i < _x._nv; ++i) {
		if(_labels[i] > -1)
		    _labels[i] = labeling(i);
	    }
	}

	/**
	 * \brief Assign the label to the out-of-domain part.
	 *
	 * @param[in] label The label given to the out-of-domain part
	 */
	void assign_label_outofdomain(int label) {
	    _labels[_x._nv] = label;
	}

	/**
	 * \brief Assign transitions with robustness margins e1,e2.
	 *
	 * This function constructs an fts according to the given system.
	 *
	 * @param[in] e1 the robustness margin at the initial point.
	 * @param[in] e2 the robustness margin at the end point.
	 * @return whether construction is successful.
	 */
	bool assign_transitions(const double e1[], const double e2[]) {
	    /* Initialize _ts (the transition system) */
	    if (!init_transitions())
		return false;
	    int n = _x._dim;
	    size_t nx = _x._nv;
	    size_t nu = _ptrsys->_ugrid._nv;
	    // std::cout << "dim=" << n << ',' << "#states=" << nx << ",#inputs=" << nu <<'\n';

	    ivec ie2(n);
	    for (int j = 0; j < n; ++j)
	    	ie2.setval(j, interval(-e2[j],e2[j]));

	    std::vector<double> xmin(n), xmax(n);
	    for (int k = 0; k < n; ++k) {
	    	xmin[k] = _x._valmin[k]-_x._gw[k]/2.0;
	    	xmax[k] = xmin[k] + _x._gw[k]*_x._size[k];
	    }
	    ivec xbds(n);
	    for (int k = 0; k < n; ++k)
		xbds.setval(k, interval(xmin[k], xmax[k]));

	    /* Out-of-domain indicator:
	     * 0:inside, 1:fully outside, 2:partially outside
	     */
	    std::vector<int> out_of_domain(nx*nu,0);

	    ivec y0(n);
	    std::vector<ivec> yt(nu, ivec(n));
	    std::vector<double> ytl(n), ytu(n);
	    size_t r, postid;
	    double idu;
	    std::vector<size_t> il(n), iu(n);
	    std::vector<size_t> stpost(nx*nu, 0), rngpost(nx*nu*n, 0);

	    /* loop state grids */
	    for (size_t row = 0; row < nx; ++row) {
		/* the avoid points (labeled by -1) has no outgoing edges */
		if (_labels[row] == -1)
		    continue;
		/* compute reachable set */
		for (int k = 0; k < n; ++k) {
		    y0.setval(k, interval(_x._data[row][k]-_x._gw[k]/2.0,
		    			  _x._data[row][k]+_x._gw[k]/2.0));
		    // y0.setval(k, interval(_x._data[row][k]-_x._gw[k]/2.0-e1[k],
		    // 			  _x._data[row][k]+_x._gw[k]/2.0+e1[k]));
		}
		_ptrsys->get_reach_set(yt, y0);
		if (!yt.empty()) {
		    /* loop inputs */
		    for (size_t col = 0; col < nu; ++col) {
			// yt[col] += ie2;
			// /* Skip the yt[col] that is out of the domain _bds.
			//  * No need to delete the transitions to avoid area, since they are sinks.
			//  * */
			// if (!xbds.isin(yt[col]))
			//     continue;
			// /** Using this line will probably result in failure in synthesis **/
			// // if (!_x._bds.isin(yt[col]))
			// //     continue;

			if(xbds.isout(yt[col])) { //fully out of domain
			    out_of_domain[row*nu+col] = 1;
			    _ts._npost[row*nu+col] = 1;
			    _ts._ptrpost[row*nu+col] = _ts._ntrans;
			    ++_ts._ntrans;
			    continue;
			}
			if(!xbds.isin(yt[col])) {//partially inside domain
			    out_of_domain[row*nu+col] = 2;
			    /* Take intersection of xbds and yt[col] */
			    for(int k = 0; k < n; ++k) {
				ytl[k] = yt[col][k].getinf()<xmin[k] ? xmin[k] : yt[col][k].getinf();
				ytu[k] = yt[col][k].getsup()>xmax[k] ? xmax[k] : yt[col][k].getsup();
			    }
			} else { //fully inside domain
			    yt[col].getinf(ytl);
			    yt[col].getsup(ytu);
			}
			/* compute the indices of the bounds of the yt[col] */
			_ts._npost[row*nu + col] = 1;
			for (int k = 0; k < n; ++k) {
			    il[k] = static_cast<size_t> ((ytl[k]-_x._valmin[k])/_x._gw[k]+0.5);
			    idu = (ytu[k]-_x._valmin[k])/_x._gw[k] + 0.5;
			    iu[k] = static_cast<size_t> (idu);
			    if (std::fabs(idu - iu[k]) < EPSIVAL)
			    	iu[k] = iu[k] - 1;
			    // if (std::fabs(fmod(ytu[k]-_x._valmin[k], _x._gw[k]) - _x._gw[k]/2.) < EPSIVAL) /* fmod is slow */
			    // 	iu[k] = static_cast<size_t> ((ytu[k]-_x._valmin[k])/_x._gw[k]);
			    // else
			    // 	iu[k] = static_cast<size_t> ((ytu[k]-_x._valmin[k]+_x._gw[k]/2.0)/_x._gw[k]);
			    stpost[row*nu+col] += _x._base[k] * il[k]; /* compute the smallest post ID of all the post grid points */
			    rngpost[n*(row*nu+col)+k] = iu[k] - il[k] + 1;
			    _ts._npost[row*nu+col] *= rngpost[n*(row*nu+col)+k];
			}
			_ts._ptrpost[row*nu+col] = _ts._ntrans;
			_ts._ntrans += _ts._npost[row*nu+col];

			/* Add an out-of-domain transition */
			if(out_of_domain[row*nu+col]) {
			    ++_ts._npost[row*nu+col];
			    ++_ts._ntrans;
			}
			// /********** logging **********/
			// if(row == 0 && col == 16) {
			//     std::cout << "y0=" << y0 << '\n'
			// 	      << "yt=" << yt[col] << '\n'
			// 	      << "out_of_domain=" << out_of_domain[row*nu+col] << '\n';
			//     if(out_of_domain[row*nu+col]>1) {
			// 	std::cout << "Intersection with domain: ";
			// 	for(int k = 0; k < n; ++k)
			// 	    std::cout << '[' << ytl[k] << ',' << ytu[k] << "] ";
			// 	std::cout << '\n';
			//     }
			//     std::cout << "starting address of post transitions: "
			// 	      << _ts._ptrpost[row*nu+col] << '\n';
			//     std::cout << "# of post transitions: "
			// 	      << _ts._npost[row*nu+col] << '\n';
			// }
			// /********** logging **********/
		    } // end input loop
		} else {
		    yt.resize(nu, ivec(n));
		}// end yt empty check
	    }  //end loop state grids
	    /* Assign out-of-domain posts */
	    if(_labels[nx] >= 0) {//assign a self-loop if the out-of-domain node is not avoided
		for(size_t col = 0; col < nu; ++col) {
		    _ts._npost[nx*nu+col] = 1;
		    _ts._ptrpost[nx*nu+col] = _ts._ntrans;
		    ++_ts._ntrans;
		}
	    }

	    /* assign _idpost, _cost and _npre */
	    _ts._idpost.resize(_ts._ntrans);
	    double w;
	    // for (size_t row = 0; row < nx; ++row) {
	    // 	for (size_t col = 0; col < nu; ++col) {
	    // 	    /* assign post grid point IDs to _idpost */
	    // 	    if (_ts._npost[row*nu+col] == 0)
	    // 		continue;
	    // 	    for (int l = 0; l < _ts._npost[row*nu+col]; ++l) {
	    // 		postid = stpost[row*nu+col];
	    // 		r = l;
	    // 		for (int k = 0; k < n; ++k) {
	    // 		    postid += (r % rngpost[n*(row*nu+col)+k])*_x._base[k];
	    // 		    r = r / rngpost[n*(row*nu+col)+k];
	    // 		}
	    // 		_ts._idpost[_ts._ptrpost[row*nu+col]+l] = postid;
	    // 		/* assign cost (worst case): maximum from all posts */
	    // 		if (_wf) {
	    // 		    w = (*_wf)(_x._data[row], _x._data[postid], _ptrsys->_ugrid._data[col]);
	    // 		    _ts._cost[row*nu+col] = _ts._cost[row*nu+col]<w ? w : _ts._cost[row*nu+col];
	    // 		}

	    // 		_ts._npre[postid*nu+col] ++;
	    // 	    }
	    // 	} /* end for col */
	    // } /* end for row */
	    int num_post;
	    size_t ptrout;
	    for (size_t row = 0; row < nx; ++row) {
		for (size_t col = 0; col < nu; ++col) {
		    // /********** logging **********/
		    // if(row == 0 && col == 16) {
		    // 	std::cout << "post nodes: ";
		    // }
		    // /********** logging **********/
		    /* assign post grid point IDs to _idpost */
		    if(out_of_domain[row*nu+col] != 1) {//intersect with domain
			if(out_of_domain[row*nu+col]) {
			    num_post = _ts._npost[row*nu+col] - 1;
			} else {
			    num_post = _ts._npost[row*nu+col];
			}
			for (int l = 0; l < num_post; ++l) {
			    postid = stpost[row*nu+col];
			    r = l;
			    for (int k = 0; k < n; ++k) {
				postid += (r % rngpost[n*(row*nu+col)+k])*_x._base[k];
				r = r / rngpost[n*(row*nu+col)+k];
			    }
			    _ts._idpost[_ts._ptrpost[row*nu+col]+l] = postid;
			    // /********** logging **********/
			    // if(row == 0 && col == 16) {
			    // 	std::cout << "ptrpost[" << _ts._ptrpost[row*nu+col]+l << "]="
			    // 		  << postid << '\n';
			    // }
			    // /********** logging **********/
			    /* assign cost (worst case): maximum from all posts */
			    if (_wf) {
				w = (*_wf)(_x._data[row], _x._data[postid], _ptrsys->_ugrid._data[col]);
				_ts._cost[row*nu+col] = _ts._cost[row*nu+col]<w ? w : _ts._cost[row*nu+col];
			    }

			    ++_ts._npre[postid*nu+col];
			    // /********** logging **********/
			    // if(postid == 0 && col == 16)
			    // 	std::cout << "current # of predecessor of node x=0 under u=16: "
			    // 		  << _ts._npre[postid*nu+col] << '\n';
			    // /********** logging **********/
			}
		    }
		    if(out_of_domain[row*nu+col]) {//fully or partially out of domain
			ptrout = _ts._ptrpost[row*nu+col]+_ts._npost[row*nu+col]-1;
			_ts._idpost[ptrout] = nx; //post node is xout
			++_ts._npre[nx*nu+col];
			// /********** logging **********/
			// if(row == 0 && col == 16) {
			//     std::cout << "ptrpost[" << ptrout << "]="
			// 	      << nx << '\n';
			// }
			// /********** logging **********/
		    }
		} /* end for col */
	    } /* end for row */
	    if(_labels[nx] >= 0) {//assign a self-loop if the out-of-domain node is not avoided
		for(size_t col = 0; col < nu; ++col) {
		    _ts._idpost[_ts._ptrpost[nx*nu+col]] = nx; //xout is a sink
		    ++_ts._npre[nx*nu+col];
		}
	    }

	    /* Determine pre's by post's: loop _npost and _idpost */
	    _ts._idpre.resize(_ts._ntrans);  // initialize the size of pre's
	    /* assign _ptrpre by _npre */
	    size_t sum = 0;
	    for (size_t row = 0; row < nx+1; ++row) {
		for (size_t col = 0; col < nu; ++col) {
		    _ts._ptrpre[row*nu+col] = sum;
		    sum += _ts._npre[row*nu+col];
		}
	    }
	    assert(sum == _ts._ntrans);

	    /* assign _idpre */
	    std::vector<size_t> precount(nu*(nx+1), 0);
	    size_t idtspre;
	    for (size_t row = 0; row < nx+1; ++row) {
		for (size_t col = 0; col < nu; ++col) {
		    // /********** logging **********/
		    // if (row == 0 && col == 16) {
		    // 	std::cout << "Assign pre transitions:\n";
		    // }
		    // /********** logging **********/
		    for (int ip = 0; ip < _ts._npost[row*nu+col]; ++ip) {
			postid = _ts._idpost[_ts._ptrpost[row*nu+col]+ip];
			idtspre = postid * nu + col;
			_ts._idpre[_ts._ptrpre[idtspre]+precount[idtspre]++] = row;
			// precount[idtspre]++;
			// /********** logging **********/
			// if (row == 0 && col == 16) {
			//     std::cout << postid << ": idpre[" << _ts._ptrpre[idtspre]
			// 	      << '+' << precount[idtspre]-1 << "]="
			// 	      << row << '\n';
			// }
			// if(_ts._ptrpre[idtspre]+precount[idtspre]-1 == 46) {
			//     std::cout << "idpre[46] is filled at: "
			// 	      << row << "->(" << col << ")->" << postid
			// 	      << '\n';
			// }
			// /********** logging **********/
		    }
		}
	    }
	    return true;
	}

	/**
	 * \brief Assign transitions by subgridding.
	 * see assign_transitions()
	 *
	 * @param[in] rp[] the pointer to an array of relative subgridding size
	 * @return whether construction is successful.
	 */
	bool assign_transitions_subgridding(const double rp[]) {
	    /*********** logging ***********/
	    // std::fstream logfile;
	    // logfile.open("y.log", std::ios::out | std::ios::ate);
	    // std::fstream logfile2;
	    // logfile2.open("post946.log", std::ios::out | std::ios::ate);
	    /*********** logging ***********/
	    if (!init_transitions())
		return false;
	    int n = _x._dim;
	    size_t nx = _x._nv;
	    size_t nu = _ptrsys->_ugrid._nv;

	    /* compute the number of sub grid points */
	    size_t subnv = 1;
	    std::vector<double> subgw(n);
	    std::vector<size_t> number(n);
	    for (int k = 0; k < n; ++k) {
		number[k] = ceil(1.0 / rp[k]);
		subgw[k] = _x._gw[k] / number[k];
		subnv *= number[k];
	    }
	    // std::cout << "number of subgrids: " << subnv << '\n';

	    /* transition computation by interval subgridding: loop states */
	    std::vector<double> xmin(n);
	    // std::vector<double> xc(n);
	    ivec v(n);
	    std::vector<size_t> subposts;
	    std::vector<std::vector<double>> sub(subnv, std::vector<double> (n));
	    std::vector<size_t>::iterator iter;
	    int np = 0;
	    /* loop state grids */
	    for (size_t row = 0; row < nx; ++row) {
		if (_labels[row] == -1) /* skip the avoid grid points (labeled by -1) */
		    continue;
		/* compute reachable set by interval subgridding */
		if (subnv == 1) {  // no subgridding
		    sub[0] = _x._data[row];
		} else {  // subnv > 1
		    for (int k = 0; k < n; ++k) {
			// xmin[k] = xc[k] - _gw[k]/2. + _rp[k]*_gw[k]/2.;
			xmin[k] = _x._data[row][k] - _x._gw[k]/2. + subgw[k]/2.;
		    }
		    _x.griddingHelper(sub, xmin, subgw, number, subnv);
		}
		std::vector< std::vector<ivec> > ys(subnv, std::vector<ivec> (nu));  // ys[vi][ui]
		for (int vi = 0; vi < subnv; ++vi) {
		    for (int k = 0; k < n; ++k) {
			v.setval(k, interval(sub[vi][k]-subgw[k]/2, sub[vi][k]+subgw[k]/2));
		    } // assign v
		    /* get a list of post intervals w.r.t. different inputs */
		    _ptrsys->get_reach_set(ys[vi], v);
		} // end for loop (subgrid)

		/* loop inputs */
		for (size_t col = 0; col < nu; ++col) {
		    std::set<size_t> posts;
		    /*********** logging ***********/
		    // logfile << col << ":\n";
		    /*********** logging ***********/

		    /* loop subgrids: collect all unique posts */
		    for (int vi = 0; vi < subnv; ++vi) {
			subposts = _x.subset(ys[vi][col], true, false);
			if (subposts.empty()) {  // out of domain
			    np = 0;
			    posts.clear();
			    break;  // jump out of the subgrid loop
			} else {
			    posts.insert(subposts.begin(), subposts.end());
			} // end if
			/*********** logging ***********/
			// logfile << ys[vi][col] << "(";
			// for (int i = 0; i < subposts.size(); ++i)
			//     logfile << subposts[i] << ',';
			// logfile << ")\n";
			/*********** logging ***********/
		    } // end collecting posts

		    /* assign posts */
		    if (!posts.empty()) {
			/*********** logging ***********/
			// logfile2 << col << '(' << posts.size() << "): ";
			/*********** logging ***********/
			_ts._npost[row*nu + col] = posts.size();
			_ts._ptrpost[row*nu + col] = _ts._ntrans;
			_ts._ntrans += posts.size();

			for (std::set<size_t>::iterator it = posts.begin(); it != posts.end(); ++it) {
			    _ts._idpost.push_back(*it);

			    /* record the number of pres for state (*it) */
			    _ts._npre[(*it) * nu + col] ++;
			    /*********** logging ***********/
			    // logfile2 << *it << ", ";
			    /*********** logging ***********/
			}
			// logfile2 << '\n';
		    }  // end transition assignment
		}  // end input loop
		// /*********** logging ***********/
		// logfile << '\n';
		// /*********** logging ***********/

	    }  // end for loop states

	    std::cout << "# of transitions: " << _ts._ntrans << '\n';
	    std::cout << "length of _idpost: " << _ts._idpost.size() << '\n';
	    assert(_ts._idpost.size() == _ts._ntrans);

	    /* assign _cost */
	    double w;
	    size_t postid;
	    for (size_t row = 0; row < nx; ++row) {
		for (size_t col = 0; col < nu; ++col) {
		    /* assign post grid point IDs to _idpost */
		    if (_ts._npost[row*nu+col] == 0)
			continue;
		    for (int l = 0; l < _ts._npost[row*nu+col]; ++l) {
			/* assign cost (worst case): maximum from all posts */
			postid = _ts._ptrpost[row*nu+col]+l;
			if (_wf) {
			    w = (*_wf)(_x._data[row], _x._data[_ts._idpost[postid]], _ptrsys->_ugrid._data[col]);
			    _ts._cost[row*nu+col] = _ts._cost[row*nu+col]<w ? w : _ts._cost[row*nu+col];
			}
		    }
		} /* end for col */
	    } /* end for row */

	    /* determine pre's by post's: loop _ts._npost and _idpost */
	    _ts._idpre.resize(_ts._ntrans);  // initialize the size of pre's
	    size_t sum = 0;
	    for (size_t row = 0; row < nx; ++row) {
		for (size_t col = 0; col < nu; ++col) {
		    _ts._ptrpre[row*nu + col] = sum;
		    sum += _ts._npre[row*nu + col];
		}
	    }
	    /* assign _idpre */
	    std::vector<size_t> precount(_ptrsys->_ugrid._nv*_x._nv, 0);
	    size_t idtspre;
	    for (size_t row = 0; row < nx; ++row) {
		for (size_t col = 0; col < nu; ++col) {
		    for (int ip = 0; ip < _ts._npost[row*nu + col]; ++ip) {

			idtspre = _ts._idpost[_ts._ptrpost[row*nu+col] + ip]*nu + col;
			_ts._idpre[_ts._ptrpre[idtspre] + precount[idtspre]] = row;
			precount[idtspre] ++;
		    }
		}
	    }

	    std::cout << "length of _idpre: " << _ts._idpre.size() << '\n';
	    assert(_ts._idpre.size() == _ts._ntrans);

	    // logfile.close();
	    // logfile2.close();

	    return true;
	}

    }; /* the abstraction class */

} // namespace rocs

#endif


// Boost.Polygon library voronoi_basic_tutorial.cpp file

//          Copyright Andrii Sydorchuk 2010-2012.
// 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)

// See http://www.boost.org for updates, documentation, and revision history.

#include <cstdio>
#include <vector>

#include <boost/polygon/voronoi.hpp>
using boost::polygon::voronoi_builder;
using boost::polygon::voronoi_diagram;
using boost::polygon::x;
using boost::polygon::y;
using boost::polygon::low;
using boost::polygon::high;

#include "voronoi_visual_utils.hpp"

struct Point {
  int a;
  int b;
  Point(int x, int y) : a(x), b(y) {}
};

struct Segment {
  Point p0;
  Point p1;
  Segment(int x1, int y1, int x2, int y2) : p0(x1, y1), p1(x2, y2) {}
};

namespace boost {
namespace polygon {

template <>
struct geometry_concept<Point> {
  typedef point_concept type;
};

template <>
struct point_traits<Point> {
  typedef int coordinate_type;

  static inline coordinate_type get(
      const Point& point, orientation_2d orient) {
    return (orient == HORIZONTAL) ? point.a : point.b;
  }
};

template <>
struct geometry_concept<Segment> {
  typedef segment_concept type;
};

template <>
struct segment_traits<Segment> {
  typedef int coordinate_type;
  typedef Point point_type;

  static inline point_type get(const Segment& segment, direction_1d dir) {
    return dir.to_int() ? segment.p1 : segment.p0;
  }
};
}  // polygon
}  // boost

// Traversing Voronoi edges using edge iterator.
int iterate_primary_edges1(const voronoi_diagram<double>& vd) {
  int result = 0;
  for (voronoi_diagram<double>::const_edge_iterator it = vd.edges().begin();
       it != vd.edges().end(); ++it) {
    if (it->is_primary())
      ++result;
  }
  return result;
}

// Traversing Voronoi edges using cell iterator.
int iterate_primary_edges2(const voronoi_diagram<double> &vd) {
  int result = 0;
  for (voronoi_diagram<double>::const_cell_iterator it = vd.cells().begin();
       it != vd.cells().end(); ++it) {
    const voronoi_diagram<double>::cell_type& cell = *it;
    const voronoi_diagram<double>::edge_type* edge = cell.incident_edge();
    // This is convenient way to iterate edges around Voronoi cell.
    do {
      if (edge->is_primary())
        ++result;
      edge = edge->next();
    } while (edge != cell.incident_edge());
  }
  return result;
}

// Traversing Voronoi edges using vertex iterator.
// As opposite to the above two functions this one will not iterate through
// edges without finite endpoints and will iterate only once through edges
// with single finite endpoint.
int iterate_primary_edges3(const voronoi_diagram<double> &vd) {
  int result = 0;
  for (voronoi_diagram<double>::const_vertex_iterator it =
       vd.vertices().begin(); it != vd.vertices().end(); ++it) {
    const voronoi_diagram<double>::vertex_type& vertex = *it;
    const voronoi_diagram<double>::edge_type* edge = vertex.incident_edge();
    // This is convenient way to iterate edges around Voronoi vertex.
    do {
      if (edge->is_primary())
        ++result;
      edge = edge->rot_next();
    } while (edge != vertex.incident_edge());
  }
  return result;
}

int main() {
  // Preparing Input Geometries.
  std::vector<Point> points;
  points.push_back(Point(0, 0));
  points.push_back(Point(1, 6));
  std::vector<Segment> segments;
  segments.push_back(Segment(-4, 5, 5, -1));
  segments.push_back(Segment(3, -11, 13, -1));

  // Construction of the Voronoi Diagram.
  voronoi_diagram<double> vd;
  construct_voronoi(points.begin(), points.end(),
                    segments.begin(), segments.end(),
                    &vd);

  // Traversing Voronoi Graph.
  {
    printf("Traversing Voronoi graph.\n");
    printf("Number of visited primary edges using edge iterator: %d\n",
        iterate_primary_edges1(vd));
    printf("Number of visited primary edges using cell iterator: %d\n",
        iterate_primary_edges2(vd));
    printf("Number of visited primary edges using vertex iterator: %d\n",
        iterate_primary_edges3(vd));
    printf("\n");
  }

  // Using color member of the Voronoi primitives to store the average number
  // of edges around each cell (including secondary edges).
  {
    printf("Number of edges (including secondary) around the Voronoi cells:\n");
    for (voronoi_diagram<double>::const_edge_iterator it = vd.edges().begin();
         it != vd.edges().end(); ++it) {
      std::size_t cnt = it->cell()->color();
      it->cell()->color(cnt + 1);
    }
    for (voronoi_diagram<double>::const_cell_iterator it = vd.cells().begin();
         it != vd.cells().end(); ++it) {
      printf("%lu ", it->color());
    }
    printf("\n");
    printf("\n");
  }

  // Linking Voronoi cells with input geometries.
  {
    unsigned int cell_index = 0;
    for (voronoi_diagram<double>::const_cell_iterator it = vd.cells().begin();
         it != vd.cells().end(); ++it) {
      if (it->contains_point()) {
        if (it->source_category() ==
            boost::polygon::SOURCE_CATEGORY_SINGLE_POINT) {
          std::size_t index = it->source_index();
          Point p = points[index];
          printf("Cell #%u contains a point: (%d, %d).\n",
                 cell_index, x(p), y(p));
        } else if (it->source_category() ==
                   boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) {
          std::size_t index = it->source_index() - points.size();
          Point p0 = low(segments[index]);
          printf("Cell #%u contains segment start point: (%d, %d).\n",
                 cell_index, x(p0), y(p0));
        } else if (it->source_category() ==
                   boost::polygon::SOURCE_CATEGORY_SEGMENT_END_POINT) {
          std::size_t index = it->source_index() - points.size();
          Point p1 = high(segments[index]);
          printf("Cell #%u contains segment end point: (%d, %d).\n",
                 cell_index, x(p1), y(p1));
        }
      } else {
        std::size_t index = it->source_index() - points.size();
        Point p0 = low(segments[index]);
        Point p1 = high(segments[index]);
        printf("Cell #%u contains a segment: ((%d, %d), (%d, %d)). \n",
               cell_index, x(p0), y(p0), x(p1), y(p1));
      }
      ++cell_index;
    }
  }
  return 0;
}


/**
 * @file 	SteinerTreeHeuristic.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jul 14, 2017
 */

#include <sstream>
#include <boost/heap/priority_queue.hpp>
#include "SteinerTreeHeuristic.h"

using std::pair;
typename graph_traits<Graph>::out_edge_iterator it, itEnd;

/**
 * Data that is stored in one node of the heap. Contains an integer and a double.
 * Comparisons are made by the double, smaller has higher priority
 */
struct heapData
{
	pair<int,double> data;

    heapData(pair<int,double> p):
        data(p)
    {}

    bool operator<(heapData const & rhs) const {
        return data.second > rhs.data.second;
    }
};

/*
 * Implementation of the improved Shortest-Path-Heuristic. A standard binary heap
 * from boost is used and keys are never decreased, but instead new nodes are pushed
 * whenever a better path is found. The resulting tree is stored implicitly in
 * treePredecessors.
 */
double SteinerTreeHeuristic::computeSteinerTree(Graph& g, int numVertices,
		int treePredecessors[], vector<int>& terminals, int root) {

	assert(NULL != treePredecessors);

	//terminals are scanned
	int numTerminals = terminals.size();
	bool* isTerminal = new bool[numVertices]();
	for (int i = 0; i < numTerminals; ++i) {
		assert(terminals[i] < numVertices);
		isTerminal[terminals[i]] = true;
	}

	if (!isTerminal[root]) {
		delete[] isTerminal;
		return -1;
	}

	heap::priority_queue<heapData> heap;

	double* distances = new double[numVertices];
	int* predecessors = new int[numVertices];
	predecessors[root] = root;
	double treeCost = 0.;

	//initialization of heap, distance array and predecessors array. note: a -1
	//in predecessors means that the vertex has not been connected to the tree yet
	for (int i = 0; i < numVertices; ++i) {
		if (i == root) {
			heap.push(std::make_pair(i,0.));
			distances[i] = 0.;
			treePredecessors[i] = root;
		} else {
			heap.push(std::make_pair(i, std::numeric_limits<double>::infinity()));
			distances[i] = std::numeric_limits<double>::infinity();
			treePredecessors[i] = -1;
		}
	}

	property_map<Graph, edge_weight_t>::type weights = get(edge_weight, g);
	property_map<Graph, vertex_index_t>::type index = get(vertex_index, g);

	int connectedTerminals = 1;

	//here the actual algorithm starts
	while (connectedTerminals < numTerminals) {
		assert(!heap.empty());	//heap should never be empty before tree contains all terminals

		pair<int,double> min = heap.top().data;
		heap.pop();

		//if we scan a terminal, all vertices on shortest path to subtree
		//are added to heap with weight 0 and included into the tree
		if (min.first != root && isTerminal[min.first] && treePredecessors[min.first] == -1) {
			++connectedTerminals;

			min.second = 0.;
			int nextVertex = predecessors[min.first];
			treePredecessors[min.first] = predecessors[min.first];

			auto edgeInfo = edge(predecessors[min.first], min.first, g);
			assert(edgeInfo.second);
			treeCost += weights[edgeInfo.first];

			//here all vertices on path from new terminal to tree are added again with key=0
			while (treePredecessors[nextVertex] == -1) {
				heap.push(std::make_pair(nextVertex, 0.));
				treePredecessors[nextVertex] = predecessors[nextVertex];
				treeCost += weights[edge(predecessors[nextVertex], nextVertex, g).first];
				nextVertex = predecessors[nextVertex];
			}
		}

		//this is basicalles the usual dijkstra with pushes instead of decreasekey operations
		for (tie(it, itEnd) = out_edges(*(vertices(g).first+min.first), g);
				it != itEnd; ++it) {
			double tmp = min.second + weights[*it];
			int targetIndex = index[target(*it, g)];
			//we only consider vertices which have not been added to the tree yet
			if (treePredecessors[targetIndex] == -1 && tmp < distances[targetIndex]) {
				distances[targetIndex] = tmp;
				predecessors[targetIndex] = min.first;
				heap.push(std::make_pair(targetIndex, tmp));
			}
		}
	}

	delete[] isTerminal;
	delete[] distances;
	delete[] predecessors;

	return treeCost;
}

bool SteinerTreeHeuristic::testAndPrintTree(Graph& g, int numVertices, int treePredecessors[],
		vector<int>& terminals, int root, string& edgeString) {

	std::stringstream stream;
	bool* visited = new bool[numVertices]();

	for (unsigned int i = 0; i < terminals.size(); ++i) {
		int curr = terminals[i];
		int vertexCount = 1;
		int edgeCount = 0;
		while (curr != root && vertexCount != numVertices && curr != -1) {
			if (!visited[curr]) {
				++edgeCount;
				//edges are added to string and every 50 lines a linebreak is
				//added for the sake of readability
				if (edgeCount % 50 == 0) stream << std::endl;
				stream << "(" << curr << "," << treePredecessors[curr] << ") ";
			}
			curr = treePredecessors[curr];
			++vertexCount;
		}

		//if a cycle is found or not all terminals are connected, result is false
		if (vertexCount == numVertices || curr == -1){
			delete[] visited;
			return false;
		}
	}

	edgeString = stream.str();

	delete[] visited;
	return true;
}

bool SteinerTreeHeuristic::testTree(Graph& g, int numVertices, int treePredecessors[],
		vector<int>& terminals, int root) {

		bool* visited = new bool[numVertices]();

		for (unsigned int i = 0; i < terminals.size(); ++i) {
			int curr = terminals[i];
			int vertexCount = 1;
			int edgeCount = 0;
			while (curr != root && vertexCount != numVertices && curr != -1) {
				if (!visited[curr]) ++edgeCount;
				curr = treePredecessors[curr];
				++vertexCount;
			}

			//if a cycle is found or not all terminals are connected, result is false
			if (vertexCount == numVertices || curr == -1){
				delete[] visited;
				return false;
			}
		}

		delete[] visited;
		return true;
}


/**
 * This file contains an example of graphlab used for gibbs sampling
 * in a pairwise markov random field to denoise a synthetic noisy
 * image.
 *
 *  \author Joseph Gonzalez
 */

// INCLUDES ===================================================================>

// Including Standard Libraries
#include <cstdlib>
#include <cassert>
#include <cstring>
#include <fstream>
#include <iostream>
#include <vector>
#include <string>
#include <algorithm>
#include <limits>
#include <cmath>

#include <boost/program_options.hpp>
#include <boost/bind.hpp>

#include <graphlab.hpp>

// ============================================================================>
//  Support code for loopy belief propagation
#include "image.hpp"

// Include the macro for the for each operation
#include <graphlab/macros_def.hpp>

// Determine the shared ata location of the shared edge factor and the
// number of samples
enum constants {EDGE_FACTOR_ID, NUM_SAMPLES_ID};

// STRUCTS (Edge and Vertex data) =============================================>
/**
 * The data associated with each variable in the pairwise markov
 * random field
 */
struct vertex_data {
  size_t asg;
  size_t updates;
  unsigned char color;
  graphlab::unary_factor potential;
  //! store the Roa-Blackwell conditional belief estimate
  graphlab::unary_factor belief;
  std::vector<double> counts;
  vertex_data() :
    asg(0), updates(0), color(0) { }
}; // End of vertex data

/**
 * The data associated with each directed edge in the pairwise markov
 * random field
 */
struct edge_data { };

typedef graphlab::graph< vertex_data, edge_data> graph_type;
typedef graphlab::types<graph_type> gl;

// GraphLab Update Function ===================================================>

/** Construct denoising ising model based on the image */
void construct_graph(image& img,
                     size_t num_rings,
                     double sigma,
                     gl::graph& graph);

/**
 * The core belief propagation update function.  This update satisfies
 * the graphlab update_function interface.
 */
template<bool UseCallback>
void gibbs_update(gl::iscope& scope,
                  gl::icallback& scheduler,
                  gl::ishared_data* shared_data);

/**
 *  The colore schedule is used by the set scheduler to describe an
 *  execution that updates all vertices of the same color (given by a
 *  checkerboard) pattern) in parallel.
 */
void color_schedule(gl::set_scheduler &sched);
// This ugly global variable is needed for the color_schedule
size_t nsamples;

// Command Line Parsing =======================================================>
struct options {
  size_t ncpus;
  size_t samples;
  size_t num_rings;
  size_t rows;
  size_t cols;
  double sigma;
  double lambda;
  std::string smoothing;
  std::string engine;
  std::string scope;
  std::string scheduler;
  std::string orig_fn;
  std::string noisy_fn;
  std::string rb_pred_fn;
  std::string counts_pred_fn;
  std::string pred_type;
};

/**
 * Parse the command line arguments.  Returns false if there was a
 * problem in parsing command line arguments
 */
bool parse_command_line(int argc, char** argv, options& opts);

/**
 * Display the program options
 */
void display_options(options& opts);

// MAIN =======================================================================>
int main(int argc, char** argv) {
  std::cout << "This program uses gibbs sampling to denoise a synthetic image."
            << std::endl;

  // set the global logger
  global_logger().set_log_level(LOG_WARNING);
  global_logger().set_log_to_console(true);

  // Parse command line arguments --------------------------------------------->
  options opts;
  bool success = parse_command_line(argc, argv, opts);
  if(!success)  return EXIT_FAILURE;
  display_options(opts);

  // Create synthetic images -------------------------------------------------->
  // Creating image for denoising
  std::cout << "Creating a synethic image." << std::endl;
  image img(opts.rows, opts.cols);
  img.paint_sunset(opts.num_rings);
  std::cout << "Saving image. " << std::endl;
  img.save(opts.orig_fn.c_str());
  std::cout << "Corrupting Image. " << std::endl;
  img.corrupt(opts.sigma);
  std::cout << "Saving corrupted image. " << std::endl;
  img.save(opts.noisy_fn.c_str());
  img.save_vec("corrupted.tsv");

  // Create the graph --------------------------------------------------------->
  gl::graph graph;
  std::cout << "Constructing pairwise Markov Random Field. " << std::endl;
  construct_graph(img, opts.num_rings, opts.sigma, graph);
  image coloring_img(opts.rows, opts.cols);
  for(size_t i = 0; i < graph.num_vertices(); ++i)
    coloring_img.pixel(i) = graph.vertex_data(i).color;
  coloring_img.save("coloring.pgm");

  // Setup global shared variables -------------------------------------------->
  gl::thread_shared_data sdm;
  // Initialize the edge agreement factor
  std::cout << "Initializing shared edge agreement factor. " << std::endl;
  // dummy variables 0 and 1 and num_rings by num_rings
  graphlab::binary_factor edge_potential(0, opts.num_rings,
                                         0, opts.num_rings);

  // Set the smoothing type
  if(opts.smoothing == "square") {
    edge_potential.set_as_agreement(opts.lambda);
  } else if (opts.smoothing == "laplace") {
    edge_potential.set_as_laplace(opts.lambda);
  } else {
    std::cout << "Invalid smoothing stype!" << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << edge_potential << std::endl;
  sdm.set_constant(EDGE_FACTOR_ID, graphlab::any(edge_potential));
  sdm.set_constant(NUM_SAMPLES_ID, graphlab::any(opts.samples));

  // Create the engine -------------------------------------------------------->
  gl::iengine* engine = NULL;
  if(opts.scheduler == "set") {
    nsamples = opts.samples;

    // Here we use the set scheduler which has some additional special setup
    engine =
      graphlab::engine_factory::new_engine(opts.engine,
                                           "set",
                                           "null",
                                           graph,
                                           opts.ncpus);
    if(engine == NULL) {
      std::cout << "Unable to construct engine!" << std::endl;
      return EXIT_FAILURE;
    }
    // Set the shared data manager for the engine
    engine->set_shared_data_manager(&sdm);
    // Attach the correct schedule
    engine->get_scheduler().set_option(gl::scheduler_options::SCHEDULING_FUNCTION,
                                       (void*)color_schedule);

  } else {
    // Here we use the other schedulers which use a more default setup
    engine =
      graphlab::engine_factory::new_engine(opts.engine,
                                           opts.scheduler,
                                           opts.scope,
                                           graph,
                                           opts.ncpus);
    if(engine == NULL) {
      std::cout << "Unable to construct engine!" << std::endl;
      return EXIT_FAILURE;
    }
    // Set the shared data manager for the engine
    engine->set_shared_data_manager(&sdm);

    // Create a shuffled set of vertices
    std::vector<gl::vertex_id_t> vertex_ids(opts.rows * opts.cols);
    for(size_t i = 0; i < vertex_ids.size(); ++i) vertex_ids[i] = i;
    std::random_shuffle(vertex_ids.begin(), vertex_ids.end());

    // Add the tasks in shuffled order (helps with fifo scheduler)
    const bool use_callback = true;
    double residual = 1.0;
    engine->get_scheduler().add_tasks(vertex_ids,
                                      gibbs_update<use_callback>,
                                      residual);
  }

  // Running the engine ------------------------------------------------------->
  std::cout << "Running the engine. " << std::endl;
  graphlab::timer timer; timer.start();
  engine->start();
  double runtime = timer.current_time();
  size_t update_count = engine->last_update_count();
  std::cout << "Finished Running engine in " << runtime
            << " seconds." << std::endl
            << "Total updates: " << update_count << std::endl
            << "Efficiency: " << (double(update_count) / runtime)
            << " updates per second "
            << std::endl;

  // Saving the output -------------------------------------------------------->
  std::cout << "Rendering the cleaned image. " << std::endl;
  image rb_img(opts.rows, opts.cols);
  image counts_img(opts.rows, opts.cols);
  if(opts.pred_type == "map") {
    for(size_t v = 0; v < graph.num_vertices(); ++v) {
      const vertex_data& vdata = graph.vertex_data(v);
      rb_img.pixel(v) = vdata.belief.max_asg();
      counts_img.pixel(v) =
        std::max_element(vdata.counts.begin(), vdata.counts.end()) -
        vdata.counts.begin();

    }
  } else if(opts.pred_type == "exp") {
    for(size_t v = 0; v < graph.num_vertices(); ++v) {
      const vertex_data& vdata = graph.vertex_data(v);
      rb_img.pixel(v) = vdata.belief.expectation();
      double expectation = 0;
      for(size_t i = 0; i < vdata.counts.size(); ++i)
        expectation += i * vdata.counts[i];
      counts_img.pixel(v) = expectation;
    }
  } else {
    std::cout << "Invalid prediction type! : " << opts.pred_type
              << std::endl;
    return EXIT_FAILURE;
  }

  std::cout << "Saving cleaned image. " << std::endl;
  rb_img.save(opts.rb_pred_fn.c_str());
  counts_img.save(opts.counts_pred_fn.c_str());

  std::cout << "Done!" << std::endl;
  return EXIT_SUCCESS;
} // End of main

// Implementations ============================================================>
template<bool UseCallback>
void gibbs_update(gl::iscope& scope,
                  gl::icallback& scheduler,
                  gl::ishared_data* shared_data) {

  assert(shared_data != NULL);
  // Get the shared data
  size_t samples = shared_data->get_constant(NUM_SAMPLES_ID).as<size_t>();

  // Grab the state from the scope -------------------------------------------->
  // Get the vertex data
  vertex_data& vdata = scope.vertex_data();

  // If this vertex has been updated sufficiently often then we don't
  // update again
  if(vdata.updates >= samples) return;

  // Construct the conditional------------------------------------------------->
  // Initialize the conditional as the node potential
  graphlab::unary_factor conditional(vdata.potential);

  const graphlab::binary_factor& edge_factor =
    shared_data->get_constant(EDGE_FACTOR_ID).as<graphlab::binary_factor>();

  // Condition on all neighbor assignments
  foreach(graphlab::edge_id_t edgeid, scope.in_edge_ids()) {
    graphlab::vertex_id_t source = scope.source(edgeid);
    // Get the neighboring assignment
    const vertex_data& neighbor = scope.neighbor_vertex_data(source);
    // Get the edge factor
    conditional.condition(edge_factor, neighbor.asg);
    conditional.normalize();
  }

  // Generate a sample -------------------------------------------------------->
  size_t sample = conditional.sample();
  vdata.asg = sample;  // Record the sample
  vdata.updates++;     // Increment the updates
  vdata.counts[vdata.asg]++; // increment the counts

  // Rao-Blackwell belief estimate
  vdata.belief.plus(conditional);

  // Finalize belief on last update ------------------------------------------->
  if(vdata.updates == samples ) { // If this is the last update
    vdata.belief.normalize();
    double Z = 0;
    for(size_t i = 0; i < vdata.counts.size(); ++i)
      Z += vdata.counts[i];
    for(size_t i = 0; i < vdata.counts.size(); ++i)
      vdata.counts[i] /= Z;
  } else if(UseCallback) {
    // Otherwise reschedule self
    double residual = 1.0;
    gl::update_task task(scope.vertex(), gibbs_update<UseCallback> );
    scheduler.add_task(task, residual);
  }
} // end of BP_update

bool parse_command_line(int argc, char** argv, options& opts) {
  // Because typing is painful
  namespace boost_po = boost::program_options;
  // Create a description for this program
  boost_po::options_description
    desc("Denoise a randomly generated image using Gibbs Sampling.");
  // Set the program options
  desc.add_options()
    ("help",   "produce this help message")
    ("ncpus",  boost_po::value<size_t>(&(opts.ncpus))->default_value(2),
     "Number of cpus to use.")
    ("samples",  boost_po::value<size_t>(&(opts.samples))->default_value(100),
     "Number of samples to generate")
    ("rings",  boost_po::value<size_t>(&(opts.num_rings))->default_value(5),
     "Number of rings in the noisy image")
    ("rows",  boost_po::value<size_t>(&(opts.rows))->default_value(200),
     "Number of rows in the noisy image")
    ("cols",  boost_po::value<size_t>(&(opts.cols))->default_value(200),
     "Number of columns in the noisy image")
    ("sigma",  boost_po::value<double>(&(opts.sigma))->default_value(1.2),
     "Standard deviation of noise.")
    ("lambda",  boost_po::value<double>(&(opts.lambda))->default_value(3),
     "Smoothness parameter (larger => smoother).")
    ("smoothing",
     boost_po::value<std::string>(&(opts.smoothing))->default_value("square"),
     "Options are {square, laplace}")
    ("engine",
     boost_po::value<std::string>(&(opts.engine))->default_value("threaded"),
     "Options are {threaded, sequential}")
    ("scope",
     boost_po::value<std::string>(&(opts.scope))->default_value("edge"),
     "Options are {vertex, edge, full}")
    ("scheduler",
     boost_po::value<std::string>(&(opts.scheduler))->default_value("fifo"),
     "Options are {fifo, priority, sampling}")
    ("orig",
     boost_po::value<std::string>(&(opts.orig_fn))->default_value("source_img.pgm"),
     "Original image file name.")
    ("noisy",
     boost_po::value<std::string>(&(opts.noisy_fn))->default_value("noisy_img.pgm"),
     "Noisy image file name.")
    ("rb_pred",
     boost_po::value<std::string>(&(opts.rb_pred_fn))->
     default_value("rb_pred_img.pgm"),
     "Predicted image file name for the Rao-Blackwell estimator.")
    ("counts_pred",
     boost_po::value<std::string>(&(opts.counts_pred_fn))->
     default_value("counts_pred_img.pgm"),
     "Predicted image file name for the raw counts estimator.")
    ("pred_type",
     boost_po::value<std::string>(&(opts.pred_type))->default_value("map"),
     "Predicted image type {map, exp}");
  // Parse the arguments
  boost_po::variables_map vm;
  boost_po::store(boost_po::parse_command_line(argc, argv, desc), vm);
  boost_po::notify(vm);
  if(vm.count("help")) {
    std::cout << desc << std::endl;
    return false;
  }
  return true;
} // end of parse command line arguments

void display_options(options& opts) {
  std::cout << "ncpus:          " << opts.ncpus << std::endl
            << "samples:        " << opts.samples << std::endl
            << "num_rings:      " << opts.num_rings << std::endl
            << "rows:           " << opts.rows << std::endl
            << "cols:           " << opts.cols << std::endl
            << "sigma:          " << opts.sigma << std::endl
            << "lambda:         " << opts.lambda << std::endl
            << "smoothing:      " << opts.smoothing << std::endl
            << "engine:         " << opts.engine << std::endl
            << "scope:          " << opts.scope << std::endl
            << "scheduler:      " << opts.scheduler << std::endl
            << "orig_fn:        " << opts.orig_fn << std::endl
            << "noisy_fn:       " << opts.noisy_fn << std::endl
            << "rb_pred_fn:     " << opts.rb_pred_fn << std::endl
            << "counts_pred_fn: " << opts.counts_pred_fn << std::endl
            << "pred_type:      " << opts.pred_type << std::endl;
}

// STRUCTS (Edge and Vertex data) =============================================>

void construct_graph(image& img,
                     size_t num_rings,
                     double sigma,
                     gl::graph& graph) {

  // Initialize the vertex data to somethine sensible
  vertex_data vdata;

  vdata.belief.resize(num_rings);
  vdata.belief.uniform(-std::numeric_limits<double>::max());
  vdata.potential.resize(num_rings);
  vdata.potential.uniform();
  vdata.potential.normalize();
  vdata.counts.resize(num_rings, 0);
  for(size_t i = 0; i < num_rings; ++i)
    vdata.counts[i] = 0;

  // Add all the vertices
  double sigmaSq = sigma*sigma;
  for(size_t i = 0; i < img.rows(); ++i) {
    for(size_t j = 0; j < img.cols(); ++j) {
      // initialize the potential and belief
      uint32_t pixel_id = img.vertid(i, j);
      vdata.potential.var() = vdata.belief.var() = pixel_id;
      // Determine the color (in checkerboard form)
      vdata.color = (i%2 == 0) ^ (j%2 == 0)? 1 : 0;
      // Set the node potential
      double obs = img.pixel(i, j);
      for(size_t pred = 0; pred < num_rings; ++pred) {
        vdata.potential.logP(pred) =
          -(obs - pred)*(obs - pred) / (2.0 * sigmaSq);
      }
      vdata.potential.normalize();
      // Set the initial assignment
      vdata.asg = vdata.potential.sample();
      // Store the actual data in the graph
      size_t vert_id = graph.add_vertex(vdata);
      // Ensure that we are using a consistent numbering
      assert(vert_id == pixel_id);
    } // end of for j in cols
  } // end of for i in rows
  // Construct an edge blob

  // Add all the edges
  for(size_t i = 0; i < img.rows(); ++i) {
    for(size_t j = 0; j < img.cols(); ++j) {
      size_t vertid = img.vertid(i,j);
      if(i-1 < img.rows())
        graph.add_edge(vertid, img.vertid(i-1, j));
      if(i+1 < img.rows())
        graph.add_edge(vertid, img.vertid(i+1, j));
      if(j-1 < img.cols())
        graph.add_edge(vertid, img.vertid(i, j-1));
      if(j+1 < img.cols())
        graph.add_edge(vertid, img.vertid(i, j+1));
    } // end of for j in cols
  } // end of for i in rows
} // End of construct graph

/** Color selection function */
template<size_t color>
bool select_color(graphlab::vertex_id_t v,
                  const vertex_data& vdata) {
  return vdata.color == color;
}

/**
 * The scheduling function used by the set scheduler to execute in
 * colored order
 */
void color_schedule(gl::set_scheduler &sched) {
  // There is a two coloring of this model
  // All sets must be created before scheduling calls
  std::vector<gl::ivertex_set*> colorsets(2);
  // Colors do not change during execution
  bool fixed_selection = true;
  gl::selector_function select_color_0( select_color<0> );
  colorsets[0] = &sched.attach(gl::rvset(select_color_0, fixed_selection),
                               sched.root_set());
  gl::selector_function select_color_1( select_color<1> );
  colorsets[1] = &sched.attach(gl::rvset(select_color_1, fixed_selection),
                               sched.root_set());
  // Actually construct the color sets
  sched.init();

  // Build the execution plan
  gl::execution_plan eplan;
  for(size_t i = 0; i < colorsets.size(); ++i) {
    std::cout << "Set " << i << " has "
              << colorsets[i]->size() << " vertices."
              << std::endl;
    const bool use_callback = false;
    eplan.execute(*colorsets[i], gibbs_update<use_callback>);
  }

  // Compile the execution plan
  graphlab::timer timer; timer.start();
  eplan.generate_plan(sched.get_graph(), sched.num_cpus());
  std::cout << "Execution plan compiled in "
            << timer.current_time() << " seconds" << std::endl;

  // Run the plan nsamples times
  for(size_t iter = 0; iter < nsamples; ++iter)
    sched.execute_plan(eplan);
}


#include <fstream>
#include <algorithm>
#include <iterator>

#include <boost/functional/value_factory.hpp>
#include <boost/array.hpp>

#include <CGAL/algorithm.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/AABB_halfedge_graph_segment_primitive.h>
#include <CGAL/Timer.h>

typedef CGAL::Epick K;
typedef K::FT FT;
typedef K::Point_3 Point;
typedef K::Vector_3 Vector;
typedef K::Segment_3 Segment;
typedef K::Ray_3 Ray;
typedef CGAL::Surface_mesh<CGAL::Point_3<CGAL::Epick> > Mesh;
typedef CGAL::AABB_face_graph_triangle_primitive<Mesh,
CGAL::Default,
CGAL::Tag_false> T_Primitive;
typedef CGAL::AABB_traits<K, T_Primitive> T_Traits;
typedef CGAL::AABB_tree<T_Traits> T_Tree;
typedef T_Tree::Primitive_id T_Primitive_id;

typedef CGAL::AABB_halfedge_graph_segment_primitive<Mesh,
CGAL::Default,
CGAL::Tag_false> E_Primitive;
typedef CGAL::AABB_traits<K, E_Primitive> E_Traits;
typedef CGAL::AABB_tree<E_Traits> E_Tree;
typedef E_Tree::Primitive_id E_Primitive_id;

int main()
{
  CGAL::Surface_mesh<CGAL::Point_3<CGAL::Epick> > m1, m2;
  std::ifstream in("data/cube.off");
  if(in)
    in >> m1;
  else{
    std::cout << "error reading cube" << std::endl;
    return 1;
  }
  in.close();
  in.open(CGAL::data_file_path("meshes/tetrahedron.off"));
  if(in)
    in >> m2;
  else{
    std::cout << "error reading tetrahedron" << std::endl;
    return 1;
  }
  in.close();
  T_Tree tree(faces(m1).first, faces(m1).second, m1);
  tree.insert(faces(m2).first, faces(m2).second, m2);
  tree.build();
  T_Tree::Bounding_box bbox = tree.bbox();
  Point bbox_center((bbox.xmin() + bbox.xmax()) / 2,
                     (bbox.ymin() + bbox.ymax()) / 2,
                     (bbox.zmin() + bbox.zmax()) / 2);
  std::vector< T_Primitive_id > intersections;
  Ray ray(bbox_center+Vector(3,-0.25,0),bbox_center+Vector(-3,+0.25,0));
  tree.all_intersected_primitives(ray,
                                  std::back_inserter(intersections));
  E_Tree e_tree(edges(m1).first, edges(m1).second, m1);
  e_tree.insert(edges(m2).first, edges(m2).second, m2);
  e_tree.build();
  std::vector< E_Primitive_id > e_intersections;
  Ray e_ray(Point(0,0,0),Point(0,1,1));
  e_tree.all_intersected_primitives(e_ray,
                                  std::back_inserter(e_intersections));

  return 0;
}