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

#ifndef BOOST_MATH_INTERPOLATORS_CARDINAL_CUBIC_B_SPLINE_DETAIL_HPP
#define BOOST_MATH_INTERPOLATORS_CARDINAL_CUBIC_B_SPLINE_DETAIL_HPP

#include <limits>
#include <cmath>
#include <vector>
#include <memory>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

namespace boost{ namespace math{ namespace interpolators{ namespace detail{

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

    Real operator()(Real x) const;

    Real prime(Real x) const;

    Real double_prime(Real x) const;

private:
    std::vector<Real> m_beta;
    Real m_h_inv;
    Real m_a;
    Real m_avg;
};

template <class Real>
Real b3_spline(Real x)
{
    using std::abs;
    Real absx = abs(x);
    if (absx < 1)
    {
        Real y = 2 - absx;
        Real z = 1 - absx;
        return boost::math::constants::sixth<Real>()*(y*y*y - 4*z*z*z);
    }
    if (absx < 2)
    {
        Real y = 2 - absx;
        return boost::math::constants::sixth<Real>()*y*y*y;
    }
    return (Real) 0;
}

template<class Real>
Real b3_spline_prime(Real x)
{
    if (x < 0)
    {
        return -b3_spline_prime(-x);
    }

    if (x < 1)
    {
        return x*(3*boost::math::constants::half<Real>()*x - 2);
    }
    if (x < 2)
    {
        return -boost::math::constants::half<Real>()*(2 - x)*(2 - x);
    }
    return (Real) 0;
}

template<class Real>
Real b3_spline_double_prime(Real x)
{
    if (x < 0)
    {
        return b3_spline_double_prime(-x);
    }

    if (x < 1)
    {
        return 3*x - 2;
    }
    if (x < 2)
    {
        return (2 - x);
    }
    return (Real) 0;
}

template <class Real>
template <class BidiIterator>
cardinal_cubic_b_spline_imp<Real>::cardinal_cubic_b_spline_imp(BidiIterator f, BidiIterator end_p, Real left_endpoint, Real step_size,
                                             Real left_endpoint_derivative, Real right_endpoint_derivative) : m_a(left_endpoint), m_avg(0)
{
    using boost::math::constants::third;

    std::size_t length = end_p - f;

    if (length < 5)
    {
        if (boost::math::isnan(left_endpoint_derivative) || boost::math::isnan(right_endpoint_derivative))
        {
            throw std::logic_error("Interpolation using a cubic b spline with derivatives estimated at the endpoints requires at least 5 points.\n");
        }
        if (length < 3)
        {
            throw std::logic_error("Interpolation using a cubic b spline requires at least 3 points.\n");
        }
    }

    if (boost::math::isnan(left_endpoint))
    {
        throw std::logic_error("Left endpoint is NAN; this is disallowed.\n");
    }
    if (left_endpoint + length*step_size >= (std::numeric_limits<Real>::max)())
    {
        throw std::logic_error("Right endpoint overflows the maximum representable number of the specified precision.\n");
    }
    if (step_size <= 0)
    {
        throw std::logic_error("The step size must be strictly > 0.\n");
    }
    // Storing the inverse of the stepsize does provide a measurable speedup.
    // It's not huge, but nonetheless worthwhile.
    m_h_inv = 1/step_size;

    // Following Kress's notation, s'(a) = a1, s'(b) = b1
    Real a1 = left_endpoint_derivative;
    // See the finite-difference table on Wikipedia for reference on how
    // to construct high-order estimates for one-sided derivatives:
    // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Forward_and_backward_finite_difference
    // Here, we estimate then to O(h^4), as that is the maximum accuracy we could obtain from this method.
    if (boost::math::isnan(a1))
    {
        // For simple functions (linear, quadratic, so on)
        // almost all the error comes from derivative estimation.
        // This does pairwise summation which gives us another digit of accuracy over naive summation.
        Real t0 = 4*(f[1] + third<Real>()*f[3]);
        Real t1 = -(25*third<Real>()*f[0] + f[4])/4  - 3*f[2];
        a1 = m_h_inv*(t0 + t1);
    }

    Real b1 = right_endpoint_derivative;
    if (boost::math::isnan(b1))
    {
        size_t n = length - 1;
        Real t0 = 4*(f[n-3] + third<Real>()*f[n - 1]);
        Real t1 = -(25*third<Real>()*f[n - 4] + f[n])/4  - 3*f[n - 2];

        b1 = m_h_inv*(t0 + t1);
    }

    // s(x) = \sum \alpha_i B_{3}( (x- x_i - a)/h )
    // Of course we must reindex from Kress's notation, since he uses negative indices which make C++ unhappy.
    m_beta.resize(length + 2, std::numeric_limits<Real>::quiet_NaN());

    // Since the splines have compact support, they decay to zero very fast outside the endpoints.
    // This is often very annoying; we'd like to evaluate the interpolant a little bit outside the
    // boundary [a,b] without massive error.
    // A simple way to deal with this is just to subtract the DC component off the signal, so we need the average.
    // This algorithm for computing the average is recommended in
    // http://www.heikohoffmann.de/htmlthesis/node134.html
    Real t = 1;
    for (size_t i = 0; i < length; ++i)
    {
        if (boost::math::isnan(f[i]))
        {
            std::string err = "This function you are trying to interpolate is a nan at index " + std::to_string(i) + "\n";
            throw std::logic_error(err);
        }
        m_avg += (f[i] - m_avg) / t;
        t += 1;
    }

    // Now we must solve an almost-tridiagonal system, which requires O(N) operations.
    // There are, in fact 5 diagonals, but they only differ from zero on the first and last row,
    // so we can patch up the tridiagonal row reduction algorithm to deal with two special rows.
    // See Kress, equations 8.41
    // The the "tridiagonal" matrix is:
    // 1  0 -1
    // 1  4  1
    //    1  4  1
    //       1  4  1
    //          ....
    //          1  4  1
    //          1  0 -1
    // Numerical estimate indicate that as N->Infinity, cond(A) -> 6.9, so this matrix is good.
    std::vector<Real> rhs(length + 2, std::numeric_limits<Real>::quiet_NaN());
    std::vector<Real> super_diagonal(length + 2, std::numeric_limits<Real>::quiet_NaN());

    rhs[0] = -2*step_size*a1;
    rhs[rhs.size() - 1] = -2*step_size*b1;

    super_diagonal[0] = 0;

    for(size_t i = 1; i < rhs.size() - 1; ++i)
    {
        rhs[i] = 6*(f[i - 1] - m_avg);
        super_diagonal[i] = 1;
    }

    // One step of row reduction on the first row to patch up the 5-diagonal problem:
    // 1 0 -1 | r0
    // 1 4 1  | r1
    // mapsto:
    // 1 0 -1 | r0
    // 0 4 2  | r1 - r0
    // mapsto
    // 1 0 -1 | r0
    // 0 1 1/2| (r1 - r0)/4
    super_diagonal[1] = 0.5;
    rhs[1] = (rhs[1] - rhs[0])/4;

    // Now do a tridiagonal row reduction the standard way, until just before the last row:
    for (size_t i = 2; i < rhs.size() - 1; ++i)
    {
        Real diagonal = 4 - super_diagonal[i - 1];
        rhs[i] = (rhs[i] - rhs[i - 1])/diagonal;
        super_diagonal[i] /= diagonal;
    }

    // Now the last row, which is in the form
    // 1 sd[n-3] 0      | rhs[n-3]
    // 0  1     sd[n-2] | rhs[n-2]
    // 1  0     -1      | rhs[n-1]
    Real final_subdiag = -super_diagonal[rhs.size() - 3];
    rhs[rhs.size() - 1] = (rhs[rhs.size() - 1] - rhs[rhs.size() - 3])/final_subdiag;
    Real final_diag = -1/final_subdiag;
    // Now we're here:
    // 1 sd[n-3] 0         | rhs[n-3]
    // 0  1     sd[n-2]    | rhs[n-2]
    // 0  1     final_diag | (rhs[n-1] - rhs[n-3])/diag

    final_diag = final_diag - super_diagonal[rhs.size() - 2];
    rhs[rhs.size() - 1] = rhs[rhs.size() - 1] - rhs[rhs.size() - 2];

    // Back substitutions:
    m_beta[rhs.size() - 1] = rhs[rhs.size() - 1]/final_diag;
    for(size_t i = rhs.size() - 2; i > 0; --i)
    {
        m_beta[i] = rhs[i] - super_diagonal[i]*m_beta[i + 1];
    }
    m_beta[0] = m_beta[2] + rhs[0];
}

template<class Real>
Real cardinal_cubic_b_spline_imp<Real>::operator()(Real x) const
{
    // See Kress, 8.40: Since B3 has compact support, we don't have to sum over all terms,
    // just the (at most 5) whose support overlaps the argument.
    Real z = m_avg;
    Real t = m_h_inv*(x - m_a) + 1;

    using std::max;
    using std::min;
    using std::ceil;
    using std::floor;

    size_t k_min = (size_t) (max)(static_cast<long>(0), boost::math::ltrunc(ceil(t - 2)));
    size_t k_max = (size_t) (max)((min)(static_cast<long>(m_beta.size() - 1), boost::math::ltrunc(floor(t + 2))), (long) 0);

    for (size_t k = k_min; k <= k_max; ++k)
    {
        z += m_beta[k]*b3_spline(t - k);
    }

    return z;
}

template<class Real>
Real cardinal_cubic_b_spline_imp<Real>::prime(Real x) const
{
    Real z = 0;
    Real t = m_h_inv*(x - m_a) + 1;

    using std::max;
    using std::min;
    using std::ceil;
    using std::floor;

    size_t k_min = (size_t) (max)(static_cast<long>(0), boost::math::ltrunc(ceil(t - 2)));
    size_t k_max = (size_t) (min)(static_cast<long>(m_beta.size() - 1), boost::math::ltrunc(floor(t + 2)));

    for (size_t k = k_min; k <= k_max; ++k)
    {
        z += m_beta[k]*b3_spline_prime(t - k);
    }
    return z*m_h_inv;
}

template<class Real>
Real cardinal_cubic_b_spline_imp<Real>::double_prime(Real x) const
{
    Real z = 0;
    Real t = m_h_inv*(x - m_a) + 1;

    using std::max;
    using std::min;
    using std::ceil;
    using std::floor;

    size_t k_min = (size_t) (max)(static_cast<long>(0), boost::math::ltrunc(ceil(t - 2)));
    size_t k_max = (size_t) (min)(static_cast<long>(m_beta.size() - 1), boost::math::ltrunc(floor(t + 2)));

    for (size_t k = k_min; k <= k_max; ++k)
    {
        z += m_beta[k]*b3_spline_double_prime(t - k);
    }
    return z*m_h_inv*m_h_inv;
}

}}}}
#endif


//
// Copyright 2020 Debabrata Mandal <mandaldebabrata123@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_GIL_IMAGE_PROCESSING_HISTOGRAM_MATCHING_HPP
#define BOOST_GIL_IMAGE_PROCESSING_HISTOGRAM_MATCHING_HPP

#include <boost/gil/algorithm.hpp>
#include <boost/gil/histogram.hpp>
#include <boost/gil/image.hpp>

#include <algorithm>
#include <cmath>
#include <map>
#include <vector>

namespace boost { namespace gil {

/////////////////////////////////////////
/// Histogram Matching(HM)
/////////////////////////////////////////
/// \defgroup HM HM
/// \brief Contains implementation and description of the algorithm used to compute
///        global histogram matching of input images.
///
///        Algorithm :-
///        1. Calculate histogram A(pixel) of input image and G(pixel) of reference image.
///        2. Compute the normalized cumulative(CDF) histograms of A and G.
///        3. Match the histograms using transofrmation  => CDF(A(px)) = CDF(G(px'))
///                                                      => px' = Inv-CDF (CDF(px))
///

/// \fn histogram_matching
/// \ingroup HM
/// \tparam SrcKeyType Key Type of input histogram
/// @param src_hist INPUT Input source histogram
/// @param ref_hist INPUT Input reference histogram
/// \brief Overload for histogram matching algorithm, takes in a single source histogram &
///        reference histogram and returns the color map used for histogram matching.
///
template <typename SrcKeyType, typename RefKeyType>
std::map<SrcKeyType, SrcKeyType>
    histogram_matching(histogram<SrcKeyType> const& src_hist, histogram<RefKeyType> const& ref_hist)
{
    histogram<SrcKeyType> dst_hist;
    return histogram_matching(src_hist, ref_hist, dst_hist);
}

/// \overload histogram_matching
/// \ingroup HM
/// \tparam SrcKeyType Key Type of input histogram
/// \tparam RefKeyType Key Type of reference histogram
/// \tparam DstKeyType Key Type of output histogram
/// @param src_hist INPUT source histogram
/// @param ref_hist INPUT reference histogram
/// @param dst_hist OUTPUT Output histogram
/// \brief Overload for histogram matching algorithm, takes in source histogram, reference
///        histogram & destination histogram and returns the color map used for histogram
///        matching as well as transforming the destination histogram.
///
template <typename SrcKeyType, typename RefKeyType, typename DstKeyType>
std::map<SrcKeyType, DstKeyType> histogram_matching(
    histogram<SrcKeyType> const& src_hist,
    histogram<RefKeyType> const& ref_hist,
    histogram<DstKeyType>& dst_hist)
{
    static_assert(
        std::is_integral<SrcKeyType>::value &&
        std::is_integral<RefKeyType>::value &&
        std::is_integral<DstKeyType>::value,
        "Source, Refernce or Destination histogram type is not appropriate.");

    using value_t = typename histogram<SrcKeyType>::value_type;
    dst_hist.clear();
    double src_sum      = src_hist.sum();
    double ref_sum      = ref_hist.sum();
    auto cumltv_srchist = cumulative_histogram(src_hist);
    auto cumltv_refhist = cumulative_histogram(ref_hist);
    std::map<SrcKeyType, RefKeyType> inverse_mapping;

    std::vector<typename histogram<RefKeyType>::key_type> src_keys, ref_keys;
    src_keys             = src_hist.sorted_keys();
    ref_keys             = ref_hist.sorted_keys();
    std::ptrdiff_t start = ref_keys.size() - 1;
    RefKeyType ref_max;
    if (start >= 0)
        ref_max = std::get<0>(ref_keys[start]);

    for (std::ptrdiff_t j = src_keys.size() - 1; j >= 0; --j)
    {
        double src_val = (cumltv_srchist[src_keys[j]] * ref_sum) / src_sum;
        while (cumltv_refhist[ref_keys[start]] > src_val && start > 0)
        {
            start--;
        }
        if (std::abs(cumltv_refhist[ref_keys[start]] - src_val) >
            std::abs(cumltv_refhist(std::min<RefKeyType>(ref_max, std::get<0>(ref_keys[start + 1]))) -
                src_val))
        {
            inverse_mapping[std::get<0>(src_keys[j])] =
                std::min<RefKeyType>(ref_max, std::get<0>(ref_keys[start + 1]));
        }
        else
        {
            inverse_mapping[std::get<0>(src_keys[j])] = std::get<0>(ref_keys[start]);
        }
        if (j == 0)
            break;
    }
    std::for_each(src_hist.begin(), src_hist.end(), [&](value_t const& v) {
        dst_hist[inverse_mapping[std::get<0>(v.first)]] += v.second;
    });
    return inverse_mapping;
}

/// \overload histogram_matching
/// \ingroup HM
/// @param src_view  INPUT source image view
/// @param ref_view  INPUT Reference image view
/// @param dst_view  OUTPUT Output image view
/// @param bin_width INPUT Histogram bin width
/// @param mask      INPUT Specify is mask is to be used
/// @param src_mask  INPUT Mask vector over input image
/// @param ref_mask  INPUT Mask vector over reference image
/// \brief Overload for histogram matching algorithm, takes in both source, reference &
///        destination image views and histogram matches the input image using the
///        reference image.
///
template <typename SrcView, typename ReferenceView, typename DstView>
void histogram_matching(
    SrcView const& src_view,
    ReferenceView const& ref_view,
    DstView const& dst_view,
    std::size_t bin_width = 1,
    bool mask = false,
    std::vector<std::vector<bool>> src_mask = {},
    std::vector<std::vector<bool>> ref_mask = {})
{
    gil_function_requires<ImageViewConcept<SrcView>>();
    gil_function_requires<ImageViewConcept<ReferenceView>>();
    gil_function_requires<MutableImageViewConcept<DstView>>();

    static_assert(
        color_spaces_are_compatible<
            typename color_space_type<SrcView>::type,
            typename color_space_type<ReferenceView>::type>::value,
        "Source and reference view must have same color space");

    static_assert(
        color_spaces_are_compatible<
            typename color_space_type<SrcView>::type,
            typename color_space_type<DstView>::type>::value,
        "Source and destination view must have same color space");

    // Defining channel type
    using source_channel_t = typename channel_type<SrcView>::type;
    using ref_channel_t    = typename channel_type<ReferenceView>::type;
    using dst_channel_t    = typename channel_type<DstView>::type;
    using coord_t          = typename SrcView::x_coord_t;

    std::size_t const channels     = num_channels<SrcView>::value;
    coord_t const width            = src_view.width();
    coord_t const height           = src_view.height();
    source_channel_t src_pixel_min = std::numeric_limits<source_channel_t>::min();
    source_channel_t src_pixel_max = std::numeric_limits<source_channel_t>::max();
    ref_channel_t ref_pixel_min    = std::numeric_limits<ref_channel_t>::min();
    ref_channel_t ref_pixel_max    = std::numeric_limits<ref_channel_t>::max();
    dst_channel_t dst_pixel_min    = std::numeric_limits<dst_channel_t>::min();
    dst_channel_t dst_pixel_max    = std::numeric_limits<dst_channel_t>::max();

    for (std::size_t i = 0; i < channels; i++)
    {
        histogram<source_channel_t> src_histogram;
        histogram<ref_channel_t> ref_histogram;
        fill_histogram(
            nth_channel_view(src_view, i), src_histogram, bin_width, false, false, mask, src_mask,
            std::tuple<source_channel_t>(src_pixel_min),
            std::tuple<source_channel_t>(src_pixel_max), true);
        fill_histogram(
            nth_channel_view(ref_view, i), ref_histogram, bin_width, false, false, mask, ref_mask,
            std::tuple<ref_channel_t>(ref_pixel_min), std::tuple<ref_channel_t>(ref_pixel_max),
            true);
        auto inverse_mapping = histogram_matching(src_histogram, ref_histogram);
        for (std::ptrdiff_t src_y = 0; src_y < height; ++src_y)
        {
            auto src_it = nth_channel_view(src_view, i).row_begin(src_y);
            auto dst_it = nth_channel_view(dst_view, i).row_begin(src_y);
            for (std::ptrdiff_t src_x = 0; src_x < width; ++src_x)
            {
                if (mask && !src_mask[src_y][src_x])
                    dst_it[src_x][0] = src_it[src_x][0];
                else
                    dst_it[src_x][0] =
                        static_cast<dst_channel_t>(inverse_mapping[src_it[src_x][0]]);
            }
        }
    }
}

}}  //namespace boost::gil

#endif


#pragma once
#include "base/assert.hpp"

#include "std/algorithm.hpp"
#include "std/cmath.hpp"
#include "std/functional.hpp"
#include "std/limits.hpp"
#include "std/type_traits.hpp"

#include <boost/integer.hpp>

namespace my
{

template <typename T> inline T Abs(T x)
{
  return (x < 0 ? -x : x);
}

// Compare floats or doubles for almost equality.
// maxULPs - number of closest floating point values that are considered equal.
// Infinity is treated as almost equal to the largest possible floating point values.
// NaN produces undefined result.
// See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
// for details.
template <typename TFloat>
bool AlmostEqualULPs(TFloat x, TFloat y, unsigned int maxULPs = 256)
{
  static_assert(is_floating_point<TFloat>::value, "");
  static_assert(numeric_limits<TFloat>::is_iec559, "");

  // Make sure maxUlps is non-negative and small enough that the
  // default NaN won't compare as equal to anything.
  ASSERT_LESS(maxULPs, 4 * 1024 * 1024, ());

  int const bits = CHAR_BIT * sizeof(TFloat);
  typedef typename boost::int_t<bits>::exact IntType;
  typedef typename boost::uint_t<bits>::exact UIntType;

  IntType xInt = *reinterpret_cast<IntType const *>(&x);
  IntType yInt = *reinterpret_cast<IntType const *>(&y);

  // Make xInt and yInt lexicographically ordered as a twos-complement int
  IntType const highestBit = IntType(1) << (bits - 1);
  if (xInt < 0)
    xInt = highestBit - xInt;
  if (yInt < 0)
    yInt = highestBit - yInt;

  UIntType const diff = Abs(xInt - yInt);

  return diff <= maxULPs;
}

// Returns true if x and y are equal up to the absolute difference eps.
// Does not produce a sensible result if any of the arguments is NaN or infinity.
// The default value for eps is deliberately not provided: the intended usage
// is for the client to choose the precision according to the problem domain,
// explicitly define the precision constant and call this function.
template <typename TFloat>
inline bool AlmostEqualAbs(TFloat x, TFloat y, TFloat eps)
{
  return fabs(x - y) < eps;
}

// Returns true if x and y are equal up to the relative difference eps.
// Does not produce a sensible result if any of the arguments is NaN, infinity or zero.
// The same considerations as in AlmostEqualAbs apply.
template <typename TFloat>
inline bool AlmostEqualRel(TFloat x, TFloat y, TFloat eps)
{
  return fabs(x - y) < eps * max(fabs(x), fabs(y));
}

template <typename TFloat> inline TFloat DegToRad(TFloat deg)
{
  return deg * TFloat(math::pi) / TFloat(180);
}

template <typename TFloat> inline TFloat RadToDeg(TFloat rad)
{
  return rad * TFloat(180) / TFloat(math::pi);
}

template <typename T> inline T id(T const & x)
{
  return x;
}

template <typename T> inline T sq(T const & x)
{
  return x * x;
}

template <typename T, typename TMin, typename TMax>
inline T clamp(T x, TMin xmin, TMax xmax)
{
  if (x > xmax)
    return xmax;
  if (x < xmin)
    return xmin;
  return x;
}

template <typename T> inline bool between_s(T a, T b, T x)
{
  return (a <= x && x <= b);
}
template <typename T> inline bool between_i(T a, T b, T x)
{
  return (a < x && x < b);
}

inline int rounds(double x)
{
  return (x > 0.0 ? int(x + 0.5) : int(x - 0.5));
}

inline size_t SizeAligned(size_t size, size_t align)
{
  // static_cast    .
  return size + (static_cast<size_t>(-static_cast<ptrdiff_t>(size)) & (align - 1));
}

template <typename T>
bool IsIntersect(T const & x0, T const & x1, T const & x2, T const & x3)
{
  return !((x1 < x2) || (x3 < x0));
}

// Computes x^n.
template <typename T> inline T PowUint(T x, uint64_t n)
{
  T res = 1;
  for (T t = x; n > 0; n >>= 1, t *= t)
    if (n & 1)
      res *= t;
  return res;
}

template <typename T> inline T NextModN(T x, T n)
{
  return x + 1 == n ? 0 : x + 1;
}

template <typename T> inline T PrevModN(T x, T n)
{
  return x == 0 ? n - 1 : x - 1;
}

inline uint32_t NextPowOf2(uint32_t v)
{
  v = v - 1;
  v |= (v >> 1);
  v |= (v >> 2);
  v |= (v >> 4);
  v |= (v >> 8);
  v |= (v >> 16);

  return v + 1;
}

// Greatest Common Divisor
template <typename T> T GCD(T a, T b)
{
  T multiplier = 1;
  T gcd = 1;
  while (true)
  {
    if (a == 0 || b == 0)
    {
      gcd = max(a, b);
      break;
    }

    if (a == 1 || b == 1)
    {
      gcd = 1;
      break;
    }

    if ((a & 0x1) == 0 && (b & 0x1) == 0)
    {
      multiplier <<= 1;
      a >>= 1;
      b >>= 1;
      continue;
    }

    if ((a & 0x1) != 0 && (b & 0x1) != 0)
    {
      T const minV = min(a, b);
      T const maxV = max(a, b);
      a = (maxV - minV) >> 1;
      b = minV;
      continue;
    }

    if ((a & 0x1) != 0)
      swap(a, b);

    a >>= 1;
  }

  return multiplier * gcd;
}

/// Calculate hash for the pair of values.
template <typename T1, typename T2>
size_t Hash(T1 const & t1, T2 const & t2)
{
  /// @todo Probably, we need better hash for 2 integral types.
  return (hash<T1>()(t1) ^ (hash<T2>()(t2) << 1));
}

}


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

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

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

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_POINT_CIRCLE_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_POINT_CIRCLE_HPP

#include <cstddef>

#include <boost/range.hpp>

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

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

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

/*!
\brief Create a circular buffer around a point
\ingroup strategies
\details This strategy can be used as PointStrategy for the buffer algorithm.
    It creates a circular buffer around a point. It can be applied
    for points and multi_points, but also for a linestring (if it is degenerate,
    so consisting of only one point) and for polygons (if it is degenerate).
    This strategy is only applicable for Cartesian coordinate systems.

\qbk{
[heading Example]
[buffer_point_circle]
[heading Output]
[$img/strategies/buffer_point_circle.png]
[heading See also]
\* [link geometry.reference.algorithms.buffer.buffer_7_with_strategies buffer (with strategies)]
\* [link geometry.reference.strategies.strategy_buffer_point_square point_square]
}
 */
class point_circle
{
public :
    //! \brief Constructs the strategy
    //! \param count number of points for the created circle (if count
    //! is smaller than 3, count is internally set to 3)
    explicit point_circle(std::size_t count = 90)
        : m_count((count < 3u) ? 3u : count)
    {}

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    //! Fills output_range with a circle around point using distance_strategy
    template
    <
        typename Point,
        typename OutputRange,
        typename DistanceStrategy
    >
    inline void apply(Point const& point,
                DistanceStrategy const& distance_strategy,
                OutputRange& output_range) const
    {
        typedef typename boost::range_value<OutputRange>::type output_point_type;

        typedef typename geometry::select_most_precise
            <
                typename geometry::select_most_precise
                    <
                        typename geometry::coordinate_type<Point>::type,
                        typename geometry::coordinate_type<output_point_type>::type
                    >::type,
                double
            >::type promoted_type;

        promoted_type const buffer_distance = distance_strategy.apply(point, point,
                        strategy::buffer::buffer_side_left);

        promoted_type const two = 2.0;
        promoted_type const two_pi = two * geometry::math::pi<promoted_type>();

        promoted_type const diff = two_pi / promoted_type(m_count);
        promoted_type a = 0;

        for (std::size_t i = 0; i < m_count; i++, a -= diff)
        {
            output_point_type p;
            set<0>(p, get<0>(point) + buffer_distance * cos(a));
            set<1>(p, get<1>(point) + buffer_distance * sin(a));
            output_range.push_back(p);
        }

        // Close it:
        output_range.push_back(output_range.front());
    }
#endif // DOXYGEN_SHOULD_SKIP_THIS

private :
    std::size_t m_count;
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_POINT_CIRCLE_HPP


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
#include <NTL/ZZ.h>
#include <algorithm>
#include <complex>

#include "norms.h"
#include "EncryptedArray.h"
#include "FHE.h"
#include "debugging.h"

NTL_CLIENT

bool verbose=false;

// Compute the L-infinity distance between two vectors
double calcMaxDiff(const vector<cx_double>& v1,
                   const vector<cx_double>& v2){

  if(lsize(v1)!=lsize(v2))
    NTL::Error("Vector sizes differ.\nFAILED\n");

  double maxDiff = 0.0;
  for (long i=0; i<lsize(v1); i++) {
    double diffAbs = std::abs(v1[i]-v2[i]);
    if (diffAbs > maxDiff)
      maxDiff = diffAbs;
  }

  return maxDiff;
}

inline bool cx_equals(const vector<cx_double>& v1,
                      const vector<cx_double>& v2,
                      double epsilon)
{
  return (calcMaxDiff(v1,v2) < epsilon);
}

void testBasicArith(const FHEPubKey& publicKey,
                    const FHESecKey& secretKey,
                    const EncryptedArrayCx& ea, double epsilon);
void testComplexArith(const FHEPubKey& publicKey,
                      const FHESecKey& secretKey,
                      const EncryptedArrayCx& ea, double epsilon);
void testRotsNShifts(const FHEPubKey& publicKey,
                     const FHESecKey& secretKey,
                     const EncryptedArrayCx& ea, double epsilon);

int main(int argc, char *argv[])
{

  // Commandline setup

  ArgMapping amap;

  long m=16;
  long r=8;
  long L=150;
  double epsilon=0.01; // Accepted accuracy

  amap.arg("m", m, "Cyclotomic index");
  amap.note("e.g., m=1024, m=2047");
  amap.arg("r", r, "Bits of precision");
  amap.arg("L", L, "Number of levels");
  amap.arg("ep", epsilon, "Accepted accuracy");
  amap.arg("verbose", verbose, "more printouts");

  amap.parse(argc, argv);

  try{

    // FHE setup keys, context, SKMs, etc

    FHEcontext context(m, /*p=*/-1, r);
    buildModChain(context, L, /*c=*/2);

    FHESecKey secretKey(context);
    secretKey.GenSecKey(); // A +-1/0 secret key
    addSome1DMatrices(secretKey); // compute key-switching matrices

    const FHEPubKey publicKey = secretKey;
    const EncryptedArrayCx& ea = context.ea->getCx();

    if (verbose) {
      ea.getPAlgebra().printout();
      cout << "r = " << context.alMod.getR() << endl;
      cout << "ctxtPrimes="<<context.ctxtPrimes
           << ", specialPrimes="<<context.specialPrimes<<endl<<endl;
    }

    // Run the tests.
    testBasicArith(publicKey, secretKey, ea, epsilon);
    testComplexArith(publicKey, secretKey, ea, epsilon);
    testRotsNShifts(publicKey, secretKey, ea, epsilon);

  }
  catch (exception& e) {
    cerr << e.what() << endl;
    cerr << "***Major FAIL***" << endl;
  }

  return 0;
}

void testBasicArith(const FHEPubKey& publicKey,
                    const FHESecKey& secretKey,
                    const EncryptedArrayCx& ea, double epsilon)
{
  if (verbose)  cout << "Test Arithmetic ";
  // Test objects

  Ctxt c1(publicKey), c2(publicKey), c3(publicKey);

  vector<cx_double> vd;
  vector<cx_double> vd1, vd2, vd3;
  ea.random(vd1);
  ea.random(vd2);

  // test encoding of shorter vectors
  vd1.resize(vd1.size()-2);
  ea.encrypt(c1, publicKey, vd1);
  vd1.resize(vd1.size()+2, 0.0);

  ea.encrypt(c2, publicKey, vd2);

  // Test - Multiplication
  c1 *= c2;
  for (long i=0; i<lsize(vd1); i++) vd1[i] *= vd2[i];

  ZZX poly;
  ea.random(vd3);
  ea.encode(poly,vd3);
  c1.addConstant(poly); // vd1*vd2 + vd3
  for (long i=0; i<lsize(vd1); i++) vd1[i] += vd3[i];

  // Test encoding, encryption of a single number
  double xx = NTL::RandomLen_long(16)/double(1L<<16); // random in [0,1]
  ea.encryptOneNum(c2, publicKey, xx);
  c1 += c2;
  for (auto& x : vd1) x += xx;

  // Test - Multiply by a mask
  vector<long> mask(lsize(vd1), 1);
  for (long i=0; i*(i+1)<lsize(mask); i++) {
    mask[i*i] = 0;
    mask[i*(i+1)] = -1;
  }

  ea.encode(poly,mask);
  c1.multByConstant(poly); // mask*(vd1*vd2 + vd3)
  for (long i=0; i<lsize(vd1); i++) vd1[i] *= mask[i];

  // Test - Addition
  ea.random(vd3);
  ea.encrypt(c3, publicKey, vd3);
  c1 += c3;
  for (long i=0; i<lsize(vd1); i++) vd1[i] += vd3[i];

  c1.negate();
  c1.addConstant(to_ZZ(1));
  for (long i=0; i<lsize(vd1); i++) vd1[i] = 1.0 - vd1[i];

  // Diff between approxNums HE scheme and plaintext floating
  ea.decrypt(c1, secretKey, vd);
#ifdef DEBUG_PRINTOUT
  printVec(cout<<"res=", vd, 10)<<endl;
  printVec(cout<<"vec=", vd1, 10)<<endl;
#endif
  if (verbose)
    cout << "(max |res-vec|_{infty}="<< calcMaxDiff(vd, vd1) << "): ";

  cx_equals(vd, vd1, epsilon)?
    cout << "GOOD\n":
    cout << "BAD\n";
}

void testComplexArith(const FHEPubKey& publicKey,
                      const FHESecKey& secretKey,
                      const EncryptedArrayCx& ea, double epsilon)
{

  // Test complex conjugate
  Ctxt c1(publicKey), c2(publicKey);

  vector<cx_double> vd;
  vector<cx_double> vd1, vd2;
  ea.random(vd1);
  ea.random(vd2);

  ea.encrypt(c1, publicKey, vd1);
  ea.encrypt(c2, publicKey, vd2);

  if (verbose)
    cout << "Test Conjugate: ";
  for_each(vd1.begin(), vd1.end(), [](cx_double& d){d=std::conj(d);});
  c1.complexConj();
  ea.decrypt(c1, secretKey, vd);
#ifdef DEBUG_PRINTOUT
  printVec(cout<<"vd1=", vd1, 10)<<endl;
  printVec(cout<<"res=", vd, 10)<<endl;
#endif
  cx_equals(vd, vd1, epsilon)?
    cout << "GOOD\n":
    cout << "BAD\n";

  // Test that real and imaginary parts are actually extracted.
  Ctxt realCtxt(c2), imCtxt(c2);
  vector<cx_double> realParts(vd2), real_dec;
  vector<cx_double> imParts(vd2), im_dec;

  if (verbose)
    cout << "Test Real and Im parts: ";
  for_each(realParts.begin(), realParts.end(), [](cx_double& d){d=std::real(d);});
  for_each(imParts.begin(), imParts.end(), [](cx_double& d){d=std::imag(d);});

  ea.extractRealPart(realCtxt);
  ea.decrypt(realCtxt, secretKey, real_dec);

  ea.extractImPart(imCtxt);
  ea.decrypt(imCtxt, secretKey, im_dec);

#ifdef DEBUG_PRINTOUT
  printVec(cout<<"vd2=", vd2, 10)<<endl;
  printVec(cout<<"real=", realParts, 10)<<endl;
  printVec(cout<<"res=", real_dec, 10)<<endl;
  printVec(cout<<"im=", imParts, 10)<<endl;
  printVec(cout<<"res=", im_dec, 10)<<endl;
#endif
  cx_equals(realParts,real_dec,epsilon) && cx_equals(imParts,im_dec,epsilon)?
    cout << "GOOD\n":
    cout << "BAD\n";
}

void testRotsNShifts(const FHEPubKey& publicKey,
                     const FHESecKey& secretKey,
                     const EncryptedArrayCx& ea, double epsilon)
{

  std::srand(std::time(0)); // set seed, current time.
  int nplaces = rand() % static_cast<int>(ea.size()/2.0) + 1;

  if (verbose)
    cout << "Test Rotation of " << nplaces << ": ";

  Ctxt c1(publicKey);
  vector<cx_double> vd1;
  vector<cx_double> vd_dec;
  ea.random(vd1);
  ea.encrypt(c1, publicKey, vd1);

#ifdef DEBUG_PRINTOUT
  printVec(cout<< "vd1=", vd1, 10)<<endl;
#endif
  std::rotate(vd1.begin(), vd1.end()-nplaces, vd1.end());
  ea.rotate(c1, nplaces);
  ea.decrypt(c1, secretKey, vd_dec);
#ifdef DEBUG_PRINTOUT
  printVec(cout<< "vd1(rot)=", vd1, 10)<<endl;
  printVec(cout<<"res: ", vd_dec, 10)<<endl;
#endif

  cx_equals(vd1, vd_dec, epsilon)?
    cout << "GOOD\n":
    cout << "BAD\n";
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// 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 <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

int main(int argc, char** argv)
{

    typedef adjacency_list< vecS, vecS, undirectedS,
        property< vertex_index_t, int >, property< edge_index_t, int > >
        graph;

    // Create a maximal planar graph on 6 vertices
    graph g(6);

    add_edge(0, 1, g);
    add_edge(1, 2, g);
    add_edge(2, 3, g);
    add_edge(3, 4, g);
    add_edge(4, 5, g);
    add_edge(5, 0, g);

    add_edge(0, 2, g);
    add_edge(0, 3, g);
    add_edge(0, 4, g);

    add_edge(1, 3, g);
    add_edge(1, 4, g);
    add_edge(1, 5, g);

    // Initialize the interior edge index
    property_map< graph, edge_index_t >::type e_index = get(edge_index, g);
    graph_traits< graph >::edges_size_type edge_count = 0;
    graph_traits< graph >::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        put(e_index, *ei, edge_count++);

    // Test for planarity - we know it is planar, we just want to
    // compute the planar embedding as a side-effect
    typedef std::vector< graph_traits< graph >::edge_descriptor > vec_t;
    std::vector< vec_t > embedding(num_vertices(g));
    if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
            boyer_myrvold_params::embedding = make_iterator_property_map(
                embedding.begin(), get(vertex_index, g))))
        std::cout << "Input graph is planar" << std::endl;
    else
        std::cout << "Input graph is not planar" << std::endl;

    typedef std::vector< graph_traits< graph >::vertex_descriptor >
        ordering_storage_t;

    ordering_storage_t ordering;
    planar_canonical_ordering(g,
        make_iterator_property_map(embedding.begin(), get(vertex_index, g)),
        std::back_inserter(ordering));

    ordering_storage_t::iterator oi, oi_end;
    oi_end = ordering.end();
    std::cout << "The planar canonical ordering is: ";
    for (oi = ordering.begin(); oi != oi_end; ++oi)
        std::cout << *oi << " ";
    std::cout << std::endl;

    return 0;
}


#include <Eigen/Core>
#include <trajopt_sco/expr_ops.hpp>
#include <trajopt_sco/modeling_utils.hpp>
#include <trajopt/trajectory_costs.hpp>

using namespace std;
using namespace sco;
using namespace Eigen;

namespace {

static MatrixXd diffAxis0(const MatrixXd& in) {
  return in.middleRows(1, in.rows()-1) - in.middleRows(0, in.rows()-1);
}

}

namespace trajopt {

//////////// Quadratic cost functions /////////////////

JointPosCost::JointPosCost(const VarVector& vars, const VectorXd& vals, const VectorXd& coeffs) :
    Cost("JointPos"), vars_(vars), vals_(vals), coeffs_(coeffs) {
    for (int i=0; i < vars.size(); ++i) {
      if (coeffs[i] > 0) {
        AffExpr diff = exprSub(AffExpr(vars[i]), AffExpr(vals[i]));
        exprInc(expr_, exprMult(exprSquare(diff), coeffs[i]));
      }
    }
}
double JointPosCost::value(const vector<double>& xvec) {
  VectorXd dofs = getVec(xvec, vars_);
  return ((dofs - vals_).array().square() * coeffs_.array()).sum();
}
ConvexObjectivePtr JointPosCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

JointVelCost::JointVelCost(const VarArray& vars, const VectorXd& coeffs) :
    Cost("JointVel"), vars_(vars), coeffs_(coeffs) {
  for (int i=0; i < vars.rows()-1; ++i) {
    for (int j=0; j < vars.cols(); ++j) {
      AffExpr vel;
      exprInc(vel, exprMult(vars(i,j), -1));
      exprInc(vel, exprMult(vars(i+1,j), 1));
      exprInc(expr_, exprMult(exprSquare(vel),coeffs_[j]));
    }
  }
}
double JointVelCost::value(const vector<double>& xvec) {
  MatrixXd traj = getTraj(xvec, vars_);
  return (diffAxis0(traj).array().square().matrix() * coeffs_.asDiagonal()).sum();
}
ConvexObjectivePtr JointVelCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

JointAccCost::JointAccCost(const VarArray& vars, const VectorXd& coeffs) :
    Cost("JointAcc"), vars_(vars), coeffs_(coeffs) {
  for (int i=0; i < vars.rows()-2; ++i) {
    for (int j=0; j < vars.cols(); ++j) {
      AffExpr acc;
      exprInc(acc, exprMult(vars(i,j), -1));
      exprInc(acc, exprMult(vars(i+1,j), 2));
      exprInc(acc, exprMult(vars(i+2,j), -1));
      exprInc(expr_, exprMult(exprSquare(acc), coeffs_[j]));
    }
  }
}
double JointAccCost::value(const vector<double>& xvec) {
  MatrixXd traj = getTraj(xvec, vars_);
  return (diffAxis0(diffAxis0(traj)).array().square().matrix() * coeffs_.asDiagonal()).sum();
}
ConvexObjectivePtr JointAccCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

}


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/**
 * @file norms.cpp - computing various norms of ring elements
 **/
#include <numeric>
#include <NTL/BasicThreadPool.h>
#include "NumbTh.h"
#include "DoubleCRT.h"
#include "norms.h"
NTL_CLIENT

long sumOfCoeffs(const zzX& f) // = f(1)
{
  long sum = 0;
  for (long i=0; i<lsize(f); i++) sum += f[i];
  return sum;
}
ZZ sumOfCoeffs(const ZZX& f) // = f(1)
{
  ZZ sum = ZZ::zero();
  for (long i=0; i<=deg(f); i++) sum += coeff(f,i);
  return sum;
}
NTL::ZZ sumOfCoeffs(const DoubleCRT& f)
{
  ZZX poly;
  f.toPoly(poly);
  return sumOfCoeffs(poly);
}

long largestCoeff(const zzX& f) // l_infty norm
{
  long mx = 0;
  for (long i=0; i<lsize(f); i++) {
    if (mx < abs(f[i]))
      mx = abs(f[i]);
  }
  return mx;
}
ZZ largestCoeff(const ZZX& f)
{
  ZZ mx = ZZ::zero();
  for (long i=0; i<=deg(f); i++) {
    if (mx < abs(coeff(f,i)))
      mx = abs(coeff(f,i));
  }
  return mx;
}
ZZ largestCoeff(const Vec<ZZ>& f)
{
  ZZ mx = ZZ::zero();
  for (auto& x : f) {
    if (mx < abs(x))
      mx = abs(x);
  }
  return mx;
}
NTL::ZZ largestCoeff(const DoubleCRT& f)
{
  ZZX poly;
  f.toPoly(poly);
  return largestCoeff(poly);
}

double coeffsL2NormSquared(const zzX& f) // l_2 norm square
{
  double s = 0.0;
  for (long i=0; i<lsize(f); i++) {
    double coef = f[i];
    s += coef * coef;
  }
  return s;
}
xdouble coeffsL2NormSquared(const ZZX& f) // l_2 norm square
{
  xdouble s(0.0);
  for (long i=0; i<=deg(f); i++) {
    xdouble coef(conv<xdouble>(coeff(f,i)));
    s += coef * coef;
  }
  return s;
}
xdouble coeffsL2NormSquared(const DoubleCRT& f) // l2 norm^2
{
  ZZX poly;
  f.toPoly(poly);
  return coeffsL2NormSquared(poly);
}

#if FFT_IMPL
// l_2 norm square of canonical embedding
double embeddingL2NormSquared(const zzX& f, const PAlgebra& palg)
{
  std::vector<cx_double> emb;
  canonicalEmbedding(emb, f, palg);
  double acc = 0.0;
  for (auto& x : emb)
    acc += std::norm(x);
  return 2*acc; // emb just has phi(m)/2 values (paired with complex conjugates)
}

//! Computing the L-infinity norm of the canonical embedding
double embeddingLargestCoeff(const zzX& f, const PAlgebra& palg)
{
  std::vector<cx_double> emb;
  canonicalEmbedding(emb, f, palg);
  double mx = 0.0;
  for (auto& x : emb) {
    double n = std::norm(x);
    if (mx < n) mx = n;
  }
  return sqrt(mx);
}

static xdouble convertAndScale(zzX& ff, const NTL::ZZX& f)
{
  const long MAX_BITS = NTL_SP_BOUND-15; // max allowed bits to avoid double overflow
                                         // in computations
  xdouble factor(1.0);
  long size = NTL::MaxBits(f);
  if (size > MAX_BITS) {
    ZZ zzFactor = ZZ(1) << (size-MAX_BITS); // divide f by this factor

    ZZX scaled = f;
    for (long i: range(f.rep.length())) RightShift(scaled.rep[i], scaled.rep[i], size-MAX_BITS);
    scaled.normalize();

    convert(factor, zzFactor);      // remember the factor
    convert(ff, scaled);            // convert to zzX
  }
  else
    convert(ff, f);                 // convert to zzX
  return factor;
}

static xdouble convertAndScale(ZZX& ff, const NTL::ZZX& f)
{
  const long MAX_BITS = 250; // max allowed bits to avoid double overflow
                             // in computations

  xdouble factor(1.0);
  long size = NTL::MaxBits(f);
  if (size > MAX_BITS) {
    ZZ zzFactor = ZZ(1) << (size-MAX_BITS); // divide f by this factor

    ZZX scaled = f;
    for (long i: range(f.rep.length())) RightShift(scaled.rep[i], scaled.rep[i], size-MAX_BITS);
    scaled.normalize();

    convert(factor, zzFactor);      // remember the factor
    ff = scaled;
  }
  else
    convert(ff, f);                 // convert to zzX
  return factor;
}

xdouble embeddingL2NormSquared(const NTL::ZZX& f, const PAlgebra& palg)
{
  zzX ff; // to hold a scaled-down version of ff;
  xdouble factor = convertAndScale(ff, f);
  return embeddingL2NormSquared(ff, palg)*factor*factor;
}

xdouble embeddingLargestCoeff(const NTL::ZZX& f, const PAlgebra& palg)
{
#if 1
  ZZX ff; // to hold a scaled-down version of ff;
  xdouble factor = convertAndScale(ff, f);
#else
  const ZZX& ff = f;
  xdouble factor { 1.0 };
#endif
  std::vector<cx_double> emb;
  canonicalEmbedding(emb, ff, palg);
  xdouble mx {0.0};
  for (auto& x : emb) {
    double re = std::real(x);
    double im = std::imag(x);
    xdouble n = xdouble(re)*xdouble(re) + xdouble(im)*xdouble(im);
    if (mx < n) mx = n;
  }
  return sqrt(mx)*factor;
}
#endif


/**
 * \file ToneStackFilter.cpp
 */

#include <boost/math/tools/polynomial.hpp>

#include "ToneStackFilter.h"
#include "IIRFilter.h"

namespace ATK
{
  template<typename DataType>
  ToneStackCoefficients<DataType>::ToneStackCoefficients(int nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels), R1(0), R2(0), R3(0), R4(0), C1(0), C2(0), C3(0), low(.5), middle(.5), high(.5)
  {
  }

  template<typename DataType>
  ToneStackCoefficients<DataType>::ToneStackCoefficients(ToneStackCoefficients&& other)
  :Parent(std::move(other)), R1(other.R1), R2(other.R2), R3(other.R3), R4(other.R4), C1(other.C1), C2(other.C2), C3(other.C3), low(other.low), middle(other.middle), high(other.high)
  {

  }

  template<typename DataType>
  void ToneStackCoefficients<DataType>::setup()
  {
    Parent::setup();

    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    DataType tempm[2] = {static_cast<DataType>(-2 * input_sampling_rate), static_cast<DataType>(2 * input_sampling_rate)};
    DataType tempp[2] = {static_cast<DataType>(1), static_cast<DataType>(1)};
    boost::math::tools::polynomial<DataType> poly1(tempm, 1);
    boost::math::tools::polynomial<DataType> poly2(tempp, 1);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    b = poly2 * poly2 * poly1 * (high*C1*R1 + middle*C3*R3 + low*(C1*R2 + C2*R2) + (C1*R3 + C2*R3));
    b += poly2 * poly1 * poly1 * (high*(C1*C2*R1*R4 + C1*C3*R1*R4) - middle*middle*(C1*C3*R3*R3 + C2*C3*R3*R3) + middle*(C1*C3*R1*R3 + C1*C3*R3*R3 + C2*C3*R3*R3)
      + low*(C1*C2*R1*R2 + C1*C2*R2*R4 + C1*C3*R2*R4) + low*middle*(C1*C3*R2*R3 + C2*C3*R2*R3)
      + (C1*C2*R1*R3 + C1*C2*R3*R4 + C1*C3*R3*R4));
    b += poly1 * poly1 * poly1 * (low*middle*(C1*C2*C3*R1*R2*R3 + C1*C2*C3*R2*R3*R4) - middle*middle*(C1*C2*C3*R1*R3*R3 + C1*C2*C3*R3*R3*R4)
      + middle*(C1*C2*C3*R1*R3*R3 + C1*C2*C3*R3*R3*R4) + high*C1*C2*C3*R1*R3*R4 - high*middle*C1*C2*C3*R1*R3*R4
      + high*low*C1*C2*C3*R1*R2*R4);

    a = poly2 * poly2 * poly2;
    a += poly2 * poly2 * poly1 * ((C1*R1 + C1*R3 + C2*R3 + C2*R4 + C3*R4) + middle*C3*R3 + low*(C1*R2 + C2*R2));
    a += poly2 * poly1 * poly1 * (middle*(C1*C3*R1*R3 - C2*C3*R3*R4 + C1*C3*R3*R3 + C2*C3*R3*R3)
      + low*middle*(C1*C3*R2*R3 + C2*C3*R2*R3) - middle*middle*(C1*C3*R3*R3 + C2*C3*R3*R3) + low*(C1*C2*R2*R4 + C1*C2*R1*R2 + C1*C3*R2*R4 + C2*C3*R2*R4)
      + (C1*C2*R1*R4 + C1*C3*R1*R4 + C1*C2*R3*R4 + C1*C2*R1*R3 + C1*C3*R3*R4 + C2*C3*R3*R4));
    a += poly1 * poly1 * poly1 * (low*middle*(C1*C2*C3*R1*R2*R3 + C1*C2*C3*R2*R3*R4) - middle*middle*(C1*C2*C3*R1*R3*R3 + C1*C2*C3*R3*R3*R4)
      + middle*(C1*C2*C3*R3*R3*R4 + C1*C2*C3*R1*R3*R3 - C1*C2*C3*R1*R3*R4)
      + low*C1*C2*C3*R1*R2*R4 + C1*C2*C3*R1*R3*R4);

    for(int i = 0; i < in_order + 1; ++i)
    {
      coefficients_in[i] = b[i] / a[out_order];
    }
    for(int i = 0; i < out_order; ++i)
    {
      coefficients_out[i] = -a[i] / a[out_order];
    }
  }

  template<typename DataType_>
  void ToneStackCoefficients<DataType_>::set_low(DataType_ low)
  {
    if(low < 0 || low > 1)
    {
      throw std::out_of_range("Low is outside the interval [0,1]");
    }
    this->low = low;

    setup();
  }

  template<typename DataType_>
  DataType_ ToneStackCoefficients<DataType_>::get_low() const
  {
    return low;
  }

  template<typename DataType_>
  void ToneStackCoefficients<DataType_>::set_middle(DataType_ middle)
  {
    if(middle < 0 || middle > 1)
    {
      throw std::out_of_range("Middle is outside the interval [0,1]");
    }
    this->middle = middle;

    setup();
  }

  template<typename DataType_>
  DataType_ ToneStackCoefficients<DataType_>::get_middle() const
  {
    return middle;
  }

  template<typename DataType_>
  void ToneStackCoefficients<DataType_>::set_high(DataType_ high)
  {
    if(high < 0 || high > 1)
    {
      throw std::out_of_range("high is outside the interval [0,1]");
    }
    this->high = high;

    setup();
  }

  template<typename DataType_>
  DataType_ ToneStackCoefficients<DataType_>::get_high() const
  {
    return high;
  }

  template<typename DataType>
  IIRFilter<ToneStackCoefficients<DataType> > ToneStackCoefficients<DataType>::buildBassmanStack()
  {
    IIRFilter<ToneStackCoefficients<DataType> > filter;
    filter.set_coefficients(static_cast<DataType>(250e3), static_cast<DataType>(1e6), static_cast<DataType>(25e3), static_cast<DataType>(45e3),
      static_cast<DataType>(250e-12), static_cast<DataType>(20e-9), static_cast<DataType>(20e-9));
    return std::move(filter);
  }

  template<typename DataType>
  IIRFilter<ToneStackCoefficients<DataType> > ToneStackCoefficients<DataType>::buildJCM800Stack()
  {
    IIRFilter<ToneStackCoefficients<DataType> > filter;
    filter.set_coefficients(static_cast<DataType>(220e3), static_cast<DataType>(1e6), static_cast<DataType>(22e3), static_cast<DataType>(33e3),
      static_cast<DataType>(470e-12), static_cast<DataType>(22e-9), static_cast<DataType>(22e-9));
    return std::move(filter);
  }

  template<typename DataType_>
  void ToneStackCoefficients<DataType_>::set_coefficients( DataType R1, DataType R2, DataType R3, DataType R4, DataType C1, DataType C2, DataType C3 )
  {
    this->R1 = R1;
    this->R2 = R2;
    this->R3 = R3;
    this->R4 = R4;
    this->C1 = C1;
    this->C2 = C2;
    this->C3 = C3;
  }

  template class ToneStackCoefficients<float>;
  template class ToneStackCoefficients<double>;

  template class IIRFilter<ToneStackCoefficients<float> >;
  template class IIRFilter<ToneStackCoefficients<double> >;
}


/**
 * @file asymptotic.cc
 * @brief Creates convergence plots for experiment 3.2.3.12
 * @author Tobias Rohner
 * @date April 2020
 * @copyright MIT License
 */

#define _USE_MATH_DEFINES

#include <lf/quad/gauss_quadrature.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <boost/program_options.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>

namespace po = boost::program_options;

// Code for empiric exploration of asymptotic convergence of norms of the
// discretization error for 1D finite element discretization of a 2-point BVP.
// Special example with a highly oscillatory solution
int main(int argc, char *argv[]) {
  po::options_description desc("Allowed options");
  // clang-format off
  desc.add_options()
  ("output,o", po::value<std::string>(), "Name of the output file")
  ("M_max,M", po::value<int>()->default_value(500), "Maximum number of cells")
  ("dM,m", po::value<int>()->default_value(5), "Increment in M")
  ("num_quad_points,n", po::value<int>()->default_value(2), "Number of points for numerical quadrature");
  // clang-format on
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  if (vm.count("output") == 0) {
    std::cout << desc << std::endl;
    exit(1);
  }
  const int M_max = vm["M_max"].as<int>();
  const int dM = vm["dM"].as<int>();
  const int num_quad_points = vm["num_quad_points"].as<int>();
  const std::string output_file = vm["output"].as<std::string>();
  const auto [quad_points, quad_weights] = lf::quad::GaussLegendre(2);

  // Load function
  const auto f = [](double x) {
    return 10000 * M_PI * M_PI * x * x * std::sin(50 * M_PI * x * x) -
           100 * M_PI * std::cos(50 * M_PI * x * x);
  };
  // Analytic solution (highly oscillatory)
  const auto u = [](double x) { return std::sin(50 * M_PI * x * x); };
  // Gradient of analytic solution
  const auto u_grad = [](double x) {
    return 100 * M_PI * x * std::cos(50 * M_PI * x * x);
  };

  Eigen::MatrixXd results(M_max / dM, 4);
  // Loop over meshes with increasing numbers of cells
  for (int M = dM; M <= M_max; M += dM) {
    // The mesh width
    const double h = 1. / M;

    // Generate the tri-diagonal stiffness matrix for an equidistant
    // mesh on [0, 1] with M cells and p.w. linear Lagrangian finite elements
    // Formulas are explained in Section 2.3 of the lecture document
    Eigen::SparseMatrix<double> A(M + 1, M + 1);
    A.reserve(3 * (M + 1));
    // Fill the diagonal
    for (int i = 0; i < M + 1; ++i) {
      A.coeffRef(i, i) += 2. / h;
    }
    // Fill the off-diagonals
    for (int i = 0; i < M; ++i) {
      A.coeffRef(i, i + 1) += -1. / h;
      A.coeffRef(i + 1, i) += -1. / h;
    }

    // Generate the load vector
    Eigen::VectorXd rhs = Eigen::VectorXd::Zero(M + 1);
    for (int i = 0; i < M; ++i) {
      const double a = static_cast<double>(i) / M;
      const Eigen::VectorXd loc_quad_points =
          Eigen::VectorXd::Constant(num_quad_points, a) + h * quad_points;
      const Eigen::VectorXd loc_quad_weights = h * quad_weights;
      // Perform the integration over the cell for both basis functions
      const auto b1 = [&](double x) { return (x - a) / h; };
      const auto b2 = [&](double x) { return 1. - b1(x); };
      for (int k = 0; k < num_quad_points; ++k) {
        rhs[i] += loc_quad_weights[k] * b2(loc_quad_points[k]) *
                  f(loc_quad_points[k]);
        rhs[i + 1] += loc_quad_weights[k] * b1(loc_quad_points[k]) *
                      f(loc_quad_points[k]);
      }
    }

    // Enforce zero dirichlet boundary conditions
    for (long k = 0; k < A.outerSize(); ++k) {
      for (Eigen::SparseMatrix<double>::InnerIterator it(A, k); it; ++it) {
        const int row = it.row();
        const int col = it.col();
        if ((row == 0 && col == 0) || (row == M && col == M)) {
          it.valueRef() = 1;
        } else if (row == 0 || row == M || col == 0 || col == M) {
          it.valueRef() = 0;
        }
      }
    }
    // Set the boundary values to zero
    rhs[0] = 0;
    rhs[M] = 0;

    // Solve the resulting linear system
    Eigen::SparseLU<Eigen::SparseMatrix<double>> solver(A);
    const Eigen::VectorXd sol = solver.solve(rhs);

    // Compute the norms and store them in the results matrix
    double norm_max = 0;
    double norm_H1_squared = 0;
    double norm_L2_squared = 0;
    for (int i = 0; i < M; ++i) {
      const double a = static_cast<double>(i) / M;
      const double b = static_cast<double>(i + 1) / M;
      const Eigen::VectorXd loc_quad_points =
          Eigen::VectorXd::Constant(num_quad_points, a) + h * quad_points;
      const Eigen::VectorXd loc_quad_weights = h * quad_weights;

      // The approximate solution on the current cell
      const auto u_h = [&](double x) {
        return sol[i + 1] * (x - a) / h + sol[i] * (1. - (x - a) / h);
      };
      // The gradient of the approximate solution on the current cell
      const auto u_h_grad = [&](double /*x*/) {
        return (sol[i + 1] - sol[i]) / h;
      };
      // The difference of the approximate and the exact solution
      const auto diff = [&](double x) { return u_h(x) - u(x); };
      // The difference in the gradient of the approximate and the exact
      // solution
      const auto diff_grad = [&](double x) { return u_h_grad(x) - u_grad(x); };

      // Compute the max norm by evaluating the functions on a fine grid
      norm_max =
          std::max(norm_max, Eigen::ArrayXd::LinSpaced(10 * M_max / M, a, b)
                                 .unaryExpr(diff)
                                 .abs()
                                 .maxCoeff());
      // Compute the H1 and L2 norms by integrating using a numerical quadrature
      for (int k = 0; k < num_quad_points; ++k) {
        norm_H1_squared += loc_quad_weights[k] * diff_grad(loc_quad_points[k]) *
                           diff_grad(loc_quad_points[k]);
        norm_L2_squared += loc_quad_weights[k] * diff(loc_quad_points[k]) *
                           diff(loc_quad_points[k]);
      }
    }
    results((M / dM) - 1, 0) = M;
    results((M / dM) - 1, 1) = norm_max;
    results((M / dM) - 1, 2) = std::sqrt(norm_H1_squared);
    results((M / dM) - 1, 3) = std::sqrt(norm_L2_squared);
  }

  // Output the resulting errors to a file
  const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::ofstream file;
  file.open(output_file);
  file << results.format(CSVFormat);
  file.close();

  return 0;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2019 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-1
 */

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <cmath>
#include <fstream>
#include <iostream>

using namespace dealii;

std::tuple<int, int, int>
grid_parameters(const Triangulation<2> &tria)
{
  return std::make_tuple(tria.n_levels(),
                         tria.n_cells(),
                         tria.n_active_cells());
}

void
first_grid()
{
  Triangulation<2> triangulation;

  GridGenerator::hyper_cube(triangulation);
  std::cout << "Number of original vertices:" << triangulation.n_vertices()
            << std::endl;

  triangulation.refine_global(4);

  std::cout << "Number of original vertices after 4 refinements:"
            << triangulation.n_vertices() << std::endl;

  {
    std::ofstream out("grid-1.svg");
    GridOut       grid_out;
    grid_out.write_svg(triangulation, out);
    std::cout << "Grid written to grid-1.svg" << std::endl;
  }

  {
    std::ofstream out("grid-1.vtk");
    GridOut       grid_out;
    grid_out.write_vtk(triangulation, out);
    std::cout << "Grid written to grid-1.vtk" << std::endl;
  }
  auto params = grid_parameters(triangulation);
  std::cout << std::get<0>(params) << " " << std::get<1>(params) << "  "
            << std::get<2>(params) << std::endl;
}

void
second_grid()
{
  Triangulation<2> triangulation;

  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 10);

  triangulation.reset_all_manifolds();
  for (unsigned int step = 0; step < 5; ++step)
    {
      std::ofstream out("grid-2-" + std::to_string(step) + ".vtk");
      GridOut       grid_out;
      grid_out.write_vtk(triangulation, out);

      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_center =
                center.distance(cell->vertex(v));

              if (std::fabs(distance_from_center - inner_radius) <=
                  1e-6 * inner_radius)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }

      triangulation.execute_coarsening_and_refinement();
    }
  {
    std::ofstream out("grid-2.svg");
    GridOut       grid_out;
    grid_out.write_svg(triangulation, out);
    std::cout << "Grid written to grid-2.svg" << std::endl;
  }
  auto params = grid_parameters(triangulation);
  std::cout << std::get<0>(params) << " " << std::get<1>(params) << "  "
            << std::get<2>(params) << std::endl;
}

void
third_grid()
{
  Triangulation<2> triangulation;
  GridGenerator::hyper_L(triangulation);
  std::cout << "Number of original vertices:" << triangulation.n_vertices()
            << std::endl;

  triangulation.refine_global(1);

  std::cout << "Number of original vertices after 1 refinement:"
            << triangulation.n_vertices() << std::endl;

  {
    std::ofstream out("grid-3.vtk");
    GridOut       grid_out;
    grid_out.write_vtk(triangulation, out);
    std::cout << "Grid written to grid-3.vtk" << std::endl;
  }

  const Point<2> corner(0, 0);
  for (unsigned int step = 0; step < 5; ++step)
    {
      std::ofstream out("grid-3-" + std::to_string(step) + ".vtk");
      GridOut       grid_out;
      grid_out.write_vtk(triangulation, out);

      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_center =
                corner.distance(cell->vertex(v));

              if (std::fabs(distance_from_center) <= 1. / 3.)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }

      triangulation.execute_coarsening_and_refinement();
    }

  auto params = grid_parameters(triangulation);
  std::cout << std::get<0>(params) << " " << std::get<1>(params) << "  "
            << std::get<2>(params) << std::endl;
}

void
circle_grid()
{
  Triangulation<2> triangulation;
  GridGenerator::hyper_ball<2>(triangulation);
  // triangulation.set_all_manifold_ids(0);
  triangulation.set_manifold(0, SphericalManifold<2>());
  const Point<2> mesh_center;
  for (const auto &cell : triangulation.active_cell_iterators())
    if (mesh_center.distance(cell->center()) > cell->diameter() / 10)
      cell->set_all_manifold_ids(0);
  triangulation.refine_global(2);
  std::ofstream out("circle.vtk");
  GridOut       grid_out;
  grid_out.write_vtk(triangulation, out);
}

void
torus_grid()
{
  Triangulation<2, 3> triangulation;
  GridGenerator::torus<2, 3>(triangulation, 4, 1);
  triangulation.refine_global(2);
  std::ofstream out("torus.vtk");
  GridOut       grid_out;
  grid_out.write_vtk(triangulation, out);
}

int
main()
{
  first_grid();
  second_grid();
  third_grid();
  circle_grid();
  torus_grid();
}


/*
 * Copyright (c) 2013-2016 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef STROBOMAP_HPP
#define STROBOMAP_HPP

#include <stdexcept>
#include <kv/ode-nv.hpp>
#include <kv/ode-autodif-nv.hpp>
#include <kv/ode-maffine.hpp>
#ifdef USE_MAFFINE2
#include <kv/ode-maffine2.hpp>
#endif
#include <kv/ode-param.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

// Generate function object of strobomap
// using function object of r.h.s of differential equation.
// Generated function object can receive
//   vector<T>,
//   vector< autodif<T> >,
//   vector< interval<T> >,
//   vector< affine<T> >,
//   vector< autodif< interval<T> > >
// as argument. In each case,
//   odelong_nv in ode-nv.hpp,
//   odelong_nv in ode-autodif-nv.hpp,
//   odelong_maffine in ode-maffine.hpp (interval version)
//   odelong_maffine in ode-maffine.hpp (affine version),
//   odelong_maffine in ode-maffine.hpp (autodif version)
// is called inside.
// (If -DUSE_MAFFINE2 then ode-maffine2.hpp is used instead.)

template <class F, class T> class StroboMap {
	public:
	F f;
	interval<T> start, end;
	ode_param<T> p;

	StroboMap(F f, interval<T> start, interval<T> end, ode_param<T> p = ode_param<T>())
	: f(f), start(start), end(end), p(p) {}

	ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> result;

		result = x;

		odelong_nv(f, result, mid(start), mid(end), p);

		return result;
	}

	ub::vector< autodif<T> > operator() (const ub::vector< autodif<T> >& x){
		ub::vector< autodif<T> > result;

		result = x;

		odelong_nv(f, result, mid(start), mid(end), p);

		return result;
	}

	ub::vector< interval<T> > operator() (const ub::vector< interval<T> >& x){
		ub::vector< interval<T> > result;
		interval<T> end2;
		int r;

		result = x;
		end2 = end;

		#ifdef USE_MAFFINE2
		r = odelong_maffine2(f, result, start, end2, p);
		#else
		r = odelong_maffine(f, result, start, end2, p);
		#endif

		if (r != 2) {
			throw std::domain_error("StroboMap(): cannot calculate validated solution.");
		}

		return result;
	}

	ub::vector< affine<T> > operator() (const ub::vector< affine<T> >& x){
		ub::vector< affine<T> > result;
		interval<T> end2;
		int r;

		result = x;
		end2 = end;

		#ifdef USE_MAFFINE2
		r = odelong_maffine2(f, result, start, end2, p);
		#else
		r = odelong_maffine(f, result, start, end2, p);
		#endif

		if (r != 2) {
			throw std::domain_error("StroboMap(): cannot calculate validated solution.");
		}

		return result;
	}

	ub::vector< autodif< interval<T> > > operator() (const ub::vector< autodif< interval<T> > >& x){
		ub::vector< autodif< interval<T> > > result;
		interval<T> end2;
		int r;

		result = x;
		end2 = end;

		r = odelong_maffine(f, result, start, end2, p);
		if (r != 2) {
			throw std::domain_error("StroboMap(): cannot calculate validated solution.");
		}

		return result;
	}
};

// Generate function object of "x-f(x)" from function object of f.

template <class F> class FixedPoint {
	public:
	F f;
	FixedPoint(F f) : f(f) {}
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		return x - f(x);
	}
};

// Generate function object for shooting method of two point
// boundary value problem from 2-variable strobomap.
//   boundary condition:
//     x(start_index) = start_value, x(end_index) = end_value
//   unknown variable:
//     x(index != start_index)
// Solve ODE from initial value (start_value, unknown) and try to
// satisfy x(end_index) = end_value by changing unknown.

template <class F, class TV> class Shooting_TPBVP {
	public:
	F f;
	TV start_value, end_value;
	int start_index, end_index;
	int variable_index;

	Shooting_TPBVP(F f, TV start_value, TV end_value, int start_index, int end_index) : f(f) , start_value(start_value), end_value(end_value), start_index(start_index), end_index(end_index) {
		variable_index = 1 - start_index;
	}

	// mid_ifnecessary<T1,T2>(x) returns x if T2 is convertible to T1,
	// and returns mid(x) if impossoble.

	#include <boost/utility/enable_if.hpp>

	template <class T1, class T2> T1 inline static mid_ifnecessary(T2& x, typename boost::enable_if_c< convertible<T2, T1>::value >::type* =0) {
		return T1(x);
	}

	template <class T1, class T2> T1 inline static mid_ifnecessary(T2& x, typename boost::enable_if_c< ! convertible<T2, T1>::value >::type* =0) {
		return T1(mid(x));
	}

	template <class T> ub::vector<T> operator() (const ub::vector<T>& s) {
		ub::vector<T> x, y, r;
		x.resize(2);
		r.resize(1);
		x(variable_index) = s(0);
		x(start_index) = mid_ifnecessary<T,TV>(start_value);
		y = f(x);
		r(0) = y(end_index) - mid_ifnecessary<T,TV>(end_value);
		return r;
	}
};

} // namespace kv

#endif // STROBOMAP_HPP


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Random.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Surface_mesh_shortest_path.h>

#include <boost/variant.hpp>
#include <boost/lexical_cast.hpp>

#include <cstdlib>
#include <iostream>
#include <fstream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef CGAL::Surface_mesh<Kernel::Point_3> Triangle_mesh;
typedef CGAL::Surface_mesh_shortest_path_traits<Kernel, Triangle_mesh> Traits;
typedef CGAL::Surface_mesh_shortest_path<Traits> Surface_mesh_shortest_path;
typedef Traits::Barycentric_coordinates Barycentric_coordinates;
typedef boost::graph_traits<Triangle_mesh> Graph_traits;
typedef Graph_traits::vertex_iterator vertex_iterator;
typedef Graph_traits::face_iterator face_iterator;
typedef Graph_traits::vertex_descriptor vertex_descriptor;
typedef Graph_traits::face_descriptor face_descriptor;
typedef Graph_traits::halfedge_descriptor halfedge_descriptor;

// A model of SurfacemeshShortestPathVisitor storing simplicies
// using boost::variant
struct Sequence_collector
{
  typedef boost::variant< vertex_descriptor,
                         std::pair<halfedge_descriptor,double>,
                         std::pair<face_descriptor, Barycentric_coordinates> > Simplex;
  std::vector< Simplex > sequence;

  void operator()(halfedge_descriptor he, double alpha)
  {

    sequence.push_back( std::make_pair(he, alpha) );
  }

  void operator()(vertex_descriptor v)
  {
    sequence.push_back( v );
  }

  void operator()(face_descriptor f, Barycentric_coordinates alpha)
  {
    sequence.push_back( std::make_pair(f, alpha) );
  }
};

// A visitor to print what a variant contains using boost::apply_visitor
struct Print_visitor : public boost::static_visitor<> {
  int i;
  Triangle_mesh& g;

  Print_visitor(Triangle_mesh& g) :i(-1), g(g) {}

  void operator()(vertex_descriptor v)
  {
    std::cout << "#" << ++i << " : Vertex : " << get(boost::vertex_index, g)[v] << "\n";
  }

  void operator()(const std::pair<halfedge_descriptor,double>& h_a)
  {
    std::cout << "#" << ++i << " : Edge : " << get(CGAL::halfedge_index, g)[h_a.first] << " , ("
                                            << 1.0 - h_a.second << " , "
                                            << h_a.second << ")\n";
  }

  void operator()(const std::pair<face_descriptor, Barycentric_coordinates>& f_bc)
  {
    std::cout << "#" << ++i << " : Face : " << get(CGAL::face_index, g)[f_bc.first] << " , ("
                                            << f_bc.second[0] << " , "
                                            << f_bc.second[1] << " , "
                                            << f_bc.second[2] << ")\n";
  }
};

int main(int argc, char** argv)
{
  const char* filename = (argc>1) ? argv[1] : "data/elephant.off";

  Triangle_mesh tmesh;
  if(!CGAL::read_polygon_mesh(filename, tmesh) ||
     !CGAL::is_triangle_mesh(tmesh))
  {
    std::cerr << "Invalid input file." << std::endl;
    return EXIT_FAILURE;
  }

  // pick up a random face
  const unsigned int randSeed = argc > 2 ? boost::lexical_cast<unsigned int>(argv[2]) : 7915421;
  CGAL::Random rand(randSeed);
  const int target_face_index = rand.get_int(0, static_cast<int>(num_faces(tmesh)));
  face_iterator face_it = faces(tmesh).first;
  std::advance(face_it,target_face_index);
  // ... and define a barycentric coordinates inside the face
  Barycentric_coordinates face_location = {{0.25, 0.5, 0.25}};

  // construct a shortest path query object and add a source point
  Surface_mesh_shortest_path shortest_paths(tmesh);
  shortest_paths.add_source_point(*face_it, face_location);

  // pick a random target point inside a face
  face_it = faces(tmesh).first;
  std::advance(face_it, rand.get_int(0, static_cast<int>(num_faces(tmesh))));

  // collect the sequence of simplicies crossed by the shortest path
  Sequence_collector sequence_collector;
  shortest_paths.shortest_path_sequence_to_source_points(*face_it, face_location, sequence_collector);

  // print the sequence using the visitor pattern
  Print_visitor print_visitor(tmesh);
  for (size_t i = 0; i < sequence_collector.sequence.size(); ++i)
    boost::apply_visitor(print_visitor, sequence_collector.sequence[i]);

  return 0;
}


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with distributed Gaussian non-negative matrix factorization.
 * We first create factors and then a data matrix
 * from these factors. THis process ensures that we know the best factorization of the input.
 * We then try to reconstruct the factors.
 */
#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
	boost::mpi::communicator& world = mfInit(argc, argv);

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 1000000;
	double sigma = 1; // standard deviation
	mf_size_type r = 10;

	// parameters for ALS
	unsigned epochs = 20;
	SlLoss loss;
	mf_size_type testNnz = nnz/100;

	BalanceType type = BALANCE_L2;;
	BalanceMethod method = BALANCE_SIMPLE;

	// parameters for distribution
	int tasksPerRank = 2;
	mf_size_type blocks = world.size() * tasksPerRank;

	mfStart();

	if (world.rank() == 0) {
	#ifndef NDEBUG
		LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
	#endif
		// generate original factors by sampling from a uniform[0,1] distribution
		Random32 random; // note: this takes a default seed (not randomized!)
		DenseMatrix wIn(size1, r);
		DenseMatrixCM hIn(r, size2);
		generateRandom(wIn, random, boost::uniform_real<>(0,1));
		generateRandom(hIn, random, boost::uniform_real<>(0,1));

		// generate a sparse matrix by selecting random entries from the generated factors
		// and add small Gaussian noise
		SparseMatrix v;
		generateRandom(v, nnz, wIn, hIn, random);
		//addRandom(v, random, boost::normal_distribution<>(0, 0.1));
		LOG4CXX_INFO(logger, "Data matrix: "
			<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
		v.sort();
		LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));
		SparseMatrixCM vc;
		copyCm(v, vc);

		// create a test matrix (without noise)
		SparseMatrix vTest;
		generateRandom(vTest, testNnz, wIn, hIn, random);
		LOG4CXX_INFO(logger, "Test matrix: "
			<< v.size1() << " x " << v.size2() << ", " << vTest.nnz() << " nonzeros");

		// generate initial factors by sampling from a uniform[0,1] distribution
		DenseMatrix w(size1, r);
		DenseMatrixCM h(r, size2);
		generateRandom(w, random, boost::uniform_real<>(0.0, 1.0));
		generateRandom(h, random, boost::uniform_real<>(0.0, 1.0));

		// distribute the input matrices and test matrix
		DistributedSparseMatrix dv = distributeMatrix("V", blocks, 1, true, v);
		LOG4CXX_INFO(logger, "Distributed data matrix: "
				<< dv.blocks1() << " x " << dv.blocks2() << " blocks");
		DistributedSparseMatrixCM dvc = distributeMatrix("VC", 1, blocks, false, vc);
		LOG4CXX_INFO(logger, "Distributed data matrix (CM): "
				<< dvc.blocks1() << " x " << dvc.blocks2() << " blocks");
		DistributedSparseMatrix dvTest = distributeMatrix("Vtest", blocks, blocks, true, vTest);
		LOG4CXX_INFO(logger, "Distributed test matrix: "
				<< dvTest.blocks1() << " x " << dvTest.blocks2() << " blocks");
		DistributedDenseMatrix dw = distributeMatrix("W", blocks, 1, true, w);
		DistributedDenseMatrixCM dh = distributeMatrix("H", 1, blocks, false, h);
		LOG4CXX_INFO(logger, "Distributed factor matrices");

		// initialize
		DapFactorizationData<> data(dv, dw, dh, tasksPerRank, &dvc);
		DsgdFactorizationData<> testJob(dvTest, dw, dh, tasksPerRank);
		Trace trace;
		Timer t;

		// run GNMF to try to reconstruct the original factors
		t.start();
		dgnmf(data, epochs, trace, type, method, &testJob);
		t.stop();
		LOG4CXX_INFO(logger, "Total time: " << t);

		// write trace to an R file
		LOG4CXX_INFO(logger, "Writing trace to " << "/tmp/dgnmf-trace.R");
		trace.toRfile("/tmp/dgnmf-trace.R", "dgnmf");
	}

	mfStop();
	mfFinalize();

	return 0;
}


#include <iostream>
#include <vector>
#include <string>
#include <Eigen/Dense>
#include "../include/layer.h"
#include <map>
#include <memory>

using namespace Eigen;

int main()
{
    using namespace MyDL;
    using std::cout;
    using std::endl;
    using std::map;
    using std::shared_ptr;
    using std::string;
    using std::unique_ptr;
    using std::vector;

    // Params
    // auto W = std::make_shared<MatrixXd>(2, 2); // 左辺はautoで問題なし
    // auto b = std::make_shared<MatrixXd>(1, 2);
    // *W = MatrixXd::Random(2, 2);
    // *b = MatrixXd::Zero(1, 2);
    map<string, shared_ptr<MatrixXd>> params;

    // Layers
    // vector<unique_ptr<BaseLayer>> layers; // 基底クラスのポインタのコンテナを用意する
    vector<shared_ptr<BaseLayer>> layers; // 基底クラスのポインタのコンテナを用意する
    map<string, unique_ptr<BaseLayer>> layers_map;
    // 抽象クラスのポインタに格納するので、unique_ptrとしてはBaseLayerに格納
    // unique_ptr<BaseLayer> p_layer1(new AddLayer()); // C++11ではmake_uniqueが存在しない
    // unique_ptr<BaseLayer> p_layer2(new MulLayer());
    // unique_ptr<BaseLayer> p_layer3(new ReLU());
    // unique_ptr<BaseLayer> p_layer4(new Sigmoid());
    // unique_ptr<BaseLayer> p_layer5(new MyDL::Affine(W, b));
    // unique_ptr<BaseLayer> p_layer6(new SoftmaxWithLoss());

    shared_ptr<BaseLayer> p_layer1 = std::make_shared<AddLayer>();
    shared_ptr<BaseLayer> p_layer2 = std::make_shared<MulLayer>();
    shared_ptr<BaseLayer> p_layer3 = std::make_shared<ReLU>();
    shared_ptr<BaseLayer> p_layer4 = std::make_shared<Sigmoid>();
    // shared_ptr<BaseLayer> p_layer5 = std::make_shared<MyDL::Affine>(W, b);
    shared_ptr<BaseLayer> p_layer5 = std::make_shared<MyDL::Affine>(2, 2); // サイズを入力するようにしてみる
    shared_ptr<BaseLayer> p_layer6 = std::make_shared<SoftmaxWithLoss>();

    // AddLayer    layer1;
    // MulLayer    layer2;
    // ReLU        layer3;
    // Sigmoid     layer4;
    // MyDL::Affine    layer5(W, b);
    // SoftmaxWithLoss layer6;

    // I/O containers
    vector<MatrixXd> inputs;
    vector<MatrixXd> outputs;
    // MatrixXd X = -2 * MatrixXd::Identity(2, 2) + MatrixXd::Ones(2, 2);
    MatrixXd X = MatrixXd::Identity(2, 2);
    MatrixXd Y = MatrixXd::Ones(2, 2);
    vector<MatrixXd> dout;
    dout.push_back(MatrixXd::Ones(2, 2));

    // inputs
    inputs.push_back(X);
    inputs.push_back(Y);

    // layers
    // layers.push_back(std::move(p_layer1)); // Layerの派生クラス(Add)のポインタをコンテナに格納
    // layers.push_back(std::move(p_layer2)); // Layerの派生クラス(Mul)のポインタをコンテナに格納
    // layers.push_back(std::move(p_layer3)); // Layerの派生クラス(ReLU)のポインタをコンテナに格納
    // layers.push_back(std::move(p_layer4)); // Layerの派生クラス(Sigmoid)のポインタをコンテナに格納
    // layers.push_back(std::move(p_layer5)); // Layerの派生クラス(Affine)のポインタをコンテナに格納
    // layers.push_back(std::move(p_layer6)); // Layerの派生クラス(SoftmaxWithLoss)のポインタをコンテナに格納

    layers.push_back(p_layer1); // Layerの派生クラス(Add)のポインタをコンテナに格納
    layers.push_back(p_layer2); // Layerの派生クラス(Mul)のポインタをコンテナに格納
    layers.push_back(p_layer3); // Layerの派生クラス(ReLU)のポインタをコンテナに格納
    layers.push_back(p_layer4); // Layerの派生クラス(Sigmoid)のポインタをコンテナに格納
    layers.push_back(p_layer5); // Layerの派生クラス(Affine)のポインタをコンテナに格納
    layers.push_back(p_layer6); // Layerの派生クラス(SoftmaxWithLoss)のポインタをコンテナに格納

    if (auto affine = std::dynamic_pointer_cast<MyDL::Affine>(p_layer5))
    {
        params["W"] = affine->pW;
        params["b"] = affine->pb;
    }

    // layers_map["Add"] = p_layer1;
    // layers_map["Affine"] = p_layer5;

    // cout << "--- map forward ---" << endl;
    // outputs = layers_map["Add"]->forward(inputs);
    // cout << outputs[0] << endl;
    // outputs = layers_map["Affine"]->forward(inputs);
    // cout << outputs[0] << endl;

    cout << "----forward----" << endl;
    for (int i = 0; i < layers.size(); i++)
    {
        cout << "----layer" << i << "----" << endl;
        outputs = layers[i]->forward(inputs); // コンテナに格納したLayerのアドレスから、メンバ関数を呼び出し(ポリモーフィズム)

        cout << outputs[0] << endl;
    }

    // Affineレイヤのパラメータを更新できるのか確認したいので、backwardの処理を走らせた後に、
    // gradsの内容を加えるという処理をし、その処理の前後でパラメータの内容が変化しているかを確認する。
    cout << "----backward----" << endl;
    for (int i = 0; i < layers.size(); i++)
    {
        vector<MatrixXd> grads;
        cout << "----layer" << i << "----" << endl;
        if (i < layers.size() - 1)
        {
            grads = layers[i]->backward(dout);
        }
        else
        {
            vector<MatrixXd> init_dout = {MatrixXd::Ones(1, 1)};
            grads = layers[i]->backward(init_dout);
        }

        for (int j = 0; j < grads.size(); j++)
        {
            cout << grads[j] << endl;
        }
    }

    if (auto affine = std::dynamic_pointer_cast<MyDL::Affine>(layers[4]))
    {
        cout << "--- Affine Param b ---" << endl;
        cout << *(affine->pb) << endl;
        cout << "params[b]:" << endl;
        cout << *(params["b"]) << endl;
        cout << "--- Affine Param W ---" << endl;
        cout << *(affine->pW) << endl;
        cout << "params[W]:" << endl;
        cout << *(params["W"]) << endl;

        *(params["b"]) -= affine->db;
        *(params["W"]) -= affine->dW;
    }
    // 別のスコープでアクセスしたときに更新されているか？
    if (auto re_affine = std::dynamic_pointer_cast<MyDL::Affine>(layers[4]))
    {
        cout << "--- after update ---" << endl; // 内部で処理しないと、affineが宣言されていない、となる。スコープ外？
        cout << *(re_affine->pb) << endl;
        cout << *(re_affine->pW) << endl;
    }

    cout << "params[b]" << endl;
    cout << *(params["b"]) << endl;
    cout << "params[W]" << endl;
    cout << *(params["W"]) << endl;

    return 0;
}


#include <iostream>
#include "Solver.hpp"

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Dense>

#include <chrono>

namespace py = pybind11;

using namespace Eigen;
using namespace std;

VectorXd solveQP( const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q, const py::EigenDRef<const VectorXd> &warm_start,const double epsilon =1e-10, const double mu_prox = 1e-7, const int max_iter=1000, const bool adaptative_rho=true){
    Solver solver;
    VectorXd solution(q.size());
    solution = solver.solveQP(P,q,warm_start,epsilon,mu_prox,max_iter,adaptative_rho);
    return solution;
}

VectorXd solveDerivativesQP(const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q, const py::EigenDRef<const VectorXd> &l, const py::EigenDRef<const VectorXd> &grad_l, const double epsilon =1e-10){
    Solver solver;
    VectorXd gamma(l.size()),bl(l.size());
    gamma = solver.dualFromPrimalQP(P,q,l,epsilon);
    bl = solver.solveDerivativesQP(P,q,l,gamma,grad_l,epsilon);
    return bl;
}

VectorXd solveQCQP( const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q,const py::EigenDRef<const VectorXd> &l_n, const py::EigenDRef<const VectorXd> &mu, const py::EigenDRef<const VectorXd> &warm_start,const double epsilon=1e-10,const double mu_prox = 1e-7, const int max_iter = 1000, const bool adaptative_rho = true){
    Solver solver;
    VectorXd solution(q.size()), mul_n(l_n.size());
    mul_n = l_n.cwiseProduct(mu);
    solution = solver.solveQCQP(P,q,mul_n,warm_start,epsilon,mu_prox,max_iter,adaptative_rho);
    return solution;
}

std::tuple<MatrixXd,MatrixXd,VectorXd> solveDerivativesQCQP(const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q, const py::EigenDRef<const VectorXd> &l_n, const py::EigenDRef<const VectorXd> &mu, const py::EigenDRef<const VectorXd> &l, const py::EigenDRef<const VectorXd> &grad_l, const double epsilon =1e-10){
    Solver solver;
    MatrixXd E1(l_n.size(),l_n.size()),E2(l_n.size(),l_n.size());
    VectorXd mul_n(l_n.size()),gamma(l.size()),blgamma(l.size());
    mul_n = l_n.cwiseProduct(mu);
    gamma = solver.dualFromPrimalQCQP(P,q,mul_n,l,epsilon);
    std::tie(E1,E2) = solver.getE12QCQP(l_n, mu, gamma);
    blgamma = solver.solveDerivativesQCQP(P,q,mul_n,l,gamma,grad_l,epsilon);
    return std::make_tuple(E1,E2,blgamma);
}

VectorXd solveLCQP( const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q, const py::EigenDRef<const VectorXd> &warm_start,const double epsilon=1e-10,const double mu_prox = 1e-7, const int max_iter = 1000, const bool adaptative_rho = true){
    Solver solver;
    VectorXd solution(q.size());
    solution = solver.solveLCQP(P,q,warm_start,epsilon,mu_prox,max_iter,adaptative_rho);
    return solution;
}

VectorXd solveDerivativesLCQP(const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q, const py::EigenDRef<const VectorXd> &l, const py::EigenDRef<const VectorXd> &grad_l, const double epsilon =1e-10){
    Solver solver;
    return solver.solveDerivativesLCQP(P,q,l,grad_l,epsilon);
}

PYBIND11_MODULE(diffsolvers, m) {
    m.doc() = "module solving QCQP and QP with ADMM, and computing the derivatives of the solution using implicit differentiation of KKT optimality conditions";
    m.def("solveQP", &solveQP, "A function which solves a QP problem with a regularized ADMM algorithm",py::arg("P"), py::arg("q"),py::arg("warm_start"),py::arg("epsilon") = 1e-10,py::arg("mu_prox")= 1e-7,py::arg("max_iter")= 1000,py::arg("adaptative_rho")= true, py::return_value_policy::reference_internal );
    m.def("solveQCQP", &solveQCQP, "A function which solves a QCQP problem with a regularized ADMM algorithm",py::arg("P"), py::arg("q"), py::arg("l_n"),py::arg("mu"), py::arg("warm_start"),py::arg("epsilon")= 1e-10,py::arg("mu_prox")= 1e-7,py::arg("max_iter")= 1000,py::arg("adaptative_rho")= true, py::return_value_policy::reference_internal );
    m.def("solveLCQP", &solveLCQP, "A function which solves a QCQP, Lorentz-cone-constrained problem with regularized ADMM algorithm",py::arg("P"), py::arg("q"), py::arg("warm_start"),py::arg("epsilon")= 1e-10,py::arg("mu_prox")= 1e-7,py::arg("max_iter")= 1000,py::arg("adaptative_rho")= true, py::return_value_policy::reference_internal );
    m.def("solveDerivativesQP", &solveDerivativesQP, "A function which solves the differentiated KKT system of a QP",py::arg("P"), py::arg("q"), py::arg("l"), py::arg("grad_l"), py::arg("epsilon")=1e-10);
    m.def("solveDerivativesQCQP", &solveDerivativesQCQP, "A function which solves the differentiated KKT system of a QCQP",py::arg("P"), py::arg("q"), py::arg("l_n"),py::arg("mu"), py::arg("l"), py::arg("grad_l"), py::arg("epsilon")=1e-10 );
    m.def("solveDerivativesLCQP", &solveDerivativesLCQP, "A function which solves the differentiated KKT system of a Lorentz-cone-constrained QCQP",py::arg("P"), py::arg("q"), py::arg("l"), py::arg("grad_l"), py::arg("epsilon")=1e-10 );

}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/
//  Based on https://github.com/sportdeath/audio_transport/
// T.Henderson and J.Solomon, Audio Transport:
// a generalized portamento via optimal transport.
// Proceedings of DAFX 2019.

#pragma once

#include "STFT.hpp"
#include "WindowFuncs.hpp"
#include "../util/FFT.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cmath>

namespace fluid {
namespace algorithm {

struct SpetralMass
{
  index  startBin;
  index  centerBin;
  index  endBin;
  double mass;
};

class AudioTransport
{
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXi = Eigen::ArrayXi;
  using ArrayXcd = Eigen::ArrayXcd;
  template <typename T>
  using Ref = Eigen::Ref<T>;
  using TransportMatrix = std::vector<std::tuple<index, index, double>>;
  template <typename T>
  using vector = std::vector<T>;

public:
  AudioTransport(index maxFFTSize)
      : mWindowSize(maxFFTSize), mFFTSize(maxFFTSize),
        mBins(maxFFTSize / 2 + 1), mFFT(maxFFTSize),
        mSTFT(maxFFTSize, maxFFTSize, maxFFTSize / 2),
        mISTFT(maxFFTSize, maxFFTSize, maxFFTSize / 2),
        mReassignSTFT(maxFFTSize, maxFFTSize, maxFFTSize / 2)
  {}

  void init(index windowSize, index fftSize, index hopSize)
  {
    mWindowSize = windowSize;
    mWindow = ArrayXd::Zero(mWindowSize);
    WindowFuncs::map()[WindowFuncs::WindowTypes::kHann](mWindowSize, mWindow);
    mWindowSquared = mWindow * mWindow;
    mFFTSize = fftSize;
    mHopSize = hopSize;
    mBins = fftSize / 2 + 1;
    mPhase = ArrayXd::Zero(mBins);
    mChanged = ArrayXi::Zero(mBins);
    mBinFreqs = ArrayXd::LinSpaced(mBins, 0, mBins - 1) * (2 * pi) / mFFTSize;
    mPhaseDiff = mBinFreqs * mHopSize;
    mSTFT = STFT(windowSize, fftSize, hopSize);
    mISTFT = ISTFT(windowSize, fftSize, hopSize);
    mReassignSTFT = STFT(windowSize, fftSize, hopSize,
                         static_cast<index>(WindowFuncs::WindowTypes::kHannD));
    mInitialized = true;
  }

  bool initialized() const { return mInitialized; }

  void processFrame(RealVectorView in1, RealVectorView in2, double weight,
                    RealMatrixView out)
  {
    using namespace _impl;
    using namespace Eigen;
    assert(mInitialized);
    ArrayXd  frame1 = asEigen<Array>(in1);
    ArrayXd  frame2 = asEigen<Array>(in2);
    ArrayXcd spectrum1(mBins);
    ArrayXcd spectrum1Dh(mBins);
    ArrayXcd spectrum2(mBins);
    ArrayXcd spectrum2Dh(mBins);
    ArrayXd  output(frame1.size());
    mSTFT.processFrame(frame1, spectrum1);
    mReassignSTFT.processFrame(frame1, spectrum1Dh);
    mSTFT.processFrame(frame2, spectrum2);
    mReassignSTFT.processFrame(frame2, spectrum2Dh);
    ArrayXcd result =
        interpolate(spectrum1, spectrum1Dh, spectrum2, spectrum2Dh, weight);
    mISTFT.processFrame(result, output);
    out.row(0) <<= asFluid(output);
    out.row(1) <<= asFluid(mWindowSquared);
  }

  vector<SpetralMass> segmentSpectrum(const Ref<ArrayXd> mag,
                                      const Ref<ArrayXd> reasignedFreq)
  {

    vector<SpetralMass> masses;
    double              totalMass = mag.sum() + epsilon;
    ArrayXi             sign = (reasignedFreq > mBinFreqs).cast<int>();
    mChanged.setZero();
    mChanged.segment(1, mBins - 1) =
        sign.segment(1, mBins - 1) - sign.segment(0, mBins - 1);
    SpetralMass currentMass{0, 0, 0, 0};
    for (index i = 1; i < mChanged.size(); i++)
    {
      if (mChanged(i) == -1)
      {
        double d1 = reasignedFreq(i - 1) - mBinFreqs(i - 1);
        double d2 = mBinFreqs(i) - reasignedFreq(i);
        currentMass.centerBin = d1 < d2 ? i - 1 : i;
      }
      if (mChanged(i) == 1)
      {
        currentMass.endBin = i;
        currentMass.mass =
            mag.segment(currentMass.startBin, i - currentMass.startBin).sum() /
            totalMass;
        masses.emplace_back(currentMass);
        currentMass = SpetralMass{i, i, i, 0};
      }
    }
    currentMass.endBin = mBins;
    currentMass.mass =
        mag.segment(currentMass.startBin, mBins - currentMass.startBin).sum() /
        totalMass;
    masses.emplace_back(currentMass);
    return masses;
  }

  TransportMatrix computeTransportMatrix(std::vector<SpetralMass> m1,
                                         std::vector<SpetralMass> m2)
  {
    TransportMatrix matrix;
    index           index1 = 0, index2 = 0;
    double          mass1 = m1[0].mass;
    double          mass2 = m2[0].mass;
    while (true)
    {
      if (mass1 < mass2)
      {
        matrix.emplace_back(index1, index2, mass1);
        mass2 -= mass1;
        index1++;
        if (index1 >= asSigned(m1.size())) break;
        mass1 = m1[asUnsigned(index1)].mass;
      }
      else
      {
        matrix.emplace_back(index1, index2, mass2);
        mass1 -= mass2;
        index2++;
        if (index2 >= asSigned(m2.size())) break;
        mass2 = m2[asUnsigned(index2)].mass;
      }
    }
    return matrix;
  }

  void placeMass(const SpetralMass mass, index bin, double scale,
                 double centerPhase, Ref<ArrayXcd> input, Ref<ArrayXcd> output,
                 double nextPhase, Ref<ArrayXd> amplitudes, Ref<ArrayXd> phases)
  {
    double phaseShift = centerPhase - std::arg(input(mass.centerBin));
    for (index i = mass.startBin; i < mass.endBin; i++)
    {
      index pos = i + bin - mass.centerBin;
      if (pos < 0 || pos >= output.size()) continue;
      double phase = phaseShift + std::arg(input(i));
      double mag = scale * std::abs(input(i));
      output(pos) += std::polar(mag, phase);
      if (mag > amplitudes(pos))
      {
        amplitudes(pos) = mag;
        phases(pos) = nextPhase;
      }
    }
  }

  ArrayXcd interpolate(Ref<ArrayXcd> in1, Ref<ArrayXcd> in1Dh,
                       Ref<ArrayXcd> in2, Ref<ArrayXcd> in2Dh,
                       double interpolation)
  {
    ArrayXd  mag1 = in1.abs().real();
    ArrayXd  mag2 = in2.abs().real();
    ArrayXcd result = ArrayXcd::Zero(mBins);
    double   mag1Sum = mag1.sum();
    double   mag2Sum = mag2.sum();
    if (mag1Sum <= 0 && mag2Sum <= 0) { return result; }
    else if (mag1Sum > 0 && mag2Sum <= 0)
    {
      return in1;
    }
    else if (mag1Sum <= 0 && mag2Sum > 0)
    {
      return in2;
    }
    ArrayXd                  phase1 = in1.arg().real();
    ArrayXd                  phase2 = in2.arg().real();
    ArrayXd                  reasignedW1 = mBinFreqs - (in1Dh / in1).imag();
    ArrayXd                  reasignedW2 = mBinFreqs - (in2Dh / in2).imag();
    ArrayXd                  newAmplitudes = ArrayXd::Zero(mBins);
    ArrayXd                  newPhases = ArrayXd::Zero(mBins);
    std::vector<SpetralMass> s1 = segmentSpectrum(mag1, reasignedW1);
    std::vector<SpetralMass> s2 = segmentSpectrum(mag2, reasignedW2);
    if (s1.size() == 0 || s2.size() == 0) { return result; }

    TransportMatrix matrix = computeTransportMatrix(s1, s2);
    for (auto t : matrix)
    {
      SpetralMass m1 = s1[asUnsigned(std::get<0>(t))];
      SpetralMass m2 = s2[asUnsigned(std::get<1>(t))];
      index  interpolatedBin = std::lrint((1 - interpolation) * m1.centerBin +
                                         interpolation * m2.centerBin);
      double interpolationFactor = interpolation;
      if (m1.centerBin != m2.centerBin)
      {
        interpolationFactor =
            ((double) interpolatedBin - (double) m1.centerBin) /
            ((double) m2.centerBin - (double) m1.centerBin);
      }
      double interpolatedFreq =
          (1 - interpolationFactor) * reasignedW1(m1.centerBin) +
          interpolationFactor * reasignedW2(m2.centerBin);
      double nextPhase = mPhase(interpolatedBin) + interpolatedFreq * mHopSize;
      double centerPhase = nextPhase - mPhaseDiff(interpolatedBin);
      placeMass(m1, interpolatedBin,
                (1 - interpolation) * std::get<2>(t) / m1.mass, centerPhase,
                in1, result, nextPhase, newAmplitudes, newPhases);
      placeMass(m2, interpolatedBin, interpolation * std::get<2>(t) / m2.mass,
                centerPhase, in2, result, nextPhase, newAmplitudes, newPhases);
    }
    mPhase = newPhases;
    return result;
  }

  index   mWindowSize{1024};
  index   mHopSize{512};
  ArrayXd mBinFreqs;
  ArrayXd mWindow;
  ArrayXd mWindowSquared;
  index   mFFTSize{1024};
  index   mBins{513};
  FFT     mFFT;
  bool    mInitialized{false};
  ArrayXd mPhase;
  ArrayXd mPhaseDiff;
  ArrayXi mChanged;
  STFT    mSTFT;
  ISTFT   mISTFT;
  STFT    mReassignSTFT;
};
} // namespace algorithm
} // namespace fluid