// 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__FLATTEN_OCP_HPP_
#define SMOOTH__FEEDBACK__FLATTEN_OCP_HPP_

/**
 * @file
 * @brief Reformulate an optimal control problem on a Lie group as an optimal control problem in the
 * tangent space around a reference trajectory.
 *
 * @todo More efficient implementation of Hessian in FlatDyn.
 * @todo Accept dxl as a template argument to avoid double differentiation.
 */

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <smooth/bundle.hpp>
#include <smooth/diff.hpp>

#include "ocp.hpp"
#include "smooth/lie_group_sparse.hpp"
#include "utils/sparse.hpp"

namespace smooth::feedback {

// \cond
namespace detail {

/// @brief The first Bernoulli numbers
static constexpr std::array<double, 23> kBn{
  1,               // 0
  -1. / 2,         // 1
  1. / 6,          // 2
  0.,              // 3
  -1. / 30,        // 4
  0,               // 5
  1. / 42,         // 6
  0,               // 7
  -1. / 30,        // 8
  0,               // 9
  5. / 66,         // 10
  0,               // 11
  -691. / 2730,    // 12
  0,               // 13
  7. / 6,          // 14
  0,               // 15
  -3617. / 510,    // 16
  0,               // 17
  43867. / 798,    // 18
  0,               // 19
  -174611. / 330,  // 20
  0,               // 21
  854513. / 138,   // 22
};

/**
 * @brief Sparse matrices containing reordered rows of algebra generators.
 */
template<LieGroup G>
inline auto generators_sparse_reordered =
  []() -> std::array<Eigen::SparseMatrix<double, Eigen::RowMajor>, Dof<G>> {
  std::array<Eigen::SparseMatrix<double, Eigen::RowMajor>, Dof<G>> ret;
  for (auto k = 0u; k < Dof<G>; ++k) {
    ret[k].resize(Dof<G>, Dof<G>);
    for (auto i = 0u; i < Dof<G>; ++i) { ret[k].row(i) = ::smooth::generators_sparse<G>[i].row(k); }
    ret[k].makeCompressed();
  }
  return ret;
}();

/**
 * @brief Derivative of the adjoint as sparse matrix
 */
template<LieGroup G>
inline Eigen::SparseMatrix<double> d_ad = []() -> Eigen::SparseMatrix<double> {
  Eigen::SparseMatrix<double> ret;
  ret.resize(Dof<G>, Dof<G> * Dof<G>);
  for (auto i = 0u; i < Dof<G>; ++i) {
    for (auto j = 0u; j < Dof<G>; ++j) {
      ret.col(j * Dof<G> + i) = smooth::generators_sparse<G>[i].row(j).transpose();
    }
  }
  ret.makeCompressed();
  return ret;
}();

/**
 * @brief Flattening of dynamics function (t, x, u) -> Tangent, and its derivatives.
 *
 * @note Do not use numeric differentiation of operator() (it differentiates inside)
 * @note Only considers first derivative of xl and ul
 */
template<LieGroup X, Manifold U, typename F, typename Xl, typename Ul>
class FlatDyn
{
private:
  using BundleT = smooth::Bundle<Eigen::Vector<double, 1>, X, U>;

  static constexpr auto Nx    = Dof<X>;
  static constexpr auto Nu    = Dof<U>;
  static constexpr auto Nouts = Nx;
  static constexpr auto Nvars = 1 + Nx + Nu;

  static constexpr auto t_B = 0;
  static constexpr auto x_B = t_B + 1;
  static constexpr auto u_B = x_B + Nx;

  using E = Tangent<X>;
  using V = Tangent<U>;

  F f;
  Xl xl;
  Ul ul;

  Eigen::SparseMatrix<double> ad_e_ = smooth::ad_sparse_pattern<X>;
  Eigen::SparseMatrix<double> ad_vi = smooth::ad_sparse_pattern<X>;

  Eigen::SparseMatrix<double> Joplus_ = smooth::d_exp_sparse_pattern<BundleT>;
  Eigen::SparseMatrix<double> Hoplus_ = smooth::d2_exp_sparse_pattern<BundleT>;

  Eigen::SparseMatrix<double> dexpinv_e_  = smooth::d_exp_sparse_pattern<X>;
  Eigen::SparseMatrix<double> d2expinv_e_ = smooth::d2_exp_sparse_pattern<X>;

  Eigen::SparseMatrix<double> J_{Nouts, Nvars};
  Eigen::SparseMatrix<double> H_{Nvars, Nouts * Nvars};

  // Would ideally like to remove these temporaries...
  Eigen::SparseMatrix<double> ji_{Nouts, Nvars}, ji_tmp_{Nouts, Nvars}, hi_{Nvars, Nouts *Nvars},
    hi_tmp_{Nvars, Nouts *Nvars};

  /// @brief Calculate jacobian of (t, x(t)+e, u(t)+v) w.r.t. (t, e, v)
  void update_joplus(const E & e, const V & v, const E & dxl, const V & dul)
  {
    dr_exp_sparse<BundleT>(Joplus_, (Tangent<BundleT>() << 1, e, v).finished());

    block_write(Joplus_, x_B, t_B, Ad<X>(smooth::exp<X>(-e)) * dxl);
    block_write(Joplus_, u_B, t_B, Ad<U>(smooth::exp<U>(-v)) * dul);

    Joplus_.makeCompressed();
  }

  /// @brief Calculate hessian of (t, x(t)+e, u(t)+v) w.r.t. (t, e, v)
  void update_hoplus(const E & e, const V & v, const E & dxl, const V & dul)
  {
    d2r_exp_sparse<BundleT>(Hoplus_, (Tangent<BundleT>() << 1, e, v).finished());

    // d (Ad_X b) = -ad_(Ad_X b) * Ad_X
    const TangentMap<X> Adexp_X  = Ad<X>(smooth::exp<X>(-e));
    const TangentMap<X> dAdexp_X = ad<X>(Adexp_X * dxl) * Adexp_X * dr_exp<X>(-e);
    const TangentMap<U> Adexp_U  = Ad<U>(smooth::exp<U>(-v));
    const TangentMap<U> dAdexp_U = ad<U>(Adexp_U * dul) * Adexp_U * dr_exp<U>(-v);

    for (auto nx = 0u; nx < Nx; ++nx) {
      const auto b0 = Nvars * (x_B + nx);
      block_write(Hoplus_, t_B, b0 + x_B, dAdexp_X.middleRows(nx, 1));
    }
    for (auto nu = 0u; nu < Nu; ++nu) {
      const auto b0 = Nvars * (u_B + nu);
      block_write(Hoplus_, t_B, b0 + u_B, dAdexp_U.middleRows(nu, 1));
    }

    Hoplus_.makeCompressed();
  }

public:
  template<typename A1, typename A2, typename A3>
  FlatDyn(A1 && a1, A2 && a2, A3 && a3)
      : f(std::forward<A1>(a1)), xl(std::forward<A2>(a2)), ul(std::forward<A3>(a3))
  {}

  template<typename T>
  CastT<T, E> operator()(const T & t, const CastT<T, E> & e, const CastT<T, V> & v) const
  {
    using XT = CastT<T, X>;

    // can not double-differentiate, so we hide derivative of xl w.r.t. t
    const double tdbl           = static_cast<double>(t);
    const auto [unused, dxlval] = diff::dr(xl, wrt(tdbl));

    return dr_expinv<XT>(e) * (f(t, rplus(xl(t), e), rplus(ul(t), v)) - dxlval.template cast<T>())
         + ad<XT>(e) * dxlval.template cast<T>();
  }

  // First derivative
  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  jacobian(double t, const E & e, const V & v) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, U>>)
  {
    const double tdbl          = static_cast<double>(t);
    const auto [xlval, dxlval] = diff::dr(xl, wrt(tdbl));
    const auto [ulval, dulval] = diff::dr(ul, wrt(tdbl));
    const auto x               = rplus(xlval, e);
    const auto u               = rplus(ulval, v);

    dr_expinv_sparse<X>(dexpinv_e_, e);
    d2r_expinv_sparse<X>(d2expinv_e_, e);
    update_joplus(e, v, dxlval, dulval);

    // value and derivative of f
    const auto fval = f(t, x, u);
    const auto & Jf = f.jacobian(t, x, u);

    // Want to differentiate  drexpinv * (f o plus - dxl) + ad dxl

    // Start with drexpinv * d (f \circ (+))
    J_ = dexpinv_e_ * Jf * Joplus_;
    // Add d ( drexpinv ) * (f \circ (+) - dxl)
    for (auto i = 0u; i < d2expinv_e_.outerSize(); ++i) {
      for (Eigen::InnerIterator it(d2expinv_e_, i); it; ++it) {
        J_.coeffRef(it.col() / Nx, 1 + (it.col() % Nx)) +=
          (fval(it.row()) - dxlval(it.row())) * it.value();
      }
    }
    // Add d ( ad ) * dxl
    for (auto i = 0u; i < d_ad<X>.outerSize(); ++i) {
      for (Eigen::InnerIterator it(d_ad<X>, i); it; ++it) {
        J_.coeffRef(it.col() / Nx, 1 + (it.col() % Nx)) += dxlval(it.row()) * it.value();
      }
    }

    J_.makeCompressed();
    return J_;
  }

  // Second derivative
  //    \sum Bn (-1)^n / n! d2r (ad_a^n f)_aa - \sum Bn / n! d2r(ad_a^n dxl)_aa
  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  hessian(double t, const E & e, const V & v) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, U>> &&
      diff::detail::diffable_order2<F, std::tuple<double, X, U>>)
  {
    const double tdbl          = static_cast<double>(t);
    const auto [xlval, dxlval] = diff::dr(xl, wrt(tdbl));
    const auto [ulval, dulval] = diff::dr(ul, wrt(tdbl));

    const auto x    = rplus(xlval, e);
    const auto u    = rplus(ul(t), v);
    const auto & Jf = f.jacobian(t, x, u);  // nx x (1 + nx + nu)
    const auto & Hf = f.hessian(t, x, u);   // (1 + nx + nu) x (nx * (1 + nx + nu))

    ad_sparse<X>(ad_e_, e);
    update_joplus(e, v, dxlval, dulval);
    update_hoplus(e, v, dxlval, dulval);

    double coef   = 1;                    // hold (-1)^i / i!
    Tangent<X> vi = f(t, x, u) - dxlval;  // (ad_a)^i * (f - dxl)
    ji_           = Jf * Joplus_;         // dr (vi)_{t, e, v}
    set_zero(hi_);
    d2r_fog(hi_, Jf, Hf, Joplus_, Hoplus_);  // d2r (vi)_{t, e, v}

    set_zero(H_);
    for (auto iter = 0u; iter < std::tuple_size_v<decltype(kBn)>; ++iter) {
      if (kBn[iter] != 0) { block_add(H_, 0, 0, hi_, kBn[iter] * coef); }

      // update hi_
      hi_tmp_.setZero();
      for (auto i = 0u; i < ad_e_.outerSize(); ++i) {
        for (Eigen::InnerIterator it(ad_e_, i); it; ++it) {
          const auto b0 = it.row() * Nvars;
          block_add(hi_tmp_, 0, b0, hi_.middleCols(it.col() * Nvars, Nvars), it.value());
        }
      }
      for (auto k = 0u; k < Nx; ++k) {
        const auto b0 = k * Nvars;
        block_add(hi_tmp_, 1, b0, generators_sparse_reordered<X>[k] * ji_);
        block_add(hi_tmp_, 0, b0 + 1, ji_.transpose() * generators_sparse_reordered<X>[k], -1);
      }
      std::swap(hi_, hi_tmp_);

      // update ji
      ji_tmp_.setZero();
      ji_tmp_ = ad_e_ * ji_;
      ad_sparse<X>(ad_vi, vi);
      block_add(ji_tmp_, 0, 1, ad_vi, -1);
      std::swap(ji_, ji_tmp_);

      // update vi
      vi.applyOnTheLeft(ad_e_);

      coef *= (-1.) / (iter + 1);
    }

    H_.makeCompressed();
    return H_;
  }
};

/**
 * @brief Flattening of inner function (t, x, u) -> Vector, and its derivatives.
 *
 * @note Only considers first derivative of xl and ul
 */
template<LieGroup X, Manifold U, std::size_t Nouts, typename F, typename Xl, typename Ul>
class FlatInnerFun
{
private:
  using BundleT = smooth::Bundle<Eigen::Vector<double, 1>, X, U>;

  F f;
  Xl xl;
  Ul ul;

  static constexpr auto Nx    = Dof<X>;
  static constexpr auto Nu    = Dof<U>;
  static constexpr auto Nvars = 1 + Nx + Nu;

  using E = Tangent<X>;
  using V = Tangent<U>;

  static constexpr auto t_B = 0;
  static constexpr auto x_B = t_B + 1;
  static constexpr auto u_B = x_B + Nx;

  Eigen::SparseMatrix<double> Joplus_ = smooth::d_exp_sparse_pattern<BundleT>;
  Eigen::SparseMatrix<double> Hoplus_ = smooth::d2_exp_sparse_pattern<BundleT>;

  Eigen::SparseMatrix<double> J_{Nouts, Nvars};
  Eigen::SparseMatrix<double> H_{Nvars, Nouts * Nvars};

  /// @brief Calculate jacobian of (t, x(t)+e, u(t)+v) w.r.t. (t, e, v)
  void update_joplus(const E & e, const V & v, const E & dxl, const V & dul)
  {
    dr_exp_sparse<BundleT>(Joplus_, (Tangent<BundleT>() << 1, e, v).finished());

    block_write(Joplus_, x_B, t_B, Ad<X>(smooth::exp<X>(-e)) * dxl);
    block_write(Joplus_, u_B, t_B, Ad<U>(smooth::exp<U>(-v)) * dul);

    Joplus_.makeCompressed();
  }

  /// @brief Calculate hessian of (t, x(t)+e, u(t)+v) w.r.t. (t, e, v)
  void update_hoplus(const E & e, const V & v, const E & dxl, const V & dul)
  {
    d2r_exp_sparse<BundleT>(Hoplus_, (Tangent<BundleT>() << 1, e, v).finished());

    // d (Ad_X b) = -ad_(Ad_X b) * Ad_X
    const TangentMap<X> Adexp_X  = Ad<X>(smooth::exp<X>(-e));
    const TangentMap<X> dAdexp_X = ad<X>(Adexp_X * dxl) * Adexp_X * dr_exp<X>(-e);
    const TangentMap<U> Adexp_U  = Ad<U>(smooth::exp<U>(-v));
    const TangentMap<U> dAdexp_U = ad<U>(Adexp_U * dul) * Adexp_U * dr_exp<U>(-v);

    for (auto nx = 0u; nx < Nx; ++nx) {
      const auto b0 = Nvars * (x_B + nx);
      block_write(Hoplus_, t_B, b0 + x_B, dAdexp_X.middleRows(nx, 1));
    }
    for (auto nu = 0u; nu < Nu; ++nu) {
      const auto b0 = Nvars * (u_B + nu);
      block_write(Hoplus_, t_B, b0 + u_B, dAdexp_U.middleRows(nu, 1));
    }

    Hoplus_.makeCompressed();
  }

public:
  template<typename A1, typename A2, typename A3>
  FlatInnerFun(A1 && a1, A2 && a2, A3 && a3)
      : f(std::forward<A1>(a1)), xl(std::forward<A2>(a2)), ul(std::forward<A3>(a3))
  {}

  template<typename T>
  Eigen::Vector<T, Nouts>
  operator()(const T & t, const CastT<T, E> & e, const CastT<T, V> & v) const
  {
    return f.template operator()<T>(t, rplus(xl(t), e), rplus(ul(t), v));
  }

  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  jacobian(double t, const E & e, const V & v) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, U>>)
  {
    const auto & [xlval, dxlval] = diff::dr(xl, wrt(t));
    const auto & [ulval, dulval] = diff::dr(ul, wrt(t));
    const auto & Jf              = f.jacobian(t, rplus(xlval, e), rplus(ulval, v));

    update_joplus(e, v, dxlval, dulval);

    J_ = Jf * Joplus_;
    J_.makeCompressed();
    return J_;
  }

  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  hessian(double t, const E & e, const V & v) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, U>> &&
      diff::detail::diffable_order2<F, std::tuple<double, X, U>>)
  {
    const auto & [xlval, dxlval] = diff::dr(xl, wrt(t));
    const auto & [ulval, dulval] = diff::dr(ul, wrt(t));
    const auto x                 = rplus(xlval, e);
    const auto u                 = rplus(ulval, v);
    const auto & Jf              = f.jacobian(t, x, u);
    const auto & Hf              = f.hessian(t, x, u);

    update_joplus(e, v, dxlval, dulval);
    update_hoplus(e, v, dxlval, dulval);

    set_zero(H_);
    d2r_fog(H_, Jf, Hf, Joplus_, Hoplus_);
    H_.makeCompressed();
    return H_;
  }
};

/**
 * @brief Flattening of endpoint function (tf, x0, xf, q) -> Vector, and its derivatives.
 *
 * @note Only considers first derivative of xl
 */
template<LieGroup X, Manifold U, std::size_t Nq, std::size_t Nouts, typename F, typename Xl>
class FlatEndptFun
{
private:
  using E       = Tangent<X>;
  using Q       = Eigen::Vector<Scalar<X>, Nq>;
  using BundleT = smooth::Bundle<Eigen::Vector<double, 1>, X, X, Q>;

  F f;
  Xl xl;

  static constexpr auto Nx    = Dof<X>;
  static constexpr auto Nvars = 1 + 2 * Nx + Nq;

  static constexpr auto tf_B = 0;
  static constexpr auto x0_B = tf_B + 1;
  static constexpr auto xf_B = x0_B + Nx;
  static constexpr auto q_B  = xf_B + Nx;

  Eigen::SparseMatrix<double> Joplus_ = d_exp_sparse_pattern<BundleT>;
  Eigen::SparseMatrix<double> Hoplus_ = d2_exp_sparse_pattern<BundleT>;

  Eigen::SparseMatrix<double> J_{Nouts, Nvars};
  Eigen::SparseMatrix<double> H_{Nvars, Nouts * Nvars};

  /// @brief Calculate jacobian of (tf, xl(0.)+e0, xl(tf)+ef, q) w.r.t. (tf, e0, ef, q)
  void update_joplus(const E & e0, const E & ef, const E & dxlf)
  {
    dr_exp_sparse<BundleT>(Joplus_, (Tangent<BundleT>() << 1, e0, ef, Q::Ones()).finished());

    block_write(Joplus_, xf_B, tf_B, Ad<X>(smooth::exp<X>(-ef)) * dxlf);

    Joplus_.makeCompressed();
  }

  /// @brief Calculate hessian of (tf, xl(0.)+e0, xl(tf)+ef, q) w.r.t. (tf, e0, ef, q)
  void update_hoplus(const E & e0, const E & ef, [[maybe_unused]] const E & dxlf)
  {
    d2r_exp_sparse<BundleT>(Hoplus_, (Tangent<BundleT>() << 1, e0, ef, Q::Ones()).finished());

    // dr (Ad_X b)_X = -ad_{Ad_X b} Ad_X
    const TangentMap<X> Adexp_f  = Ad<X>(smooth::exp<X>(-ef));
    const TangentMap<X> dAdexp_f = ad<X>(Adexp_f * dxlf) * Adexp_f * dr_exp<X>(-ef);

    for (auto nx = 0u; nx < Nx; ++nx) {
      const auto b0 = Nvars * (xf_B + nx);
      block_write(Hoplus_, tf_B, b0 + xf_B, dAdexp_f.middleRows(nx, 1));
    }

    Hoplus_.makeCompressed();
  }

public:
  template<typename A1, typename A2>
  FlatEndptFun(A1 && a1, A2 && a2) : f(std::forward<A1>(a1)), xl(std::forward<A2>(a2))
  {}

  template<typename T>
  auto operator()(
    const T & tf, const CastT<T, E> & e0, const CastT<T, E> & ef, const CastT<T, Q> & q) const
  {
    return f.template operator()<T>(tf, rplus(xl(T(0.)), e0), rplus(xl(tf), ef), q);
  }

  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  jacobian(double tf, const E & e0, const E & ef, const Q & q) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, X, Q>>)
  {
    const auto & [xlfval, dxlfval] = diff::dr(xl, wrt(tf));
    const auto & Jf                = f.jacobian(tf, rplus(xl(0.), e0), rplus(xlfval, ef), q);

    update_joplus(e0, ef, dxlfval);

    J_ = Jf * Joplus_;
    J_.makeCompressed();
    return J_;
  }

  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  hessian(double tf, const E & e0, const E & ef, const Q & q) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, X, Q>> &&
      diff::detail::diffable_order2<F, std::tuple<double, X, X, Q>>)
  {
    const auto & [xlfval, dxlfval] = diff::dr(xl, wrt(tf));
    const auto x0                  = rplus(xl(0.), e0);
    const auto xf                  = rplus(xlfval, ef);
    const auto & Jf                = f.jacobian(tf, x0, xf, q);  // Nouts x Nx
    const auto & Hf                = f.hessian(tf, x0, xf, q);   // Nx x (Nouts * Nx)

    update_joplus(e0, ef, dxlfval);
    update_hoplus(e0, ef, dxlfval);

    set_zero(H_);
    d2r_fog(H_, Jf, Hf, Joplus_, Hoplus_);
    H_.makeCompressed();
    return H_;
  }
};

}  // namespace detail
// \endcond

/**
 * @brief Flatten a LieGroup OCP by defining it in the tangent space around a trajectory.
 *
 * @param ocp OCPType defined on a LieGroup
 * @param xl nominal state trajectory
 * @param ul nominal state trajectory
 *
 * @note The flattened problem defines analytical jacobians and hessians if \p ocp does.
 *
 * @warn The Hessian of the flattened dynamics is not implemented in an efficient manner.
 *
 * @return FlatOCPType in variables (xe, ue) obtained via variables change x = xl ⊕ xe, u = ul ⊕
 * ue,
 */
auto flatten_ocp(const OCPType auto & ocp, auto && xl, auto && ul)
{
  using ocp_t = std::decay_t<decltype(ocp)>;
  using X     = typename ocp_t::X;
  using U     = typename ocp_t::U;
  using Xl    = decltype(xl);
  using Ul    = decltype(ul);

  static constexpr auto Nq = ocp_t::Nq;

  return OCP<
    Tangent<X>,
    Tangent<U>,
    detail::FlatEndptFun<X, U, Nq, 1, decltype(ocp.theta), Xl>,
    detail::FlatDyn<X, U, decltype(ocp.f), Xl, Ul>,
    detail::FlatInnerFun<X, U, Nq, decltype(ocp.g), Xl, Ul>,
    detail::FlatInnerFun<X, U, ocp_t::Ncr, decltype(ocp.cr), Xl, Ul>,
    detail::FlatEndptFun<X, U, Nq, ocp_t::Nce, decltype(ocp.ce), Xl>>{
    .theta = detail::FlatEndptFun<X, U, Nq, 1, decltype(ocp.theta), Xl>{ocp.theta, xl},
    .f     = detail::FlatDyn<X, U, decltype(ocp.f), Xl, Ul>{ocp.f, xl, ul},
    .g     = detail::FlatInnerFun<X, U, Nq, decltype(ocp.g), Xl, Ul>{ocp.g, xl, ul},
    .cr    = detail::FlatInnerFun<X, U, ocp_t::Ncr, decltype(ocp.cr), Xl, Ul>{ocp.cr, xl, ul},
    .crl   = ocp.crl,
    .cru   = ocp.cru,
    .ce    = detail::FlatEndptFun<X, U, Nq, ocp_t::Nce, decltype(ocp.ce), Xl>{ocp.ce, xl},
    .cel   = ocp.cel,
    .ceu   = ocp.ceu,
  };
}

/**
 * @brief Unflatten a FlatOCPSolution
 *
 * If flat_sol is a solution to flat_ocp = flatten_ocp(ocp, xl_fun, ul_fun),
 * then unflatten_ocpsol(flat_sol, xl_fun, ul_fun) is a solution to ocp.
 */
template<LieGroup X, Manifold U>
auto unflatten_ocpsol(const auto & flatsol, auto && xl_fun, auto && ul_fun)
{
  using ocpsol_t = std::decay_t<decltype(flatsol)>;

  auto u_unflat = [ul_fun = std::forward<decltype(ul_fun)>(ul_fun),
                   usol   = flatsol.u](double t) -> U { return rplus(ul_fun(t), usol(t)); };

  auto x_unflat = [xl_fun = std::forward<decltype(xl_fun)>(xl_fun),
                   xsol   = flatsol.x](double t) -> X { return rplus(xl_fun(t), xsol(t)); };

  return OCPSolution<X, U, ocpsol_t::Nq, ocpsol_t::Ncr, ocpsol_t::Nce>{
    .t0         = flatsol.t0,
    .tf         = flatsol.tf,
    .Q          = flatsol.Q,
    .u          = std::move(u_unflat),
    .x          = std::move(x_unflat),
    .lambda_q   = flatsol.lambda_q,
    .lambda_ce  = flatsol.lambda_ce,
    .lambda_dyn = flatsol.lambda_dyn,
    .lambda_cr  = flatsol.lambda_cr,
  };
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__FLATTEN_OCP_HPP_


// gradientDescent.hpp
#ifndef COURSERA_GRADIENTDESCENT_HPP
#define COURSERA_GRADIENTDESCENT_HPP

#include <memory>
#include <armadillo>

void gradientDescent(std::shared_ptr<arma::fvec> &J_history, std::shared_ptr<arma::fvec> &theta,
                     const std::shared_ptr<arma::fmat> &X, const std::shared_ptr<arma::fvec> &y,
                     const float alpha, const size_t num_iters);

#endif // COURSERA_GRADIENTDESCENT_HPP


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  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_SIMD_FUNCTION_AVERAGE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_AVERAGE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
                  {

  /*!
    @ingroup group-arithmetic
    This function object computes the arithmetic mean of its parameters.

    @par Header <boost/simd/abs.hpp>

    @par Notes

    Using `average(x, y)` for floating entries is similar to  `(x+y)/2`

    for integer types, it returns a rounded value at a distance guaranteed
    to be less than or equal to 0.5 of the average floating value, but may differ
    by unity from the truncation given by `(x+y)/2`.

    @par Note:
      This function does not overflow.

    @see meanof

    @par Example:

      @snippet average.cpp average

    @par Possible output:

      @snippet average.txt average

  **/
  Value average(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/average.hpp>
#include <boost/simd/function/simd/average.hpp>

#endif


/**
 * \ file LMSFilter.cpp
 */

#include <array>
#include <fstream>

#include <ATK/Adaptive/LMSFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Tools/SumFilter.h>

#include <Eigen/Core>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 1200;

BOOST_AUTO_TEST_CASE(LMSFilter_size_negative_test)
{
  ATK::LMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_size(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(LMSFilter_size_set_test)
{
  ATK::LMSFilter<double> filter(100);
  filter.set_size(10);
  BOOST_CHECK_EQUAL(filter.get_size(), 10);
}

BOOST_AUTO_TEST_CASE(LMSFilter_mode_set_test)
{
  ATK::LMSFilter<double> filter(100);
  filter.set_mode(ATK::LMSFilter<double>::Mode::NORMALIZED);
  BOOST_CHECK(filter.get_mode() == ATK::LMSFilter<double>::Mode::NORMALIZED);
}

BOOST_AUTO_TEST_CASE(LMSFilter_memory_negative_test)
{
  ATK::LMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_memory(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(LMSFilter_memory_test)
{
  ATK::LMSFilter<double> filter(100);
  filter.set_memory(0.5);
  BOOST_CHECK_EQUAL(filter.get_memory(), 0.5);
}

BOOST_AUTO_TEST_CASE( LMSFilter_memory_positive1_test )
{
  ATK::LMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_memory(1), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(LMSFilter_mu_negative_test)
{
  ATK::LMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_mu(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(LMSFilter_mu_test)
{
  ATK::LMSFilter<double> filter(100);
  filter.set_mu(0.5);
  BOOST_CHECK_EQUAL(filter.get_mu(), 0.5);
}

BOOST_AUTO_TEST_CASE( LMSFilter_mu_positive1_test )
{
  ATK::LMSFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_mu(1), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE(LMSFilter_learning_set_test)
{
  ATK::LMSFilter<double> filter(100);
  BOOST_CHECK_EQUAL(filter.get_learning(), true);
  filter.set_learning(false);
  BOOST_CHECK_EQUAL(filter.get_learning(), false);
}

BOOST_AUTO_TEST_CASE( LMSFilter_memory_99_test )
{
  std::array<double, PROCESSSIZE> data;
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/input_lms.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(data.data()), PROCESSSIZE * sizeof(double));
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  ATK::LMSFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(.999);
  filter.set_mu(.01);

  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);

  filter.process(PROCESSSIZE);

  std::array<double, PROCESSSIZE> outdata;
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/output_lms.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(outdata.data()), PROCESSSIZE * sizeof(double));
  }

  for (unsigned int i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_CHECK_CLOSE(outdata[i], filter.get_output_array(0)[i], 0.0001);
  }
}


/*
Copyright 2014 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

/** \file
Test product.hpp.
*/

#define BOOST_TEST_MODULE test_math_product
#include "utility/test/boost_unit_test.hpp"

#include "math/product.hpp"

#include <string>
#include <vector>

#include <boost/mpl/assert.hpp>

#include "math/arithmetic_magma.hpp"
#include "math/cost.hpp"
#include "math/sequence.hpp"
#include "math/check/check_magma.hpp"

BOOST_AUTO_TEST_SUITE (test_suite_product)

BOOST_AUTO_TEST_CASE (test_product_annihilator) {
    typedef math::product <
        math::over <math::sequence <char>, math::cost <double>>>
        without_inverse;
    typedef math::product <
        math::over <math::sequence <char>, math::cost <double>>,
        math::with_inverse <math::callable::times>> with_divide;
    typedef math::product <
        math::over <math::sequence <char>, math::cost <double>>,
        math::with_inverse <math::callable::plus>> with_minus;

    auto sequence_one = math::one <math::sequence <char>>();
    auto sequence_annihilator =
        math::annihilator <math::sequence <char>> (math::times);
    auto cost_one = math::one <math::cost <double>> ();
    auto cost_annihilator =
        math::annihilator <math::cost <double>> (math::times);

    // divide.
    BOOST_MPL_ASSERT_NOT ((math::has <math::callable::divide <math::left> (
        without_inverse, without_inverse)>));
    BOOST_MPL_ASSERT_NOT ((math::has <math::callable::invert <
        math::left, math::callable::times> (without_inverse)>));

    BOOST_MPL_ASSERT ((math::has <math::callable::divide <math::left> (
        with_divide, with_divide)>));
    // sequence does not have invert.
    BOOST_MPL_ASSERT_NOT ((math::has <math::callable::invert <
        math::left, math::callable::times> (with_divide)>));

    BOOST_MPL_ASSERT_NOT ((math::has <math::callable::divide <math::left> (
        with_minus, with_minus)>));
    BOOST_MPL_ASSERT_NOT ((math::has <
        math::callable::invert <math::left, math::callable::times> (
            with_minus)>));

    // minus.
    BOOST_MPL_ASSERT_NOT ((math::has <math::callable::minus <math::left> (
        without_inverse, without_inverse)>));
    BOOST_MPL_ASSERT_NOT ((math::has <math::callable::minus <math::left> (
        with_divide, with_divide)>));
    // minus is not actually implemented, since sequence does not have it.
    BOOST_MPL_ASSERT_NOT ((math::has <math::callable::minus <math::left> (
        with_minus, with_minus)>));

    // All components annihilators.
    BOOST_CHECK (math::is_annihilator (math::times,
        without_inverse (sequence_annihilator, cost_annihilator)));
    BOOST_CHECK (math::is_annihilator (math::times,
        with_divide (sequence_annihilator, cost_annihilator)));
    BOOST_CHECK (math::is_annihilator (math::times,
        with_minus (sequence_annihilator, cost_annihilator)));

    // One component annihilator.
    BOOST_CHECK (!math::is_annihilator (math::times,
        without_inverse (sequence_one, cost_annihilator)));
    BOOST_CHECK (math::is_annihilator (math::times,
        with_divide (sequence_one, cost_annihilator)));
    BOOST_CHECK (!math::is_annihilator (math::times,
        with_minus (sequence_one, cost_annihilator)));

    BOOST_CHECK (!math::is_annihilator (math::times,
        without_inverse (sequence_annihilator, cost_one)));
    BOOST_CHECK (math::is_annihilator (math::times,
        with_divide (sequence_annihilator, cost_one)));
    BOOST_CHECK (!math::is_annihilator (math::times,
        with_minus (sequence_annihilator, cost_one)));

    // No components annihilators.
    BOOST_CHECK (!math::is_annihilator (math::times,
        without_inverse (sequence_one, cost_one)));
    BOOST_CHECK (!math::is_annihilator (math::times,
        with_divide (sequence_one, cost_one)));
    BOOST_CHECK (!math::is_annihilator (math::times,
        with_minus (sequence_one, cost_one)));

    // Annihilators compare equal.
    BOOST_CHECK_EQUAL (
        without_inverse (sequence_annihilator, cost_annihilator),
        without_inverse (sequence_annihilator, cost_annihilator));
    BOOST_CHECK (!math::compare (
        without_inverse (sequence_annihilator, cost_annihilator),
        without_inverse (sequence_annihilator, cost_annihilator)));

    BOOST_CHECK_EQUAL (
        with_minus (sequence_annihilator, cost_annihilator),
        with_minus (sequence_annihilator, cost_annihilator));
    BOOST_CHECK (!math::compare (
        with_minus (sequence_annihilator, cost_annihilator),
        with_minus (sequence_annihilator, cost_annihilator)));

    BOOST_CHECK_EQUAL (
        with_divide (sequence_annihilator, cost_annihilator),
        with_divide (sequence_annihilator, cost_one));
    BOOST_CHECK (!math::compare (
        with_divide (sequence_annihilator, cost_annihilator),
        with_divide (sequence_annihilator, cost_one)));

    BOOST_CHECK_EQUAL (
        with_divide (sequence_annihilator, cost_annihilator),
        with_divide (sequence_one, cost_annihilator));
    BOOST_CHECK (!math::compare (
        with_divide (sequence_annihilator, cost_annihilator),
        with_divide (sequence_one, cost_annihilator)));

    BOOST_CHECK_EQUAL (
        with_divide (sequence_annihilator, cost_one),
        with_divide (sequence_one, cost_annihilator));
    BOOST_CHECK (!math::compare (
        with_divide (sequence_annihilator, cost_one),
        with_divide (sequence_one, cost_annihilator)));

    // Annihilators and non-annihilators compare unequal.
    BOOST_CHECK (! math::equal (
        without_inverse (sequence_annihilator, cost_annihilator),
        without_inverse (sequence_annihilator, cost_one)));
    BOOST_CHECK (! math::equal (
        with_minus (sequence_annihilator, cost_annihilator),
        with_minus (sequence_annihilator, cost_one)));

    BOOST_CHECK (! math::equal (
        without_inverse (sequence_annihilator, cost_annihilator),
        without_inverse (sequence_one, cost_annihilator)));
    BOOST_CHECK (! math::equal (
        with_minus (sequence_annihilator, cost_annihilator),
        with_minus (sequence_one, cost_annihilator)));

    BOOST_CHECK (! math::equal (
        without_inverse (sequence_annihilator, cost_one),
        without_inverse (sequence_one, cost_annihilator)));
    BOOST_CHECK (! math::equal (
        with_minus (sequence_annihilator, cost_one),
        with_minus (sequence_one, cost_annihilator)));

    BOOST_CHECK (! math::equal (
        with_divide (sequence_annihilator, cost_one),
        with_divide (sequence_one, cost_one)));

    // compare.
    BOOST_CHECK (math::compare (
        with_divide (sequence_one, cost_one),
        with_divide (sequence_annihilator, cost_one)));
    BOOST_CHECK (!math::compare (
        with_divide (sequence_annihilator, cost_one),
        with_divide (sequence_one, cost_one)));

    BOOST_CHECK (!math::compare (
        without_inverse (sequence_annihilator, cost_annihilator),
        without_inverse (sequence_one, cost_annihilator)));
    BOOST_CHECK (math::compare (
        without_inverse (sequence_one, cost_annihilator),
        without_inverse (sequence_annihilator, cost_annihilator)));
}

BOOST_AUTO_TEST_CASE (test_product_spot_three) {
    typedef math::product <
        math::over <math::sequence <char>, int, math::cost <float>>> product;

    product a_4_4 (
        math::sequence <char> (std::string ("a")), 4, math::cost <float> (4.));
    product bc_2_2 (
        math::sequence <char> (std::string ("bc")), 2, math::cost <float> (2.));

    BOOST_CHECK ((math::not_equal (a_4_4, bc_2_2)));

    product abc_8_6 = a_4_4 * bc_2_2;
    BOOST_CHECK_EQUAL (range::at_c <0> (abc_8_6.components()),
        math::sequence <char> (std::string ("abc")));
    BOOST_CHECK_EQUAL (range::at_c <1> (abc_8_6.components()), 8);
    BOOST_CHECK_EQUAL (
        range::at_c <2> (abc_8_6.components()), math::cost <float> (6.));

    product a_12_4 = abc_8_6 + a_4_4;
    BOOST_CHECK_EQUAL (range::at_c <0> (a_12_4.components()),
        math::sequence <char> (std::string ("a")));
    BOOST_CHECK_EQUAL (range::at_c <1> (a_12_4.components()), 12);
    BOOST_CHECK_EQUAL (
        range::at_c <2> (a_12_4.components()), math::cost <float> (4.));
}

BOOST_AUTO_TEST_CASE (test_product_spot_floats) {
    typedef math::product <math::over <float, float>,
        math::with_inverse <math::callable::times>> product;

    product a (4, 4);
    product inverse = math::invert <math::callable::times> (a);
    BOOST_CHECK_EQUAL (inverse, product (.25, .25));
}

BOOST_AUTO_TEST_CASE (test_product_automaton_determinisation) {
    /*
    This is a simple use case: determinisation of a finite-state automaton.
    This does things like turning (a*b) + (a*c) into a*(b+c).
    Imagine an automaton with two paths:
    (a, 1), (b, 3)
    (a, 2), (c, 4)
    This can be turned into, e.g.
    (a, 1), (b, 3)
    (a, 1), (c, 5)
    and now the first transition is the same and can be shared.
    */

    using math::divide;
    using math::left;
    typedef math::with_inverse <math::callable::times> inverses;

    typedef math::single_sequence <char> symbol;
    typedef math::product <math::over <symbol, math::cost <double>>, inverses>
        product;
    product a_1 (symbol ('a'), 1.);
    product a_2 (symbol ('a'), 2.);
    product b_3 (symbol ('b'), 3.);
    product c_4 (symbol ('c'), 4.);

    auto merged_untyped = a_1 + a_2;
    // Explicitly convert to "product".
    product merged (merged_untyped);
    auto residue1 = divide <left> (a_1, merged);
    auto residue2 = divide <left> (a_2, merged);

    auto follow1 = residue1 * b_3;
    auto follow2 = residue2 * c_4;

    BOOST_CHECK_EQUAL (merged, a_1);
    BOOST_CHECK_EQUAL (follow1, b_3);
    BOOST_CHECK_EQUAL (follow2, product (symbol ('c'), 5.));

    BOOST_CHECK_EQUAL (merged * follow1, a_1 * b_3);
    BOOST_CHECK_EQUAL (merged * follow2, a_2 * c_4);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/test/unit_test.hpp>

#include <sway/math.h>

using namespace sway;

BOOST_AUTO_TEST_SUITE(TColorTestSuite)

/*!
 * \brief
 *    Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(TColorTestCase_DefaultConstructor) {
	const math::col4f_t color;

	BOOST_CHECK_EQUAL(color.getR(), 0.0f);
	BOOST_CHECK_EQUAL(color.getG(), 0.0f);
	BOOST_CHECK_EQUAL(color.getB(), 0.0f);
	BOOST_CHECK_EQUAL(color.getA(), 1.0f);
}

/*!
 * \brief
 *    Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 *    которые были заданы.
 */
BOOST_AUTO_TEST_CASE(TColorTestCase_ComponentConstructor) {
	const f32_t r = 0.1f, g = 0.2f, b = 0.3f, a = 1.0f;
	const math::col4f_t color(r, g, b, a);

	BOOST_CHECK_EQUAL(color.getR(), r);
	BOOST_CHECK_EQUAL(color.getG(), g);
	BOOST_CHECK_EQUAL(color.getB(), b);
	BOOST_CHECK_EQUAL(color.getA(), a);
}

/*!
 * \brief
 *    Убеждаемся, что преобразование в TVector4<type> проходит правильно.
 */
BOOST_AUTO_TEST_CASE(TColorTestCase_ConvertToVector4) {
	const f32_t r = 0.1f, g = 0.2f, b = 0.3f, a = 1.0f;

	math::col4f_t color(r, g, b, a);
	math::vec4f_t vec4 = color.toVec4();

	BOOST_CHECK_EQUAL(vec4.getX(), r);
	BOOST_CHECK_EQUAL(vec4.getY(), g);
	BOOST_CHECK_EQUAL(vec4.getZ(), b);
	BOOST_CHECK_EQUAL(vec4.getW(), a);
}

BOOST_AUTO_TEST_SUITE_END()


///////////////////////////////////////////////////////////////////////////////
// tail_quantile.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_TAIL_QUANTILE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_TAIL_QUANTILE_HPP_DE_01_01_2006

#include <vector>
#include <limits>
#include <functional>
#include <sstream>
#include <stdexcept>
#include <cmath>             // For ceil
#include <boost/throw_exception.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // tail_quantile_impl
    //  Tail quantile estimation based on order statistics
    /**
        @brief Tail quantile estimation based on order statistics (for both left and right tails)

        The estimation of a tail quantile \f$\hat{q}\f$ with level \f$\alpha\f$ based on order statistics requires the
        chaching of at least the \f$\lceil n\alpha\rceil\f$ smallest or the \f$\lceil n(1-\alpha)\rceil\f$ largest samples,
        \f$n\f$ being the total number of samples. The largest of the \f$\lceil n\alpha\rceil\f$ smallest samples or the
        smallest of the \f$\lceil n(1-\alpha)\rceil\f$ largest samples provides an estimate for the quantile:

        \f[
            \hat{q}_{n,\alpha} = X_{\lceil \alpha n \rceil:n}
        \f]

        @param quantile_probability
    */
    template<typename Sample, typename LeftRight>
    struct tail_quantile_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef Sample result_type;

        tail_quantile_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            std::size_t cnt = count(args);

            std::size_t n = static_cast<std::size_t>(
                std::ceil(
                    cnt * ( ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability] )
                )
            );

            // If n is in a valid range, return result, otherwise return NaN or throw exception
            if ( n < static_cast<std::size_t>(tail(args).size()))
            {
               // Note that the cached samples of the left are sorted in ascending order,
               // whereas the samples of the right tail are sorted in descending order
               return *(boost::begin(tail(args)) + n - 1);
            }
            else
            {
                if (std::numeric_limits<result_type>::has_quiet_NaN)
                {
                    return std::numeric_limits<result_type>::quiet_NaN();
                }
                else
                {
                    std::ostringstream msg;
                    msg << "index n = " << n << " is not in valid range [0, " << tail(args).size() << ")";
                    boost::throw_exception(std::runtime_error(msg.str()));
                    return Sample(0);
                }
            }
        }
    };
} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::tail_quantile<>
//
namespace tag
{
    template<typename LeftRight>
    struct tail_quantile
      : depends_on<count, tail<LeftRight> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::tail_quantile_impl<mpl::_1, LeftRight> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::tail_quantile
//
namespace extract
{
    extractor<tag::quantile> const tail_quantile = {};
}

using extract::tail_quantile;

// for the purposes of feature-based dependency resolution,
// tail_quantile<LeftRight> provide the same feature as quantile
template<typename LeftRight>
struct feature_of<tag::tail_quantile<LeftRight> >
  : feature_of<tag::quantile>
{
};

// So that tail_quantile can be automatically substituted with
// weighted_tail_quantile when the weight parameter is non-void.
template<typename LeftRight>
struct as_weighted_feature<tag::tail_quantile<LeftRight> >
{
    typedef tag::weighted_tail_quantile<LeftRight> type;
};

template<typename LeftRight>
struct feature_of<tag::weighted_tail_quantile<LeftRight> >
  : feature_of<tag::tail_quantile<LeftRight> >
{};

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Pawel Dlotko
 *
 *    Copyright (C) 2016 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "Persistence_intervals_with_distances_test"
#include <boost/test/unit_test.hpp>
#include <gudhi/reader_utils.h>
#include <gudhi/Persistence_intervals_with_distances.h>
#include <gudhi/common_persistence_representations.h>
#include <gudhi/Unitary_tests_utils.h>

#include <iostream>

using namespace Gudhi;
using namespace Gudhi::Persistence_representations;

BOOST_AUTO_TEST_CASE(check_bottleneck_distances_computation) {
  Persistence_intervals_with_distances p("data/file_with_diagram");
  Persistence_intervals_with_distances q("data/file_with_diagram_1");

  double dist = p.distance(q);

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(dist, 0.389043, Gudhi::Persistence_representations::epsi);
}

BOOST_AUTO_TEST_CASE(check_default_parameters_in_distance) {
  Persistence_intervals_with_distances p("data/file_with_diagram");
  Persistence_intervals_with_distances q("data/file_with_diagram_1");

  double default_parameter_distance = p.distance(q);
  double max_parameter_distance = p.distance(q, std::numeric_limits<double>::max());
  double inf_parameter_distance = p.distance(q, std::numeric_limits<double>::infinity());

  GUDHI_TEST_FLOAT_EQUALITY_CHECK(default_parameter_distance, max_parameter_distance);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(inf_parameter_distance, max_parameter_distance);
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(inf_parameter_distance, max_parameter_distance);
}


#include "headers/rmat.h"
#include "headers/util.h"
#include <boost/numeric/ublas/matrix.hpp>

using boost::numeric::ublas::matrix;
using will::util::hash32Prob;

RmatConfig::RmatConfig(double _a, double _b, double _c) : a(_a), b(_b), c(_c) {
  d = 1. - _a + _b + _c;
  totalA = _a;
  totalB = totalA + _b;
  totalC = totalB + _c;
  if (d < 0) {
    std::cout << "Warning: a + b + c > 1" << std::endl;
  }
}

std::list<Edge> listRmat(size_t n, size_t nEdges, const RmatConfig &cfg) {
  cilk::reducer<cilk::op_list_append<Edge>> red;
  listRmatHelper(red, nEdges, cfg, 0, 0, n, n);
  const std::list<Edge> &edgeList = red.get_value();
  // std::vector<Edge> out(edgeList.size());
  // std::copy(edgeList.begin(), edgeList.end(), out.begin());
  return std::move(edgeList);
}

void listRmatHelper(cilk::reducer<cilk::op_list_append<Edge>> &red, size_t nEdges,
                    const RmatConfig &cfg, size_t x0, size_t y0, size_t x1, size_t y1) {
  if (nEdges == 0) return;
  size_t xMid = (x0 + x1) / 2;
  size_t yMid = (y0 + y1) / 2;
  size_t nCells = (x1 - x0) * (y1 - y0);

  if (nCells == 1) {
    red->push_back({y0, x0});
    return;
  }

  cilk::reducer< cilk::op_add<size_t> > redA(0);
  cilk::reducer< cilk::op_add<size_t> > redB(0);
  cilk::reducer< cilk::op_add<size_t> > redC(0);
  cilk::reducer< cilk::op_add<size_t> > redD(0);

  cilk_for(size_t i = 0; i < nEdges; ++i) {
    double prob = hash32Prob(i);
    if (prob <= cfg.totalA)
      *redA += 1;
    else if (prob <= cfg.totalB)
      *redB += 1;
    else if (prob <= cfg.totalC)
      *redC += 1;
    else
      *redD += 1;
  }
  // Required for some reason, can't use .get_value() directly in recursive call
  size_t numA = redA.get_value();
  size_t numB = redB.get_value();
  size_t numC = redC.get_value();
  size_t numD = redD.get_value();

  if (nCells == 4) {
    if (numA > 0) red->push_back({ y0, x0 });
    if (numB > 0) red->push_back({ y0, x1 - 1 });
    if (numC > 0) red->push_back({ y1 - 1, x0 });
    if (numD > 0) red->push_back({ y1 - 1, x1 - 1 });
  } else if (nCells > 4) {
    cilk_spawn listRmatHelper(red, numA, cfg, x0, y0, xMid, yMid);
    cilk_spawn listRmatHelper(red, numB, cfg, xMid, y0, x1, yMid);
    cilk_spawn listRmatHelper(red, numC, cfg, x0, yMid, xMid, y1);
    listRmatHelper(red, numD, cfg, xMid, yMid, x1, y1);
    cilk_sync;
  }
}


/**
 * Simulate: Diffusion-Limited Aggregation from Form+Code in Art, Design, and Architecture
 * implemented in C++ by Patrick Tierney (patrick.l.tierney@gmail.com || http://ptierney.com)
 *
 * Requires Cinder 0.8.2 available at http://libcinder.org
 *
 * Project files are located at https://github.com/hlp/form-and-code
 *
 * For more information about Form+Code visit http://formandcode.com
 */

#include <vector>

#include <boost/shared_ptr.hpp>
#include <boost/date_time.hpp>

#include "cinder/gl/gl.h"
#include "cinder/gl/Texture.h"
#include "cinder/app/AppBasic.h"
#include "cinder/Rand.h"
#include "cinder/CinderMath.h"

class Particle;

// soon to be std::shared_ptr
typedef boost::shared_ptr<Particle> ParticlePtr;

class Simulate_DLA : public ci::app::AppBasic {
public:
    void prepareSettings(Settings* settings);
    void setup();
    void update();
    void draw();
    void shutdown();

    std::vector<bool> field;

private:
    int particleCount;
    std::vector<ParticlePtr> particles;
    GLubyte* data;
    int dataSize;
};

class Particle {
public:
    Particle(Simulate_DLA& diffusionApp) : field(diffusionApp.field) {
        stuck = false;
        width = diffusionApp.getWindowWidth();
        height = diffusionApp.getWindowHeight();

        reset();
    }

    void reset() {
        // keep choosing random spots until an empty one is found
        do {
            x = ci::Rand::randInt(width);
            y = ci::Rand::randInt(height);
        } while (field[y * width + x]);
    }

    void update() {
        // move around
        if (!stuck) {
            // get random int [-1, 1] (hence 2)
            x += ci::Rand::randInt(-1, 2);
            y += ci::Rand::randInt(-1, 2);

            if (x < 0 || y < 0 || x >= width || y >= height) {
                reset();
                return;
            }

            // test if something is next to us
            if (!alone()) {
                stuck = true;
                field[y * width + x] = true;
            }
        }
    }

    // returns true if no neighboring pixels
    bool alone() {
        int cx = x;
        int cy = y;

        // get positions
        int lx = cx-1;
        int rx = cx+1;
        int ty = cy-1;
        int by = cy+1;

        if (cx <= 0 || cx >= width ||
            lx <= 0 || lx >= width ||
            rx <= 0 || rx >= width ||
            cy <= 0 || cy >= height ||
            ty <= 0 || ty >= height ||
            by <= 0 || by >= height) return true;

        // pre multiply the ys
        cy *= width;
        by *= width;
        ty *= width;

        // N, W, E, S
        if (field[cx + ty] ||
            field[lx + cy] ||
            field[rx + cy] ||
            field[cx + by]) return false;

        // NW, NE, SW, SE
        if (field[lx + ty] ||
            field[lx + by] ||
            field[rx + ty] ||
            field[rx + by]) return false;

        return true;
    }

    bool stuck;
    int x, y;

private:
    int width, height;
    std::vector<bool>& field;
};

void Simulate_DLA::prepareSettings(Settings* settings) {
    settings->setWindowSize(1024, 700);
}

void Simulate_DLA::setup() {
    // this number might need to be smaller for some computers
    particleCount = 20000;
    particles.resize(particleCount);

    // create an array that stores the position of our particles and set them to false
    field.resize(getWindowWidth() * getWindowHeight());

    for (std::vector<bool>::iterator it = field.begin(); it != field.end(); ++it) {
        *it = false;
    }

    // add seed in the center
    int fcenterX = getWindowWidth() / 2;
    int fcenterY = getWindowHeight() / 2;
    field[fcenterX + fcenterY * getWindowWidth()] = true;

    // make particles
    for (int i = 0; i < particles.size(); ++i) {
        particles[i] = ParticlePtr(new Particle(*this));
    }

    // create pixel buffer
    dataSize = getWindowWidth() * getWindowHeight() * 3;
    data = new GLubyte[dataSize];

    // set all pixels to white
    for (int i = 0; i < dataSize; i++) {
        data[i] = (GLubyte) 255;
    }
}

void Simulate_DLA::update() {
    for(int i = 0; i < particleCount; i++) {
        particles[i]->update();
        if (particles[i]->stuck) {
            data[particles[i]->y * getWindowWidth() * 3 + particles[i]->x * 3] = (GLubyte) 0;
            data[particles[i]->y * getWindowWidth() * 3 + particles[i]->x * 3 + 1] = (GLubyte) 0;
            data[particles[i]->y * getWindowWidth() * 3 + particles[i]->x * 3 + 2] = (GLubyte) 0;
        }
    }
}

void Simulate_DLA::draw() {
    glDrawPixels(getWindowWidth(), getWindowHeight(), GL_RGB, GL_UNSIGNED_BYTE, data);
}

void Simulate_DLA::shutdown() {
    delete [] data;
}

CINDER_APP_BASIC(Simulate_DLA, ci::app::RendererGl)


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <iostream>

#include <graphblas/graphblas.hpp>
#include <algorithms/triangle_count.hpp>

using namespace grb;
using namespace algorithms;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE triangle_count_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
/*
static double const INF = std::numeric_limits<double>::max();

static std::vector<double> gr={0,1,1,2,2,2,2,3,3,3,3,4,4,4,5,6,6,6,8,8};
static std::vector<double> gc={3,3,6,4,5,6,8,0,1,4,6,2,3,8,2,1,2,3,2,4};
static std::vector<double> gv(gr.size(), 1);

static Matrix<double, DirectedMatrixTag> G_tn(9,9);

//static Matrix<double, DirectedMatrixTag> G_tn_answer(
//    {{2, 2, 3, 1, 2, 4, 2, INF, 3},
//     {2, 2, 2, 1, 2, 3, 1, INF, 3},
//     {3, 2, 2, 2, 1, 1, 1, INF, 1},
//     {1, 1, 2, 2, 1, 3, 1, INF, 2},
//     {2, 2, 1, 1, 2, 2, 2, INF, 1},
//     {4, 3, 1, 3, 2, 2, 2, INF, 2},
//     {2, 1, 1, 1, 2, 2, 2, INF, 2},
//     {INF, INF, INF, INF, INF, INF, INF, INF, INF},
//     {3, 3, 1, 2, 1, 2, 2, INF, 2}},
//    INF);

static std::vector<double> tr={0,0,1,1,2,2,2,2,3,3,4,4};
static std::vector<double> tc={1,2,0,2,0,1,3,4,2,4,2,3};
static std::vector<double> tv(tr.size(), 1);

//static Matrix<double, DirectedMatrixTag> test5x5(
//    {{INF,   1,   1, INF, INF},
//     {  1, INF,   1, INF, INF},
//     {  1,   1, INF,   1,   1},
//     {INF, INF,   1, INF,   1},
//     {INF, INF,   1,   1, INF}},
//    INF);
static Matrix<double, DirectedMatrixTag> test5x5(5,5,INF);
*/

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_triangle_count)
{
    //Matrix<double, DirectedMatrixTag> testtriangle(
    //                       {{0,1,1,1,0},
    //                        {1,0,1,0,1},
    //                        {1,1,0,1,1},
    //                        {1,0,1,0,1},
    //                        {0,1,1,1,0}});

    std::vector<double> ar={0, 0, 0, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4, 4};
    std::vector<double> ac={1, 2, 3, 0, 2, 4, 0, 1, 3, 4, 0, 2, 4, 1, 2, 3};
    std::vector<double> av={1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
    Matrix<double, DirectedMatrixTag> testtriangle(5,5);
    testtriangle.build(ar.begin(), ac.begin(), av.begin(), av.size());

    IndexType result = triangle_count(testtriangle);
    BOOST_CHECK_EQUAL(result, 4);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_triangle_count_masked)
{
    //Matrix<double, DirectedMatrixTag> testtriangle(
    //                       {{0,1,1,1,0},
    //                        {1,0,1,0,1},
    //                        {1,1,0,1,1},
    //                        {1,0,1,0,1},
    //                        {0,1,1,1,0}});

    std::vector<double> ar={0, 0, 0, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4, 4};
    std::vector<double> ac={1, 2, 3, 0, 2, 4, 0, 1, 3, 4, 0, 2, 4, 1, 2, 3};
    std::vector<double> av={1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
    Matrix<double, DirectedMatrixTag> testtriangle(5,5);
    testtriangle.build(ar.begin(), ac.begin(), av.begin(), av.size());

    Matrix<double, DirectedMatrixTag> L(5,5), U(5,5);
    grb::split(testtriangle, L, U);

    IndexType result = triangle_count_masked(L);
    BOOST_CHECK_EQUAL(result, 4);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_triangle_count_masked_noT)
{
    //Matrix<double, DirectedMatrixTag> testtriangle(
    //                       {{0,1,1,1,0},
    //                        {1,0,1,0,1},
    //                        {1,1,0,1,1},
    //                        {1,0,1,0,1},
    //                        {0,1,1,1,0}});

    std::vector<double> ar={0, 0, 0, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4, 4};
    std::vector<double> ac={1, 2, 3, 0, 2, 4, 0, 1, 3, 4, 0, 2, 4, 1, 2, 3};
    std::vector<double> av={1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
    Matrix<double, DirectedMatrixTag> testtriangle(5,5);
    testtriangle.build(ar.begin(), ac.begin(), av.begin(), av.size());

    Matrix<double, DirectedMatrixTag> L(5,5), U(5,5);
    grb::split(testtriangle, L, U);

    IndexType result = triangle_count_masked_noT(L);
    BOOST_CHECK_EQUAL(result, 4);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_triangle_counting_newGBTL)
{
    //Matrix<double, DirectedMatrixTag> testtriangle(
    //                       {{0,1,1,1,0},
    //                        {1,0,1,0,1},
    //                        {1,1,0,1,1},
    //                        {1,0,1,0,1},
    //                        {0,1,1,1,0}});

    std::vector<double> ar={0, 0, 0, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4, 4};
    std::vector<double> ac={1, 2, 3, 0, 2, 4, 0, 1, 3, 4, 0, 2, 4, 1, 2, 3};
    std::vector<double> av={1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
    Matrix<double, DirectedMatrixTag> testtriangle(5,5), L(5,5), U(5,5);
    testtriangle.build(ar.begin(), ac.begin(), av.begin(), av.size());
    grb::split(testtriangle, L, U);

    IndexType result = triangle_count_newGBTL(L, U);
    BOOST_CHECK_EQUAL(result, 4);
}

BOOST_AUTO_TEST_SUITE_END()


/* boost random/inversive_congruential.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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 most recent version including documentation.
 *
 * $Id: inversive_congruential.hpp,v 1.9 2004/07/27 03:43:32 dgregor Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_INVERSIVE_CONGRUENTIAL_HPP
#define BOOST_RANDOM_INVERSIVE_CONGRUENTIAL_HPP

#include <iostream>
#include <cassert>
#include <boost/config.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/detail/const_mod.hpp>

namespace boost {
namespace random {

// Eichenauer and Lehn 1986
template<class IntType, IntType a, IntType b, IntType p, IntType val>
class inversive_congruential
{
public:
  typedef IntType result_type;
#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
  static const bool has_fixed_range = true;
  static const result_type min_value = (b == 0 ? 1 : 0);
  static const result_type max_value = p-1;
#else
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
#endif
  BOOST_STATIC_CONSTANT(result_type, multiplier = a);
  BOOST_STATIC_CONSTANT(result_type, increment = b);
  BOOST_STATIC_CONSTANT(result_type, modulus = p);

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return b == 0 ? 1 : 0; }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return p-1; }

  explicit inversive_congruential(IntType y0 = 1) : value(y0)
  {
    BOOST_STATIC_ASSERT(b >= 0);
    BOOST_STATIC_ASSERT(p > 1);
    BOOST_STATIC_ASSERT(a >= 1);
    if(b == 0)
      assert(y0 > 0);
  }
  template<class It> inversive_congruential(It& first, It last)
  { seed(first, last); }

  void seed(IntType y0 = 1) { value = y0; if(b == 0) assert(y0 > 0); }
  template<class It> void seed(It& first, It last)
  {
    if(first == last)
      throw std::invalid_argument("inversive_congruential::seed");
    value = *first++;
  }
  IntType operator()()
  {
    typedef const_mod<IntType, p> do_mod;
    value = do_mod::mult_add(a, do_mod::invert(value), b);
    return value;
  }

  bool validation(result_type x) const { return val == x; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, inversive_congruential x)
  { os << x.value; return os; }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, inversive_congruential& x)
  { is >> x.value; return is; }
#endif

  friend bool operator==(inversive_congruential x, inversive_congruential y)
  { return x.value == y.value; }
  friend bool operator!=(inversive_congruential x, inversive_congruential y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(inversive_congruential rhs) const
  { return value == rhs.value; }
  bool operator!=(inversive_congruential rhs) const
  { return !(*this == rhs); }
#endif
private:
  IntType value;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const bool inversive_congruential<IntType, a, b, p, val>::has_fixed_range;
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const typename inversive_congruential<IntType, a, b, p, val>::result_type inversive_congruential<IntType, a, b, p, val>::min_value;
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const typename inversive_congruential<IntType, a, b, p, val>::result_type inversive_congruential<IntType, a, b, p, val>::max_value;
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const typename inversive_congruential<IntType, a, b, p, val>::result_type inversive_congruential<IntType, a, b, p, val>::multiplier;
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const typename inversive_congruential<IntType, a, b, p, val>::result_type inversive_congruential<IntType, a, b, p, val>::increment;
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const typename inversive_congruential<IntType, a, b, p, val>::result_type inversive_congruential<IntType, a, b, p, val>::modulus;
#endif

} // namespace random

typedef random::inversive_congruential<int32_t, 9102, 2147483647-36884165,
  2147483647, 0> hellekalek1995;

} // namespace boost

#endif // BOOST_RANDOM_INVERSIVE_CONGRUENTIAL_HPP


#include <boost/ut.hpp>// single header
// import boost.ut;        // single module (C++20)
#include "tl/random.hpp"
#include "tl/algorithm.hpp"
#include <string_view>
int
  main()
{
  using namespace boost::ut::literals;
  using namespace boost::ut::operators::terse;
  using namespace boost::ut;
  using namespace std::string_view_literals;
  using namespace std::string_literals;
  [[maybe_unused]] suite random = [] {
    const auto check_type = []<tl::concepts::is_integral integralT>() {
      const auto random_values = tl::random::iota<char, 10U>();
      expect(tl::algorithm::any_of(random_values, [](const auto &value) -> bool {
        return value != integralT{};
      }));
    };
    const auto check_types = [&check_type]<tl::concepts::is_integral... integralT2>()
    {
      (check_type.template operator()<integralT2>(), ...);
    };
    "create random char array"_test = [&check_types] {
      check_types.operator()<std::int8_t,
                             std::uint8_t,
                             std::int16_t,
                             std::uint16_t,
                             std::int32_t,
                             std::uint32_t,
                             std::int64_t,
                             std::uint64_t>();
    };
  };
}


// Copyright 2019 Hans Dembinski
//
// 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)

// clang-format off

//[ getting_started_listing_05

//////////////// Begin: put this in header file ////////////////////

#include <algorithm>           // std::max_element
#include <boost/format.hpp>    // only needed for printing
#include <boost/histogram.hpp> // make_histogram, integer, indexed
#include <iostream>            // std::cout, std::endl
#include <sstream>             // std::ostringstream
#include <tuple>
#include <vector>

// use this when axis configuration is fix to get highest performance
struct HolderOfStaticHistogram {
  // put axis types here
  using axes_t = std::tuple<
    boost::histogram::axis::regular<>,
    boost::histogram::axis::integer<>
  >;
  using hist_t = boost::histogram::histogram<axes_t>;
  hist_t hist_;
};

// use this when axis configuration should be flexible
struct HolderOfDynamicHistogram {
  // put all axis types here that you are going to use
  using axis_t = boost::histogram::axis::variant<
    boost::histogram::axis::regular<>,
    boost::histogram::axis::variable<>,
    boost::histogram::axis::integer<>
  >;
  using axes_t = std::vector<axis_t>;
  using hist_t = boost::histogram::histogram<axes_t>;
  hist_t hist_;
};

//////////////// End: put this in header file ////////////////////

int main() {
  using namespace boost::histogram;

  HolderOfStaticHistogram hs;
  hs.hist_ = make_histogram(axis::regular<>(5, 0, 1), axis::integer<>(0, 3));
  // now assign a different histogram
  hs.hist_ = make_histogram(axis::regular<>(3, 1, 2), axis::integer<>(4, 6));
  // hs.hist_ = make_histogram(axis::regular<>(5, 0, 1)); does not work;
  // the static histogram cannot change the number or order of axis types

  HolderOfDynamicHistogram hd;
  hd.hist_ = make_histogram(axis::regular<>(5, 0, 1), axis::integer<>(0, 3));
  // now assign a different histogram
  hd.hist_ = make_histogram(axis::regular<>(3, -1, 2));
  // and assign another
  hd.hist_ = make_histogram(axis::integer<>(0, 5), axis::integer<>(3, 5));
}

//]


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

#include <boost/math/special_functions/beta.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include <stdexcept>

// Use the Boost `ibeta` implementation to generate an array of weights for the
// Harrell-Davis quantile estimator.
//
// For an array `v` of length `l` and quantile `q`,
//   v[i] = I_x1(a, b) - I_x0(a, b)
// where
//   x0 = i / l
//   x1 = (i + 1) / l
//   a = l * q
//   b = l * (1 - q)
//   and I_x() is the regularized beta function.
//
// Harrell, F., & Davis, C. (1982). A New Distribution-Free Quantile Estimator.
// Biometrika, 69(3), 635-640. doi:10.2307/2335999
//
// By default, gives a tab-seperated-values (tsv) output on stdout, but can also
// output raw doubles as bytes, so can be piped as input to another program
// (e.g. into a JS TypedArray).

using ExtendedDouble = long double;
// Other options for checking with extended precision, but little difference
// unless comparison also in extended precision. Note: no float128.hpp in Clang.
// #include <boost/multiprecision/cpp_bin_float.hpp>
// using ExtendedDouble = boost::multiprecision::cpp_bin_float_50;

int main(int argc, char** argv) {
  int arr_length = 0;
  ExtendedDouble quantile = 0;

  // Output tab separated if true, binary if not.
  bool output_tsv = true;

  try {
    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()("help,h", "print help message")(
        "length,l", po::value<int>(&arr_length)->required(),
        "length of the target array")(
        "quantile,q", po::value<ExtendedDouble>(&quantile)->required(),
        "quantile to use")(
        "format,f", po::value<std::string>()->default_value("tsv"),
        "Output format: either 'tsv' or 'bin'. Defaults to 'tsv'.");

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

    if (vm.count("help")) {
      std::cout << "Print a Harrell-Davis quantile estimator weight array.\n"
                << desc << "\n";
      return 1;
    }

    if (vm.count("format")) {
      std::string output_format = vm["format"].as<std::string>();
      if (output_format == "bin") {
        output_tsv = false;
      } else if (output_format != "tsv") {
        throw po::validation_error(po::validation_error::invalid_option_value,
                                   "format");
      }
    }

    // Ensure we have > double precision.
    int max_digits_10 = std::numeric_limits<ExtendedDouble>::max_digits10;
    if (max_digits_10 < 21) {
      throw std::runtime_error(
          "Insuffient precision available for calculation");
    }

    // Notify about missing arg(s) after --help, checking --format, etc.
    po::notify(vm);
  } catch (std::exception& e) {
    std::cerr << "Error: " << e.what() << "\n";
    return 1;
  }

  if (output_tsv) {
    // Column headers for tsv.
    std::cout << "index\tvalue"
              << "\n";
    // Make sure enough digits of the upcoming numbers are printed.
    std::cout << std::setprecision(
        std::numeric_limits<ExtendedDouble>::max_digits10);
  }

  ExtendedDouble a = arr_length * quantile;
  ExtendedDouble b = arr_length * (1 - quantile);
  ExtendedDouble total = 0;

  for (int i = 0; i < arr_length; i++) {
    ExtendedDouble start =
        boost::math::ibeta(a, b, i / (ExtendedDouble)arr_length);
    ExtendedDouble end =
        boost::math::ibeta(a, b, (i + 1) / (ExtendedDouble)arr_length);
    ExtendedDouble value = end - start;
    total += value;

    if (output_tsv) {
      std::cout << i << "\t" << value << "\n";
    } else {
      // Only write doubles (not higher precision) to stdout so they can be
      // imported in JS.
      // Uncomment for boost::multiprecision.
      // double d_value = value.convert_to<double>();
      double d_value = (double)value;
      std::cout.write(reinterpret_cast<char*>(&d_value), sizeof d_value);
    }
  }

  if (output_tsv) {
    std::cerr << std::setprecision(
                     std::numeric_limits<ExtendedDouble>::max_digits10)
              << "Complete with a total of " << std::setprecision(20) << total
              << "\n";
  }
}


// transform algorithm example
#include <iostream>
#include <algorithm>
#include <vector>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>

using namespace std;
using namespace boost::lambda;

int op_increase (int i) { return ++i; }
int op_sum (int i, int j) { return i+j; }

int main () {
  vector<int> first;
  vector<int> second;
  vector<int>::iterator it;

  // set some values:
  for (int i=1; i<6; i++) first.push_back (i*10); //  first: 10 20 30 40 50

  second.resize(first.size());     // allocate space
  transform (first.begin(), first.end(), second.begin(), op_increase);
                                                  // second: 11 21 31 41 51

  transform (first.begin(), first.end(), second.begin(), first.begin(), op_sum);
                                                  //  first: 21 41 61 81 101

  cout << "first contains:";
  for (it=first.begin(); it!=first.end(); ++it)
    cout << " " << *it;

  cout << endl;

  vector<int> third;
  third.resize(first.size());
  transform(first.begin(), first.end(), third.begin(), _1 * _1);
  cout << "third: ";
  for_each(third.begin(), third.end(), cout << _1 << ' ');
  cout << endl;
  return 0;
}