// 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_HPP_
#define SMOOTH__FEEDBACK__COLLOCATION_HPP_

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

#include <cstddef>
#include <numeric>
#include <ranges>
#include <vector>

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

namespace smooth::feedback {

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.
   */
  inline Mesh() : intervals_(1, Interval{.K = Kmin, .tau0 = 0}) {}

  /**
   * @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 { return intervals_[i].K; }

  /**
   * @breif 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.
   */
  inline void refine_ph(std::size_t i, std::size_t D)
  {
    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 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 and quadrature weights (DOES include extra point)
   */
  inline std::pair<Eigen::VectorXd, Eigen::VectorXd> interval_nodes_and_weights(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;

    Eigen::VectorXd ns, ws;
    utils::static_for<Kmax + 2 - Kmin>([&](auto i) {
      static constexpr auto K = Kmin + i;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
        ns = Eigen::Map<const Eigen::VectorXd>(nw_ext_s.first.data(), k + 1);
        ws = Eigen::Map<const Eigen::VectorXd>(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 alpha = (tauf - tau0) / 2;

    return {
      Eigen::VectorXd::Constant(ns.size(), tau0) + alpha * (ns + Eigen::VectorXd::Ones(ns.size())),
      alpha * ws,
    };
  }

  /**
   * @brief All Mesh nodes and quadrature weights (DOES include extra point)
   */
  inline std::pair<Eigen::VectorXd, Eigen::VectorXd> all_nodes_and_weights() const
  {
    Eigen::VectorXd n(N_colloc() + 1), w(N_colloc() + 1);

    std::size_t cntr = 0;
    for (auto i = 0u; i < intervals_.size(); ++i) {
      auto [ni, wi] = interval_nodes_and_weights(i);

      const std::size_t Ni = ni.size();

      // exclude last point that belongs to next interval..
      n.segment(cntr, Ni - 1) = ni.head(Ni - 1);
      w.segment(cntr, Ni - 1) = wi.head(Ni - 1);

      cntr += Ni - 1;
    }

    n.tail(1).setConstant(1);
    w.tail(1).setConstant(0);

    return {n, w};
  }

  /**
   * @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.
   */
  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;
    utils::static_for<Kmax + 2 - Kmin>([&](auto i) {
      static constexpr auto K = Kmin + i;
      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);
        ;
      }
    });

    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.
   */
  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 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 derivative to evaluate
   * @param extend set to true if a value is provided for t=+1
   */
  template<typename RetT, std::ranges::sized_range R>
  RetT eval(double t, const R & r, std::size_t p = 0, bool extend = true) const
  {
    [[maybe_unused]] const std::size_t N = N_colloc();

    if (extend) {
      assert(std::ranges::size(r) == N + 1);
    } else {
      assert(std::ranges::size(r) == N);
    }

    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;

    Eigen::RowVectorXd W;

    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);
          const auto U                   = monomial_derivative<K>(u, p);
          W = MatMap(U[0].data(), 1, k + 1) * MatMap(B_ext_s[0].data(), k + 1, k + 1);
          assert(std::size_t(W.size()) == k + 1);
        } else {
          static constexpr auto nw_s = lgr_nodes<K>();
          static constexpr auto B_s  = lagrange_basis<K - 1>(nw_s.first);
          const auto U               = monomial_derivative<K - 1>(u, p);
          W                          = MatMap(U[0].data(), 1, k) * MatMap(B_s[0].data(), k, k);
          assert(std::size_t(W.size()) == k);
        }
      }
    });

    using namespace std::views;

    std::size_t N_before = 0;
    for (auto i = 0u; i < ival; ++i) { N_before += intervals_[i].K; }
    const auto r_ival = r | drop(int64_t(N_before));
    RetT ret          = W(0) * *std::ranges::begin(r_ival);

    for (auto i = 1u; const auto & v : r_ival | drop(1) | take(W.size() - 1)) { ret += W(i++) * 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<T, Mesh>;

/**
 * @brief Evaluate a function on all collocation points.
 *
 * Returns a nf x N matrix
 *
 *  F= [ f(t_0, X_0, U_0)  f(t_1, X_1, u_1) ... f(t_{N-1}, X_{N-1}, U_{N-1})]
 *
 * with the function evaluated at all collocation points t_i in the Mesh m.
 *
 * @tparam Der return derivatives w.r.t variables
 *
 * @param nf dimensionality of f image
 * @param f function (t, X, U) -> R^nf
 * @param m Mesh of time
 * @param t0 initial time variable
 * @param tf final time variable
 * @param X state variables (size nx x N+1)
 * @param U input variables (size nu x N)
 *
 * @return If Deriv == false,
 * If Deriv == true, {F, dvecF_dt0, dvecF_dtf, dvecF_dvecX, dvecF_dvecU}, where vec(X) stacks the
 * columns of X into a single column vector.
 */
template<bool Deriv>
auto colloc_eval(
  const std::size_t nf,
  auto && f,
  const MeshType auto & m,
  const double t0,
  const double tf,
  const Eigen::MatrixXd & X,
  const Eigen::MatrixXd & U)
{
  assert(m.N_colloc() + 1 == static_cast<std::size_t>(X.cols()));  //  extra variable at the end
  assert(m.N_colloc() == static_cast<std::size_t>(U.cols()));  // one input per collocation point

  const std::size_t nx = X.rows();
  const std::size_t nu = U.rows();

  // all nodes in mesh
  const auto [tau_s, w_s] = m.all_nodes_and_weights();

  Eigen::MatrixXd Fval(nf, tau_s.size() - 1);

  Eigen::SparseMatrix<double> dvecF_dt0, dvecF_dtf, dvecF_dvecX, dvecF_dvecU;

  if constexpr (Deriv) {
    dvecF_dt0.resize(Fval.size(), 1);
    dvecF_dtf.resize(Fval.size(), 1);
    dvecF_dvecX.resize(Fval.size(), X.size());
    dvecF_dvecU.resize(Fval.size(), U.size());

    dvecF_dt0.reserve(Fval.size());
    dvecF_dtf.reserve(Fval.size());

    Eigen::VectorXi FX_pattern = Eigen::VectorXi::Constant(X.size(), nf);
    FX_pattern.tail(nx).setZero();
    dvecF_dvecX.reserve(FX_pattern);
    dvecF_dvecU.reserve(Eigen::VectorXi::Constant(U.size(), nf));
  }

  for (auto i = 0u; i + 1 < tau_s.size(); ++i) {
    const double T = t0 + (tf - t0) * tau_s(i);

    const Eigen::VectorXd x = X.col(i);
    const Eigen::VectorXd u = U.col(i);

    if constexpr (Deriv) {
      const auto [fval, dfval] = diff::dr(f, wrt(T, x, u));

      assert(fval.rows() == Eigen::Index(nf));
      assert(dfval.rows() == Eigen::Index(nf));
      assert(dfval.cols() == Eigen::Index(1 + nu + nx));

      Fval.col(i) = fval;

      for (auto row = 0u; row < nf; ++row) {
        dvecF_dt0.insert(nf * i + row, 0) = dfval(row, 0) * (1. - tau_s(i));
        dvecF_dtf.insert(nf * i + row, 0) = dfval(row, 0) * tau_s(i);
        for (auto col = 0u; col < nx; ++col) {
          dvecF_dvecX.insert(nf * i + row, i * nx + col) = dfval(row, 1 + col);
        }
        for (auto col = 0u; col < nu; ++col) {
          dvecF_dvecU.insert(nf * i + row, i * nu + col) = dfval(row, 1 + nx + col);
        }
      }
    } else {
      Fval.col(i) = f(T, x, u);
    }
  }

  if constexpr (Deriv) {
    dvecF_dt0.makeCompressed();
    dvecF_dtf.makeCompressed();
    dvecF_dvecX.makeCompressed();
    dvecF_dvecU.makeCompressed();

    return std::make_tuple(
      std::move(Fval),
      std::move(dvecF_dt0),
      std::move(dvecF_dtf),
      std::move(dvecF_dvecX),
      std::move(dvecF_dvecU));
  } else {
    return Fval;
  }
}

/**
 * @brief Evaluate a function at endpoints.
 *
 * Returns a nf vector
 *
 *  F = f(t_0, t_f, x_0, x_f, q)
 *
 * @tparam Der return derivatives w.r.t variables
 *
 * @param nf dimensionality of f image
 * @param nf state space degrees of freedom
 * @param f function (t0, tf, x0, xf, q) -> R^nf
 * @param t0 initial time
 * @param tf final time
 * @param X state variables (size nx x N+1)
 * @param q integrals
 *
 * @return If Deriv == false,
 * If Deriv == true, {F, dF_dt0, dF_dtf, dF_dvecX, dF_dQ},
 */
template<bool Deriv>
auto colloc_eval_endpt(
  const std::size_t nf,
  const std::size_t nx,
  auto && f,
  [[maybe_unused]] const double t0,
  const double tf,
  const Eigen::MatrixXd & X,
  const Eigen::VectorXd & Q)
{
  assert(static_cast<std::size_t>(X.rows()) == nx);

  // NOTE: for now t0 = 0 and we don't want t0 in signatures
  assert(t0 == 0);

  const Eigen::VectorXd x0 = X.leftCols(1);
  const Eigen::VectorXd xf = X.rightCols(1);

  if constexpr (!Deriv) {
    return f(tf, x0, xf, Q);
  } else {
    const auto [Fval, J] = diff::dr(f, wrt(tf, x0, xf, Q));

    assert(static_cast<std::size_t>(J.rows()) == nf);
    assert(static_cast<std::size_t>(J.cols()) == 1 + 2 * nx + Q.size());

    Eigen::SparseMatrix<double> dF_dt0, dF_dtf, dF_dvecX, dF_dQ;

    dF_dt0.resize(nf, 1);
    // dF_dt0.reserve(nf);
    // for (auto i = 0u; i < nf; ++i) { dF_dt0.insert(i, 0) = J(i, 0); }

    dF_dtf.resize(nf, 1);
    dF_dtf.reserve(nf);
    for (auto i = 0u; i < nf; ++i) { dF_dtf.insert(i, 0) = J(i, 0); }

    dF_dvecX.resize(nf, X.size());
    Eigen::VectorXi pattern = Eigen::VectorXi::Zero(X.size());
    pattern.head(nx).setConstant(nf);
    pattern.tail(nx).setConstant(nf);
    dF_dvecX.reserve(pattern);

    for (auto row = 0u; row < nf; ++row) {
      for (auto col = 0u; col < nx; ++col) {
        dF_dvecX.insert(row, col)                 = J(row, 1 + col);
        dF_dvecX.insert(row, X.size() - nx + col) = J(row, 1 + nx + col);
      }
    }

    dF_dQ.resize(nf, Q.size());
    dF_dQ.reserve(Eigen::VectorXi::Constant(Q.size(), nf));

    for (auto row = 0u; row < nf; ++row) {
      for (auto col = 0u; col < Q.size(); ++col) {
        dF_dQ.insert(row, col) = J(row, 1 + 2 * nx + col);
      }
    }

    dF_dt0.makeCompressed();
    dF_dtf.makeCompressed();
    dF_dvecX.makeCompressed();
    dF_dQ.makeCompressed();

    return std::make_tuple(Fval, dF_dt0, dF_dtf, dF_dvecX, dF_dQ);
  }
}

/**
 * @brief Evaluate dynamics constraint in all collocation points of a Mesh.
 *
 * @tparam Der return derivatives w.r.t variables
 *
 * @param nx state space degrees of freedom
 * @param f right-hand side of dynamics with signature (t, x, u) -> dx where x and dx are size nx
 * x 1 and u is size nu x 1
 * @param m mesh with a total of N collocation points
 * @param tf final time (variable of size 1)
 * @param x state values (variable of size nx x N+1)
 * @param u input values (variable of size nu x N)
 *
 * @return {F, dvecF_dt0, dvecF_dtf, dvecF_dvecX, dvecF_dvecU},
 * where vec(X) stacks the columns of X into a single column vector.
 */
template<bool Deriv>
auto colloc_dyn(
  const std::size_t nx,
  auto && f,
  const MeshType auto & m,
  const double t0,
  const double tf,
  const Eigen::MatrixXd & X,
  const Eigen::MatrixXd & U)
{
  assert(m.N_colloc() + 1 == static_cast<std::size_t>(X.cols()));  // extra at the end
  assert(m.N_colloc() == static_cast<std::size_t>(U.cols()));      // one per collocation point
  assert(nx == static_cast<std::size_t>(X.rows()));                // one per collocation point

  Eigen::MatrixXd Fval;
  Eigen::MatrixXd XD(nx, m.N_colloc());
  Eigen::SparseMatrix<double> dvecF_dt0, dvecF_dtf, dvecF_dvecX, dvecF_dvecU, dvecXD_dvecX;

  if constexpr (!Deriv) {
    Fval = colloc_eval<0>(nx, f, m, t0, tf, X, U);
  } else {
    std::tie(Fval, dvecF_dt0, dvecF_dtf, dvecF_dvecX, dvecF_dvecU) =
      colloc_eval<1>(nx, f, m, t0, tf, X, U);

    dvecXD_dvecX.resize(XD.size(), X.size());

    // reserve sparsity pattern
    Eigen::VectorXi pattern = Eigen::VectorXi::Zero(X.size());
    for (auto M = 0u, i = 0u; i < m.N_ivals(); ++i) {
      const std::size_t K = m.N_colloc_ival(i);
      pattern.segment(M, (K + 1) * nx) += Eigen::VectorXi::Constant((K + 1) * nx, K);
      M += K * nx;
    }
    dvecXD_dvecX.reserve(pattern);
  }

  for (auto i = 0u, M = 0u; i < m.N_ivals(); M += m.N_colloc_ival(i), ++i) {
    const std::size_t K     = m.N_colloc_ival(i);
    const Eigen::MatrixXd D = m.interval_diffmat(i);
    XD.block(0, M, nx, K)   = X.block(0, M, nx, K + 1) * D;

    if constexpr (Deriv) {
      // vec(X * D) = kron(D', I) * vec(X), so derivative w.r.t vec(X) = kron(D', I)
      for (auto i = 0u; i < K; ++i) {
        for (auto j = 0u; j < K + 1; ++j) {
          for (auto diag = 0u; diag < nx; ++diag) {
            dvecXD_dvecX.coeffRef(M * nx + i * nx + diag, M * nx + j * nx + diag) += D(j, i);
          }
        }
      }
    }
  }

  Eigen::VectorXd Fv = (XD - (tf - t0) * Fval).reshaped();

  // scale equalities by by quadrature weights
  const auto N      = m.N_colloc();
  const auto [n, w] = m.all_nodes_and_weights();

  // vec(A * W) = kron(W', I) * vec(A), so we apply kron(W', I) on the left

  Eigen::SparseMatrix<double> W(N, N);
  W.reserve(Eigen::VectorXi::Ones(N));
  for (auto i = 0u; i < N; ++i) { W.insert(i, i) = w(i); }

  const Eigen::SparseMatrix<double> W_kron_I = kron_identity(W, nx);

  Fv.applyOnTheLeft(W_kron_I);

  if constexpr (!Deriv) {
    return Fv;
  } else {
    dvecXD_dvecX.makeCompressed();

    Eigen::SparseMatrix<double> dF_dt0 = -(tf - t0) * dvecF_dt0;
    dF_dt0 += Fval.reshaped().sparseView();  // OK since dvecF_dtf is dense
    dF_dt0 = W_kron_I * dF_dt0;

    Eigen::SparseMatrix<double> dF_dtf = -(tf - t0) * dvecF_dtf;
    dF_dtf -= Fval.reshaped().sparseView();  // OK since dvecF_dtf is dense
    dF_dtf = W_kron_I * dF_dtf;

    Eigen::SparseMatrix<double> dF_dvecX = dvecXD_dvecX;
    dF_dvecX -= (tf - t0) * dvecF_dvecX;
    dF_dvecX = W_kron_I * dF_dvecX;

    Eigen::SparseMatrix<double> dF_dvecU = -(tf - t0) * W_kron_I * dvecF_dvecU;

    dF_dt0.makeCompressed();
    dF_dtf.makeCompressed();
    dF_dvecX.makeCompressed();
    dF_dvecU.makeCompressed();

    return std::make_tuple(
      std::move(Fv),
      std::move(dF_dt0),
      std::move(dF_dtf),
      std::move(dF_dvecX),
      std::move(dF_dvecU));
  }
}

/**
 * @brief Calculate relative dynamics errors for each interval in mesh.
 *
 * @param nx state space dimension
 * @param f dynamics function
 * @param m Mesh
 * @param t0 initial time variable
 * @param tf final time variable
 * @param x state trajectory
 * @param u input trajectory
 *
 * @return vector with relative errors for every interval in m
 */
Eigen::VectorXd mesh_dyn_error(
  const std::size_t nx,
  auto && f,
  const MeshType auto & m,
  const double t0,
  const double tf,
  const std::function<Eigen::VectorXd(double)> xfun,
  const std::function<Eigen::VectorXd(double)> ufun)
{
  const auto N = m.N_ivals();

  // create a new mesh where each interval is extended
  Mesh mext = m;
  for (auto i = 0u; i < N; ++i) {
    const std::size_t K = m.N_colloc_ival(i);
    mext.set_N_colloc_ival(i, K + 1);
  }

  Eigen::VectorXd ival_errs(N);

  // for each interval
  for (auto i = 0u, M = 0u; i < N; M += m.N_colloc_ival(i), ++i) {
    const std::size_t Kext = mext.N_colloc_ival(i);

    const auto [tau_s, weights] = mext.interval_nodes_and_weights(i);

    assert(std::size_t(tau_s.size()) == Kext + 1);

    // evaluate X and F at those points
    Eigen::MatrixXd Fval(nx, Kext + 1);
    Eigen::MatrixXd Xval(nx, Kext + 1);
    for (auto j = 0u; j < Kext + 1; ++j) {
      const double tj = t0 + (tf - t0) * tau_s(j);

      // evaluate x and u values at tj using current degree polynomials
      const auto Xj = xfun(tj);
      const auto Uj = ufun(tj);

      // evaluate right-hand side of dynamics at tj
      Fval.col(j) = f(tj, Xj, Uj);

      // store x values for later comparison
      Xval.col(j) = Xj;
    }

    // "integrate" system inside interval
    const Eigen::MatrixXd Xval_est =
      Xval.col(0).replicate(1, Kext) + (tf - t0) * Fval.leftCols(Kext) * mext.interval_intmat(i);

    // absolute error in interval
    Eigen::VectorXd e_abs = (Xval_est - Xval.rightCols(Kext)).colwise().norm();
    Eigen::VectorXd e_rel = e_abs / (1. + Xval.rightCols(Kext).colwise().norm().maxCoeff());

    // mex relative error on interval
    ival_errs(i) = e_rel.maxCoeff();
  }

  return ival_errs;
}

/**
 * @brief Refine intervals in mesh to satisfy a target error criterion.
 * @param[in, out] m mesh to refine
 * @param[in] errs relative errors for all intervals (@see mesh_dyn_error())
 * @param[in] target_err target relative error
 */
void mesh_refine(MeshType auto & m, const Eigen::VectorXd & errs, const double target_err)
{
  const auto N = m.N_ivals();

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

  for (auto i = 0u; i < N; ++i) {
    const auto Nmi = N - 1 - i;
    const auto Ki  = m.N_colloc_ival(Nmi);

    if (errs(Nmi) > target_err) {
      const auto Ktarget = Ki + std::lround(std::log(errs(Nmi) / target_err) / std::log(Ki) + 1);
      m.refine_ph(Nmi, Ktarget);
    }
  }
}

/**
 * @brief Evaluate integral constraint on Mesh.
 *
 * @tparam Der return derivatives w.r.t variables
 *
 * @param nq number of integrals
 * @param g integrand with signature (t, x, u) -> R^{nq} where x is size nx x 1 and u is size nu x
 * 1
 * @param m mesh
 * @param t0 initial time (variable of size 1)
 * @param tf final time (variable of size 1)
 * @param I values (variable of size nq)
 * @param X state values (variable of size nx x N+1)
 * @param U input values (variable of size nu x N)
 *
 * @return {G, dvecG_dt0, dvecG_dtf, dvecG_dvecX, dvecG_dvecU},
 * where vec(X) stacks the columns of X into a single column vector.
 */
template<bool Deriv>
auto colloc_int(
  const std::size_t nq,
  auto && g,
  const MeshType auto & m,
  const double t0,
  const double tf,
  const Eigen::VectorXd & I,
  const Eigen::MatrixXd & X,
  const Eigen::MatrixXd & U)
{
  assert(static_cast<std::size_t>(I.size()) == nq);

  const std::size_t N = m.N_colloc();

  const auto [n, w] = m.all_nodes_and_weights();

  if constexpr (Deriv == false) {
    const auto Gv              = colloc_eval<Deriv>(nq, g, m, t0, tf, X, U);
    const Eigen::VectorXd Iest = Gv * w.head(N);
    Eigen::VectorXd Rv         = (tf - t0) * Iest - I;
    return Rv;
  } else {
    const auto [Gv, dvecG_dt0, dvecG_dtf, dvecG_dvecX, dvecG_dvecU] =
      colloc_eval<Deriv>(nq, g, m, t0, tf, X, U);
    const Eigen::VectorXd Iest = Gv * w.head(N);

    Eigen::VectorXd Rv = (tf - t0) * Iest - I;

    const Eigen::SparseMatrix<double> w_kron_I =
      (tf - t0) * kron_identity(w.head(N).transpose(), nq);

    Eigen::SparseMatrix<double> dR_dt0 = w_kron_I * dvecG_dt0;
    for (auto i = 0u; i < Iest.size(); ++i) { dR_dt0.coeffRef(i, 0) -= Iest(i); }

    Eigen::SparseMatrix<double> dR_dtf = w_kron_I * dvecG_dtf;
    for (auto i = 0u; i < Iest.size(); ++i) { dR_dtf.coeffRef(i, 0) += Iest(i); }

    Eigen::SparseMatrix<double> dR_dvecI = -sparse_identity(nq);

    Eigen::SparseMatrix<double> dR_dvecX = w_kron_I * dvecG_dvecX;

    Eigen::SparseMatrix<double> dR_dvecU = w_kron_I * dvecG_dvecU;

    dR_dt0.makeCompressed();
    dR_dtf.makeCompressed();
    dR_dvecI.makeCompressed();
    dR_dvecX.makeCompressed();
    dR_dvecU.makeCompressed();

    return std::make_tuple(
      std::move(Rv),
      std::move(dR_dt0),
      std::move(dR_dtf),
      std::move(dR_dvecI),
      std::move(dR_dvecX),
      std::move(dR_dvecU));
  }
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__COLLOCATION_HPP_


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_COMPLEX_GENERIC_POW_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_COMPLEX_GENERIC_POW_HPP_INCLUDED
#include <nt2/exponential/functions/pow.hpp>
#include <nt2/include/functions/pow.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/sincos.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/seladd.hpp>
#include <nt2/include/functions/if_else.hpp>
#include <nt2/include/functions/is_real.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/arg.hpp>
#include <nt2/include/functions/logical_not.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_real.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>
#include <nt2/sdk/simd/logical.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)
                            , (generic_< complex_<floating_<A0> > >)
                              (generic_< complex_<floating_<A0> > >)
                            )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      return exp(a1*log(a0));
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              , (generic_< complex_<floating_<A0> > >)
                                (generic_< floating_<A1> >)
                            )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
    {
      typedef typename meta::as_real<result_type>::type rtype;
      typedef typename meta::as_imaginary< rtype>::type itype;
      rtype t = nt2::arg(a0);
      rtype a = nt2::abs(a0);
      return nt2::pow(a, a1)*nt2::exp(itype(t*a1));
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              , (generic_< complex_<floating_<A0> > >)
                                (generic_< dry_<floating_<A1> > >)
                            )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return pow(a0, nt2::real(a1));
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              , (generic_< floating_<A0> >)
                                (generic_< complex_<floating_<A1> > >)
                            )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
      {
        typedef typename meta::as_dry<A0>::type dtype;
        return nt2::exp(a1*nt2::log(dtype(a0)));
      }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              , (generic_< dry_ < floating_<A0> > > )
                                (generic_< complex_<floating_<A1> > >)
                            )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
      {
        return nt2::exp(a1*nt2::log(a0));
      }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              , (generic_< complex_<floating_<A0> > >)
                              (generic_< imaginary_<floating_<A1> > >)
                              )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
      {
        return nt2::exp(a1*nt2::log(a0));
      }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              , (generic_< imaginary_<floating_<A0> > >)
                              (generic_< complex_<floating_<A1> > >)
                              )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
      {
        return nt2::exp(a1*nt2::log(a0));
      }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              , (generic_< imaginary_<floating_<A0> > >)
                              (generic_< floating_<A1> >)
                              )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
      {
        return nt2::exp(a1*nt2::log(a0));
      }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              , (generic_< imaginary_<floating_<A0> > >)
                              (generic_< dry_ < floating_<A1> > >)
                              )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
      {
        return nt2::exp(a1*nt2::log(a0));
      }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              ,  (generic_< floating_<A0> >)
                              (generic_< imaginary_<floating_<A1> > >)
                              )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    typedef typename meta::as_dry<A0>::type dtype;
    NT2_FUNCTOR_CALL(2)
      {
        return nt2::exp(a1*nt2::log(dtype(a0)));
      }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              ,  (generic_< dry_ < floating_<A0> > > )
                              (generic_< imaginary_<floating_<A1> > >)
                              )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
      {
        return nt2::exp(a1*nt2::log(a0));
      }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::pow_, tag::cpu_
                              , (A0)(A1)
                              ,  (generic_< dry_ < floating_<A0> > > )
                              (generic_< dry_<floating_<A1> > >)
                              )
  {
    typedef typename meta::as_complex<A0>::type result_type;
    NT2_FUNCTOR_CALL(2)
      {
        return nt2::exp(a1*nt2::log(a0));
      }
  };
} }

#endif


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

// Written by Simon Praetorius (adopted from previous implementation)

#ifndef ITL_GMRES_HOUSEHOLDER_INCLUDE
#define ITL_GMRES_HOUSEHOLDER_INCLUDE

#include <algorithm>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/multi_vector.hpp>
#include <boost/numeric/mtl/operation/givens.hpp>
#include <boost/numeric/mtl/operation/two_norm.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>

#include "solver/itl/details.hpp"

namespace itl
{

  /// Generalized Minimal Residual method (without restart) using householder othogonalization.
  /** It computes at most kmax_in iterations (or size(x) depending on what is smaller)
      regardless on whether the termination criterion is reached or not.   **/
  template <typename Matrix, typename Vector, typename LeftPreconditioner, typename Iteration>
  int gmres_householder_full(const Matrix& A, Vector& x, const Vector& b,
                             LeftPreconditioner& L, Iteration& iter)
  {
    using mtl::irange;
    using std::abs;
    using math::reciprocal;
    using mtl::iall;
    using mtl::imax;
    using mtl::signum;
    using mtl::vector::dot;
    using mtl::conj;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    typedef typename mtl::Collection<Vector>::size_type  Size;

    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");

    const Scalar zero= math::zero(Scalar()), dbl_tol= 1.e-16;
    Scalar       rho, bnrm2, temp, beta;
    Size         k, kmax(std::min(size(x), Size(iter.max_iterations() - iter.iterations())));
    Vector       w(b - A *x), r(solve(L,w));
    mtl::matrix::multi_vector<Vector>   V(Vector(resource(x), zero), kmax+1);
    mtl::vector::dense_vector<Scalar>   sn(kmax, zero), cs(kmax, zero), s(kmax+1, zero), y(kmax, zero);  // replicated in distributed solvers
    mtl::matrix::dense2D<Scalar>        H(kmax, kmax);

    bnrm2 = two_norm(b);
    if (bnrm2 < dbl_tol)
      bnrm2 = 1.0;

    temp = two_norm(r);				// norm of preconditioned residual
    rho = temp * reciprocal(bnrm2);
    if (iter.finished(rho))			// initial guess is good enough solution
      return iter;

    // u = r + sign(r(0))*||r||*e0
    beta = signum(r[0])*temp;
    w = r;
    w[0] += beta;
    w *= reciprocal(two_norm(w));

    V.vector(0) = w;
    H = zero;
    s[0] = -beta;

    // GMRES iteration
    for (k= 0; k < kmax && !iter.finished(rho); ++k, ++iter)
    {

      w = (-2.0 * V.vector(k)[k])*V.vector(k);
      w[k] += 1.0;
      // v := P_0*...*P_{k-2}*(P_{k-1} * e_k)
      for (Size i= k; i > 0; i--)
      {
        temp = 2.0 * dot(V.vector(i-1), w);
        w -= temp * V.vector(i-1);
      }

      temp = two_norm(w);
      if (temp == zero)
        return iter.fail(2, "GMRES: breakdown");

      // Explicitly normalize v to reduce the effects of round-off.
      w *= reciprocal(temp);
      w = solve(L, Vector(A*w));

      // P_{k-1}*...*P_0*Av
      for (Size i = 0; i <= k; i++)
      {
        temp = 2.0 * dot(V.vector(i), w);
        w -= temp * V.vector(i);
      }

      temp = two_norm(w);
      if (temp == zero)
        return iter.fail(3, "GMRES: breakdown");

      irange range_to_end(k+1,imax);
      set_to_zero(V.vector(k+1));
      V.vector(k+1)[range_to_end] = w[range_to_end];
      beta = two_norm(V.vector(k+1));
      if (beta != 0.0)
      {
        beta *= signum(w[k+1]);
        V.vector(k+1)[k+1] += beta;
        V.vector(k+1) *= reciprocal(two_norm(V.vector(k+1)));

        w[k+1] = -beta;
      }

      for (Size i= 0; i < k; i++)
      {
        temp   =  conj(cs[i])*w[i] + conj(sn[i])*w[i+1];
        w[i+1] = -sn[i]*w[i] + cs[i]*w[i+1];
        w[i]   =  temp;
      }

      details::rotmat(w[k], w[k+1], cs[k], sn[k]);

      s[k+1] = -sn[k]*s[k];
      s[k]   = conj(cs[k])*s[k];
      w[k]   = cs[k]*w[k] + sn[k]*w[k+1];
      w[k+1] = 0.0;

      irange range(num_rows(H));
      H[iall][k] = w[range];

      rho = std::abs(s[k+1]) / bnrm2;
    }

    // reduce k, to get regular matrix
    //     while (k > 0 && std::abs(s[k-1]) <= iter.atol()) k--;

    // iteration is finished -> compute x: solve H*y=s as far as rank of H allows
    irange range(k);
    for (; !range.empty(); --range)
    {
      try
      {
        y[range] = upper_trisolve(H[range][range], s[range]);
      }
      catch (mtl::matrix_singular)
      {
        continue;    // if singular then try with sub-matrix
      }
      break;
    }

    if (range.finish() < k)
      std::cerr << "GMRES orhogonalized with " << k << " vectors but matrix singular, can only use "
                << range.finish() << " vectors!\n";
    if (range.empty())
      return iter.fail(3, "GMRES did not find any direction to correct x");

    kmax = k-1;

    w = V.vector(kmax) * (-2.0 * y[kmax] * conj(V.vector(kmax)[kmax]));
    w[kmax] += y[kmax];
    for (Size i= kmax; i > 0; i--)
    {
      w[i-1] += y[i-1];
      temp = 2.0 * dot(V.vector(i-1), w);
      w -= temp * V.vector(i-1);
    }
    x += w;

    r = b - A*x;
    return iter.terminate(r);
  }

  /// Generalized Minimal Residual method with restart
  template <typename Matrix, typename Vector, typename LeftPreconditioner,
            typename Iteration>
  int gmres_householder(const Matrix& A, Vector& x, const Vector& b,
                        LeftPreconditioner& L,
                        Iteration& iter, typename mtl::Collection<Vector>::size_type restart)
  {
    do
    {
      Iteration inner(iter);
      inner.set_max_iterations(std::min(int(iter.iterations()+restart), iter.max_iterations()));
      inner.suppress_resume(true);
      gmres_householder_full(A, x, b, L, inner);
      iter.update_progress(inner);
    }
    while (!iter.finished());

    return iter;
  }

} // namespace itl

#endif // ITL_GMRES_HOUSEHOLDER_INCLUDE


#pragma once
#include <stdint.h>
#include "settings.hpp"
#include "weight_initialization.hpp"
#include "activations.hpp"
#if USE_EIGEN == 1
#include <Eigen/Dense>
#endif

namespace nn
{
	enum class LayerType
	{
		kInput,
		kFC,
		kSoftmax
	};

	// any layer has some amount of units and activation function
#if USE_EIGEN == 1
	class layer
	{
	public:
		using MatrixType = Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>;

		layer(LayerType type,
			uint32_t unitsInLayer,
			uint32_t unitsInPreviousLayer,
			ActivationType activationType,
			WeightInitializationType weightInitializationType) :
			m_type(type),
			m_unitsInLayer(unitsInLayer),
			m_unitsInPreviousLayer(unitsInPreviousLayer)
		{
			// set activation type
			if (activationType == ActivationType::kSigmoid)
			{
				m_activation = activation<ActivationType::kSigmoid>;
				m_activationDerivative = activation_derivative<ActivationType::kSigmoid>;
			}
			else if (activationType == ActivationType::kLinear)
			{
				m_activation = activation<ActivationType::kLinear>;
				m_activationDerivative = activation_derivative<ActivationType::kLinear>;
			}
			else if (activationType == ActivationType::kTanh)
			{
				m_activation = activation<ActivationType::kTanh>;
				m_activationDerivative = activation_derivative<ActivationType::kTanh>;
			}
			else if (activationType == ActivationType::kRelu)
			{
				m_activation = activation<ActivationType::kRelu>;
				m_activationDerivative = activation_derivative<ActivationType::kRelu>;
			}
			else if (activationType == ActivationType::kLRelu)
			{
				m_activation = activation<ActivationType::kLRelu>;
				m_activationDerivative = activation_derivative<ActivationType::kLRelu>;
			}

			if (type != LayerType::kInput)
			{
				if (weightInitializationType == WeightInitializationType::kGaussian)
				{
					m_weight = MatrixType::Zero(UnitsInLayer(), UnitsInPreviousLayer()).unaryExpr(weight_initalization<WeightInitializationType::kZeros>());
					m_nabla_w = MatrixType::Zero(UnitsInLayer(), UnitsInPreviousLayer());
					m_bias = MatrixType::Zero(UnitsInLayer(), 1).unaryExpr(weight_initalization<WeightInitializationType::kZeros>());
					m_nabla_b = MatrixType::Zero(UnitsInLayer(), 1);
				}
				else if (weightInitializationType == WeightInitializationType::kSequentialDebug)
				{
					m_weight = MatrixType::Zero(UnitsInLayer(), UnitsInPreviousLayer()).unaryExpr(weight_initalization<WeightInitializationType::kSequentialDebug>());
					m_nabla_w = MatrixType::Zero(UnitsInLayer(), UnitsInPreviousLayer());
					m_bias = MatrixType::Zero(UnitsInLayer(), 1).unaryExpr(weight_initalization<WeightInitializationType::kSequentialDebug>());
					m_nabla_b = MatrixType::Zero(UnitsInLayer(), 1);
				}
				else if (weightInitializationType == WeightInitializationType::kUniform)
				{
					m_weight = MatrixType::Zero(UnitsInLayer(), UnitsInPreviousLayer()).unaryExpr(weight_initalization<WeightInitializationType::kUniform>());
					m_nabla_w = MatrixType::Zero(UnitsInLayer(), UnitsInPreviousLayer());
					m_bias = MatrixType::Zero(UnitsInLayer(), 1).unaryExpr(weight_initalization<WeightInitializationType::kUniform>());
					m_nabla_b = MatrixType::Zero(UnitsInLayer(), 1);
				}
				else if (weightInitializationType == WeightInitializationType::kGaussian)
				{
					m_weight = MatrixType::Zero(UnitsInLayer(), UnitsInPreviousLayer()).unaryExpr(weight_initalization<WeightInitializationType::kGaussian>());
					m_nabla_w = MatrixType::Zero(UnitsInLayer(), UnitsInPreviousLayer());
					m_bias = MatrixType::Zero(UnitsInLayer(), 1).unaryExpr(weight_initalization<WeightInitializationType::kGaussian>());
					m_nabla_b = MatrixType::Zero(UnitsInLayer(), 1);
				}
				else if (weightInitializationType == WeightInitializationType::kWeightedGaussian)
				{
					m_weight = MatrixType::Zero(UnitsInLayer(), UnitsInPreviousLayer()).unaryExpr(weight_initalization<WeightInitializationType::kWeightedGaussian>(UnitsInLayer()));
					m_nabla_w = MatrixType::Zero(UnitsInLayer(), UnitsInPreviousLayer());
					m_bias = MatrixType::Zero(UnitsInLayer(), 1).unaryExpr(weight_initalization<WeightInitializationType::kWeightedGaussian>(UnitsInLayer()));
					m_nabla_b = MatrixType::Zero(UnitsInLayer(), 1);
				}
			}
		}

		~layer() {}

		void computeWeightedSum(const MatrixType& input)
		{
			m_z.noalias() = m_weight * input;
			for (int i = 0; i < m_z.cols(); ++i)
				m_z.col(i).noalias() += m_bias;

		}
		void setActivations(const MatrixType& input) { m_a.noalias() = input; }
		void computeActivations(const MatrixType& input)
		{
			if (m_type == LayerType::kFC)
				m_a.noalias() = input.unaryExpr(m_activation);
			else if (m_type == LayerType::kSoftmax)
			{
				if (m_a.rows() != input.rows() || m_a.cols() != input.cols())
					m_a = MatrixType::Zero(input.rows(), input.cols());
				MatrixType maxCol(m_a.rows(), 1);
				for (int i = 0; i < input.cols(); ++i)
				{
					maxCol.setConstant(input.maxCoeff()); // prevent softmax overflow
					m_a.col(i).noalias() = (input.col(i) - maxCol).unaryExpr(&expf);
					m_a.col(i) /= m_a.col(i).sum();
				}
			}
		}
		void computeActivationDerivatives(const MatrixType& input)
		{
			if (m_type == LayerType::kFC)
				m_da.noalias() = input.unaryExpr(m_activationDerivative);
			else if (m_type == LayerType::kSoftmax)
				m_da = MatrixType::Ones(m_a.rows(), m_a.cols());
		}

		MatrixType computeWeightedSumExplicit(const MatrixType& input)
		{
			MatrixType result = m_weight * input;
			for (int i = 0; i < result.cols(); ++i)
				result.col(i).noalias() += m_bias;
			return result;
		}

		MatrixType computeActivationsExplicit(const MatrixType& input)
		{
			MatrixType result;
			if (m_type == LayerType::kFC)
				result.noalias() = input.unaryExpr(m_activation);
			else if (m_type == LayerType::kSoftmax)
			{
				if (result.rows() != input.rows() || result.cols() != input.cols())
					result = MatrixType::Zero(input.rows(), input.cols());
				MatrixType maxCol(result.rows(), 1);
				for (int i = 0; i < input.cols(); ++i)
				{
					maxCol.setConstant(input.maxCoeff()); // prevent softmax overflow
					result.col(i).noalias() = (input.col(i) - maxCol).unaryExpr(&expf);
					result.col(i) /= result.col(i).sum();
				}
			}
			return result;
		}
		MatrixType computeActivationDerivativesExplicit(const MatrixType& input)
		{
			MatrixType result;
			if (m_type == LayerType::kFC)
				result.noalias() = input.unaryExpr(m_activationDerivative);
			else if (m_type == LayerType::kSoftmax)
				result = MatrixType::Ones(input.rows(), input.cols());
			return result;
		}

		const MatrixType& getWeightedSum() const { return m_z; }
		const MatrixType& getActivations() const { return m_a; }
		const MatrixType& getActivationDerivatives() const { return m_da; }
		const MatrixType& getWeights() const { return m_weight; }
		MatrixType& getWeights() { return m_weight; }
		MatrixType& getBias() { return m_bias; }
		MatrixType& getNablaB() { return m_nabla_b; }
		MatrixType& getNablaW() { return m_nabla_w; }

		uint32_t UnitsInLayer() const { return m_unitsInLayer; }
		uint32_t UnitsInPreviousLayer() const { return m_unitsInPreviousLayer; }
	private:
		LayerType m_type;
		uint32_t m_unitsInLayer;
		uint32_t m_unitsInPreviousLayer;
		MatrixType m_z;
		MatrixType m_a;
		MatrixType m_da;

		MatrixType m_weight;
		MatrixType m_nabla_w;
		MatrixType m_bias;
		MatrixType m_nabla_b;
		ActivationFunction m_activation = nullptr;
		ActivationFunction m_activationDerivative = nullptr;
	};
#endif
}


#include <Eigen/Core>
#include <Eigen/StdVector>
#include <boost/make_unique.hpp>
#include <boost/optional/optional_io.hpp>
#include <catch2/catch.hpp>
#include <random>
#include <string>
#include "common/convex_hull.hpp"
#include "common/facets.hpp"
#include "common/geom.hpp"
#include "common/point_source_panner.hpp"
#include "ear/bs2051.hpp"
#include "ear/exceptions.hpp"
#include "helper/compare.hpp"

using namespace ear;

TEST_CASE("test_virtual_ngon") {
  Eigen::MatrixXd spkPositions(4, 3);
  spkPositions << cartT(30.0, 0.0, 1.0), cartT(-30.0, 0.0, 1.0),
      cartT(30.0, 30.0, 1.0), cartT(-30.0, 30.0, 1.0);
  Eigen::Vector4d virtualDownmix(0.2, 0.2, 0.3, 0.3);
  Eigen::Vector3d virtualPos = virtualDownmix.transpose() * spkPositions;
  auto ng =
      std::make_shared<VirtualNgon>(Eigen::Vector4i::LinSpaced(4, 0, 4),
                                    spkPositions, virtualPos, virtualDownmix);
  // panning to the virtual speaker, the output should be the normalised downmix
  boost::optional<Eigen::VectorXd> pv = ng->handle(virtualPos);
  REQUIRE(pv != boost::none);
  REQUIRE(pv.get().size() == 4);
  REQUIRE(pv->isApprox(virtualDownmix / virtualDownmix.norm()));

  std::default_random_engine generator;
  std::uniform_real_distribution<double> distribution(0.0, 1.0);
  auto random = [&](double) { return distribution(generator); };
  for (int i = 0; i < 100; ++i) {
    Eigen::Vector4d proportion = Eigen::Vector4d::NullaryExpr(4, random);
    // we can calculate a position within the ngon
    Eigen::Vector3d pos = spkPositions.transpose() * proportion;
    pos /= pos.norm();
    // which when rendered
    boost::optional<Eigen::VectorXd> pv = ng->handle(pos);
    REQUIRE(pv != boost::none);
    // can be multiplied by the speaker positions to produce another position
    Eigen::Vector3d posCalc = pv->transpose() * spkPositions;
    posCalc /= posCalc.norm();
    // that should be the same as the one we started with
    REQUIRE(pos.isApprox(posCalc));
  }
}

TEST_CASE("test_quad") {
  Eigen::MatrixXd spkPositions(4, 3);
  spkPositions << cartT(30.0, -15.0, 1.0), cartT(-30.0, -15.0, 1.0),
      cartT(30.0, 15.0, 1.0), cartT(-30.0, 15.0, 1.0);
  auto quad = std::make_shared<QuadRegion>(Eigen::Vector4i::LinSpaced(4, 0, 4),
                                           spkPositions);

  using PosGainsT = std::pair<Eigen::Vector3d, Eigen::Vector4d>;
  std::vector<PosGainsT, Eigen::aligned_allocator<PosGainsT>> posGains;
  posGains.push_back(
      std::make_pair(spkPositions.row(0), Eigen::Vector4d(1.0, 0.0, 0.0, 0.0)));
  posGains.push_back(
      std::make_pair(spkPositions.row(1), Eigen::Vector4d(0.0, 1.0, 0.0, 0.0)));
  posGains.push_back(
      std::make_pair(spkPositions.row(2), Eigen::Vector4d(0.0, 0.0, 1.0, 0.0)));
  posGains.push_back(
      std::make_pair(spkPositions.row(3), Eigen::Vector4d(0.0, 0.0, 0.0, 1.0)));
  posGains.push_back(
      std::make_pair(cart(0.0, 0.0, 1.0), Eigen::Vector4d(0.5, 0.5, 0.5, 0.5)));
  for (const auto& pair : posGains) {
    auto pv = quad->handle(pair.first);
    REQUIRE(pv != boost::none);
    REQUIRE(pv->isApprox(pair.second));
  }
};

TEST_CASE("test_stereo_downmix") {
  Eigen::MatrixXd spkPositions(2, 3);
  spkPositions << cartT(30.0, 0.0, 1.0), cartT(-30.0, 0.0, 1.0);

  auto p = std::make_shared<StereoPannerDownmix>(
      Eigen::Vector2i::LinSpaced(2, 0, 1), spkPositions);

  std::vector<std::pair<Eigen::Vector3d, Eigen::Vector2d>> posGains;
  posGains.push_back(std::make_pair(cart(0.0, 0.0, 1.0),
                                    Eigen::Vector2d(sqrt(0.5), sqrt(0.5))));
  posGains.push_back(
      std::make_pair(cart(-30.0, 0.0, 1.0), Eigen::Vector2d(0.0, 1.0)));
  posGains.push_back(
      std::make_pair(cart(-110.0, 0.0, 1.0), Eigen::Vector2d(0.0, sqrt(0.5))));
  posGains.push_back(std::make_pair(cart(-180.0, 0.0, 1.0),
                                    Eigen::Vector2d(sqrt(0.25), sqrt(0.25))));

  for (auto pair : posGains) {
    auto pv = p->handle(pair.first);
    REQUIRE(pv->isApprox(pair.second));
  }
}

Eigen::MatrixXd createDownmixMatrixFromIndices(std::vector<int> indices,
                                               size_t size) {
  Eigen::MatrixXd downmix = Eigen::MatrixXd::Identity(size, size);
  for (int index : indices) {
    Eigen::VectorXd downmixRow = Eigen::VectorXd::Zero(size);
    downmixRow(index) = 1.0;
    downmix.conservativeResize(downmix.rows() + 1, Eigen::NoChange);
    downmix.row(downmix.rows() - 1) = downmixRow;
  }
  return downmix;
}

TEST_CASE("extra_pos_vertical_nominal") {
  std::vector<Channel> extraChannels;
  Eigen::MatrixXd downmix;
  std::vector<PolarPosition> expectedPositions;

  SECTION("0+5+0") {
    Layout layout = getLayout("0+5+0").withoutLfe();
    std::tie(extraChannels, downmix) = extraPosVerticalNominal(layout);
    std::vector<PolarPosition> expectedPositions = {
        PolarPosition(30.0, -30.0, 1.0),   PolarPosition(-30.0, -30.0, 1.0),
        PolarPosition(0.0, -30.0, 1.0),    PolarPosition(110.0, -30.0, 1.0),
        PolarPosition(-110.0, -30.0, 1.0), PolarPosition(30.0, 30.0, 1.0),
        PolarPosition(-30.0, 30.0, 1.0),   PolarPosition(0.0, 30.0, 1.0),
        PolarPosition(110.0, 30.0, 1.0),   PolarPosition(-110.0, 30.0, 1.0)};
    REQUIRE(extraChannels.size() == expectedPositions.size());
    for (size_t i = 0; i < extraChannels.size(); ++i) {
      REQUIRE(extraChannels[i].polarPosition() == expectedPositions[i]);
    }
    Eigen::MatrixXd expectedDownmix = createDownmixMatrixFromIndices(
        std::vector<int>{0, 1, 2, 3, 4, 0, 1, 2, 3, 4},
        layout.channels().size());
    REQUIRE(downmix == expectedDownmix);
  };

  SECTION("2+5+0") {
    Layout layout = getLayout("2+5+0").withoutLfe();
    std::tie(extraChannels, downmix) = extraPosVerticalNominal(layout);
    std::vector<PolarPosition> expectedPositions = {
        PolarPosition(30.0, -30.0, 1.0),   PolarPosition(-30.0, -30.0, 1.0),
        PolarPosition(0.0, -30.0, 1.0),    PolarPosition(110.0, -30.0, 1.0),
        PolarPosition(-110.0, -30.0, 1.0), PolarPosition(110.0, 30.0, 1.0),
        PolarPosition(-110.0, 30.0, 1.0)};
    REQUIRE(extraChannels.size() == expectedPositions.size());
    for (size_t i = 0; i < extraChannels.size(); ++i) {
      REQUIRE(extraChannels[i].polarPosition() == expectedPositions[i]);
    }
    Eigen::MatrixXd expectedDownmix = createDownmixMatrixFromIndices(
        std::vector<int>{0, 1, 2, 3, 4, 3, 4}, layout.channels().size());
    REQUIRE(downmix == expectedDownmix);
  };

  SECTION("4+5+0/4+5+1") {
    for (const std::string& layoutName : {"4+5+0", "4+5+1"}) {
      Layout layout = getLayout("4+5+0").withoutLfe();
      std::tie(extraChannels, downmix) = extraPosVerticalNominal(layout);
      std::vector<PolarPosition> expectedPositions = {
          PolarPosition(30.0, -30.0, 1.0), PolarPosition(-30.0, -30.0, 1.0),
          PolarPosition(0.0, -30.0, 1.0), PolarPosition(110.0, -30.0, 1.0),
          PolarPosition(-110.0, -30.0, 1.0)};
      REQUIRE(extraChannels.size() == expectedPositions.size());
      for (size_t i = 0; i < extraChannels.size(); ++i) {
        REQUIRE(extraChannels[i].polarPosition() == expectedPositions[i]);
      }
      Eigen::MatrixXd expectedDownmix = createDownmixMatrixFromIndices(
          std::vector<int>{0, 1, 2, 3, 4}, layout.channels().size());
      REQUIRE(downmix == expectedDownmix);
    }
  };

  SECTION("3+7+0") {
    Layout layout = getLayout("3+7+0").withoutLfe();
    std::tie(extraChannels, downmix) = extraPosVerticalNominal(layout);
    std::vector<PolarPosition> expectedPositions = {
        PolarPosition(0.0, -30.0, 1.0),   PolarPosition(30.0, -30.0, 1.0),
        PolarPosition(-30.0, -30.0, 1.0), PolarPosition(90.0, -30.0, 1.0),
        PolarPosition(-90.0, -30.0, 1.0), PolarPosition(135.0, -30.0, 1.0),
        PolarPosition(-135.0, -30.0, 1.0)};
    REQUIRE(extraChannels.size() == expectedPositions.size());
    for (size_t i = 0; i < extraChannels.size(); ++i) {
      REQUIRE(extraChannels[i].polarPosition() == expectedPositions[i]);
    }
    Eigen::MatrixXd expectedDownmix = createDownmixMatrixFromIndices(
        std::vector<int>{0, 1, 2, 5, 6, 7, 8}, layout.channels().size());
    REQUIRE(downmix == expectedDownmix);
  };

  SECTION("4+9+0") {
    Layout layout = getLayout("4+9+0").withoutLfe();
    std::tie(extraChannels, downmix) = extraPosVerticalNominal(layout);
    std::vector<PolarPosition> expectedPositions = {
        PolarPosition(30.0, -30.0, 1.0),   PolarPosition(-30.0, -30.0, 1.0),
        PolarPosition(0.0, -30.0, 1.0),    PolarPosition(90.0, -30.0, 1.0),
        PolarPosition(-90.0, -30.0, 1.0),  PolarPosition(135.0, -30.0, 1.0),
        PolarPosition(-135.0, -30.0, 1.0), PolarPosition(15.0, -30.0, 1.0),
        PolarPosition(-15.0, -30.0, 1.0)};
    REQUIRE(extraChannels.size() == expectedPositions.size());
    for (size_t i = 0; i < extraChannels.size(); ++i) {
      REQUIRE(extraChannels[i].polarPosition() == expectedPositions[i]);
    }
    Eigen::MatrixXd expectedDownmix = createDownmixMatrixFromIndices(
        std::vector<int>{0, 1, 2, 3, 4, 5, 6, 11, 12},
        layout.channels().size());
    REQUIRE(downmix == expectedDownmix);
  };

  SECTION("9+10+3") {
    Layout layout = getLayout("9+10+3").withoutLfe();
    std::tie(extraChannels, downmix) = extraPosVerticalNominal(layout);
    std::vector<PolarPosition> expectedPositions = {
        PolarPosition(135.0, -30.0, 1.0), PolarPosition(-135.0, -30.0, 1.0),
        PolarPosition(180.0, -30.0, 1.0), PolarPosition(90.0, -30.0, 1.0),
        PolarPosition(-90.0, -30.0, 1.0)};
    REQUIRE(extraChannels.size() == expectedPositions.size());
    for (size_t i = 0; i < extraChannels.size(); ++i) {
      REQUIRE(extraChannels[i].polarPosition() == expectedPositions[i]);
    }
    Eigen::MatrixXd expectedDownmix = createDownmixMatrixFromIndices(
        std::vector<int>{3, 4, 7, 8, 9}, layout.channels().size());
    REQUIRE(downmix == expectedDownmix);
  };

  SECTION("0+7+0") {
    Layout layout = getLayout("0+7+0").withoutLfe();
    std::tie(extraChannels, downmix) = extraPosVerticalNominal(layout);
    std::vector<PolarPosition> expectedPositions = {
        PolarPosition(30.0, -30.0, 1.0),   PolarPosition(-30.0, -30.0, 1.0),
        PolarPosition(0.0, -30.0, 1.0),    PolarPosition(90.0, -30.0, 1.0),
        PolarPosition(-90.0, -30.0, 1.0),  PolarPosition(135.0, -30.0, 1.0),
        PolarPosition(-135.0, -30.0, 1.0), PolarPosition(30.0, 30.0, 1.0),
        PolarPosition(-30.0, 30.0, 1.0),   PolarPosition(0.0, 30.0, 1.0),
        PolarPosition(90.0, 30.0, 1.0),    PolarPosition(-90.0, 30.0, 1.0),
        PolarPosition(135.0, 30.0, 1.0),   PolarPosition(-135.0, 30.0, 1.0)};
    REQUIRE(extraChannels.size() == expectedPositions.size());
    for (size_t i = 0; i < extraChannels.size(); ++i) {
      REQUIRE(extraChannels[i].polarPosition() == expectedPositions[i]);
    }
    Eigen::MatrixXd expectedDownmix = createDownmixMatrixFromIndices(
        std::vector<int>{0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6},
        layout.channels().size());
    REQUIRE(downmix == expectedDownmix);
  };

  SECTION("4+7+0") {
    Layout layout = getLayout("4+7+0").withoutLfe();
    std::tie(extraChannels, downmix) = extraPosVerticalNominal(layout);
    std::vector<PolarPosition> expectedPositions = {
        PolarPosition(30.0, -30.0, 1.0),  PolarPosition(-30.0, -30.0, 1.0),
        PolarPosition(0.0, -30.0, 1.0),   PolarPosition(90.0, -30.0, 1.0),
        PolarPosition(-90.0, -30.0, 1.0), PolarPosition(135.0, -30.0, 1.0),
        PolarPosition(-135.0, -30.0, 1.0)};
    REQUIRE(extraChannels.size() == expectedPositions.size());
    for (size_t i = 0; i < extraChannels.size(); ++i) {
      REQUIRE(extraChannels[i].polarPosition() == expectedPositions[i]);
    }
    Eigen::MatrixXd expectedDownmix = createDownmixMatrixFromIndices(
        std::vector<int>{0, 1, 2, 3, 4, 5, 6}, layout.channels().size());
    REQUIRE(downmix == expectedDownmix);
  };
};

std::vector<std::pair<Eigen::Vector3d, Eigen::VectorXd>> generatePositionGains(
    const Layout& layout) {
  std::vector<std::pair<Eigen::Vector3d, Eigen::VectorXd>> ret;
  auto positions = layout.positions();
  for (size_t i = 0; i < positions.size(); ++i) {
    Eigen::VectorXd expectedGain =
        Eigen::VectorXd::Zero(layout.channels().size());
    expectedGain(i) = 1.0;
    ret.push_back(
        std::make_pair(toCartesianVector3d(positions[i]), expectedGain));
  }
  return ret;
}

TEST_CASE("test_polar_point_source_panner") {
  Eigen::MatrixXd positions(4, 3);
  positions << cartT(30.0, 0.0, 1.0),  //
      cartT(0.0, 0.0, 1.0),  //
      cartT(-30.0, 0.0, 1.0),  //
      cartT(0.0, 30.0, 1.0);
  std::vector<Eigen::Vector3i> outputChannelsVec = {{0, 1, 3}, {2, 1, 3}};
  std::vector<std::unique_ptr<RegionHandler>> regions_1;
  for (const auto& outputChannels : outputChannelsVec) {
    regions_1.push_back(boost::make_unique<Triplet>(
        outputChannels, positions(outputChannels, {0, 1, 2})));
  }

  // should fail if not given enough channels
  REQUIRE_THROWS(PolarPointSourcePanner(
      std::move(regions_1), static_cast<int>(positions.rows() - 1)));

  std::vector<std::unique_ptr<RegionHandler>> regions_2;
  for (const auto& outputChannels : outputChannelsVec) {
    regions_2.push_back(boost::make_unique<Triplet>(
        outputChannels, positions(outputChannels, {0, 1, 2})));
  }
  PolarPointSourcePanner psp(std::move(regions_2));
  REQUIRE(psp.numberOfOutputChannels() == positions.rows());
  for (int i = 0; i < psp.numberOfOutputChannels(); ++i) {
    Eigen::VectorXd pv_req =
        Eigen::VectorXd::Zero(psp.numberOfOutputChannels());
    pv_req(i) = 1.0;
    auto pv = psp.handle(positions(i, {0, 1, 2}));
    REQUIRE(pv != boost::none);
    REQUIRE(pv->isApprox(pv_req));
  }
  REQUIRE(psp.handle(Eigen::Vector3d{0.0, -1.0, 0.0}) == boost::none);
}

Eigen::VectorXi getChannelFlipVector(const Eigen::MatrixXd& spkPositions) {
  Eigen::VectorXi channelFlipX = Eigen::VectorXi::Zero(spkPositions.rows());
  Eigen::VectorXd spkNorm(spkPositions.rows());
  Eigen::RowVector3d spkPositionFlipX;
  Eigen::VectorXd::Index minIndex;
  for (int i = 0; i < spkPositions.rows(); ++i) {
    spkPositionFlipX << -spkPositions(i, 0), spkPositions(i, 1),
        spkPositions(i, 2);
    spkNorm = (spkPositions.rowwise() - spkPositionFlipX).rowwise().norm();
    spkNorm.minCoeff(&minIndex);
    channelFlipX(i) = static_cast<int>(minIndex);
  }
  return channelFlipX;
}

TEST_CASE("test_all_layouts") {
  for (const auto& l : loadLayouts()) {
    Layout layout = l.withoutLfe();
    Eigen::MatrixXd spkPositions(layout.positions().size(), 3);
    for (int i = 0; i < spkPositions.rows(); ++i) {
      spkPositions.row(i) = toCartesianVector3d(layout.positions().at(i));
    }

    SECTION(layout.name()) {
      std::shared_ptr<PointSourcePanner> psp = configurePolarPanner(layout);
      Eigen::VectorXi channelFlipX = getChannelFlipVector(spkPositions);

      // calculate gains for every position on a grid
      Eigen::VectorXd azimuths = Eigen::VectorXd::LinSpaced(21, -180.0, 180.0);
      Eigen::VectorXd elevations = Eigen::VectorXd::LinSpaced(11, -90.0, 90.0);

      std::vector<std::vector<Eigen::VectorXd>> pvs(61);
      for (int a = 0; a < azimuths.size(); ++a) {
        for (int e = 0; e < elevations.size(); ++e) {
          double az = azimuths(a);
          double el = elevations(e);
          Eigen::VectorXd position = cartT(az, el, 1.0);
          auto pv = psp->handle(position);
          REQUIRE(pv != boost::none);
          REQUIRE((pv->array() >= 0.0).any());

          // check that the gains are normalised

          // stereo is normalised only at the front, and at the back at -3dB
          if (layout.name() == std::string("0+2+0")) {
            if (abs(az) <= 30.0 && el == 0.0) {
              REQUIRE(pv->norm() == Approx(1.0));
            } else if (abs(az) >= 110.0 && el == 0.0) {
              REQUIRE(pv->norm() == Approx(sqrt(0.5)));
            }
          } else {
            REQUIRE(pv->norm() == Approx(1.0));
          }

          // check that the velocity vector matches the source position
          bool doPositionCheck = true;
          if (layout.name() == std::string("0+2+0")) {
            if (abs(az) >= 30.0 || el != 0.0) {
              doPositionCheck = false;
            }
          } else if (layout.name() == std::string("0+5+0") ||
                     layout.name() == std::string("2+5+0") ||
                     layout.name() == std::string("0+7+0")) {
            if (el != 0.0) {
              doPositionCheck = false;
            }
          }
          // only 9+10+3 has no remapping above the horizontal plane
          // all layouts have remapping below the horizontal plane
          if (layout.name() == std::string("9+10+3")) {
            if (el < 0.0) {
              doPositionCheck = false;
            }
          } else if (el != 0.0) {
            doPositionCheck = false;
          }

          if (doPositionCheck) {
            Eigen::VectorXd vv = pv.get().transpose() * spkPositions;
            vv /= vv.norm();
            INFO("az: " << az << " != " << azimuth(vv));
            INFO("el: " << el << " != " << elevation(vv));
            REQUIRE(vv.isApprox(cart(az, el, 1.0)));
          }

          Eigen::Vector3d positionFlipX{-position(0), position(1), position(2)};
          auto pvFlipX = psp->handle(positionFlipX);

          REQUIRE(pv.get().isApprox(pvFlipX.get()(channelFlipX)));
        }
      }
    }
  }
}

TEST_CASE("configure_full_polar_panner") {
  SECTION("0+5+0") {
    auto layout = getLayout("0+5+0").withoutLfe();
    auto psp = configurePolarPanner(layout);
    for (const auto& pair : generatePositionGains(layout)) {
      auto pv = psp->handle(pair.first);
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(pair.second));
    }
    SECTION("azimuth=15.0") {
      Eigen::VectorXd expectedGain =
          Eigen::VectorXd::Zero(layout.channels().size());
      expectedGain({0, 2}) << 1.0 / std::sqrt(2.0), 1.0 / std::sqrt(2.0);
      auto pv = psp->handle(cart(15.0, 0.0, 1.0));
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(expectedGain));
    };
    SECTION("azimuth=-15.0") {
      Eigen::VectorXd expectedGain =
          Eigen::VectorXd::Zero(layout.channels().size());
      expectedGain({1, 2}) << 1.0 / std::sqrt(2.0), 1.0 / std::sqrt(2.0);
      auto pv = psp->handle(cart(-15.0, 0.0, 1.0));
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(expectedGain));
    };
  };
  SECTION("2+5+0") {
    auto layout = getLayout("2+5+0").withoutLfe();
    auto psp = configurePolarPanner(layout);
    for (const auto& pair : generatePositionGains(layout)) {
      auto pv = psp->handle(pair.first);
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(pair.second));
    }
  };
  SECTION("4+5+0") {
    auto layout = getLayout("4+5+0").withoutLfe();
    auto psp = configurePolarPanner(layout);
    for (const auto& pair : generatePositionGains(layout)) {
      auto pv = psp->handle(pair.first);
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(pair.second));
    }
  };
  SECTION("4+5+1") {
    auto layout = getLayout("4+5+1").withoutLfe();
    auto psp = configurePolarPanner(layout);
    for (const auto& pair : generatePositionGains(layout)) {
      auto pv = psp->handle(pair.first);
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(pair.second));
    }
  };
  SECTION("3+7+0") {
    auto layout = getLayout("3+7+0").withoutLfe();
    auto psp = configurePolarPanner(layout);
    for (const auto& pair : generatePositionGains(layout)) {
      auto pv = psp->handle(pair.first);
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(pair.second));
    }
  };
  SECTION("4+9+0") {
    auto layout = getLayout("4+9+0").withoutLfe();
    auto psp = configurePolarPanner(layout);
    for (const auto& pair : generatePositionGains(layout)) {
      auto pv = psp->handle(pair.first);
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(pair.second));
    }
  };
  SECTION("9+10+3") {
    auto layout = getLayout("9+10+3").withoutLfe();
    auto psp = configurePolarPanner(layout);
    for (const auto& pair : generatePositionGains(layout)) {
      auto pv = psp->handle(pair.first);
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(pair.second));
    }
  };
  SECTION("0+7+0") {
    auto layout = getLayout("0+7+0").withoutLfe();
    auto psp = configurePolarPanner(layout);
    for (const auto& pair : generatePositionGains(layout)) {
      auto pv = psp->handle(pair.first);
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(pair.second));
    }
  };
  SECTION("4+7+0") {
    auto layout = getLayout("4+7+0").withoutLfe();
    auto psp = configurePolarPanner(layout);
    for (const auto& pair : generatePositionGains(layout)) {
      auto pv = psp->handle(pair.first);
      REQUIRE(pv != boost::none);
      REQUIRE(pv.get().isApprox(pair.second));
    }
  };
};

TEST_CASE("screen_loudspeaker_positions") {
  auto layout = getLayout("4+9+0").withoutLfe();

  SECTION("too wide") {
    SECTION("+") {
      for (auto& channel : layout.channels())
        if (channel.name() == "M+SC") channel.polarPosition({40.0, 0.0, 1.0});
      REQUIRE_THROWS_AS(configurePolarPanner(layout), not_implemented);
    }
    SECTION("-") {
      for (auto& channel : layout.channels())
        if (channel.name() == "M-SC") channel.polarPosition({-40.0, 0.0, 1.0});
      REQUIRE_THROWS_AS(configurePolarPanner(layout), not_implemented);
    }
  }

  SECTION("not in range") {
    SECTION("+") {
      for (auto& channel : layout.channels())
        if (channel.name() == "M+SC") channel.polarPosition({30.0, 0.0, 1.0});
      REQUIRE_THROWS_AS(configurePolarPanner(layout), invalid_argument);
    }

    SECTION("-") {
      for (auto& channel : layout.channels())
        if (channel.name() == "M-SC") channel.polarPosition({-30.0, 0.0, 1.0});
      REQUIRE_THROWS_AS(configurePolarPanner(layout), invalid_argument);
    }
  }
}

TEST_CASE("hull") {
  for (auto& layoutFull : loadLayouts()) {
    if (layoutFull.name() == "0+2+0") continue;
    auto layout = layoutFull.withoutLfe();

    SECTION(layout.name()) {
      std::vector<Eigen::Vector3d> positionsReal;
      std::vector<Eigen::Vector3d> positionsNominal;
      std::set<int> virtualVerts;
      Eigen::MatrixXd downmix;
      std::tie(positionsReal, positionsNominal, virtualVerts, downmix) =
          getAugmentedLayout(layout);

      std::vector<Facet> facets_precomputed = FACETS.at(layout.name());

      // check that we're not close to a tolerance that is too big or small
      for (double tolerance : {1e-6, 1e-5, 1e-4}) {
        SECTION("tol = " + std::to_string(tolerance)) {
          std::vector<Facet> facets_calculated =
              convex_hull(positionsNominal, tolerance);

          std::sort(facets_precomputed.begin(), facets_precomputed.end());
          std::sort(facets_calculated.begin(), facets_calculated.end());

          REQUIRE(facets_precomputed == facets_calculated);
        }
      }
    }
  }
}

#ifdef CATCH_CONFIG_ENABLE_BENCHMARKING
TEST_CASE("hull_benchmark", "[.benchmark]") {
  auto layout = getLayout("9+10+3").withoutLfe();

  std::vector<Eigen::Vector3d> positionsReal, positionsNominal;
  std::set<int> virtualVerts;
  Eigen::MatrixXd downmix;
  std::tie(positionsReal, positionsNominal, virtualVerts, downmix) =
      getAugmentedLayout(layout);

  BENCHMARK("hull 9+10+3") { return convex_hull(positionsNominal); };
}
#endif


#define DEBUG 1
/**
 * File    : C.cpp
 * Author  : Kazune Takahashi
 * Created : 2020/7/3 5:48:49
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/integer/common_factor_rt.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::integer::gcd; // for C++14 or for cpp_int
using boost::integer::lcm; // for C++14 or for cpp_int
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1'000'000'007LL};
// constexpr ll MOD{998'244'353LL}; // be careful
constexpr ll MAX_SIZE{3'000'010LL};
// constexpr ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
bool ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
    return true;
  }
  return false;
}
template <typename T>
bool ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
    return true;
  }
  return false;
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(Mint const &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(Mint const &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(Mint const &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(Mint const &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(Mint const &a) const { return Mint(*this) += a; }
  Mint operator-(Mint const &a) const { return Mint(*this) -= a; }
  Mint operator*(Mint const &a) const { return Mint(*this) *= a; }
  Mint operator/(Mint const &a) const { return Mint(*this) /= a; }
  bool operator<(Mint const &a) const { return x < a.x; }
  bool operator<=(Mint const &a) const { return x <= a.x; }
  bool operator>(Mint const &a) const { return x > a.x; }
  bool operator>=(Mint const &a) const { return x >= a.x; }
  bool operator==(Mint const &a) const { return x == a.x; }
  bool operator!=(Mint const &a) const { return !(*this == a); }
  Mint power(ll N) const
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, Mint<MOD> const &rhs) { return rhs + lhs; }
template <ll MOD>
Mint<MOD> operator-(ll lhs, Mint<MOD> const &rhs) { return -rhs + lhs; }
template <ll MOD>
Mint<MOD> operator*(ll lhs, Mint<MOD> const &rhs) { return rhs * lhs; }
template <ll MOD>
Mint<MOD> operator/(ll lhs, Mint<MOD> const &rhs) { return Mint<MOD>{lhs} / rhs; }
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a) { return stream >> a.x; }
template <ll MOD>
ostream &operator<<(ostream &stream, Mint<MOD> const &a) { return stream << a.x; }
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
// ----- for C++17 -----
template <typename T, typename enable_if<is_integral<T>::value>::type * = nullptr>
size_t popcount(T x) { return bitset<64>(x).count(); }
size_t popcount(string const &S) { return bitset<200010>{S}.count(); }
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// -----Geometry Library-- ---
// Referring to the great source codes:
//   - Maehara-san's algorithm library: http://www.prefield.com/algorithm/index.html
// Many thanks.

// ----- Basic Classes -----

constexpr ld EPSILON{1e-12};

// ----- Point -----

using Point = complex<ld>;
bool operator<(Point const &p, Point const &q)
{
  return real(p) != real(q) ? real(p) < real(q) : imag(p) < imag(q);
}
istream &operator>>(istream &is, Point &p)
{
  ld x, y;
  is >> x >> y;
  p = Point{x, y};
  return is;
}

ld OuterProduct(Point const &p, Point const &q)
{
  return imag(conj(p) * q);
}
ld InnerProduct(Point const &p, Point const &q)
{
  return real(conj(p) * q);
}

Point Normalize(Point const &p)
{
  return p / abs(p);
}

// ---- ccw -----

int ccw(Point a, Point b, Point c)
{
  b -= a;
  c -= a;
  auto tmp{OuterProduct(b, c)};
  if (tmp > 0)
  {
    return +1; // counter clockwise
  }
  if (tmp < 0)
  {
    return -1; // clockwise
  }
  if (InnerProduct(b, c) < 0)
  {
    return +2; // c--a--b on line
  }
  if (norm(b) < norm(c))
  {
    return -2; // a--b--c on line
  }
  return 0;
}

// ----- Geom -----

using Geom = vector<Point>;

Geom &operator+=(Geom &g, Point const &p)
{
  for (auto &q : g)
  {
    q += p;
  }
  return g;
}
Geom operator+(Geom const &g, Point const &p)
{
  Geom h{g};
  return h += p;
}
Geom &operator-=(Geom &g, Point const &p)
{
  return g += (-p);
}
Geom operator-(Geom const &g, Point const &p)
{
  return g + (-p);
}

// ----- Line -----

struct Segment;

struct Line : public Geom
{
  Line() {}
  Line(Point const &p, Point const &q)
  {
    push_back(p);
    push_back(q);
  }
};

// ----- Segment -----

struct Segment : public Line
{
  Segment() {}
  Segment(Point const &p, Point const &q)
  {
    push_back(p);
    push_back(q);
  }
};

// ----- Circle -----

struct Circle
{
  Point p;
  ld r;

  Circle() {}
  Circle(Point const &p, ld r) : p(p), r(r) {}
};

// ----- Functions -----

// ----- Rotate -----

Point Rotate(Point const &p, ld radian = M_PI / 2)
{
  return p * Point{cos(radian), sin(radian)};
}
Geom Rotate(Geom g, ld radian = M_PI / 2)
{
  for (auto &p : g)
  {
    p = Rotate(p, radian);
  }
  return g;
}

// ----- Projection and Reflection (Point and Line) -----

Point Projection(Line const &l, Point const &p)
{
  ld t{InnerProduct(p - l[0], l[0] - l[1]) / norm(l[0] - l[1])};
  return l[0] + t * (l[0] - l[1]);
}

Point Reflection(Line const &l, Point const &p)
{
  return p + ld{2} * (Projection(l, p) - p);
}

// ----- Intersect -----

bool Intersect(Line const &l, Line const &m)
{
  return abs(OuterProduct(l[1] - l[0], m[1] - m[0])) > EPSILON || // non-parallel
         abs(OuterProduct(l[1] - l[0], m[0] - l[0])) < EPSILON;   // same line
}
bool Intersect(Line const &l, Segment const &s)
{
  return OuterProduct(l[1] - l[0], s[0] - l[0]) * OuterProduct(l[1] - l[0], s[1] - l[0]) < EPSILON;
}
bool Intersect(Segment const &s, Line const &l)
{
  return Intersect(l, s);
}
bool Intersect(Line const &l, Point const &p)
{
  return abs(OuterProduct(l[1] - p, l[0] - p)) < EPSILON;
}
bool Intersect(Point const &p, Line const &l)
{
  return Intersect(l, p);
}
bool Intersect(Segment const &s, Segment const &t)
{
  return ccw(s[0], s[1], t[0]) * ccw(s[0], s[1], t[1]) <= 0 &&
         ccw(t[0], t[1], s[0]) * ccw(t[0], t[1], s[1]) <= 0;
}
bool Intersect(Segment const &s, Point const &p)
{
  return abs(s[0] - p) + abs(s[1] - p) - abs(s[1] - s[0]) < EPSILON; // triangle inequality
}
bool Intersect(Point const &p, Segment const &s)
{
  return Intersect(s, p);
}
bool Intersect(Circle const &a, Circle const &b)
{
  return a.r + b.r + EPSILON < abs(a.p - b.p);
}

// ----- Dist -----

ld Dist(Point const &p, Point const &q)
{
  return abs(p - q);
}
ld Dist(Line const &l, Point const &p)
{
  return abs(p - Projection(l, p));
}
ld Dist(Point const &p, Line const &l)
{
  return Dist(l, p);
}
ld Dist(Line const &l, Line const &m)
{
  return Intersect(l, m) ? 0 : Dist(l, m[0]);
}
ld Dist(Line const &l, Segment const &s)
{
  if (Intersect(l, s))
  {
    return 0;
  }
  return min(Dist(l, s[0]), Dist(l, s[1]));
}
ld Dist(Segment const &s, Line const &l)
{
  return Dist(l, s);
}
ld Dist(Segment const &s, Point const &p)
{
  auto r{Projection(static_cast<Line>(s), p)};
  if (Intersect(s, r))
  {
    return abs(r - p);
  }
  return min(abs(s[0] - p), abs(s[1] - p));
}
ld Dist(Point const &p, Segment const &s)
{
  return Dist(s, p);
}
ld Dist(Segment const &s, Segment const &t)
{
  if (Intersect(s, t))
  {
    return 0;
  }
  return min({Dist(s, t[0]), Dist(s, t[1]), Dist(t, s[0]), Dist(t, s[1])});
}

// ----- IntersectionPoints ------

vector<Point> IntersectionPoints(Circle const &a, Circle const &b)
{
  auto d{Dist(a.p, b.p)};
  auto l{(a.r * a.r - b.r * b.r + d * d) / (2 * d)};
  auto tmp{a.r * a.r - l * l};
  if (tmp <= 0)
  {
    return {};
  }
  auto h{sqrt(tmp)};
  vector<Point> res;
  auto v{Normalize(b.p - a.p)};
  auto w{Rotate(v)};
  res.push_back(a.p + v * l + w * h);
  res.push_back(a.p + v * l - w * h);
  return res;
}

vector<Point> IntersectionPoints(Line const &l, Line const &m)
{
  auto A{OuterProduct(l[1] - l[0], m[1] - m[0])};
  auto B{OuterProduct(l[1] - l[0], l[1] - m[0])};
  if (abs(A) < EPSILON && abs(B) < EPSILON)
  {
    return {m[0], m[1], l[0], l[1]}; // same line
  }
  if (abs(A) < EPSILON)
  {
    assert(false); // Precondition is not satisfied.
  }
  return {m[0] + B / A * (m[1] - m[0])};
}

// ----- Contains -----

enum class ContainState
{
  OUT,
  ON,
  IN
};

ContainState Contains(Geom const &g, Point const &p)
{
  bool in{false};
  for (auto i{size_t{0}}; i < g.size(); ++i)
  {
    auto a{g[i] - p};
    auto b{g[(i + 1) % g.size()] - p};
    if (imag(a) > imag(b))
    {
      swap(a, b);
    }
    if (imag(a) <= 0 && 0 < imag(b) && OuterProduct(a, b) < 0)
    {
      in = !in;
    }
    if (abs(OuterProduct(a, b)) < EPSILON && InnerProduct(a, b) < EPSILON)
    {
      return ContainState::ON;
    }
  }
  return in ? ContainState::IN : ContainState::OUT;
}

ContainState Contains(Circle const &c, Point const &p)
{
  auto d{Dist(c.p, p)};
  if (abs(d - c.r) < EPSILON)
  {
    return ContainState::ON;
  }
  if (d > c.r)
  {
    return ContainState::OUT;
  }
  return ContainState::IN;
}

bool ContainStateToBool(ContainState s)
{
  return s == ContainState::IN || s == ContainState::ON;
}

template <typename T, typename U>
bool DoesContain(T const &a, U const &b)
{
  return ContainStateToBool(Contains(a, b));
}

// ----- Solve -----

class Solve
{

public:
  Solve()
  {
  }

  void flush()
  {
  }

private:
};

// ----- main() -----

/*
int main()
{
  Solve solve;
  solve.flush();
}
*/

int main()
{
  ld A, B, H, M;
  cin >> A >> B >> H >> M;
  Point p{polar(A, 2 * M_PI * H / 12 + 2 * M_PI * M / (12 * 60))};
  Point q{polar(B, 2 * M_PI * M / 60)};
  cout << fixed << setprecision(12) << abs(p - q) << endl;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "EntropyRegularizedEisner"

#include <boost/test/unit_test.hpp>
namespace utf = boost::unit_test;

#include <vector>

#include "diffdp/algorithm/eisner.h"
#include "diffdp/math.h"
#include "dynet/expr.h"

// using boost test with intolerance fails (too precise),
// so let's just use the same test as in Dynet.
bool check_grad(float g, float g_act)
{
    float f = std::fabs(g - g_act);
    float m = std::max(std::fabs(g), std::fabs(g_act));
    if (f > 0.01 && m > 0.f)
        f /= m;

    if (f > 0.01 || std::isnan(f))
        return false;
    else
        return true;
}

BOOST_AUTO_TEST_CASE(test_softmax)
{
    int argc = 1;
    char **argv;
    dynet::initialize(argc, argv);

    std::vector<float> input(10);
    std::vector<float> output(10);
    std::vector<float> input_grad(10);
    std::vector<float> output_grad(10);
    for (unsigned i = 0 ; i < input.size() ; ++i)
    input.at(i) = i;

    {
        diffdp::softmax(output.begin(), input.begin(), input.size());

        dynet::ComputationGraph cg;
        auto e_output = dynet::softmax(dynet::input(cg, {10}, input));
        auto dynet_output = as_vector(cg.forward(e_output));

        for (unsigned i = 0 ; i < 10 ; ++i)
            BOOST_CHECK(check_grad(output.at(i), dynet_output.at(i)));
    }

    for (unsigned input_id = 0 ; input_id < input.size() ; ++input_id)
    {
        for (unsigned output_id = 0 ; output_id < input.size() ; ++output_id)
        {

            // compute gradient

            std::fill(input_grad.begin(), input_grad.end(), 0.f);
            std::fill(output_grad.begin(), output_grad.end(), 0.f);
            //std::fill(output.begin(), output.end(), 0.f);

            diffdp::softmax(output.begin(), input.begin(), input.size());
            output_grad.at(output_id) = 1.f;
            diffdp::backprop_softmax(
                    input_grad.begin(), output_grad.begin(),
                    input.begin(), output.begin(),
                    input.size()
            );
            const float computed_gradient = input_grad.at(input_id);

            // dynet gradient
            dynet::ComputationGraph cg;

            auto e_input = dynet::input(cg, {10}, input);
            auto e_softmax = dynet::softmax(e_input);
            auto e_output = dynet::pick(e_softmax, output_id);
            cg.forward(e_output);
            cg.backward(e_output, true);

            auto dynet_gradient_all = as_vector(e_input.gradient());
            float dynet_gradient = dynet_gradient_all.at(input_id);

            BOOST_CHECK(check_grad(computed_gradient, dynet_gradient));
        }

    }
}


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

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

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_ANDOYER_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_ANDOYER_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/algorithms/detail/flattening.hpp>

#include <boost/geometry/strategies/distance.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Point-point distance approximation taking flattening into account
\ingroup distance
\tparam Spheroid The reference spheroid model
\tparam CalculationType \tparam_calculation
\author After Andoyer, 19xx, republished 1950, republished by Meeus, 1999
\note Although not so well-known, the approximation is very good: in all cases the results
are about the same as Vincenty. In my (Barend's) testcases the results didn't differ more than 6 m
\see http://nacc.upc.es/tierra/node16.html
\see http://sci.tech-archive.net/Archive/sci.geo.satellite-nav/2004-12/2724.html
\see http://home.att.net/~srschmitt/great_circle_route.html (implementation)
\see http://www.codeguru.com/Cpp/Cpp/algorithms/article.php/c5115 (implementation)
\see http://futureboy.homeip.net/frinksamp/navigation.frink (implementation)
\see http://www.voidware.com/earthdist.htm (implementation)
*/
template
<
    typename Spheroid,
    typename CalculationType = void
>
class andoyer
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef Spheroid model_type;

    inline andoyer()
        : m_spheroid()
    {}

    explicit inline andoyer(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        return calc<typename calculation_type<Point1, Point2>::type>
            (
                get_as_radian<0>(point1), get_as_radian<1>(point1),
                get_as_radian<0>(point2), get_as_radian<1>(point2)
            );
    }

    inline Spheroid const& model() const
    {
        return m_spheroid;
    }

private :
    template <typename CT, typename T>
    inline CT calc(T const& lon1,
                T const& lat1,
                T const& lon2,
                T const& lat2) const
    {
        CT const G = (lat1 - lat2) / 2.0;
        CT const lambda = (lon1 - lon2) / 2.0;

        if (geometry::math::equals(lambda, 0.0)
            && geometry::math::equals(G, 0.0))
        {
            return 0.0;
        }

        CT const F = (lat1 + lat2) / 2.0;

        CT const sinG2 = math::sqr(sin(G));
        CT const cosG2 = math::sqr(cos(G));
        CT const sinF2 = math::sqr(sin(F));
        CT const cosF2 = math::sqr(cos(F));
        CT const sinL2 = math::sqr(sin(lambda));
        CT const cosL2 = math::sqr(cos(lambda));

        CT const S = sinG2 * cosL2 + cosF2 * sinL2;
        CT const C = cosG2 * cosL2 + sinF2 * sinL2;

        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;
        CT const c3 = 3;

        if (geometry::math::equals(S, c0) || geometry::math::equals(C, c0))
        {
            return c0;
        }

        CT const radius_a = CT(get_radius<0>(m_spheroid));
        CT const flattening = geometry::detail::flattening<CT>(m_spheroid);

        CT const omega = atan(math::sqrt(S / C));
        CT const r3 = c3 * math::sqrt(S * C) / omega; // not sure if this is r or greek nu
        CT const D = c2 * omega * radius_a;
        CT const H1 = (r3 - c1) / (c2 * C);
        CT const H2 = (r3 + c1) / (c2 * S);

        return D * (c1 + flattening * (H1 * sinF2 * cosG2 - H2 * cosF2 * sinG2) );
    }

    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Spheroid, typename CalculationType>
struct tag<andoyer<Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct return_type<andoyer<Spheroid, CalculationType>, P1, P2>
    : andoyer<Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template <typename Spheroid, typename CalculationType>
struct comparable_type<andoyer<Spheroid, CalculationType> >
{
    typedef andoyer<Spheroid, CalculationType> type;
};

template <typename Spheroid, typename CalculationType>
struct get_comparable<andoyer<Spheroid, CalculationType> >
{
    static inline andoyer<Spheroid, CalculationType> apply(andoyer<Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct result_from_distance<andoyer<Spheroid, CalculationType>, P1, P2>
{
    template <typename T>
    static inline typename return_type<andoyer<Spheroid, CalculationType>, P1, P2>::type
        apply(andoyer<Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

template <typename Point1, typename Point2>
struct default_strategy<point_tag, point_tag, Point1, Point2, geographic_tag, geographic_tag>
{
    typedef strategy::distance::andoyer
                <
                    srs::spheroid
                        <
                            typename select_coordinate_type<Point1, Point2>::type
                        >
                > type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_ANDOYER_HPP


#include <rviz_helper/kr_marker.hpp>
#include <Eigen/Geometry>

namespace kr {
namespace viz {
Marker &Marker::covariance(const geometry_msgs::PoseWithCovariance &pose_cov) {
  Eigen::Matrix<double, 6, 6> cov;
  for (int i = 0; i < 6; i++) {
    for (int j = 0; j < 6; j++) {
      cov(i, j) = pose_cov.covariance[(i * 6) + j];
    }
  }
  covariance(cov.block<3,3>(0,0));
  position(pose_cov.pose.position);
  return *this;
}

Marker &Marker::covariance(const Eigen::Matrix3d &cov) {
  //  we assume here that covariance is symmetric positive definite
  Eigen::JacobiSVD<Eigen::Matrix3d> SVD = cov.jacobiSvd(Eigen::ComputeFullU);

  Eigen::Matrix3d U = SVD.matrixU();  //  U == V in this case
  Eigen::Vector3d S = SVD.singularValues();

  //  U is a rotation matrix of the ellipse, convert to quaternion
  Eigen::Quaterniond quat(U*U.determinant());
  quat.normalize();

  mark_.pose.orientation.w = quat.w();
  mark_.pose.orientation.x = quat.x();
  mark_.pose.orientation.y = quat.y();
  mark_.pose.orientation.z = quat.z();

  //  set scale
  for (int i = 0; i < 3; i++) {
    S[i] = std::sqrt(S[i]);
  }
  scale(S[0], S[1], S[2]);

  return *this;
}

}  // namespace viz
}  // namespace kr


/* -*-C++-*- */
/*
   (c) Copyright 1996-2005, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    \brief mathematical functions not found in the standard math library.
*/

#ifndef LINTEL_MATHSPECIALFUNCTIONS_HPP
#define LINTEL_MATHSPECIALFUNCTIONS_HPP

#include <math.h>

#include <boost/version.hpp>
#include <boost/config.hpp>
/* TODO: when we decide to stop supporting older boost versions, this
   should be removed. Also, note the equivalent checks later in the file
   and in the .cpp
*/
#if BOOST_VERSION >= 103500
#include <boost/math/special_functions/erf.hpp>
#else
   // no erf() on windows
#  if defined(BOOST_MSVC)
#     error need boost version >= 1.35
#  endif
#endif

//////////////////////////////////////////////////////////////////////////////
// Functions
//////////////////////////////////////////////////////////////////////////////

// The inverse of the Erf function, defined over the domain -1 < y < 1.
// TODO: should probably be deprecated in favor of boost::math::erfc() ?
double inverseErf(double y);

// The cumulative distribution function of the unit Normal RV U;
// unitNormalCDF(x) = Prob[U < x] = (1+erf(x/sqrt(2)))/2
inline double unitNormalCDF(double x)
{
#if BOOST_VERSION >= 103500
    return 0.5*(1.0+ boost::math::erf<double>(0.70710678118654752440084436210485*x));
#else
    return 0.5*(1.0+ erf(0.70710678118654752440084436210485*x));
#endif
}

// The probability that the absolute value of a measurement of a
// unit normal-distributed quantity is above X.  This is the same
// as the two-side folded cumulative distribution function of the
// unit normal RV U:
inline double probAbsNormal(double x)
{
#if BOOST_VERSION >= 103500
    return 1. - boost::math::erf<double>(0.70710678118654752440084436210485*fabs(x));
#else
    return 1. - erf(0.70710678118654752440084436210485*fabs(x));
#endif
}

// The probability density function of the unit Normal RV U;
// unitNormalPDF(x) = unitNormalCDF'(x) = exp(-x*x/2)/sqrt(2 Pi)
inline double unitNormalPDF(double x)
  {return 0.39894228040143267793994605993438 * exp(-0.5*x*x) ;};

// The inverse of the cumulative distribution function of the unit Normal
// RV U, also known as inversePhi.
// inverseUnitNormalCDF(x) == y  <=>  x == unitNormalCDF(y)
// inverseUnitNormalCDF(x) = sqrt(2) * inverseErf(2*x -1)
inline double inverseUnitNormalCDF(double x)
  {return 1.4142135623730950488016887242097*(inverseErf(2.0*x - 1.0));};

//////////////////////////////////////////////////////////////////////////////
// Data type conversion utilities
//////////////////////////////////////////////////////////////////////////////

// Verify that the double argument can be converted (without rounding error)
// to an integer data type.  You get to specify the size of the integer
// data type; only the size of a long or a long long is supported.
// "Without rounding error" means that the double is really close to
// being an actual integer.
extern bool isDoubleIntegral(double d, size_t target_size);

// Convert the double to an integer, of type long or long long
extern long convertDoubleLong(double d);
extern long long convertDoubleLongLong(double d);

#endif


#include "dense_solver.hpp"
#include <Eigen/Dense>
#include <iostream>
#include <stdexcept>

std::unique_ptr<dense_solver_interface> make_dense_solver(
    const std::string& type) {
  using std::make_unique;
  using namespace Eigen;

  if (type == "PartialPivLU")
    return make_unique<dense_solver<PartialPivLU<MatrixXd>>>();
  else if (type == "FullPivLU")
    return make_unique<dense_solver<FullPivLU<MatrixXd>>>();
  else if (type == "HouseholderQR")
    return make_unique<dense_solver<HouseholderQR<MatrixXd>>>();
  else if (type == "ColPivHouseholderQR")
    return make_unique<dense_solver<ColPivHouseholderQR<MatrixXd>>>();
  else if (type == "FullPivHouseholderQR")
    return make_unique<dense_solver<FullPivHouseholderQR<MatrixXd>>>();
  else if (type == "CompleteOrthogonalDecomposition")
    return make_unique<
        dense_solver<CompleteOrthogonalDecomposition<MatrixXd>>>();
  else if (type == "BDCSVD")
    return make_unique<dense_solver<BDCSVD<MatrixXd>>>();
  else if (type == "JacobiSVD")
    return make_unique<dense_solver<JacobiSVD<MatrixXd>>>();
  else if (type == "LLT")
    return make_unique<dense_solver<LLT<MatrixXd>>>();
  else if (type == "LDLT")
    return make_unique<dense_solver<LDLT<MatrixXd>>>();
  else {
    std::stringstream ss;
    ss << "Error: " << __FILE__ << ": " << __LINE__
       << "\n  Invalid dense solver type found: " << type
       << "\n  Available types are:\n"
       << "    PartialPivLU,\n"
       << "    FullPivLU,\n"
       << "    HouseholderQR,\n"
       << "    ColPivHouseholderQR,\n"
       << "    FullPivHouseholderQR,\n"
       << "    CompleteOrthogonalDecomposition,\n"
       << "    BDCSVD,\n"
       << "    JacobiSVD,\n"
       << "    LLT,\n"
       << "    LDLT" << std::endl;
    throw std::invalid_argument(ss.str());
  }
}