// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// 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__INTERP__BEZIER_HPP_
#define SMOOTH__INTERP__BEZIER_HPP_

/**
 * @file
 * @brief bezier splines on lie groups.
 */

#include <ranges>

#include <Eigen/Sparse>
#include <Eigen/SparseLU>

#include "smooth/concepts.hpp"
#include "smooth/internal/utils.hpp"

#include "common.hpp"

namespace smooth {

/**
 * @brief Bezier curve on [0, 1].
 * @tparam N Polonimial degree of curve.
 * @tparam G LieGroup type.
 *
 * The curve is defined by
 * \f[
 *  g(t) = g_0 * \exp(\tilde B_1(t) v_1) * ... \exp(\tilde B_N(t) v_N)
 * \f]
 * where \f$\tilde B_i(t)\f$ are cumulative Bernstein basis functions and
 * \f$v_i = g_i \ominus g_{i-1}\f$ are the control point differences.
 */
template<std::size_t N, LieGroup G>
class Bezier
{
public:
  /**
   * @brief Default constructor creates a constant curve on [0, 1] equal to identity.
   */
  Bezier() : g0_(G::Identity()) { vs_.fill(G::Tangent::Zero()); }

  /**
   * @brief Create curve from rvalue parameter values.
   *
   * @param g0 starting value
   * @param vs differences [v_1, ..., v_n] between control points
   */
  Bezier(G && g0, std::array<typename G::Tangent, N> && vs) : g0_(std::move(g0)), vs_(std::move(vs))
  {}

  /**
   * @brief Create curve from parameter values.
   *
   * @tparam Rv range containing control point differences
   * @param g0 starting value
   * @param vs differences [v_1, ..., v_n] between control points
   *
   * @note Range value type of \p Rv must be the tangent type of \p G.
   */
  template<std::ranges::range Rv>
  Bezier(const G & g0, const Rv & vs) : g0_(g0)
  {
    if (std::ranges::size(vs) != N) { throw std::runtime_error("Wrong number of control points"); }
    std::copy(std::ranges::begin(vs), std::ranges::end(vs), vs_.begin());
  }

  /// @brief Copy constructor
  Bezier(const Bezier &) = default;
  /// @brief Move constructor
  Bezier(Bezier &&) = default;
  /// @brief Copy assignment
  Bezier & operator=(const Bezier &) = default;
  /// @brief Move assignment
  Bezier & operator=(Bezier &&) = default;
  /// @brief Destructor
  ~Bezier() = default;

  /**
   * @brief Evaluate Bezier curve.
   *
   * @param[in] t time point to evaluate at
   * @param[out] vel output body velocity at evaluation time
   * @param[out] acc output body acceleration at evaluation time
   * @return spline value at time t
   *
   * @note Input \p t is clamped to interval [0, 1]
   */
  G eval(double t, detail::OptTangent<G> vel = {}, detail::OptTangent<G> acc = {}) const
  {
    double tc = std::clamp<double>(t, 0, 1);

    constexpr auto Mstatic = detail::cum_coefmat<CSplineType::BEZIER, double, N>().transpose();
    Eigen::Map<const Eigen::Matrix<double, N + 1, N + 1, Eigen::RowMajor>> M(Mstatic[0].data());

    return cspline_eval<N>(g0_, vs_, M, tc, vel, acc);
  }

private:
  G g0_;
  std::array<typename G::Tangent, N> vs_;
};

/**
 * @brief Piecewise curve built from Bezier segments.
 *
 * The curve is given by
 * \f[
 *  \mathbf{x}(t) = p_i \left( \frac{t - t_i}{t_{i+1} - t_{i}} \right)
 * \f]
 * for \f$ t \in [t_i, t_{i+1}]\f$
 * where \f$p_i\f$ is a Bezier curve on \f$[0, 1]\f$.
 */
template<std::size_t N, LieGroup G>
class PiecewiseBezier
{
public:
  /**
   * @brief Default constructor creates a constant curve defined on [0, 1] equal to identity.
   */
  PiecewiseBezier() : knots_{0, 1}, segments_{Bezier<N, G>{}} {}

  /**
   * @brief Create a PiecewiseBezier from knot times and Bezier segments.
   *
   * @param knots points \f$ t_i \f$
   * @param segments Bezier curves \f$ p_i \f$
   */
  PiecewiseBezier(std::vector<double> && knots, std::vector<Bezier<N, G>> && segments)
      : knots_(std::move(knots)), segments_(std::move(segments))
  {}

  /**
   * @brief Create a PiecewiseBezier from knot times and Bezier segments.
   *
   * @param knots points \f$ t_i \f$
   * @param segments Bezier curves \f$ p_i \f$
   */
  template<std::ranges::range Rt, std::ranges::range Rs>
  PiecewiseBezier(const Rt & knots, const Rs & segments)
      : knots_(std::ranges::begin(knots), std::ranges::end(knots)),
        segments_(std::ranges::begin(segments), std::ranges::end(segments))
  {}

  /// @brief Copy constructor
  PiecewiseBezier(const PiecewiseBezier &) = default;
  /// @brief Move constructor
  PiecewiseBezier(PiecewiseBezier &&) = default;
  /// @brief Copy assignment
  PiecewiseBezier & operator=(const PiecewiseBezier &) = default;
  /// @brief Move assignment
  PiecewiseBezier & operator=(PiecewiseBezier &&) = default;
  /// @brief Destructor
  ~PiecewiseBezier() = default;

  /// @brief Minimal time where curve is defined.
  double t_min() const { return knots_.front(); }

  /// @brief Maximal time where curve is defined.
  double t_max() const { return knots_.back(); }

  /**
   * @brief Evalauate PiecewiseBezier curve.
   *
   * @param[in] t time point to evaluate at
   * @param[out] vel output body velocity at evaluation time
   * @param[out] acc output body acceleration at evaluation time
   * @return curve value at time t
   */
  G eval(double t, detail::OptTangent<G> vel = {}, detail::OptTangent<G> acc = {}) const
  {
    /// find index
    // TODO binary search
    std::size_t istar = 0;
    while (istar + 2 < knots_.size() && knots_[istar + 1] <= t) { ++istar; }

    double T = knots_[istar + 1] - knots_[istar];

    const double u = (t - knots_[istar]) / T;

    G g = segments_[istar].eval(u, vel, acc);

    if (vel.has_value()) { vel.value() /= T; }
    if (acc.has_value()) { acc.value() /= (T * T); }

    return g;
  }

private:
  std::vector<double> knots_;
  std::vector<Bezier<N, G>> segments_;
};

/**
 * @brief Fit a linear PiecewiseBezier curve to data.
 *
 * The resulting curve passes through the data points and has piecewise
 * constant velocity.
 *
 * @warning Result has discontinuous derivatives at knot points
 *
 * @tparam Rt, Rg range types
 * @param tt interpolation times
 * @param gg interpolation values
 */
template<std::ranges::range Rt, std::ranges::range Rg>
PiecewiseBezier<1, std::ranges::range_value_t<Rg>> fit_linear_bezier(const Rt & tt, const Rg & gg)
{
  if (std::ranges::size(tt) < 2 || std::ranges::size(gg) < 2) {
    throw std::runtime_error("Not enough points");
  }
  using G = std::ranges::range_value_t<Rg>;

  const std::size_t N = std::min<std::size_t>(std::ranges::size(tt), std::ranges::size(gg)) - 1;

  std::vector<Bezier<1, G>> segments(N);

  auto it_g = std::ranges::begin(gg);
  auto it_t = std::ranges::begin(tt);

  for (auto i = 0u; i != N; ++i, ++it_t, ++it_g) {
    segments[i] = Bezier<1, G>(*it_g, std::array<typename G::Tangent, 1>{(*(it_g + 1) - *it_g)});
  }

  auto take_view = tt | std::views::take(N + 1);
  std::vector<double> knots(std::ranges::begin(take_view), std::ranges::end(take_view));

  return PiecewiseBezier<1, G>(std::move(knots), std::move(segments));
}

/**
 * @brief Fit a quadratic PiecewiseBezier curve to data.
 *
 * The resulting curve passes through the data points and has
 * continuous derivatives.
 *
 * @warning Result may exhibit oscillatory behavior since second derivative
 * is free.
 *
 * @tparam Rt, Rg range types
 * @param tt interpolation times
 * @param gg interpolation values
 */
template<std::ranges::range Rt, std::ranges::range Rg>
PiecewiseBezier<2, std::ranges::range_value_t<Rg>> fit_quadratic_bezier(
  const Rt & tt, const Rg & gg)
{
  if (std::ranges::size(tt) < 2 || std::ranges::size(gg) < 2) {
    throw std::runtime_error("Not enough points");
  }
  using G             = std::ranges::range_value_t<Rg>;
  const std::size_t N = std::min<std::size_t>(std::ranges::size(tt), std::ranges::size(gg)) - 1;

  std::vector<Bezier<2, G>> segments(N);

  auto it_g = std::ranges::begin(gg);
  auto it_t = std::ranges::begin(tt);

  typename G::Tangent v0 = (*(it_g + 1) - *it_g) / (*(it_t + 1) - *it_t);

  for (auto i = 0u; i != N; ++i, ++it_t, ++it_g) {
    const double dt = *(it_t + 1) - *it_t;

    // scaled velocity
    const typename G::Tangent va = v0 * dt;

    // create segment
    const typename G::Tangent v1 = va / 2;
    const typename G::Tangent v2 = *(it_g + 1) - (*it_g * G::exp(va / 2));

    segments[i] = Bezier<2, G>(*it_g, std::array<typename G::Tangent, 2>{v1, v2});

    // unscaled end velocity for interval
    v0 = v2 * 2 / dt;
  }

  auto take_view = tt | std::views::take(N + 1);
  std::vector<double> knots(std::ranges::begin(take_view), std::ranges::end(take_view));

  return PiecewiseBezier<2, G>(std::move(knots), std::move(segments));
}

/**
 * @brief Fit a cubic PiecewiseBezier curve to data.
 *
 * The resulting curve passes through the data points, has continuous first
 * derivatives, and approximately continuous second derivatives.
 *
 * @tparam Rt, Rg range types
 * @param tt interpolation times
 * @param gg interpolation values
 */
template<std::ranges::range Rt, std::ranges::range Rg>
PiecewiseBezier<3, std::ranges::range_value_t<Rg>> fit_cubic_bezier(const Rt & tt, const Rg & gg)
{
  if (std::ranges::size(tt) < 2 || std::ranges::size(gg) < 2) {
    throw std::runtime_error("Not enough points");
  }
  using G = std::ranges::range_value_t<Rg>;

  // number of intervals
  const std::size_t N = std::min<std::size_t>(std::ranges::size(tt), std::ranges::size(gg)) - 1;

  std::size_t NumVars = G::Dof * 3 * N;

  Eigen::SparseMatrix<typename G::Scalar> lhs;
  lhs.resize(NumVars, NumVars);
  Eigen::Matrix<int, -1, 1> nnz = Eigen::Matrix<int, -1, 1>::Constant(NumVars, 3);
  nnz.head(G::Dof).setConstant(2);
  nnz.tail(G::Dof).setConstant(2);
  lhs.reserve(nnz);

  Eigen::Matrix<typename G::Scalar, -1, 1> rhs(NumVars);
  rhs.setZero();

  // variable layout:
  //
  // [ v_{1, 0}; v_{2, 0}; v_{3, 0}; v_{1, 1}; v_{2, 1}; v_{3, 1}; ...]
  //
  // where v_ji is a Dof-length vector

  const auto idx = [&](int j, int i) { return 3 * G::Dof * i + G::Dof * (j - 1); };

  std::size_t row_counter = 0;

  //// LEFT END POINT  ////

  // zero second derivative at start:
  // v_{1, 0} = v_{2, 0}
  const std::size_t v10_start = idx(1, 0);
  const std::size_t v20_start = idx(2, 0);
  for (auto n = 0u; n != G::Dof; ++n) {
    lhs.insert(row_counter + n, v10_start + n) = 1;
    lhs.insert(row_counter + n, v20_start + n) = -1;
  }
  row_counter += G::Dof;

  //// INTERIOR END POINT  ////

  auto it_t = std::ranges::begin(tt);
  auto it_g = std::ranges::begin(gg);

  for (auto i = 0u; i + 1 < N; ++i, ++it_t, ++it_g) {
    const std::size_t v1i_start = idx(1, i);
    const std::size_t v2i_start = idx(2, i);
    const std::size_t v3i_start = idx(3, i);

    const std::size_t v1ip_start = idx(1, i + 1);
    const std::size_t v2ip_start = idx(2, i + 1);

    // segment lengths
    const typename G::Scalar Ti  = *(it_t + 1) - *it_t;
    const typename G::Scalar Tip = *(it_t + 2) - *(it_t + 1);

    // pass through control points
    // v_{1, i} + v_{2, i} + v_{3, i} = x_{i+1} - x_i
    for (auto n = 0u; n != G::Dof; ++n) {
      lhs.insert(row_counter + n, v1i_start + n) = 1;
      lhs.insert(row_counter + n, v2i_start + n) = 1;
      lhs.insert(row_counter + n, v3i_start + n) = 1;
    }
    rhs.segment(row_counter, G::Dof) = *(it_g + 1) - *(it_g);
    row_counter += G::Dof;

    // velocity continuity
    // v_{3, i} = v_{1, i+1}
    for (auto n = 0u; n != G::Dof; ++n) {
      lhs.insert(row_counter + n, v3i_start + n)  = 1 * Tip;
      lhs.insert(row_counter + n, v1ip_start + n) = -1 * Ti;
    }
    row_counter += G::Dof;

    // acceleration continuity (approximate for Lie groups)
    // v_{2, i} - v_{3, i} = v_{2, i+1} - v_{1, i+1}
    for (auto n = 0u; n != G::Dof; ++n) {
      lhs.insert(row_counter + n, v2i_start + n)  = 1 * (Tip * Tip);
      lhs.insert(row_counter + n, v3i_start + n)  = -1 * (Tip * Tip);
      lhs.insert(row_counter + n, v1ip_start + n) = -1 * (Ti * Ti);
      lhs.insert(row_counter + n, v2ip_start + n) = 1 * (Ti * Ti);
    }
    row_counter += G::Dof;
  }

  //// RIGHT END POINT  ////

  const std::size_t v1_nm_start = idx(1, N - 1);
  const std::size_t v2_nm_start = idx(2, N - 1);
  const std::size_t v3_nm_start = idx(3, N - 1);

  // end at last control point
  // v_{1, n-1} + v_{2, n-1} v_{3, n-1} = x_{n} - x_{n-1}
  for (auto n = 0u; n != G::Dof; ++n) {
    lhs.insert(row_counter + n, v1_nm_start + n) = 1;
    lhs.insert(row_counter + n, v2_nm_start + n) = 1;
    lhs.insert(row_counter + n, v3_nm_start + n) = 1;
  }
  rhs.segment(row_counter, G::Dof) = *(it_g + 1) - *it_g;
  row_counter += G::Dof;

  // zero second derivative at end:
  // v_{2, n-1} = v_{3, n-1}
  for (auto n = 0u; n != G::Dof; ++n) {
    lhs.insert(row_counter + n, v2_nm_start + n) = 1;
    lhs.insert(row_counter + n, v3_nm_start + n) = -1;
  }

  //// DONE FILLING SPARSE MATRIX ////

  lhs.makeCompressed();

  //// SOLVE SYSTEM ////

  Eigen::SparseLU<decltype(lhs), Eigen::COLAMDOrdering<int>> solver(lhs);
  Eigen::VectorXd result = solver.solve(rhs);

  //// EXTRACT SOLUTION ////

  std::vector<Bezier<3, G>> segments;
  segments.reserve(N);

  it_g = std::ranges::begin(gg);

  for (auto i = 0u; i != N; ++i, ++it_g) {
    const std::size_t v1i_start = idx(1, i);
    const std::size_t v3i_start = idx(3, i);

    typename G::Tangent v1 = result.template segment<G::Dof>(v1i_start);
    typename G::Tangent v3 = result.template segment<G::Dof>(v3i_start);
    // re-compute v2 to compensate for linearization
    // this ensures points are interpolated, but the cost is
    // potential non-continuity of the second derivative
    typename G::Tangent v2 = *(it_g + 1) * G::exp(-v3) - *(it_g)*G::exp(v1);

    segments.emplace_back(
      *it_g, std::array<typename G::Tangent, 3>{std::move(v1), std::move(v2), std::move(v3)});
  }

  auto take_view = tt | std::views::take(N + 1);
  std::vector<double> knots(std::ranges::begin(take_view), std::ranges::end(take_view));

  return PiecewiseBezier<3, G>(std::move(knots), std::move(segments));
}

}  // namespace smooth

#endif  // SMOOTH__INTERP__BEZIER_HPP_


#include "CubicInterpolation/BicubicSplines.h"
#include "CubicInterpolation/InterpolantBuilder.h"

#include <Eigen/Dense>
#include <boost/math/differentiation/finite_difference.hpp>
#include <boost/math/interpolators/cardinal_cubic_b_spline.hpp>
#include <boost/serialization/access.hpp>
#include <cmath>
#include <vector>

#include <chrono>
#include <iostream>

namespace cubic_splines {
namespace detail {

template <typename T> auto calculate_node(T val, unsigned int n_max) {
  auto n = std::floor(val);
  if (n < 0)
    return 0u;
  if (static_cast<unsigned int>(n) > n_max - 2u)
    return n_max - 2u;
  return static_cast<unsigned int>(n);
}

template <typename T> auto exponent_vector(T x) {
  auto x_vec = ::Eigen::Matrix<T, 4, 1>(4);
  for (size_t i = 0; i < 4; ++i) {
    x_vec(i) = 1;
    for (size_t j = 1; j <= i; ++j)
      x_vec(i) *= x;
  }
  return x_vec;
}
} // namespace detail

namespace detail {}

template <typename T> struct BicubicSplines<T>::RuntimeData {
  using MatrixX = ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic>;
  using Matrix4 = ::Eigen::Matrix<T, 4, 4>;

  MatrixX y, dydx1, dydx2, d2ydx1dx2;
  Matrix4 m =
      (Matrix4() << 1, 0, -3, 2, 0, 0, 3, -2, 0, 1, -2, 1, 0, 0, -1, 1).finished();

  RuntimeData() = default;

  RuntimeData(size_t n1, size_t n2)
      : y(n1, n2), dydx1(n1, n2), dydx2(n1, n2), d2ydx1dx2(n1, n2){};

  template <typename T1>
  RuntimeData(T1 &&_y, T1 &&_dydx1, T1 &&_dydx2, T1 &&_d2ydx1dx2)
      : y(std::forward<T1>(_y)), dydx1(std::forward<T1>(_dydx1)),
        dydx2(std::forward<T1>(_dydx2)), d2ydx1dx2(std::forward<T1>(_d2ydx1dx2)){};

  template <typename T1> static ::std::vector<T> to_vector(T1 m) {
    return ::std::vector<T>(m.data(), m.data() + m.rows() * m.cols());
  }

  auto get_dimensions() const {
    return std::array<long int, 2>{static_cast<long int>(y.rows()),
                                   static_cast<long int>(y.cols())};
  }

  StorageData to_storage_data() const;
};

template <typename T> struct BicubicSplines<T>::StorageData {
  using MatrixX = ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic>;

  ::std::array<long int, 2> size;
  ::std::vector<T> y, dydx1, dydx2, d2ydx1dx2;

  friend class boost::serialization::access;
  template <class Archive> void serialize(Archive &ar, const unsigned int) {
    ar &size;
    ar &y;
    ar &dydx1;
    ar &dydx2;
    ar &d2ydx1dx2;
  }

  template <typename V> inline auto to_matrix(V v) const {
    return ::Eigen::Map<MatrixX>(v.data(), size[0], size[1]);
  }

public:
  StorageData() = default;

  template <typename T1>
  StorageData(std::array<long int, 2> _size, T1 &&_y, T1 &&_dydx1, T1 &&_dydx2,
              T1 &&_d2ydx1dx2)
      : size(std::move(_size)), y(std::forward<T1>(_y)), dydx1(std::forward<T1>(_dydx1)),
        dydx2(std::forward<T1>(_dydx2)), d2ydx1dx2(std::forward<T1>(_d2ydx1dx2)){};

  auto to_runtime_data() const {
    return RuntimeData(to_matrix(y), to_matrix(dydx1), to_matrix(dydx2),
                       to_matrix(d2ydx1dx2));
  }
};

template <typename T>
typename BicubicSplines<T>::StorageData
BicubicSplines<T>::RuntimeData::to_storage_data() const {
  return StorageData(get_dimensions(), to_vector(y), to_vector(dydx1), to_vector(dydx2),
                     to_vector(d2ydx1dx2));
}

template <typename T>
BicubicSplines<T>::BicubicSplines(BicubicSplines::RuntimeData _data)
    : data(::std::make_unique<BicubicSplines::RuntimeData>(_data)) {}

template <typename T>
BicubicSplines<T>::BicubicSplines(Definition const &def, std::string path,
                                  std::string filename) {
  try {
    auto storage_data = load<BicubicSplines>(path, filename);
    *this = BicubicSplines(storage_data.to_runtime_data());
  } catch (std::system_error const &ex) {
    if (ex.code().value() != ENOENT)
      throw(ex);
    *this = BicubicSplines(def);
    save(data->to_storage_data(), path, filename);
  }
}

template <typename T>
std::tuple<T, T> BicubicSplines<T>::back_transform(Definition const &def,
                                                   unsigned long n1,
                                                   unsigned long n2) const {
  auto x1 = def.axis[0]->back_transform(n1);
  auto x2 = def.axis[1]->back_transform(n2);
  return std::make_tuple(x1, x2);
}

template <typename T>
template <typename T1>
std::array<T, 2>
BicubicSplines<T>::_prime(T1 func, std::array<std::unique_ptr<Axis<T>>, 2> const &axis,
                          unsigned int n1, unsigned int n2) {
  using boost::math::differentiation::finite_difference_derivative;
  auto f_x1 = [&func, ax = axis[0].get(), x2 = axis[1]->back_transform(n2)](T t1) {
    return func(ax->back_transform(t1), x2);
  };
  auto f_x2 = [&func, ax = axis[1].get(), x1 = axis[0]->back_transform(n1)](T t2) {
    return func(x1, ax->back_transform(t2));
  };
  auto dydx = std::array<T, 2>();
  dydx[0] = finite_difference_derivative(f_x1, static_cast<T>(n1));
  dydx[1] = finite_difference_derivative(f_x2, static_cast<T>(n2));
  return dydx;
}

/* template <typename T> */
/* template <typename T1> */
/* std::vector<std::tuple<unsigned int, T>> */
/* BicubicSplines<T>::_prime(std::vector<T> const &yi, T1 func, unsigned int n_max) { */
/*   auto spline = boost::math::interpolators::cardinal_cubic_b_spline<T>( */
/*       yi.data(), yi.size(), 0, 1, func(0u), func(n_max)); */
/*   auto diff = std::vector<std::tuple<unsigned int, T>>(); */
/*   for (auto i = 0u; i < n_max; ++i) */
/*     diff.emplace_back(i, spline.prime(i)); */
/*   return diff; */
/* } */

template <typename T>
template <typename T1>
T BicubicSplines<T>::_double_prime(Definition const &def, T1 func, unsigned int n1,
                                   unsigned int n2) {
  using boost::math::differentiation::finite_difference_derivative;
  auto f_x1 = [&func, ax = def.axis[0].get()](T t1, T t2) {
    return func(ax->back_transform(t1), t2);
  };
  auto dydx1 = [&f_x1, n1](T t2) {
    return finite_difference_derivative([&f_x1, t2](T t1) { return f_x1(t1, t2); },
                                        static_cast<T>(n1));
  };
  auto dydx1_x2 = [&dydx1, ax = def.axis[1].get()](T t2) {
    return dydx1(ax->back_transform(t2));
  };
  return finite_difference_derivative(dydx1_x2, static_cast<T>(n2));
}

template <typename T>
BicubicSplines<T>::BicubicSplines(Definition const &def)
    : data(std::make_shared<RuntimeData>(def.axis[0]->required_nodes(),
                                         def.axis[1]->required_nodes())) {
  using boost::math::differentiation::finite_difference_derivative;
  auto func = [&def](T x1, T x2) {
    if (def.f_trafo)
      return def.f_trafo->transform(def.f(x1, x2));
    return def.f(x1, x2);
  };

  for (auto n1 = 0u; n1 < data->y.rows(); ++n1) {
    for (auto n2 = 0u; n2 < data->y.cols(); ++n2) {
      auto x = back_transform(def, n1, n2);
      data->y(n1, n2) = func(std::get<0>(x), std::get<1>(x));
    }
  }

  auto n_rows = data->y.rows();
  auto n_cols = data->y.cols();
  if (def.approx_derivates) {
    for (auto n1 = 0u; n1 < n_cols; ++n1) {
      auto yi = RuntimeData::to_vector(data->y.col(n1));
      auto diff = [this, &def, &func, n1](unsigned int n) {
        return _prime(func, def.axis, n, n1)[0];
      };
      auto spline = boost::math::interpolators::cardinal_cubic_b_spline<T>(
          yi.data(), yi.size(), 0, 1, diff(0u), diff(n_rows - 1));
      for (auto row = 0; row < n_rows; ++row)
        data->dydx1(n1, row) = spline.prime(row);
    }
    data->dydx1.transposeInPlace();

    ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic> y_rowise = data->y;
    y_rowise.transposeInPlace();
    for (auto n2 = 0u; n2 < n_rows; ++n2) {
      auto yi = RuntimeData::to_vector(y_rowise.col(n2));
      auto diff = [this, &def, &func, n2](unsigned int n) {
        return _prime(func, def.axis, n2, n)[1];
      };
      auto spline = boost::math::interpolators::cardinal_cubic_b_spline<T>(
          yi.data(), yi.size(), 0, 1, diff(0u), diff(n_cols - 1));
      for (auto col = 0; col < n_cols; ++col)
        data->dydx2(col, n2) = spline.prime(col);
    }
    data->dydx2.transposeInPlace();
  } else {
    for (auto n1 = 0u; n1 < n_rows; ++n1) {
      for (auto n2 = 0u; n2 < n_cols; ++n2) {
        auto dydx = _prime(func, def.axis, n1, n2);
        data->dydx1(n1, n2) = dydx[0];
        data->dydx2(n1, n2) = dydx[1];
      }
    }
  }
  if (def.approx_derivates) {
    ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic> dydx1_rowise = data->dydx1;
    dydx1_rowise.transposeInPlace();
    for (auto n2 = 0u; n2 < n_rows; ++n2) {
      auto yi = RuntimeData::to_vector(dydx1_rowise.col(n2));
      auto diff = [this, &def, &func, n2](unsigned int n) {
        return _double_prime(def, func, n2, n);
      };
      auto spline = boost::math::interpolators::cardinal_cubic_b_spline<T>(
          yi.data(), yi.size(), 0, 1, diff(0u), diff(n_cols - 1));
      for (auto col = 0; col < n_cols; ++col)
        data->d2ydx1dx2(col, n2) = spline.prime(col);
    }
    data->d2ydx1dx2.transposeInPlace();
  } else {
    for (auto n1 = 0u; n1 < n_rows; ++n1) {
      for (auto n2 = 0u; n2 < n_cols; ++n2)
        data->d2ydx1dx2(n1, n2) = _double_prime(def, func, n1, n2);
    }
  }
}

template <typename T> T BicubicSplines<T>::evaluate(T x0, T x1) const {
  auto n0 = detail::calculate_node(x0, data->y.cols());
  auto n1 = detail::calculate_node(x1, data->y.rows());
  auto temp = ::Eigen::Matrix<T, 4, 4>(4, 4);
  temp.template block<2, 2>(0, 0) = data->y.block(n0, n1, 2, 2);
  temp.template block<2, 2>(2, 0) = data->dydx1.block(n0, n1, 2, 2);
  temp.template block<2, 2>(0, 2) = data->dydx2.block(n0, n1, 2, 2);
  temp.template block<2, 2>(2, 2) = data->d2ydx1dx2.block(n0, n1, 2, 2);
  auto v1 = detail::exponent_vector(x0 - n0);
  auto v2 = detail::exponent_vector(x1 - n1);
  return v1.dot((data->m.transpose() * (temp * data->m)) * v2);
}

template <typename T> std::array<T, 2> BicubicSplines<T>::prime(T x0, T x1) const {
  using boost::math::differentiation::finite_difference_derivative;
  auto grad = std::array<T, 2>();
  grad[0] =
      finite_difference_derivative([this, x1](T x_0) { return evaluate(x_0, x1); }, x0);
  grad[1] =
      finite_difference_derivative([this, x0](T x_1) { return evaluate(x0, x_1); }, x1);
  return grad;
}

template <typename T> T BicubicSplines<T>::double_prime(T x0, T x1) const {
  using boost::math::differentiation::finite_difference_derivative;
  return finite_difference_derivative(
      [this, x1](T x_1) {
        return finite_difference_derivative(
            [this, x_1](T x_2) { return evaluate(x_1, x_2); }, x1);
      },
      x0);
}
} // namespace cubic_splines

template class cubic_splines::BicubicSplines<double>;
template class cubic_splines::BicubicSplines<float>;


#include <cstdio>
#include <iomanip>
#include <iostream>
#include <limits>
#include <stdexcept>

#include <Eigen/Dense>

#include <spii/spii.h>
#include <spii/solver.h>

#include "sgdsolver.hpp"

namespace spii {

void SGDSolver::solve(const Function& function, SolverResults* results) const {
  // Get the dimensionality of the problem.
  size_t dimension = function.get_number_of_scalars();
  std::cout << "Dimension is " << dimension << std::endl;

  if (dimension == 0) {
    results->exit_condition = SolverResults::FUNCTION_TOLERANCE;
    return;
  }

  Eigen::VectorXd current_point, gradient;
  Eigen::VectorXd next_point, next_point_gradient;

  function.copy_user_to_global(&current_point);
  Eigen::VectorXd stepdirection(dimension);

  results->exit_condition = SolverResults::INTERNAL_ERROR;
  int iteration = 0;

  double function_value = std::numeric_limits<double>::quiet_NaN();
  double gradient_norm = std::numeric_limits<double>::quiet_NaN();

  double previous_function_value = 0;
  double gain = 1.0;

  double exponentially_weighed_average_gain = gain;

  while (iteration < 500) {
    function_value = function.evaluate(current_point, &gradient);
    gradient_norm = std::max(gradient.maxCoeff(), -gradient.minCoeff());
    stepdirection = -gradient;

    gain = previous_function_value - function_value;

    printf("[%d:] Loss is %+.17e, norm is %+.17e, gain is %+.17e\n",
      iteration, function_value, gradient_norm, gain);

    double stepsize = (1.0 / gradient_norm) * (1.0/(iteration+1));

    // Evaluate the function at the next point and see if we are making
    // progress.
    double next_function_value;
    while (stepsize > 1.0e-10) {
      next_point = current_point + (10*stepsize) * stepdirection;
      next_function_value = function.evaluate(next_point,
        &next_point_gradient);
      // In order to make progress, the next_function_value needs to be smaller
      // than the current_function_value, so the gain needs to be positive.
      gain = function_value - next_function_value;
      if ((gain <= 0) || std::isnan(gain)) {
        printf("[%d:] gain is %+.17e, reducing stepsize * norm from %+.17e\n",
          iteration, gain, stepsize * gradient_norm);
        stepsize = stepsize / 100.0;
      } else {
        printf("[%d:] gain is %+.17e reducing loss to %+.17e\n", iteration,
          gain, next_function_value);
        current_point = next_point;
        gradient = next_point_gradient;
        break;
      }
    }

    if (stepsize <= 1.0e-100) {
      exponentially_weighed_average_gain = gain + (0.001 *
        exponentially_weighed_average_gain);
    } else {
      exponentially_weighed_average_gain = gain +
        (0.5 * exponentially_weighed_average_gain);
    }

    printf("[%d:] Exponentially weighed avg gain: %+.17e\n", iteration,
      exponentially_weighed_average_gain);

    if (exponentially_weighed_average_gain < 1.0e-20) {
      printf("[%d:] Exponentially weighed avg gain too small, aborting\n", iteration);
      if (exponentially_weighed_average_gain < 0.0) {
        printf("[%d:] Can't be negative, exiting.\n", iteration);
        exit(-1);
      }
      break;
    }

    if (function_value < 1.0) {
      printf("[%d:] Loss less than 1, aborting\n", iteration);
      break;
    }
    ++iteration;
    previous_function_value = function_value;
  }

  function.copy_global_to_user(current_point);
}

} // namespace spii


#include <fstream>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <cassert>
#include <algorithm>
#include <cmath>
#include <iterator>
#include "sobol.h"
#include "Transformable.h"
#include "RunManagerAbstract.h"
#include "ParamTransformSeq.h"
#include "ModelRunPP.h"
#include "Stats.h"
#include "FileManager.h"

using namespace std;
using namespace Eigen;

Sobol::Sobol(Pest &_pest_scenario,
	FileManager &_file_manager, ObjectiveFunc *_obj_func_ptr,
	const ParamTransformSeq &_par_transform,
	int _n_sample, PARAM_DIST _par_dist, unsigned int _seed)
	: GsaAbstractBase(_pest_scenario, _file_manager, _obj_func_ptr, _par_transform,
		_par_dist, _seed), n_sample(_n_sample)
	{
	}

VectorXd Sobol::gen_rand_vec(long nsample, double min, double max)
{
	VectorXd v(nsample);
	long v_len = v.size();

	if (par_dist == PARAM_DIST::normal)
	{
		std::normal_distribution<> distribution((max + min) / 2.0, (max - min) / 4.0);
		for (long i = 0; i < v_len; ++i)
		{
			v[i] = distribution(rand_engine);
			while (v[i] < min || v[i] > max) v[i] = distribution(rand_engine);
		}
	}
	else
	{
		std::uniform_real_distribution<double> distribution(min, max);
		for (long i = 0; i < v_len; ++i)
		{
			v[i] = distribution(rand_engine);
		}
	}
	return v;
}

void Sobol::gen_m1_m2()
{
	long npar = adj_par_name_vec.size();
	//generate random matrices
	double par_min;
	double par_max;
	VectorXd v1;
	VectorXd v2;
	m1 = MatrixXd::Zero(n_sample, npar);
	m2 = MatrixXd::Zero(n_sample, npar);
	for (int i=0; i<npar; ++i)
	{
		string &p_name = adj_par_name_vec[i];
		par_min = min_numeric_pars[p_name];
		par_max = max_numeric_pars[p_name];
		v1 = gen_rand_vec(n_sample, par_min, par_max);
		v2 = gen_rand_vec(n_sample, par_min, par_max);
		m1.col(i) = v1;
		m2.col(i) = v2;
	}
}

MatrixXd Sobol::gen_N_matrix(const MatrixXd &m1, const MatrixXd &m2, const vector<int> &idx_vec)
{
  MatrixXd n = m2;
  for (int i : idx_vec)
  {
	n.col(i) = m1.col(i);
  }
  return n;
}

void Sobol::add_model_runs(RunManagerAbstract &run_manager, const MatrixXd &n)
{
	for (int i=0; i<n_sample; ++i)
	{
		VectorXd tmp_vec =  n.row(i);
		Parameters tmp_pars(adj_par_name_vec, tmp_vec);
		base_partran_seq_ptr->numeric2model_ip(tmp_pars);
		run_manager.add_run(tmp_pars);
	}
}

void Sobol::assemble_runs(RunManagerAbstract &run_manager)
{
	MatrixXd c;
	run_manager.reinitialize();
	gen_m1_m2();

	//calculate a0
	int n_adj_par = adj_par_name_vec.size();

	add_model_runs(run_manager, m1);
	add_model_runs(run_manager, m2);

	//cout << m1 << endl << endl;
	//cout << m2 << endl << endl;
	//calculate first order runs a1,....an
	vector<int> idx_vec;
	for (int ai=0; ai<n_adj_par; ++ai)
	{
		idx_vec.clear();

		idx_vec.push_back(ai);
		c = gen_N_matrix(m1, m2, idx_vec);
		//cout << c << endl << endl;
		add_model_runs(run_manager, c);
	}
}

vector<double> Sobol::get_obs_vec(RunManagerAbstract &run_manager, int run_set, ModelRun &model_run, const string &obs_name)
{
	ModelRun run0 = model_run;

	int run_b = run_set * n_sample;
	int run_e = run_b + n_sample;

	Parameters pars0;
	Observations obs0;
	int nrun = 0;
	vector<double> obs_vec = vector<double>(n_sample, MISSING_DATA);
	for (int run_id = run_b; run_id<run_e; ++run_id)
	{
		double obs = MISSING_DATA;
		bool success = run_manager.get_run(run_id, pars0, obs0);
		if (success)
		{
			run0.update_ctl(pars0, obs0);
			obs = obs0.get_rec(obs_name);
			if (obs == Observations::no_data) obs = MISSING_DATA;
		}
		obs_vec[nrun] = obs;
		nrun++;
	}
	return obs_vec;
}

vector<double> Sobol::get_phi_vec(RunManagerAbstract &run_manager, int run_set, ModelRun &model_run)
{
	ModelRun run0 = model_run;

	int run_b = run_set * n_sample;
	int run_e = run_b + n_sample;

	Parameters pars0;
	Observations obs0;
	int nrun = 0;
	vector<double> phi_vec = vector<double>(n_sample, MISSING_DATA);
	for(int run_id=run_b; run_id<run_e; ++run_id)
	{
		double phi = MISSING_DATA;
		bool success = run_manager.get_run(run_id, pars0, obs0);
		if (success)
		{
			run0.update_ctl(pars0, obs0);
			phi = run0.get_phi(0.0);
		}
		phi_vec[nrun] = phi;
		nrun++;
	}
	return phi_vec;
}

void Sobol::calc_sen(RunManagerAbstract &run_manager, ModelRun model_run)
{
	ofstream &fout_sbl = file_manager_ptr->open_ofile_ext("sbl");
	fout_sbl << "Sobol Sensitivity for PHI" << endl;
	calc_sen_single(run_manager, model_run, fout_sbl, string());

	vector<string> obs_names = run_manager.get_obs_name_vec();
	for (const string &iobs : obs_names)
	{
		fout_sbl << endl << endl;
		fout_sbl << "Sobol Sensitivity for observation \"" << iobs <<"\"" << endl;
		calc_sen_single(run_manager, model_run, fout_sbl, iobs);
	}

	file_manager_ptr->close_file("sbl");
}

void Sobol::calc_sen_single(RunManagerAbstract &run_manager, ModelRun model_run, ofstream &fout_sbl, const string &obs_name)
{
	vector<double> ya;
	vector<double> yb;
	if (obs_name.empty())
	{
		ya = get_phi_vec(run_manager, 0, model_run);
		yb = get_phi_vec(run_manager, 1, model_run);
	}
	else
	{
		ya = get_obs_vec(run_manager, 0, model_run, obs_name);
		yb = get_obs_vec(run_manager, 1, model_run, obs_name);
	}

	vector<double> ya_yb_prod = vec_array_prod(ya, yb, MISSING_DATA);
	vector<double> y_ab;
	y_ab.reserve(ya.size() + yb.size()); // preallocate memory
	y_ab.insert(y_ab.end(), ya.begin(), ya.end());
	y_ab.insert(y_ab.end(), yb.begin(), yb.end());

	//Compute Mean for the S_i's
	double mean_sq_si = vec_mean_missing_data(ya_yb_prod, MISSING_DATA);
	// Compute Var for S_i's
	pair<double, size_t> data = sum_of_prod_missing_data(y_ab, y_ab, MISSING_DATA);
	double var_si = data.first / (data.second - 2.0) - mean_sq_si;

	//Compute Mean for the S_ti's
	double mean_sq_sti = pow(vec_mean_missing_data(y_ab, MISSING_DATA), 2.0);
	// Compute Var for S_ti's
	double sti_u = sobol_u_missing_data(yb, yb, MISSING_DATA);
	double var_sti = sti_u - mean_sq_sti;
	fout_sbl << "E(Y) = " << sqrt(mean_sq_si) << ";  Var(Y) = " << var_si << " (for S_i calculations)" << endl;
	fout_sbl << "E(Y) = " << sqrt(mean_sq_sti) << ";  Var(Y) = " << var_sti << " (for S_ti calculations)" << endl;
	size_t npar = adj_par_name_vec.size();

	fout_sbl << "parameter_name, s_i, st_i, n_runs" << endl;
	for (size_t i=0; i<npar; ++i)
	{
		vector<double> yci;
		if (obs_name.empty())
		{
			yci = get_phi_vec(run_manager, i + 2, model_run);
		}
		else
		{
			yci = get_obs_vec(run_manager, i + 2, model_run, obs_name);
		}

		pair<double, int> sumprod_num = sum_of_prod_missing_data(ya, yci, MISSING_DATA);
		long int n_runs = sumprod_num.second;
		double sobol_uj = sumprod_num.first / (sumprod_num.second - 1.0);
		double si = (sobol_uj - mean_sq_si) / var_si;

		double sobol_umj = sobol_u_missing_data(yb, yci, MISSING_DATA);
		double sti = 1 - (sobol_umj - mean_sq_sti) / var_sti;

		fout_sbl << adj_par_name_vec[i] << ", " << si << ", " << sti << ", " << n_runs << endl;
	}
}


/*

Copyright (c) 2005-2018, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef DELTANOTCHODESYSTEM_HPP_
#define DELTANOTCHODESYSTEM_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include <cmath>
#include <iostream>

#include "AbstractOdeSystem.hpp"

/**
 * Represents the Delta-Notch ODE system described by Collier et al,
 * "Pattern formation by lateral inhibition with feedback: a mathematical
 * model of delta-notch intercellular signalling" (Journal of Theoretical
 * Biology 183:429-446, 1996).
 */
class DeltaNotchOdeSystem : public AbstractOdeSystem
{
private:

    friend class boost::serialization::access;
    /**
     * Serialize the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractOdeSystem>(*this);
    }

    ///\todo extract model parameters as member variables

public:

    /**
     * Default constructor.
     *
     * @param stateVariables optional initial conditions for state variables (only used in archiving)
     */
    DeltaNotchOdeSystem(std::vector<double> stateVariables=std::vector<double>());

    /**
     * Destructor.
     */
    ~DeltaNotchOdeSystem();

    /**
     * Compute the RHS of the  Collier et al. system of ODEs.
     *
     * Returns a vector representing the RHS of the ODEs at each time step, y' = [y1' ... yn'].
     * An ODE solver will call this function repeatedly to solve for y = [y1 ... yn].
     *
     * @param time used to evaluate the RHS.
     * @param rY value of the solution vector used to evaluate the RHS.
     * @param rDY filled in with the resulting derivatives (using  Collier et al. system of equations).
     */
    void EvaluateYDerivatives(double time, const std::vector<double>& rY, std::vector<double>& rDY);
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(DeltaNotchOdeSystem)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a DeltaNotchOdeSystem.
 */
template<class Archive>
inline void save_construct_data(
    Archive & ar, const DeltaNotchOdeSystem * t, const unsigned int file_version)
{
    const std::vector<double>& state_variables = t->rGetConstStateVariables();
    ar & state_variables;
}

/**
 * De-serialize constructor parameters and initialise a DeltaNotchOdeSystem.
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, DeltaNotchOdeSystem * t, const unsigned int file_version)
{
    std::vector<double> state_variables;
    ar & state_variables;

    // Invoke inplace constructor to initialise instance
    ::new(t)DeltaNotchOdeSystem(state_variables);
}
}
} // namespace ...

#endif /*DELTANOTCHODESYSTEM_HPP_*/


/*
 * MIT License
 *
 * Copyright (c) 2020 Robert Grupp
 *
 * 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.
 */

#include "xregOpenCLMath.h"

#include <boost/compute/utility/source.hpp>

const char* xreg::kOPENCL_MATH_UTILS_SRC = BOOST_COMPUTE_STRINGIZE_SOURCE(

float3 xregFloat4HmgToFloat3(const float4 a)
{
  return (float3) (a.x, a.y, a.z);
}

float xregFloat1Norm(const float x)
{
  return fabs(x);
}

float xregFloat2Norm(const float2 x)
{
  return sqrt((x.x * x.x) + (x.y * x.y));
}

float xregFloat3Norm(const float3 x)
{
  return sqrt((x.x * x.x) + (x.y * x.y) + (x.z * x.z));
}

float xregFloat4HmgNorm(const float4 x)
{
  return sqrt((x.x * x.x) + (x.y * x.y) + (x.z * x.z));
}

float xregFloat4Norm(const float4 x)
{
  return sqrt((x.x * x.x) + (x.y * x.y) + (x.z * x.z)  + (x.w * x.w));
}

float xregFloat8Norm(const float8 x)
{
  return sqrt((x.s0 * x.s0) + (x.s1 * x.s1) + (x.s2 * x.s2)  + (x.s3 * x.s3) +
              (x.s4 * x.s4) + (x.s5 * x.s5) + (x.s6 * x.s6)  + (x.s7 * x.s7));
}

float xregFloat16Norm(const float16 x)
{
  return sqrt((x.s0 * x.s0) + (x.s1 * x.s1) + (x.s2 * x.s2)  + (x.s3 * x.s3) +
              (x.s4 * x.s4) + (x.s5 * x.s5) + (x.s6 * x.s6)  + (x.s7 * x.s7) +
              (x.s8 * x.s8) + (x.s9 * x.s9) + (x.sa * x.sa)  + (x.sb * x.sb) +
              (x.sc * x.sc) + (x.sd * x.sd) + (x.se * x.se)  + (x.sf * x.sf));
}

float2 xregFloat2Normalize(const float2 x)
{
  return x / xregFloat2Norm(x);
}

float3 xregFloat3Normalize(const float3 x)
{
  return x / xregFloat3Norm(x);
}

float4 xregFloat4Normalize(const float4 x)
{
  return x / xregFloat4Norm(x);
}

float8 xregFloat8Normalize(const float8 x)
{
  return x / xregFloat8Norm(x);
}

float16 xregFloat16Normalize(const float16 x)
{
  return x / xregFloat16Norm(x);
}

float xregFloat1NormSq(const float x)
{
  return x * x;
}

float xregFloat2NormSq(const float2 x)
{
  return (x.x * x.x) + (x.y * x.y);
}

float xregFloat3NormSq(const float3 x)
{
  return (x.x * x.x) + (x.y * x.y) + (x.z * x.z);
}

float xregFloat4HmgNormSq(const float4 x)
{
  return (x.x * x.x) + (x.y * x.y) + (x.z * x.z);
}

float xregFloat4NormSq(const float4 x)
{
  return (x.x * x.x) + (x.y * x.y) + (x.z * x.z)  + (x.w * x.w);
}

float xregFloat8NormSq(const float8 x)
{
  return (x.s0 * x.s0) + (x.s1 * x.s1) + (x.s2 * x.s2)  + (x.s3 * x.s3) +
         (x.s4 * x.s4) + (x.s5 * x.s5) + (x.s6 * x.s6)  + (x.s7 * x.s7);
}

float xregFloat16NormSq(const float16 x)
{
  return (x.s0 * x.s0) + (x.s1 * x.s1) + (x.s2 * x.s2)  + (x.s3 * x.s3) +
         (x.s4 * x.s4) + (x.s5 * x.s5) + (x.s6 * x.s6)  + (x.s7 * x.s7) +
         (x.s8 * x.s8) + (x.s9 * x.s9) + (x.sa * x.sa)  + (x.sb * x.sb) +
         (x.sc * x.sc) + (x.sd * x.sd) + (x.se * x.se)  + (x.sf * x.sf);
}

float3 xregFrm4x4XformFloat3Pt(const float16 frm, const float3 a)
{
  float3 b;

  b.x = (frm.s0 * a.x) + (frm.s1 * a.y) + (frm.s2 * a.z) + frm.s3;
  b.y = (frm.s4 * a.x) + (frm.s5 * a.y) + (frm.s6 * a.z) + frm.s7;
  b.z = (frm.s8 * a.x) + (frm.s9 * a.y) + (frm.sa * a.z) + frm.sb;

  return b;
}

float3 xregFrm4x4XformFloat3Vec(const float16 frm, const float3 a)
{
  float3 b;

  b.x = (frm.s0 * a.x) + (frm.s1 * a.y) + (frm.s2 * a.z);
  b.y = (frm.s4 * a.x) + (frm.s5 * a.y) + (frm.s6 * a.z);
  b.z = (frm.s8 * a.x) + (frm.s9 * a.y) + (frm.sa * a.z);

  return b;
}

float4 xregFrm4x4XformFloat4Pt(const float16 frm, const float4 a)
{
  float4 b;

  b.x = (frm.s0 * a.x) + (frm.s1 * a.y) + (frm.s2 * a.z) + frm.s3;
  b.y = (frm.s4 * a.x) + (frm.s5 * a.y) + (frm.s6 * a.z) + frm.s7;
  b.z = (frm.s8 * a.x) + (frm.s9 * a.y) + (frm.sa * a.z) + frm.sb;
  b.w = 1;

  return b;
}

float4 xregFrm4x4XformFloat4Vec(const float16 frm, const float4 a)
{
  float4 b;

  b.x = (frm.s0 * a.x) + (frm.s1 * a.y) + (frm.s2 * a.z);
  b.y = (frm.s4 * a.x) + (frm.s5 * a.y) + (frm.s6 * a.z);
  b.z = (frm.s8 * a.x) + (frm.s9 * a.y) + (frm.sa * a.z);
  b.w = 0;

  return b;
}

float16 xregFrm4x4Composition(const float16 f, const float16 g)
{
  float16 h;

  h.s0 = (f.s0 * g.s0) + (f.s1 * g.s4) + (f.s2 * g.s8);
  h.s1 = (f.s0 * g.s1) + (f.s1 * g.s5) + (f.s2 * g.s9);
  h.s2 = (f.s0 * g.s2) + (f.s1 * g.s6) + (f.s2 * g.sa);
  h.s3 = f.s3 + (f.s0 * g.s3) + (f.s1 * g.s7) + (f.s2 * g.sb);
  h.s4 = (f.s4 * g.s0) + (f.s5 * g.s4) + (f.s6 * g.s8);
  h.s5 = (f.s4 * g.s1) + (f.s5 * g.s5) + (f.s6 * g.s9);
  h.s6 = (f.s4 * g.s2) + (f.s5 * g.s6) + (f.s6 * g.sa);
  h.s7 = f.s7 + (f.s4 * g.s3) + (f.s5 * g.s7) + (f.s6 * g.sb);
  h.s8 = (f.s8 * g.s0) + (f.s9 * g.s4) + (f.sa * g.s8);
  h.s9 = (f.s8 * g.s1) + (f.s9 * g.s5) + (f.sa * g.s9);
  h.sa = (f.s8 * g.s2) + (f.s9 * g.s6) + (f.sa * g.sa);
  h.sb = f.sb + (f.s8 * g.s3) + (f.s9 * g.s7) + (f.sa * g.sb);
  h.sc = 0;
  h.sd = 0;
  h.se = 0;
  h.sf = 1;

  return h;
}

float16 xregFrm4x4SE3Inv(const float16 h)
{
  float16 h_inv;

  // transpose for rotation inverse: R^-1 = R^T

  // row 1
  h_inv.s0 = h.s0;
  h_inv.s1 = h.s4;
  h_inv.s2 = h.s8;

  // row 2
  h_inv.s4 = h.s1;
  h_inv.s5 = h.s5;
  h_inv.s6 = h.s9;

  // row 3
  h_inv.s8 = h.s2;
  h_inv.s9 = h.s6;
  h_inv.sa = h.sa;

  // translation inverse: t^-1 = -R^T * t
  h_inv.s3 = -((h_inv.s0 * h.s3) + (h_inv.s1 * h.s7) + (h_inv.s2 * h.sb));
  h_inv.s7 = -((h_inv.s4 * h.s3) + (h_inv.s5 * h.s7) + (h_inv.s6 * h.sb));
  h_inv.sb = -((h_inv.s8 * h.s3) + (h_inv.s9 * h.s7) + (h_inv.sa * h.sb));

  h_inv.sc = 0;
  h_inv.sd = 0;
  h_inv.se = 0;
  h_inv.sf = 1;

  return h_inv;
}

float2 xregFrm3x3XformFloat2Pt(const float16 frm, const float2 a)
{
  float2 b;

  b.x = (frm.s0 * a.x) + (frm.s1 * a.y) + frm.s2;
  b.y = (frm.s3 * a.x) + (frm.s4 * a.y) + frm.s5;

  return b;
}

float2 xregFrm3x3XformFloat2Vec(const float16 frm, const float2 a)
{
  float2 b;

  b.x = (frm.s0 * a.x) + (frm.s1 * a.y);
  b.y = (frm.s3 * a.x) + (frm.s4 * a.y);

  return b;
}

float16 xregFrm3x3Identity()
{
  float16 A;

  A.s0 = 1;
  A.s1 = 0;
  A.s2 = 0;

  A.s3 = 0;
  A.s4 = 1;
  A.s5 = 0;

  A.s6 = 0;
  A.s7 = 0;
  A.s8 = 1;

  return A;
}

float16 xregFrm4x4Identity()
{
  float16 A;

  A.s0 = 1;
  A.s1 = 0;
  A.s2 = 0;
  A.s3 = 0;

  A.s4 = 0;
  A.s5 = 1;
  A.s6 = 0;
  A.s7 = 0;

  A.s8 = 0;
  A.s9 = 0;
  A.sa = 1;
  A.sb = 0;

  A.sc = 0;
  A.sd = 0;
  A.se = 0;
  A.sf = 1;

  return A;
}

float xregFloat2Inner(const float2 x, const float2 y)
{
  return (x.x * y.x) + (x.y * y.y);
}

float xregFloat3Inner(const float3 x, const float3 y)
{
  return (x.x * y.x) + (x.y * y.y) + (x.z * y.z);
}

float xregFloat4HmgInner(const float4 x, const float4 y)
{
  return (x.x * y.x) + (x.y * y.y) + (x.z * y.z);
}

float xregFloat4Inner(const float4 x, const float4 y)
{
  return (x.x * y.x) + (x.y * y.y) + (x.z * y.z) + (x.w * y.w);
}

float xregFloat8Inner(const float8 x, const float8 y)
{
  return (x.s0 * y.s0) + (x.s1 * y.s1) + (x.s2 * y.s2) + (x.s3 * y.s3) +
         (x.s4 * y.s4) + (x.s5 * y.s5) + (x.s6 * y.s6) + (x.s7 * y.s7);
}

float xregFloat16Inner(const float16 x, const float16 y)
{
  return (x.s0 * y.s0) + (x.s1 * y.s1) + (x.s2 * y.s2) + (x.s3 * y.s3) +
         (x.s4 * y.s4) + (x.s5 * y.s5) + (x.s6 * y.s6) + (x.s7 * y.s7) +
         (x.s8 * y.s8) + (x.s9 * y.s9) + (x.sa * y.sa) + (x.sb * y.sb) +
         (x.sc * y.sc) + (x.sd * y.sd) + (x.se * y.se) + (x.sf * y.sf);
}

// Useful for debugging:

void xregPrintMat4x4(const float16 x)
{
  printf("[ %+9.3f , %+9.3f , %+9.3f , %+9.3f ;\n"
         "  %+9.3f , %+9.3f , %+9.3f , %+9.3f ;\n"
         "  %+9.3f , %+9.3f , %+9.3f , %+9.3f ;\n"
         "  %+9.3f , %+9.3f , %+9.3f , %+9.3f ]\n",
         x.s0, x.s1, x.s2, x.s3,
         x.s4, x.s5, x.s6, x.s7,
         x.s8, x.s9, x.sa, x.sb,
         x.sc, x.sd, x.se, x.sf);
}

);


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014-2016
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
Library author: Francesco Banterle
This file author: Giorgio Marcias

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_ALGORITHMS_MITSUNAGA_NAYAR_CRF_HPP
#define PIC_ALGORITHMS_MITSUNAGA_NAYAR_CRF_HPP

#include<algorithm>
#include<limits>
#include<vector>

#include "../base.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/LU"
#else
    #include <Eigen/LU>
#endif

#endif

namespace pic {

/**
 * @brief MitsunagaNayarClassic computes the inverse CRF of a camera as a polynomial function.
 * @param samples           Sample array of size nSamples x #exposures.
 * @param nSamples          Number of samples, for each exposure.
 * @param exposures         Array of exposure timings (size: #exposures = 'Q' as in the Mitsunaga & Nayar paper).
 * @param coefficients      The output coefficients ('c' in the paper) resulting from the computation.
 * @param computeRatios     false if exact exposures are passed, true to approximate exposure ratios as in the paper.
 * @param R                 The output estimated exposure ratios, i.e. R[q1][q2] = 'R_{q1,q2}' as in the book.
 * @param eps               Threshold for stopping the approximation process.
 * @param max_iterations    Maximum number of iterations.
 * @return The error as in the paper.
 */
PIC_INLINE float MitsunagaNayarClassic(int *samples, const std::size_t nSamples, const std::vector<float> &exposures,
                                   std::vector<float> &coefficients, const bool computeRatios, std::vector<float> &R,
                                   const float eps, const std::size_t max_iterations)
{
#ifndef PIC_DISABLE_EIGEN
    float eval, val, tmp1, tmp2;
    const std::size_t Q = exposures.size();
    const std::size_t N = coefficients.size() - 1;

    const float Mmax = 1.f;

    for (float &_c : coefficients) {
        _c = 0.f;
    }

    if (!samples || Q < 2 || coefficients.size() < 2) {
        return std::numeric_limits<float>::infinity();
    }

    R.assign(Q < 2 ? 0 : Q - 1, 1.f);
    for (int q = 0; q < R.size(); ++q) {
        R[q] = exposures[q] / exposures[q+1];
    }

    //Check valid samples
    std::vector<std::vector<float>> g(nSamples, std::vector<float>(Q, 0.f));
    std::size_t P = 0;
    for (std::size_t p = 0; p < nSamples; ++p) {
        bool valid = false;
        for (std::size_t q = 0; q < Q-1; ++q) {
            if (samples[p * Q + q] >= 0 && samples[p * Q + q+1] >= 0) {
                valid = true;
                break;
            }
        }
        if (valid) {
            for (std::size_t q = 0; q < Q; ++q) {
                if (samples[p * Q + q] >= 0) {
                    g[P][q] = samples[p * Q + q] / 255.f;
                } else {
                    g[P][q] = -1.f;
                }
            }
            ++P;
        }
    }
    g.resize(P);

    if (g.empty()) {
        return std::numeric_limits<float>::infinity();
    }

    //Precompute test with exponentials
    std::vector<Eigen::VectorXf> test;
    if (computeRatios) {
        test.assign(256, Eigen::VectorXf::Zero(N+1));
        for (std::size_t i = 0; i < 256; ++i) {
            test[i][0] = 1.f;
            for (std::size_t n = 1; n <= N; ++n) {
                test[i][n] = (i / 255.f) * test[i][n-1];
            }
        }
    }

    //Precompute M with exponentials
    std::vector<std::vector<std::vector<float>>> M(P,
                                                   std::vector<std::vector<float>>(Q,
                                                                                std::vector<float>(N+1, 0.f)));
    for (std::size_t p = 0; p < P; ++p) {
        for (std::size_t q = 0; q < Q; ++q) {
            M[p][q][0] = 1.f;
            for (std::size_t n = 1; n <= N; ++n) {
                M[p][q][n] = g[p][q] * M[p][q][n-1];
            }
        }
    }

    std::vector<std::vector<std::vector<float>>> d(P,
                                                   std::vector<std::vector<float>>(Q-1,
                                                                                std::vector<float>(N+1, 1.f)));
    Eigen::MatrixXf A = Eigen::MatrixXf::Zero(N, N);
    Eigen::VectorXf x, b = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf c(N+1), prev_c = Eigen::VectorXf::Zero(N+1);

    std::size_t iter = 0;

    do {
        //Compute d
        for (std::size_t p = 0; p < P; ++p) {
            for (std::size_t q = 0; q < Q-1; ++q) {
                if (g[p][q] >= 0.f && g[p][q+1] >= 0.f) {
                    for (std::size_t n = 0; n <= N; ++n) {
                        d[p][q][n] = M[p][q][n] - R[q] * M[p][q+1][n];
                    }
                } else {
                    d[p][q].assign(N+1, 0.f);
                }
            }
        }

        //Build the matrix A of the linear system
        A.setZero(N, N);
        for (std::size_t i = 0; i < N; ++i) {
            for (std::size_t j = 0; j < N; ++j) {
                for (std::size_t p = 0; p < P; ++p) {
                    for (std::size_t q = 0; q < Q - 1; ++q) {
                        A(i, j) += d[p][q][i] * (d[p][q][j] - d[p][q][N]);
                    }
                }
            }
        }

        //Build the vector of knowns b
        b.setZero(N);
        for (std::size_t i = 0; i < N; ++i) {
            for (std::size_t p = 0; p < P; ++p) {
                for (std::size_t q = 0; q < Q - 1; ++q) {
                    b(i) -= Mmax * d[p][q][i] * d[p][q][N];
                }
            }
        }

        //Solve the linear system
        x = A.partialPivLu().solve(b);
        c << x, Mmax - x.sum();

        if (computeRatios) {
            //Evaluate approximation increment
            eval = std::numeric_limits<float>::lowest();
            for (const Eigen::VectorXf &_M : test) {
                val = std::abs((c - prev_c).dot(_M));
                if (val > eval) {
                    eval = val;
                }
            }

            //Update R
            for (std::size_t q = 0; q < Q-1; ++q) {
                R[q] = 0.f;
                tmp1 = 0.f;
                tmp2 = 0.f;
                for (std::size_t p = 0; p < P; ++p) {
                    if (g[p][q] >= 0.f && g[p][q+1] >= 0.f) {
                        for (std::size_t n = 0; n <= N; ++n) {
                            tmp1 += c[n] * M[p][q][n];
                            tmp2 += c[n] * M[p][q+1][n];
                        }
                    }
                }
                R[q] += tmp1 / tmp2;
            }

            ++iter;
        }
    } while (computeRatios && eval > eps && iter < max_iterations);

    for (std::size_t n = 0; n <= N; ++n) {
        coefficients[n] = c[n];
    }

    //Evaluate error
    eval = 0.f;
    for (std::size_t q = 0; q < Q-1; ++q) {
        for (std::size_t p = 0; p < P; ++p) {
            if (g[p][q] >= 0.f && g[p][q+1] >= 0.f) {
                val = 0.f;
                for (std::size_t n = 0; n <= N; ++n) {
                    val += coefficients[n] * (M[p][q][n] - R[q] * M[p][q+1][n]);
                }
                eval += val * val;
            }
        }
    }

    return eval;
#else
    return -1.0f;
#endif
}

/**
 * @brief MitsunagaNayarFull computes the inverse CRF of a camera as a polynomial function, using all exposure ratios.
 * @param samples           Sample array of size nSamples x #exposures.
 * @param nSamples          Number of samples, for each exposure.
 * @param exposures         Array of exposure timings (size: #exposures = 'Q' as in the Mitsunaga & Nayar paper).
 * @param coefficients      The output coefficients ('c' in the paper) resulting from the computation.
 * @param computeRatios     false if exact exposures are passed, true to approximate exposure ratios as in the paper.
 * @param R                 The output estimated exposure ratios, i.e. R[q1][q2] = 'R_{q1,q2}' as in the book.
 * @param eps               Threshold for stopping the approximation process.
 * @param max_iterations    Maximum number of iterations.
 * @return The error as in the paper.
 */
PIC_INLINE float MitsunagaNayarFull(int *samples, const std::size_t nSamples, const std::vector<float> &exposures,
                                std::vector<float> &coefficients, bool computeRatios, std::vector<std::vector<float>> &R,
                                const float eps, const std::size_t max_iterations)
{
#ifndef PIC_DISABLE_EIGEN
    float eval, val, tmp1, tmp2;
    const std::size_t Q = exposures.size();
    const std::size_t N = coefficients.size() - 1;

    const float Mmax = 1.f;

    for (float &_c : coefficients) {
        _c = 0.f;
    }
    R.assign(Q < 2 ? 0 : Q, std::vector<float>(Q < 2 ? 0 : Q, 1.f));
    for (int q1 = 0; q1 < R.size(); ++q1) {
        for (int q2 = 0; q2 < R[q1].size(); ++q2) {
            if (q2 == q1) {
                R[q1][q2] = 1.f;
            } else {
                R[q1][q2] = exposures[q1] / exposures[q2];
            }
        }
    }
    if (!samples || Q < 2 || coefficients.size() < 2) {
        return std::numeric_limits<float>::infinity();
    }

    //Check valid samples
    std::vector<std::vector<float>> g(nSamples, std::vector<float>(Q, 0.f));
    std::size_t P = 0;
    for (std::size_t p = 0; p < nSamples; ++p) {
        std::size_t valid = 0;
        for (std::size_t q = 0; q < Q; ++q) {
            if (samples[p * Q + q] >= 0) {
                ++valid;
            }
        }
        if (valid > 1) {
            for (std::size_t q = 0; q < Q; ++q) {
                if (samples[p * Q + q] >= 0) {
                    g[P][q] = samples[p * Q + q] / 255.f;
                } else {
                    g[P][q] = -1.f;
                }
            }
            ++P;
        }
    }
    g.resize(P);

    if (g.empty()) {
        return std::numeric_limits<float>::infinity();
    }

    //Precompute test with exponentials
    std::vector<Eigen::VectorXf> test;
    if (computeRatios) {
        test.assign(256, Eigen::VectorXf::Zero(N+1));
        for (std::size_t i = 0; i < 256; ++i) {
            test[i][0] = 1.f;
            for (std::size_t n = 1; n <= N; ++n) {
                test[i][n] = (i / 255.f) * test[i][n-1];
            }
        }
    }

    //Precompute M with exponentials
    std::vector<std::vector<std::vector<float>>> M(P,
                                                   std::vector<std::vector<float>>(Q,
                                                                                std::vector<float>(N+1, 0.f)));
    for (std::size_t p = 0; p < P; ++p) {
        for (std::size_t q = 0; q < Q; ++q) {
            M[p][q][0] = 1.f;
            for (std::size_t n = 1; n <= N; ++n) {
                M[p][q][n] = g[p][q] * M[p][q][n-1];
            }
        }
    }

    std::vector<std::vector<std::vector<std::vector<float>>>> d(P,
                                                                std::vector<std::vector<std::vector<float>>>(Q,
                                                                            std::vector<std::vector<float>>(Q,
                                                                                std::vector<float>(N+1, 1.f))));
    Eigen::MatrixXf A = Eigen::MatrixXf::Zero(N, N);
    Eigen::VectorXf x, b = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf c(N+1), prev_c = Eigen::VectorXf::Zero(N+1);

    std::size_t iter = 0;

    do {
        //Compute d
        for (std::size_t p = 0; p < P; ++p) {
            for (std::size_t q1 = 0; q1 < Q; ++q1) {
                for (std::size_t q2 = 0; q2 < Q; ++q2) {
                    d[p][q1][q2].assign(N+1, 0.f);
                    if (q2 != q1) {
                        for (std::size_t n = 0; n <= N; ++n) {
                            if (g[p][q1] >= 0.f && g[p][q2] >= 0.f) {
                                d[p][q1][q2][n] = M[p][q1][n] - R[q1][q2] * M[p][q2][n];
                            } else {
                                d[p][q1][q2][n] = 0.f;
                            }
                        }
                    }
                }
            }
        }

        //Build the matrix A of the linear system
        A.setZero(N, N);
        for (std::size_t i = 0; i < N; ++i) {
            for (std::size_t j = 0; j < N; ++j) {
                for (std::size_t p = 0; p < P; ++p) {
                    for (std::size_t q1 = 0; q1 < Q; ++q1) {
                        for (std::size_t q2 = 0; q2 < Q; ++q2) {
                            if (q2 != q1) {
                                A(i, j) += d[p][q1][q2][i] * (d[p][q1][q2][j] - d[p][q1][q2][N]);
                            }
                        }
                    }
                }
            }
        }

        //Build the vector of knowns b
        b.setZero(N);
        for (std::size_t i = 0; i < N; ++i) {
            for (std::size_t p = 0; p < P; ++p) {
                for (std::size_t q1 = 0; q1 < Q; ++q1) {
                    for (std::size_t q2 = 0; q2 < Q; ++q2) {
                        if (q2 != q1) {
                            b(i) -= Mmax * d[p][q1][q2][i] * d[p][q1][q2][N];
                        }
                    }
                }
            }
        }

        //Solve the linear system
        x = A.partialPivLu().solve(b);
        c << x, Mmax - x.sum();

        if (computeRatios) {
            //Evaluate approximation increment
            eval = std::numeric_limits<float>::lowest();
            for (const Eigen::VectorXf &_M : test) {
                val = std::abs((c - prev_c).dot(_M));
                if (val > eval) {
                    eval = val;
                }
            }

            //Update R
            for (std::size_t q1 = 0; q1 < Q; ++q1) {
                for (std::size_t q2 = 0; q2 < Q; ++q2) {
                    R[q1][q2] = 0.f;
                    tmp1 = 0.f;
                    tmp2 = 0.f;
                    for (std::size_t p = 0; p < P; ++p) {
                        if (g[p][q1] >= 0.f && g[p][q2] >= 0.f) {
                            for (std::size_t n = 0; n <= N; ++n) {
                                tmp1 += c[n] * M[p][q1][n];
                                tmp2 += c[n] * M[p][q2][n];
                            }
                        }
                    }
                    R[q1][q2] += tmp1 / tmp2;
                }
            }

            ++iter;
        }
    } while (computeRatios && eval > eps && iter < max_iterations);

    for (std::size_t n = 0; n <= N; ++n) {
        coefficients[n] = c[n];
    }

    //Evaluate error
    eval = 0.f;
    for (std::size_t q1 = 0; q1 < Q; ++q1) {
        for (std::size_t q2 = 0; q2 < Q; ++q2) {
            if (q2 != q1) {
                for (std::size_t p = 0; p < P; ++p) {
                    if (g[p][q1] >= 0.f && g[p][q2] >= 0.f) {
                        val = 0.f;
                        for (std::size_t n = 0; n <= N; ++n) {
                            val += coefficients[n] * (M[p][q1][n] - R[q1][q2] * M[p][q2][n]);
                        }
                        eval += val * val;
                    }
                }
            }
        }
    }

    return eval;
#else
    return -1.0f;
#endif
}

}

#endif // PIC_ALGORITHMS_MITSUNAGA_NAYAR_CRF_HPP


#pragma once

#include <armadillo>

#include "heattransfer/radial.hpp"

/*!
 * \brief Implements 1d radial unsteady heat transfer.
 *
 * This is documented in Jan Fredrik Helgaker's PhD thesis and in
 *  - Sensitivity of pipeline gas flow model to the selection of the equation of state
 *  - Transient flow in natural gas pipeline - The effect of pipeline thermal model
 */
class UnsteadyHeatTransfer : public RadialHeatTransfer
{
public:
    /*!
     * \brief Construct from complete description of pipeline and surroundings.
     * \param diameter Inner diameter [m]
     * \param pipeWall PipeWall instance
     * \param burialDepth Distance from top of pipe to top of burial medium [m]
     * \param burialMedium BurialMedium instance
     * \param ambientFluid AmbientFluid instanc.
     */
    UnsteadyHeatTransfer(
            const double diameter,
            const PipeWall& pipeWall,
            const double burialDepth,
            const BurialMedium& burialMedium,
            const AmbientFluid& ambientFluid);

    /*!
     * \brief Evaluate 1d radial unsteady heat transfer model.
     *
     * Operates on a HeatTransferState and returns a new HeatTransferState.
     * Requires the discretization temperature HeatTransferState::m_temperature.
     *
     * \param current Current HeatTransferState
     * \param timeStep Time step [s]
     * \param ambientTemperature Ambient temperature [K]
     * \param gasPressure Gas pressure [Pa]
     * \param gasTemperature Gas temperature [K]
     * \param gasReynoldsNumber Reynolds number of gas [-]
     * \param gasHeatCapacity Gas heat capacity (\f$c_p\f$) [J/(kg K)]
     * \param gasViscosity Gas dynamic viscosity [Pa s] = [kg/m*s]
     * \return HeatTransferState with new heat flux.
     */
    virtual HeatTransferState evaluate(
            const HeatTransferState& current,
            const double timeStep,
            const double ambientTemperature,
            const double gasPressure,
            const double gasTemperature,
            const double gasReynoldsNumber,
            const double gasHeatCapacity,
            const double gasViscosity) const override;

    /*!
     * \brief Internal method used for evaluating the unsteady heat transfer model.
     *
     * This is exposed for testing purposes. We typically use pointers anyway,
     * so this is not accessible without casting to UnsteadyHeatTransfer.
     *
     * \param shellTemperature The temperature of each discretization layer [K]
     * \param timeStep Time step [s]
     * \param ambientTemperature Ambient temperature [K]
     * \param gasPressure Gas pressure [Pa]
     * \param gasTemperature Gas temperature [K]
     * \param gasReynoldsNumber Reynolds number of gas [-]
     * \param gasHeatCapacity Gas heat capacity (\f$c_p\f$) [J/(kg K)]
     * \param gasViscosity Gas dynamic viscosity [Pa s] = [kg/m*s]
     * \return HeatTransferState with new heat flux.
     */
    HeatTransferState evaluateInternal(
            const arma::vec& shellTemperature,
            const double timeStep,
            const double ambientTemperature,
            const double gasPressure,
            const double gasTemperature,
            const double gasReynoldsNumber,
            const double gasHeatCapacity,
            const double gasViscosity) const;

    /*!
     * \brief Thermalize the unsteady heat transfer model to steady state.
     *
     * This just performs an infinite time step to find the temperature
     * distribution in the discretization layers at steady state.
     * This could also be done analytically, if we had the analytic solution.
     *
     * \param ambientTemperature Ambient temperature [K]
     * \param gasPressure Gas pressure [Pa]
     * \param gasTemperature Gas temperature [K]
     * \param gasReynoldsNumber Reynolds number of gas [-]
     * \param gasHeatCapacity Gas heat capacity (\f$c_p\f$) [J/(kg K)]
     * \param gasViscosity Gas dynamic viscosity [Pa s] = [kg/m*s]
     * \return HeatTransferState with new heat flux.
     */
    HeatTransferState thermalizeToSteadyState(
            const double ambientTemperature,
            const double gasPressure,
            const double gasTemperature,
            const double gasReynoldsNumber,
            const double gasHeatCapacity,
            const double gasViscosity) const;

private:
    //! Heat transfer coefficient for each discretization layer.
    //! This is the k_i from eq. (2.26) in JFH PhD thesis.
    arma::vec m_heatTransferCoefficient;

    /*!
     * \brief Internal (private) method used for solving the equations in the 1d
     * radial unsteady heat transfer model.
     *
     * \param shellTemperature The temperature of each discretization layer [K]
     * \param timeStep Time step [s]
     * \param ambientTemperature Ambient temperature [K]
     * \param gasPressure Gas pressure [Pa]
     * \param gasTemperature Gas temperature [K]
     * \param gasReynoldsNumber Reynolds number of gas [-]
     * \param gasHeatCapacity Gas heat capacity (\f$c_p\f$) [J/(kg K)]
     * \param gasViscosity Gas dynamic viscosity [Pa s] = [kg/m*s]
     * \return HeatTransferState with new heat flux.
     */
    arma::vec solveEquations(
            const arma::vec& shellTemperature,
            const double timeStep,
            const double gasPressure,
            const double gasTemperature,
            const double ambientTemperature,
            const double gasReynoldsNumber,
            const double gasHeatCapacity,
            const double gasViscosity) const;
};


//=======================================================================
// Copyright 1997-2001 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/graph_utility.hpp>
/*
  Sample output:
  A directed graph:
  a --> b f h
  b --> c a
  c --> d b
  d --> e
  e --> d
  f --> g
  g --> f d
  h --> i
  i --> h j e c
  j -->

  Total number of components: 4
  Vertex a is in component 3
  Vertex b is in component 3
  Vertex c is in component 3
  Vertex d is in component 0
  Vertex e is in component 0
  Vertex f is in component 1
  Vertex g is in component 1
  Vertex h is in component 3
  Vertex i is in component 3
  Vertex j is in component 2
 */

int main(int, char*[])
{
  using namespace boost;
  const char* name = "abcdefghij";

  GraphvizDigraph G;
  read_graphviz("scc.dot", G);

  std::cout << "A directed graph:" << std::endl;
  print_graph(G, name);
  std::cout << std::endl;

  typedef graph_traits<GraphvizGraph>::vertex_descriptor Vertex;

  std::vector<int> component(num_vertices(G)), discover_time(num_vertices(G));
  std::vector<default_color_type> color(num_vertices(G));
  std::vector<Vertex> root(num_vertices(G));
  int num = strong_components(G, &component[0],
                              root_map(&root[0]).
                              color_map(&color[0]).
                              discover_time_map(&discover_time[0]));

  std::cout << "Total number of components: " << num << std::endl;
  std::vector<int>::size_type i;
  for (i = 0; i != component.size(); ++i)
    std::cout << "Vertex " << name[i]
         <<" is in component " << component[i] << std::endl;

  return 0;
}


//=======================================================================
// Copyright 2000 University of Notre Dame.
// Authors: Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef EDMUNDS_KARP_MAX_FLOW_HPP
#define EDMUNDS_KARP_MAX_FLOW_HPP

#include <boost/config.hpp>
#include <vector>
#include <algorithm> // for std::min and std::max
#include <boost/config.hpp>
#include <boost/pending/queue.hpp>
#include <boost/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/breadth_first_search.hpp>

namespace boost {

  // The "labeling" algorithm from "Network Flows" by Ahuja, Magnanti,
  // Orlin.  I think this is the same as or very similar to the original
  // Edmunds-Karp algorithm.  This solves the maximum flow problem.

  namespace detail {

    template <class Graph, class ResCapMap>
    filtered_graph<Graph, is_residual_edge<ResCapMap> >
    residual_graph(Graph& g, ResCapMap residual_capacity) {
      return filtered_graph<Graph, is_residual_edge<ResCapMap> >
        (g, is_residual_edge<ResCapMap>(residual_capacity));
    }

    template <class Graph, class PredEdgeMap, class ResCapMap,
              class RevEdgeMap>
    inline void
    augment(Graph& g,
            typename graph_traits<Graph>::vertex_descriptor src,
            typename graph_traits<Graph>::vertex_descriptor sink,
            PredEdgeMap p,
            ResCapMap residual_capacity,
            RevEdgeMap reverse_edge)
    {
      typename graph_traits<Graph>::edge_descriptor e;
      typename graph_traits<Graph>::vertex_descriptor u;
      typedef typename property_traits<ResCapMap>::value_type FlowValue;

      // find minimum residual capacity along the augmenting path
      FlowValue delta = (std::numeric_limits<FlowValue>::max)();
      e = p[sink];
      do {
        BOOST_USING_STD_MIN();
        delta = min BOOST_PREVENT_MACRO_SUBSTITUTION(delta, residual_capacity[e]);
        u = source(e, g);
        e = p[u];
      } while (u != src);

      // push delta units of flow along the augmenting path
      e = p[sink];
      do {
        residual_capacity[e] -= delta;
        residual_capacity[reverse_edge[e]] += delta;
        u = source(e, g);
        e = p[u];
      } while (u != src);
    }

  } // namespace detail

  template <class Graph,
            class CapacityEdgeMap, class ResidualCapacityEdgeMap,
            class ReverseEdgeMap, class ColorMap, class PredEdgeMap>
  typename property_traits<CapacityEdgeMap>::value_type
  edmunds_karp_max_flow
    (Graph& g,
     typename graph_traits<Graph>::vertex_descriptor src,
     typename graph_traits<Graph>::vertex_descriptor sink,
     CapacityEdgeMap cap,
     ResidualCapacityEdgeMap res,
     ReverseEdgeMap rev,
     ColorMap color,
     PredEdgeMap pred)
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;

    typename graph_traits<Graph>::vertex_iterator u_iter, u_end;
    typename graph_traits<Graph>::out_edge_iterator ei, e_end;
    for (tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
      for (tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
        res[*ei] = cap[*ei];

    color[sink] = Color::gray();
    while (color[sink] != Color::white()) {
      boost::queue<vertex_t> Q;
      breadth_first_search
        (detail::residual_graph(g, res), src, Q,
         make_bfs_visitor(record_edge_predecessors(pred, on_tree_edge())),
         color);
      if (color[sink] != Color::white())
        detail::augment(g, src, sink, pred, res, rev);
    } // while

    typename property_traits<CapacityEdgeMap>::value_type flow = 0;
    for (tie(ei, e_end) = out_edges(src, g); ei != e_end; ++ei)
      flow += (cap[*ei] - res[*ei]);
    return flow;
  } // edmunds_karp_max_flow()

  namespace detail {
    //-------------------------------------------------------------------------
    // Handle default for color property map

    // use of class here is a VC++ workaround
    template <class ColorMap>
    struct edmunds_karp_dispatch2 {
      template <class Graph, class PredMap, class P, class T, class R>
      static typename edge_capacity_value<Graph, P, T, R>::type
      apply
      (Graph& g,
       typename graph_traits<Graph>::vertex_descriptor src,
       typename graph_traits<Graph>::vertex_descriptor sink,
       PredMap pred,
       const bgl_named_params<P, T, R>& params,
       ColorMap color)
      {
        return edmunds_karp_max_flow
          (g, src, sink,
           choose_const_pmap(get_param(params, edge_capacity), g, edge_capacity),
           choose_pmap(get_param(params, edge_residual_capacity),
                       g, edge_residual_capacity),
           choose_const_pmap(get_param(params, edge_reverse), g, edge_reverse),
           color, pred);
      }
    };
    template<>
    struct edmunds_karp_dispatch2<detail::error_property_not_found> {
      template <class Graph, class PredMap, class P, class T, class R>
      static typename edge_capacity_value<Graph, P, T, R>::type
      apply
      (Graph& g,
       typename graph_traits<Graph>::vertex_descriptor src,
       typename graph_traits<Graph>::vertex_descriptor sink,
       PredMap pred,
       const bgl_named_params<P, T, R>& params,
       detail::error_property_not_found)
      {
        typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
        typedef typename graph_traits<Graph>::vertices_size_type size_type;
        size_type n = is_default_param(get_param(params, vertex_color)) ?
          num_vertices(g) : 1;
        std::vector<default_color_type> color_vec(n);
        return edmunds_karp_max_flow
          (g, src, sink,
           choose_const_pmap(get_param(params, edge_capacity), g, edge_capacity),
           choose_pmap(get_param(params, edge_residual_capacity),
                       g, edge_residual_capacity),
           choose_const_pmap(get_param(params, edge_reverse), g, edge_reverse),
           make_iterator_property_map(color_vec.begin(), choose_const_pmap
                                      (get_param(params, vertex_index),
                                       g, vertex_index), color_vec[0]),
           pred);
      }
    };

    //-------------------------------------------------------------------------
    // Handle default for predecessor property map

    // use of class here is a VC++ workaround
    template <class PredMap>
    struct edmunds_karp_dispatch1 {
      template <class Graph, class P, class T, class R>
      static typename edge_capacity_value<Graph, P, T, R>::type
      apply(Graph& g,
            typename graph_traits<Graph>::vertex_descriptor src,
            typename graph_traits<Graph>::vertex_descriptor sink,
            const bgl_named_params<P, T, R>& params,
            PredMap pred)
      {
        typedef typename property_value< bgl_named_params<P,T,R>, vertex_color_t>::type C;
        return edmunds_karp_dispatch2<C>::apply
          (g, src, sink, pred, params, get_param(params, vertex_color));
      }
    };
    template<>
    struct edmunds_karp_dispatch1<detail::error_property_not_found> {

      template <class Graph, class P, class T, class R>
      static typename edge_capacity_value<Graph, P, T, R>::type
      apply
      (Graph& g,
       typename graph_traits<Graph>::vertex_descriptor src,
       typename graph_traits<Graph>::vertex_descriptor sink,
       const bgl_named_params<P, T, R>& params,
       detail::error_property_not_found)
      {
        typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
        typedef typename graph_traits<Graph>::vertices_size_type size_type;
        size_type n = is_default_param(get_param(params, vertex_predecessor)) ?
          num_vertices(g) : 1;
        std::vector<edge_descriptor> pred_vec(n);

        typedef typename property_value< bgl_named_params<P,T,R>, vertex_color_t>::type C;
        return edmunds_karp_dispatch2<C>::apply
          (g, src, sink,
           make_iterator_property_map(pred_vec.begin(), choose_const_pmap
                                      (get_param(params, vertex_index),
                                       g, vertex_index), pred_vec[0]),
           params,
           get_param(params, vertex_color));
      }
    };

  } // namespace detail

  template <class Graph, class P, class T, class R>
  typename detail::edge_capacity_value<Graph, P, T, R>::type
  edmunds_karp_max_flow
    (Graph& g,
     typename graph_traits<Graph>::vertex_descriptor src,
     typename graph_traits<Graph>::vertex_descriptor sink,
     const bgl_named_params<P, T, R>& params)
  {
    typedef typename property_value< bgl_named_params<P,T,R>, vertex_predecessor_t>::type Pred;
    return detail::edmunds_karp_dispatch1<Pred>::apply
      (g, src, sink, params, get_param(params, vertex_predecessor));
  }

  template <class Graph>
  typename property_traits<
    typename property_map<Graph, edge_capacity_t>::const_type
  >::value_type
  edmunds_karp_max_flow
    (Graph& g,
     typename graph_traits<Graph>::vertex_descriptor src,
     typename graph_traits<Graph>::vertex_descriptor sink)
  {
    bgl_named_params<int, buffer_param_t> params(0);
    return edmunds_karp_max_flow(g, src, sink, params);
  }

} // namespace boost

#endif // EDMUNDS_KARP_MAX_FLOW_HPP


/// \file
// --------------------------------------------------------------------------
// This file is part of the reference implementation for the paper
//    QFib: Fast and Efficient Brain Tractogram Compression
//    C. Mercier*, S. Rousseau*, P. Gori, I. Bloch and T. Boubekeur
//    NeuroInformatics 2020
//    DOI: 10.1007/s12021-020-09452-0
//
// All rights reserved. Use of this source code is governed by a
// MIT license that can be found in the LICENSE file.
// --------------------------------------------------------------------------
#pragma once

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

#include "saveload.hpp"
#include <vector>

namespace utl
{
///
/// \brief meanMaxError Function to compute average and maximum error between two sets of fibers containing the exact same number of points and fibers
/// \param fibersA Vector containing the first set of fibers
/// \param fibersB Vector containing the second set of fibers
/// \param meanError Average error
/// \param maxError Maximum error
/// \param totalNbPts Variable containing the total number of points in the bundle, used for average error computation
///

void meanMaxError(const std::vector< std::vector<Vector3f> > & fibersA, const std::vector< std::vector<Vector3f> > & fibersB, double & meanError, float & maxError, uint64_t & totalNbPts)
{
	meanError = 0;
	maxError = 0.f;
	assert(fibersA.size() == fibersB.size());
	for(unsigned f = 0; f < fibersA.size(); ++f)
	{
		assert(fibersA[f].size() == fibersB[f].size());
		for(unsigned p = 0; p < fibersA[f].size(); ++p)
		{
			float err = (fibersA[f][p] - fibersB[f][p]).norm();
			maxError = err > maxError ? err : maxError;
			meanError  += err / double(totalNbPts);
		}
	}
}

///
/// \brief error Function to compute the error between two sets of fibers with the exact same number of points and fibers
/// This error is computed depending on the length of the fibers
/// \param fibersA Vector containing the first set of fibers
/// \param fibersB Vector containing the second set of fibers
/// \param minError Minimum error
/// \param meanError Average error
/// \param maxError Maximum error
/// \param minEndPointError Minimum error on every end point
/// \param meanEndPointError Average error on every end point
/// \param maxEndPointError Maximum error on every end point
/// \param totalNbPts Variable containing the total number of points in the bundle, used for average error computation
/// \param steps Number of length separation to perform
/// \param minValue Minimum length to consider
/// \param maxValue Maximum length to consider
///

void error(const std::vector<std::vector<Vector3f> > & fibersA, const std::vector< std::vector<Vector3f> > & fibersB,
		   vector<float>& minError, vector<double>& meanError, vector<float>& maxError,
		   float & minEndPointError, float & meanEndPointError, float & maxEndPointError, uint64_t & totalNbPts,
		   unsigned steps = 10, float minValue = 35, float maxValue = 260)
{
	minEndPointError = std::numeric_limits<float>::max();
	meanEndPointError = 0.f;
	maxEndPointError = 0.f;
	unsigned nbSteps = (maxValue - minValue)/steps;
	minError.resize(nbSteps, std::numeric_limits<float>::max());
	meanError.resize(nbSteps, 0);
	maxError.resize(nbSteps, 0);

	unsigned nbEndPoints = fibersA.size();
	assert(fibersA.size() == fibersB.size());
	for(unsigned f = 0; f < fibersA.size(); ++f)
	{
		unsigned tablePos = (float)((fibersA[f].size() - 1) * (fibersA[f][1]-fibersA[f][0]).norm() - minValue) / (float)(maxValue - minValue) * (nbSteps-1);
		if (tablePos>nbSteps-1) tablePos = nbSteps - 1;
		assert(fibersA[f].size() == fibersB[f].size());
		for(unsigned p = 0; p < fibersA[f].size()-1; ++p)
		{
			float err = (fibersA[f][p] - fibersB[f][p]).norm();
			meanError[tablePos]  += err / double(totalNbPts);
			maxError[tablePos] = err > maxError[tablePos] ? err : maxError[tablePos];
			minError[tablePos] = err < minError[tablePos] ? err : minError[tablePos];
		}
		float err = (fibersA[f][fibersA[f].size()-1] - fibersB[f][fibersA[f].size()-1]).norm();
		meanError[tablePos]  += err / double(totalNbPts);
		maxError[tablePos] = err > maxError[tablePos] ? err : maxError[tablePos];
		minError[tablePos] = err < minError[tablePos] ? err : minError[tablePos];
		minEndPointError = err < minEndPointError ? err : minEndPointError;
		meanEndPointError += err / double(nbEndPoints);
		maxEndPointError = err > maxEndPointError ? err : maxEndPointError;
	}
}
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_DETAIL_SCALAR_D_EXPO_REDUCTION_HPP_INCLUDED
#define BOOST_SIMD_ARCH_DETAIL_SCALAR_D_EXPO_REDUCTION_HPP_INCLUDED

#include <boost/simd/arch/detail/scalar/horner.hpp>
#include <boost/simd/constant/invlog10_2.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/constant/log10_2hi.hpp>
#include <boost/simd/constant/log10_2lo.hpp>
#include <boost/simd/constant/log_10.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/log_2hi.hpp>
#include <boost/simd/constant/log_2lo.hpp>
#include <boost/simd/constant/maxlog.hpp>
#include <boost/simd/constant/maxlog10.hpp>
#include <boost/simd/constant/maxlog2.hpp>
#include <boost/simd/constant/minlog.hpp>
#include <boost/simd/constant/minlog10.hpp>
#include <boost/simd/constant/minlog2.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/simd/fma.hpp>
#include <boost/simd/function/simd/fnms.hpp>
#include <boost/simd/function/simd/inc.hpp>
#include <boost/simd/function/simd/oneminus.hpp>
#include <boost/simd/function/simd/oneplus.hpp>
#include <boost/simd/function/simd/round2even.hpp>
#include <boost/simd/function/simd/sqr.hpp>
#include <boost/simd/logical.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

    template < class A0> struct exp_reduction < A0, natural_tag, double>
    {
      using l_t = logical<A0>;
      static BOOST_FORCEINLINE l_t isgemaxlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 >= Maxlog<A0>());
      }

      static BOOST_FORCEINLINE l_t isleminlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 <= Minlog<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce( A0 a0
                                        , A0& hi, A0& lo, A0& x) BOOST_NOEXCEPT
      {
        A0 k = round2even(Invlog_2<A0>()*a0);
        hi = fnms(k, Log_2hi<A0>(), a0); //a0-k*L
        lo = k*Log_2lo<A0>();
        x  = hi-lo;
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 x) BOOST_NOEXCEPT
      {
        A0 const t = sqr(x);
        return fnms(t, horner<BOOST_SIMD_HORNER_COEFF_T(A0, 5,
                                                        ( 0x3e66376972bea4d0ull
                                                        , 0xbebbbd41c5d26bf1ull
                                                        , 0x3f11566aaf25de2cull
                                                        , 0xbf66c16c16bebd93ull
                                                        , 0x3fc555555555553eull
                                                        )
                                                       )>(t), x); //x-h*t
      }

      static BOOST_FORCEINLINE A0 finalize(A0 x, A0 c, A0 hi, A0 lo) BOOST_NOEXCEPT
      {
        return oneminus(((lo-(x*c)/(Two<A0>()-c))-hi));
      }

    };

    template < class A0 > struct exp_reduction < A0, two_tag, double>
    {
      using l_t = logical<A0>;
      static BOOST_FORCEINLINE l_t isgemaxlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 >= Maxlog2<A0>());
      }

      static BOOST_FORCEINLINE l_t isleminlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 <= Minlog2<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce(A0 a0, A0, A0, A0& x) BOOST_NOEXCEPT
      {
        A0 k = round2even(a0);
        x = (a0 - k)*Log_2<A0>();
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 x) BOOST_NOEXCEPT
      {
        const A0 t =  sqr(x);
        return fnms(t, horner<BOOST_SIMD_HORNER_COEFF_T(A0, 5,
                                                        ( 0x3e66376972bea4d0ull
                                                        , 0xbebbbd41c5d26bf1ull
                                                        , 0x3f11566aaf25de2cull
                                                        , 0xbf66c16c16bebd93ull
                                                        , 0x3fc555555555553eull
                                                        )
                                                       )> (t), x); //x-h*t
      }

      static BOOST_FORCEINLINE A0 finalize( A0 x, A0 c, A0, A0& ) BOOST_NOEXCEPT
      {
        return oneminus(((-(x*c)/(Two<A0>()-c))-x));
      }
    };

    template < class A0 > struct exp_reduction < A0, ten_tag, double>
    {
      using l_t = logical<A0>;
      static BOOST_FORCEINLINE l_t isgemaxlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 >= Maxlog10<A0>());
      }

      static BOOST_FORCEINLINE l_t isleminlog(A0 a0) BOOST_NOEXCEPT
      {
        return (a0 <= Minlog10<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce(A0 a0, A0&, A0&, A0& x) BOOST_NOEXCEPT
      {
        A0 k  = round2even(Invlog10_2<A0>()*a0);
        x = fnms(k, Log10_2hi<A0>(), a0);
        x = fnms(k, Log10_2lo<A0>(), x);
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 x) BOOST_NOEXCEPT
      {
        A0 xx = sqr(x);
        A0 px = x*horner<BOOST_SIMD_HORNER_COEFF_T(A0, 4,
                                                   (0x3fa4fd75f3062dd4ull,
                                                    0x40277d9474c55934ull,
                                                    0x40796b7a050349e4ull,
                                                    0x40a2b4798e134a01ull)
                                                  )> (xx);
        A0 x2 =  px/(horner<BOOST_SIMD_HORNER_COEFF_T(A0, 4,
                                                      (0x3ff0000000000000ull,
                                                       0x405545fdce51ca08ull,
                                                       0x4093e05eefd67782ull,
                                                       0x40a03f37650df6e2ull)
                                                     )> (xx)-px);
        return oneplus(x2+x2);
      }

      static BOOST_FORCEINLINE A0 finalize(A0, A0 c, A0,  A0 ) BOOST_NOEXCEPT
      {
        return c;
      }
    };
  }
} }
#endif