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

// Written by Simon Praetorius

#ifndef ITL_FGMRES_INCLUDE
#define ITL_FGMRES_INCLUDE

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

#include "details.hpp"

namespace itl
{

  /// Flexible Generalized Minimal Residual method (without restart)
  /// Cite: Youcef Saad, A Flexible Inner-Outer Preconditioned GMRES Algorithm, 1993
  /** It computes at most kmax_in iterations (or size(x) depending on what is smaller)
      regardless on whether the termination criterion is reached or not.   **/
  template <typename Matrix, typename Vector, typename Preconditioner, typename Iteration>
  int fgmres_full(const Matrix& A, Vector& x, const Vector& b,
                  Preconditioner& P, Iteration& iter)
  {
    using mtl::irange;
    using mtl::iall;
    using std::abs;
    using std::sqrt;
    using math::reciprocal;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    typedef typename mtl::Collection<Vector>::size_type  Size;

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

    const Scalar                zero= math::zero(Scalar()), breakdown_tol= 1.e-16, kappa = 10.0;
    Scalar                      rho, bnrm2, temp, hr;
    Size                        k, kmax(std::min(size(x), Size(iter.max_iterations() - iter.iterations())));
    Vector                      w(resource(x)), r(b - A*x);
    mtl::matrix::multi_vector<Vector>   V(Vector(resource(x), zero), kmax+1);
    mtl::matrix::multi_vector<Vector>   Z(Vector(resource(x), zero), kmax+1);
    mtl::matrix::dense2D<Scalar>        H(kmax+1, kmax);

    mtl::vector::dense_vector<Scalar>   sn(kmax, zero), cs(kmax, zero), s(kmax+1, zero), y(kmax, zero);  // replicated in distributed solvers

    bnrm2 = two_norm(b);
    if (bnrm2 == zero)
    {
      set_to_zero(x);
      // b == 0 => solution = 0
      return iter.terminate(bnrm2);
    }

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

    V.vector(0) = r * reciprocal(rho);
    H = zero;
    s[0] = rho;

    // FGMRES iteration
    for (k= 0; k < kmax && !iter.finished(rho) ; ++k, ++iter)
    {
      Z.vector(k) = solve(P, V.vector(k));
      w = A * Z.vector(k);
      temp = two_norm(w);

      for (Size j= 0; j < k+1; j++)
      {
        H[j][k] = dot(V.vector(j), w);
        w -= H[j][k] * V.vector(j);
      }
      H[k+1][k]= two_norm(w);

      // reorthogonalization, only if "heuristic" condition is fulfilled
      if (H[k+1][k] < temp * reciprocal(kappa))
      {
        for (Size i= 0; i < k+1; i++)
        {
          hr = dot(w, V.vector(i));
          H[i][k] += hr;
          w -= hr * V.vector(i);
        }
        temp = two_norm(w);

        if (temp < H[k+1][k] * reciprocal(kappa))
        {
          set_to_zero(w);
          H[k+1][k] = 0.0;
        }
        else
        {
          H[k+1][k] = temp;
        }
      }

      if (H[k+1][k] < breakdown_tol)
        return iter.fail(2, "FGMRES: Singular matrix - nearly hard breakdown");

      V.vector(k+1) = w * reciprocal(H[k+1][k]);

      // k Given's rotations
      for(Size i= 0; i < k; i++)
      {
        temp =        cs[i]*H[i][k] + sn[i]*H[i+1][k];
        H[i+1][k] = - sn[i]*H[i][k] + cs[i]*H[i+1][k];
        H[i][k] = temp;
      }

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

      s[k+1] = -sn[k]*s[k];
      s[k]   =  cs[k]*s[k];
      H[k][k] = cs[k]*H[k][k] + sn[k]*H[k+1][k];
      H[k+1][k] = 0.0;

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

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

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

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

    x += Z.vector(range) * y[range];

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

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

    return iter;
  }

} // namespace itl

#endif // ITL_FGMRES_INCLUDE


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
/**
 * @file basis.hpp
 * @author Boston Cleek
 * @date 20 Nov 2020
 * @brief Fourier cosine basis
 */
#ifndef BASIS_HPP
#define BASIS_HPP

#include <armadillo>

namespace ergodic_exploration
{
using arma::imat;
using arma::mat;
using arma::vec;

/**
 * @brief Fourier cosine basis */
class Basis
{
private:
  template <class ModelT>
  friend class ErgodicControl;

public:
  /**
   * @brief Constructor
   * @param lx - x-axis lenght of domain
   * @param ly - y-axis lenght of domain
   * @param num_basis - number of each basis functions per dimension
   */
  Basis(double lx, double ly, unsigned int num_basis);

  /**
   * @brief Compose cosine basis functions given positon
   * @param x - robot position [x y]
   * @return cosine basis functions (num_basis^2 x 1)
   * @details x must be on domain [0 lx] x [0 ly]
   */
  vec fourierBasis(const vec& x) const;

  /**
   * @brief Compose gradient cosine basis functions
   * @param x - robot position [x y]
   * @return gradient of each basis function (2 x num_basis^2)
   * @details x must be on domain [0 lx] x [0 ly]
   */
  mat gradFourierBasis(const vec& x) const;

  /**
   * @brief Compose trajectory fourier coefficients
   * @param xt - trajectory
   * @return trajectory fourier coefficients (num_basis^2 x 1)
   * @details xt must be on domain [0 lx] x [0 ly] and robot heading is not required
   */
  vec trajCoeff(const mat& xt) const;

  /**
   * @brief Compose spatial fourier coefficients
   * @param phi_vals - target evaluated at each grid cell in phi_grid
   * @param phi_grid - discretization of fourier domain
   * @return spatial fourier coefficients (num_basis^2 x 1)
   * @details phi_grid is not the occupancy grid, the first row contains the
   * x-cordinates and the second row contains the corresponding y-coordinates.
   * The index of the elements in phi_vals must correspond to the columns in phi_grid.
   */
  vec spatialCoeff(const vec& phi_vals, const mat& phi_grid) const;

private:
  double lx_, ly_;            // length of domain
  unsigned int total_basis_;  // number of basis functions
  vec lamdak_;                // frequency coefficients weights
  imat k_;                    // basis number
};
}  // namespace ergodic_exploration
#endif


/* p_integrand.cpp */
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/interpolators/cubic_hermite.hpp>
#include <algorithm>

extern "C"
{
/* Survival function for R^phi*W */
int RW_marginal_C(double *xval, double phi, double gamma, int n_xval, double *result){
    double tmp2 = pow(gamma/2, phi)/boost::math::tgamma(0.5);
    double tmp1, tmp0, a;
    a = 0.5-phi;

    for(int i=0; i<n_xval; i++){
        tmp1 = gamma/(2*pow(xval[i],1/phi));
        tmp0 = tmp2/(a*xval[i]);
        result[i] = boost::math::gamma_p(0.5L,tmp1) + boost::math::tgamma((long double)(a+1),tmp1)*tmp0-pow(tmp1,a)*exp(-tmp1)*tmp0;
    }
    return 1;
}

/* Marginal distribution function for R^phi*W + epsilon */
int pRW_me_interp_C(double *xval, double *xp, double *surv_p, double tau_sqd, double phi, double gamma, int n_xval, int n_grid, double *result){
    bool tau_bool = (tau_sqd > 0.05);
    double tp[n_grid];
    double integrand_p[n_grid];
    double tmp, tmp_res; /* temporary constant */
    double tmp_sum = 0; /* temporary trapesoid sum */
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double sd_const_pi =sqrt(2*M_PI)*sd;
    int i,j, tmp_int; /* iterative constants */

    for (i = 0; i < n_xval; i++) {
        if(tau_bool & (xval[i]<820)){
            /* Calculate integrand on a grid */
            for(j=0; j<n_grid;j++){
                tmp = xval[i]-xp[j];
                tp[j] = tmp;
                integrand_p[j] = exp(-tmp*tmp/(2*tau_sqd)) * surv_p[j];
            }

            /* Numerical integral using the trapesoid method */
            for(j=0; j<(n_grid-1);j++){
                tmp_sum+= (tp[j+1]-tp[j])*(integrand_p[j] + integrand_p[j+1])/2;
            }
            tmp_res = 0.5*erfc(-xval[i]/sd_const)-tmp_sum/sd_const_pi;
            tmp_sum = 0;

            /* CDF value must be greater than 0 */
            if(tmp_res < 0){
                tmp_res = 0;
            }
            result[i] = tmp_res;
        }
        else{
            tmp_int = RW_marginal_C(&xval[i], phi, gamma, 1, &tmp_res);
            result[i] = 1-tmp_res;
        }
    }

    return 1;
}

/* Transform to uniform scales from RW mixtures */
int RW_me_2_unifs(double *X, double *xp, double *Surv, double tau_sqd, double *phi, double gamma,
                    int n_s, int n_grid, int n_t, double *unifs){
    int tmp_int, X_lookup, Surv_lookup;
    for (int i = 0; i<n_s;i++){
        X_lookup = i*n_t;
        Surv_lookup = i*n_grid;
        tmp_int = pRW_me_interp_C(X+X_lookup, xp, Surv+Surv_lookup, tau_sqd, phi[i], gamma, n_t, n_grid, unifs+X_lookup);
    }

    return 1;
}

/* Get the quantile range for certain probability levels */
int find_xrange_pRW_me_C(double min_p, double max_p, double min_x, double max_x, double *xp, double *surv_p, double tau_sqd, double phi, double gamma, int n_grid, double *x_range){
    if (min_x >= max_x){
        printf("Initial value of mix_x must be smaller than max_x.\n");
        exit(EXIT_FAILURE);
    }

    /* First the min */
    double p_min_x;
    int tmp_int;
    tmp_int = pRW_me_interp_C(&min_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_min_x);
    while (p_min_x > min_p){
        min_x = min_x-40/phi;
        tmp_int = pRW_me_interp_C(&min_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_min_x);
    }

    x_range[0] = min_x;

    /* Now the max */
    double p_max_x;
    tmp_int = pRW_me_interp_C(&max_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_max_x);
    while (p_max_x < max_p){
        max_x = max_x*2; /* Upper will set to 20 initially */
        tmp_int = pRW_me_interp_C(&max_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_max_x);
    }

    x_range[1] = max_x;
    return 1;
}

/* PCHIP 1-D monotonic cubic interpolation */
using boost::math::interpolators::cubic_hermite;
int pchip(double *x_input, double *y_input, double *x_pred, double *y_pred_res, int n, int n_pred){
    std::vector<double> x(n);
    memcpy(&x[0], x_input, n*sizeof(double));
    std::vector<double> y(n);
    memcpy(&y[0], y_input, n*sizeof(double));

    double left_endpoint_derivative = std::numeric_limits<double>::quiet_NaN();
    double right_endpoint_derivative = std::numeric_limits<double>::quiet_NaN();
    std::vector<double> s(x.size(), std::numeric_limits<double>::quiet_NaN());
    if (isnan(left_endpoint_derivative)){
        // O(h) finite difference derivative:
        // This, I believe, is the only derivative guaranteed to be monotonic:
        s[0] = (y[1]-y[0])/(x[1]-x[0]);
    }else{
        s[0] = left_endpoint_derivative;
    }

    for (int k = 1; k < n-1; ++k) {
        double hkm1 = x[k] - x[k-1];
        double dkm1 = (y[k] - y[k-1])/hkm1;

        double hk = x[k+1] - x[k];
        double dk = (y[k+1] - y[k])/hk;
        double w1 = 2*hk + hkm1;
        double w2 = hk + 2*hkm1;
        if ( (dk > 0 && dkm1 < 0) || (dk < 0 && dkm1 > 0) || dk == 0 || dkm1 == 0){
            s[k] = 0;
        }else{
            s[k] = (w1+w2)/(w1/dkm1 + w2/dk);
        }

    }
    // Quadratic extrapolation at the other end:
    if (isnan(right_endpoint_derivative)){
                s[n-1] = (y[n-1]-y[n-2])/(x[n-1] - x[n-2]);
    }else{
                s[n-1] = right_endpoint_derivative;
    }

    auto spline = cubic_hermite<std::vector<double>>(std::move(x), std::move(y), std::move(s));

    for(int j=0; j<n_pred; j++){
        y_pred_res[j] = spline(x_pred[j]);
    }
    return 1;
}

/* Quantile inverse function for R^phi*W + epsilon*/
int qRW_me_interp(double *p, double *xp, double *surv_p, double tau_sqd, double phi, double gamma,
                  int n_p, int n_grid, double *result,
                  double *cdf_vals, double *x_vals, int n_x=400, double lower=5, double upper=20
                ){
    int tmp_int, i, zeros;
    double min_p, max_p, delta, tmp_double;
    bool large_delta_large_x = false;

    // (1) When phi is varying over space, we need to calculate one quantile for each phi value.
    // Given more accuarte [lower,upper] values for the single p, we can decrease n_x.
    if(n_p<2){ n_x=floor(100*(p[0]+0.1));}
    double x_vals1[n_x+150];
    double cdf_vals1[n_x+150];

    // (2) Generate x_vals and cdf_vals to interpolate
    if(isnan(x_vals[0])){
        double x_range[2];
        max_p = *std::max_element(p, p+n_p);
        min_p = *std::min_element(p, p+n_p);
        tmp_int = find_xrange_pRW_me_C(min_p, max_p, lower, upper, xp, surv_p, tau_sqd,
                                       phi, gamma, n_grid, x_range);
        if (isinf(x_range[1])){   // Upper is set to 20 initially
            x_range[1] = std::pow(10,20);
            large_delta_large_x = true;
        }
        if (x_range[0]<=0){
            delta = (0.0001 - x_range[0]) / 149;
            for(i=0; i<150; i++){
                x_vals1[i] = x_range[0] + delta * i;
            }
            delta = (log(x_range[1])-log(0.0001001)) / (n_x-1);
            for(i=0; i<n_x; i++){
                x_vals1[i+150] = exp(log(0.0001001) + delta * i);
            }
            n_x=150+n_x;
        }else{
            delta = (log(x_range[1])-log(x_range[0])) / (n_x-1);
            for(i=0; i<n_x; i++){
                x_vals1[i] = exp(log(x_range[0]) + delta * i);
            }
        }
        tmp_int = pRW_me_interp_C(x_vals1, xp, surv_p, tau_sqd, phi, gamma, n_x, n_grid, cdf_vals1);
    }else{
        double x_vals1[n_x];
        double cdf_vals1[n_x];
        memcpy(x_vals1, x_vals, n_x*sizeof(double));
        if(isnan(cdf_vals[0])){
            tmp_int = pRW_me_interp_C(x_vals1, xp, surv_p, tau_sqd, phi, gamma, n_x, n_grid, cdf_vals1);
        }else{
            memcpy(cdf_vals1, cdf_vals, n_x*sizeof(double));
        }
    }

    // (3) Obtain the quantile level using the interpolated function
    if(!large_delta_large_x){
        zeros = 0;
        tmp_double = cdf_vals1[zeros];
        while (tmp_double==0) {
            tmp_double = cdf_vals1[++zeros];
        }
        tmp_int = pchip(cdf_vals1+zeros, x_vals1+zeros, p, result, n_x-zeros, n_p);
    }else{
        int exceeds = 0;
        for (i=0;i<n_p;i++){
            if(p[i]>cdf_vals1[n_x-1]) exceeds+=1;
        }
        tmp_int = pchip(cdf_vals1, x_vals1, p, result, n_x, n_p-exceeds);
        for (i=n_p-exceeds;i<n_p;i++){
            result[i] = x_vals[n_x-1];
        }
    }
    return 1;
}

/* Density function for R^phi*W */
int RW_density_C(double *xval, double phi, double gamma, int n_xval, double *result){
    double tmp2 = pow(gamma/2, phi)/boost::math::tgamma(0.5);
    double tmp1, tmp0, a;
    a = 0.5-phi;

    for(int i=0; i<n_xval; i++){
        tmp1 = gamma/(2*pow(xval[i],1/phi));
        tmp0 = tmp2/(a*pow(xval[i],2));
        result[i] = (boost::math::tgamma((long double)(a+1),tmp1)-pow(tmp1,a)*exp(-tmp1))*tmp0;
    }
    return 1;
}

/* Marginal density function for R^phi*W + epsilon */
int dRW_me_interp_C(double *xval, double *xp, double *den_p, double tau_sqd, double phi, double gamma, int n_xval, int n_grid, double *result){
    double thresh_large = 820;
    if(tau_sqd < 1) {
        thresh_large = 50;
    }
    bool tau_bool = (tau_sqd > 0.05);

    double tp[n_grid];
    double integrand_p[n_grid];
    double tmp, tmp_res; /* temporary constant */
    double tmp_sum = 0; /* temporary trapesoid sum */
    double sd = sqrt(tau_sqd);
    double sd_const_pi =sqrt(2*M_PI)*sd;
    int i,j, tmp_int; /* iterative constants */

    for (i = 0; i < n_xval; i++) {
        if(tau_bool & (xval[i]<thresh_large)){
            /* Calculate integrand on a grid */
            for(j=0; j<n_grid;j++){
                tmp = xval[i]-xp[j];
                tp[j] = tmp;
                integrand_p[j] = exp(-tmp*tmp/(2*tau_sqd)) * den_p[j];
            }

            /* Numerical integral using the trapesoid method */
            for(j=0; j<(n_grid-1);j++){
                tmp_sum+= (tp[j+1]-tp[j])*(integrand_p[j] + integrand_p[j+1])/2;
            }
            tmp_res = tmp_sum/sd_const_pi;
            tmp_sum = 0;
            result[i] = tmp_res;
        }else if((tau_bool & (xval[i]>=thresh_large))|(!tau_bool & (xval[i]>0))){
            tmp_int = RW_density_C(&xval[i], phi, gamma, 1, &tmp_res);
            result[i] = tmp_res;
        }else{
            result[i] = 0;
        }
    }

    return 1;
}

int density_interp_grid(double *xp, double *phi, double gamma, int n_phi, int n_grid, double *Den, double *Surv){
    int counter = 0;
    int i,j, tmp_int;
    double tmp_surv, tmp_den;
    double tmp2, tmp1, tmp0, a, tmp_incomp, tmp_phi, tmp_phi_inv, tmp_xp;
    double gamma_half = gamma/2;

    for(i=0; i<n_phi; i++){
        tmp_phi = phi[i];
        a = 0.5-tmp_phi;
        tmp2 = std::pow(gamma_half, tmp_phi)/(a*sqrt(M_PI));
        tmp_phi_inv = 1/tmp_phi;
        for(j=0; j<n_grid; j++){
            tmp_xp = xp[j];
            tmp1 = gamma_half/std::pow(tmp_xp,tmp_phi_inv);
            tmp0 = tmp2/tmp_xp;
            tmp_incomp = (boost::math::tgamma((long double)(a+1),tmp1)-std::pow(tmp1,a)*exp(-tmp1))*tmp0;
            Surv[counter] = boost::math::gamma_p(0.5L,tmp1) + tmp_incomp;
            Den[counter++] = tmp_incomp/tmp_xp;
        }
    }
    return 1;
}

double dgev_C(double y, double loc, double scale, double shape, bool log_out){
    double t = std::pow(1+shape*((y-loc)/scale), -1/shape);
    double result;
    if(log_out){
        result = -log(scale)+(shape+1)*log(t)-t;
    }else{
        result = std::pow(t, shape+1)*exp(-t)/scale;
    }
    return result;
}

double dnorm_C(double y, double mean, double sd, bool log_out){
    double t=(y-mean)/sd;
    double result;
    if(log_out){
        result = -0.5*log(2*M_PI)-log(sd)-0.5*t*t;
    }else{
        result = exp(-0.5*t*t)/(sqrt(2*M_PI)*sd);
    }
    return result;
}

/* Thresh_X and Thresh_X_above are required */
/* xp, den_p and surv_p are required */
/* Calculate column_wise in C order OR one time  */
double marg_transform_data_mixture_me_likelihood_C(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double *phi, double gamma,
                                                double *xp, double *Den, int n_s, int n_grid){
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double ll=0;
    double RW_den;
    int i, tmp_int, Den_lookup;

    for (i=0; i<n_s; i++){
        if(cen[i]){
            ll += log(0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else if(cen_above[i]){
            ll += log(1-0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else{
            Den_lookup = i*n_grid;
            tmp_int = dRW_me_interp_C(&X[i], xp, &Den[Den_lookup], tau_sqd, phi[i], gamma, 1, n_grid, &RW_den);
            ll += dnorm_C(X[i], X_s[i], sd, true)+dgev_C(Y[i], Loc[i], Scale[i], Shape[i], true)-log(RW_den);
        }
    }
    return ll;
}

/* Calculate row_wise in F order OR one location */
double marg_transform_data_mixture_me_likelihood_F(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double phi, double gamma,
                                                double *xp, double *den_p, int n_t, int n_grid){
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double ll=0;
    double RW_den;
    int i, tmp_int;

    for (i=0; i<n_t; i++){
        if(cen[i]){
            ll += log(0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else if(cen_above[i]){
            ll += log(1-0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else{
            tmp_int = dRW_me_interp_C(&X[i], xp, den_p, tau_sqd, phi, gamma, 1, n_grid, &RW_den);
            ll += dnorm_C(X[i], X_s[i], sd, true)+dgev_C(Y[i], Loc[i], Scale[i], Shape[i], true)-log(RW_den);
        }
    }
    return ll;
}

/* Calculate all locations and all times */
double marg_transform_data_mixture_me_likelihood_global(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double *phi, double gamma,
                                                double *xp, double *Den, int n_s, int n_t, int n_grid){

    double ll=0;
    int site, tmp_int, Den_lookup, X_lookup;

    for (site=0; site<n_s; site++){
        X_lookup = site*n_t;
        Den_lookup = site*n_grid;
        ll += marg_transform_data_mixture_me_likelihood_F(&Y[X_lookup], &X[X_lookup], &X_s[X_lookup], &cen[X_lookup], &cen_above[X_lookup], &Loc[X_lookup], &Scale[X_lookup], &Shape[X_lookup], tau_sqd, phi[site], gamma, xp, &Den[Den_lookup], n_t, n_grid);
    }
    return ll;
}

/* Calculate thresh_X and thresh_X_above for an array of phi values */
// First way: iterate through each prob at each location (Not efficient)
int Thresh_X_try(double *phi, double *xp, double *Surv, double prob_below, double prob_above, double tau_sqd, double gamma,
                    double below_approx, double above_approx,
                    int n_phi, int n_grid, double *Thresh_X, double *Thresh_X_above){
    int Surv_lookup, tmp_int;
    int n_x=100;
    double cdf_vals[n_x], x_vals[n_x];
    double tmp_res;
    cdf_vals[0] = std::numeric_limits<double>::quiet_NaN(); x_vals[0] = std::numeric_limits<double>::quiet_NaN();

    for (int i = 0; i<n_phi;i++){
        Surv_lookup = i*n_grid;
        tmp_int = qRW_me_interp(&prob_below, xp, &Surv[Surv_lookup], tau_sqd, phi[i], gamma,
                          1, n_grid, &tmp_res,
                          cdf_vals, x_vals, n_x, below_approx-10, below_approx+10);
        Thresh_X[i] = tmp_res;
        tmp_int = qRW_me_interp(&prob_above, xp, &Surv[Surv_lookup], tau_sqd, phi[i], gamma,
                          1, n_grid, &tmp_res,
                          cdf_vals, x_vals, n_x, above_approx-100, above_approx+200);
        Thresh_X_above[i] = tmp_res;
    }

    return 1;
}

// Second way: calculate probs altogether at each location
int X_update(double *phi, double *xp, double *Surv, double *probs, double tau_sqd, double gamma,
                    double below_approx, double above_approx,
                    int n_phi, int n_grid, int n_probs, double *X_res){
    int Surv_lookup, tmp_int, X_lookup;
    int n_x=100;
    double cdf_vals[n_x], x_vals[n_x];
    cdf_vals[0] = std::numeric_limits<double>::quiet_NaN(); x_vals[0] = std::numeric_limits<double>::quiet_NaN();

    for (int i = 0; i<n_phi;i++){
        Surv_lookup = i*n_grid;
        X_lookup = i*n_probs;
        tmp_int = qRW_me_interp(probs, xp, &Surv[Surv_lookup], tau_sqd, phi[i], gamma,
                          n_probs, n_grid, X_res + X_lookup,
                          cdf_vals, x_vals, n_x, below_approx-10, above_approx+200);
    }

    return 1;
}

int unifs_2_RW_me(double *unifs, double *xp, double *Surv, double tau_sqd, double *phi, double gamma,
                    double below_approx, double above_approx,
                    int n_phi, int n_grid, int n_probs_each_loc, double *X_res){
    int Surv_lookup, tmp_int, X_lookup;
    int n_x=100;
    double cdf_vals[n_x], x_vals[n_x];
    cdf_vals[0] = std::numeric_limits<double>::quiet_NaN(); x_vals[0] = std::numeric_limits<double>::quiet_NaN();

    for (int i = 0; i<n_phi;i++){
        Surv_lookup = i*n_grid;
        X_lookup = i*n_probs_each_loc;
        tmp_int = qRW_me_interp(unifs + X_lookup, xp, &Surv[Surv_lookup], tau_sqd, phi[i], gamma,
                          n_probs_each_loc, n_grid, X_res + X_lookup,
                          cdf_vals, x_vals, n_x, below_approx-10, above_approx+200);
    }

    return 1;
}

void print_c(double *Y, int n_grid){
    printf("%4.2f %4.2f\n",*Y,*(Y+n_grid-1));
}

double print_Vec(double *Y, int n_grid, int n_s){
    int Den_lookup;
    for(int i=0; i<n_s; i++){
        Den_lookup = i*n_grid;
        print_c(&Y[Den_lookup], n_grid);
    }

    return Y[0];
}

}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__DIFF_HPP_
#define SMOOTH__DIFF_HPP_

/**
 * @file
 * @brief Differentiation on Manifolds.
 */

#include <Eigen/Core>
#include <type_traits>

#include "internal/utils.hpp"
#include "manifold.hpp"
#include "wrt.hpp"

namespace smooth {

// differentiation module
namespace diff {
namespace detail {

/**
 * @brief Numerical first-order differentiation in tangent space.
 *
 * @param f function to differentiate
 * @param x reference tuple of function arguments
 * @return \p std::pair containing value and right derivative: \f$(f(x), \mathrm{d}^r f_x)\f$
 *
 * @note All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<std::size_t K = 1>
  requires(K >= 1 && K <= 2)
auto dr_numerical(auto && f, auto && x)
{
  using Wrt    = decltype(x);
  using Result = decltype(std::apply(f, x));
  using Scalar = ::smooth::Scalar<Result>;

  static constexpr auto NumArgs = std::tuple_size_v<std::decay_t<Wrt>>;

  static_assert(Manifold<Result>, "f(x) is not a Manifold");

  const Scalar eps = std::sqrt(Eigen::NumTraits<Scalar>::epsilon());

  // arguments are modified below, so we create a copy of those that come in as const
  auto x_nc = wrt_copy_if_const(std::forward<Wrt>(x));
  Result F  = std::apply(f, x_nc);

  // static sizes
  static constexpr Eigen::Index Nx = wrt_Dof<Wrt>();
  static constexpr Eigen::Index Ny = Dof<Result>;

  // dynamic sizes
  Eigen::Index nx = std::apply([](auto &&... args) { return (dof(args) + ...); }, x_nc);
  Eigen::Index ny = dof<Result>(F);

  // output variable
  Eigen::Matrix<Scalar, Ny, Nx> J(ny, nx);

  if constexpr (K == 1) {
    Eigen::Index I0 = 0;
    utils::static_for<NumArgs>([&](auto i) {
      auto & w = std::get<i>(x_nc);
      using W  = std::decay_t<decltype(w)>;

      static constexpr Eigen::Index Nx_j = Dof<W>;
      const int nx_j                     = dof<W>(w);

      for (auto j = 0; j != nx_j; ++j) {
        Scalar eps_j = eps;
        if constexpr (std::is_base_of_v<Eigen::MatrixBase<W>, W>) {
          // scale step size if we are in Rn
          eps_j *= abs(w[j]);
          if (eps_j == 0.) { eps_j = eps; }
        }
        w             = rplus<W>(w, (eps_j * Eigen::Vector<Scalar, Nx_j>::Unit(nx_j, j)).eval());
        J.col(I0 + j) = rminus<Result>(std::apply(f, x_nc), F) / eps_j;
        w             = rplus<W>(w, (-eps_j * Eigen::Vector<Scalar, Nx_j>::Unit(nx_j, j)).eval());
      }
      I0 += nx_j;
    });

    return std::make_pair(std::move(F), std::move(J));
  }

  if constexpr (K == 2) {
    static_assert(Ny == 1, "2nd derivative only implemented for scalar functions");

    const auto sqrteps = std::sqrt(eps);

    Eigen::Matrix<Scalar, Nx, Nx> H(nx, nx);

    Eigen::Index I0 = 0;
    utils::static_for<NumArgs>([&](auto i0) {
      auto & w0                           = std::get<i0>(x_nc);
      using W0                            = std::decay_t<decltype(w0)>;
      static constexpr Eigen::Index Nx_i0 = Dof<W0>;
      const int nx_i0                     = dof<W0>(w0);

      Eigen::Index I1 = 0;
      utils::static_for<NumArgs>([&](auto i1) {
        if (i1 > i0) { return; }

        auto & w1                           = std::get<i1>(x_nc);
        using W1                            = std::decay_t<decltype(w1)>;
        static constexpr Eigen::Index Nx_i1 = Dof<W1>;
        const int nx_i1                     = dof<W1>(w1);

        for (auto k0 = 0; k0 != nx_i0; ++k0) {
          Scalar eps0 = sqrteps;
          if constexpr (std::is_base_of_v<Eigen::MatrixBase<W0>, W0>) {
            eps0 *= abs(w0[k0]);
            if (eps0 == 0.) { eps0 = sqrteps; }
          }

          w0               = rplus<W0>(w0, eps0 * Eigen::Vector<Scalar, Nx_i0>::Unit(nx_i0, k0));
          const Result F10 = std::apply(f, x_nc);
          w0               = rplus<W0>(w0, -eps0 * Eigen::Vector<Scalar, Nx_i0>::Unit(nx_i0, k0));

          J(0, I0 + k0) = (F10 - F) / eps0;

          for (auto k1 = 0; k1 < (i0 == i1 ? k0 + 1 : nx_i1); ++k1) {
            Scalar eps1 = sqrteps;
            if constexpr (std::is_base_of_v<Eigen::MatrixBase<W1>, W1>) {
              eps1 *= abs(w1[k1]);
              if (eps1 == 0.) { eps1 = sqrteps; }
            }

            // do this in order to ensure we return to same point on spaces with non-zero brackets
            w1               = rplus<W1>(w1, eps1 * Eigen::Vector<Scalar, Nx_i1>::Unit(nx_i1, k1));
            const Result F01 = std::apply(f, x_nc);
            w0               = rplus<W0>(w0, eps0 * Eigen::Vector<Scalar, Nx_i0>::Unit(nx_i0, k0));
            const Result F11 = std::apply(f, x_nc);
            w0               = rplus<W0>(w0, -eps0 * Eigen::Vector<Scalar, Nx_i0>::Unit(nx_i0, k0));
            w1               = rplus<W1>(w1, -eps1 * Eigen::Vector<Scalar, Nx_i1>::Unit(nx_i1, k1));

            // hessian is symmetric
            H(I0 + k0, I1 + k1) = H(I1 + k1, I0 + k0) = (F11 - F01 - F10 + F) / eps0 / eps1;
          }
        }
        I1 += nx_i1;
      });
      I0 += nx_i0;
    });

    return std::make_tuple(std::move(F), std::move(J), std::move(H));
  }
}

}  // namespace detail

/**
 * @brief Available differentiation methods
 */
enum class Type {
  Numerical,  ///< Numerical (forward) derivatives
  Autodiff,   ///< Uses the autodiff (https://autodiff.github.io) library; requires  \p
              ///< compat/autodiff.hpp
  Ceres,      ///< Uses the Ceres (http://ceres-solver.org) built-in autodiff; requires \p
              ///< compat/ceres.hpp
  Analytic,   ///< Hand-coded derivative, requires that function returns \p std::pair \f$(f(x),
              ///< \mathrm{d}^r f_x) \f$
  Default     ///< Automatically select type based on availability
};

static constexpr Type DefaultType =
#ifdef SMOOTH_DIFF_AUTODIFF
  Type::Autodiff;
#elif defined SMOOTH_DIFF_CERES
  Type::Ceres;
#else
  Type::Numerical;
#endif

/**
 * @brief Differentiation in tangent space
 *
 * @tparam K differentiation order (1 or 2)
 * @tparam D differentiation method to use
 *
 * @param f function to differentiate
 * @param x reference tuple of function arguments
 * @return {f(x), dr f(x)} for K = 1, {f(x), dr f(x), d2r f(x)} for K = 2
 *
 * @note Only scalar functions suppored for K = 2
 *
 * @note All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<std::size_t K, Type D>
auto dr(auto && f, auto && x)
{
  using F   = decltype(f);
  using Wrt = decltype(x);

  if constexpr (D == Type::Numerical) {
    return detail::dr_numerical<K>(std::forward<F>(f), std::forward<Wrt>(x));
  } else if constexpr (D == Type::Autodiff) {
#ifdef SMOOTH_DIFF_AUTODIFF
    return dr_autodiff<K>(std::forward<F>(f), std::forward<Wrt>(x));
#else
    static_assert(D != Type::Autodiff, "compat/autodiff.hpp header not included");
#endif
  } else if constexpr (D == Type::Ceres) {
    static_assert(K == 1, "Only K = 1 supported with Ceres");
#ifdef SMOOTH_DIFF_CERES
    return dr_ceres(std::forward<F>(f), std::forward<Wrt>(x));
#else
    static_assert(D != Type::Ceres, "compat/ceres.hpp header not included");
#endif
  } else if constexpr (D == Type::Analytic) {
    auto F  = std::apply(f, x);
    auto dF = std::apply(std::bind_front(std::mem_fn(&std::decay_t<decltype(f)>::jacobian), f), x);
    if constexpr (K == 1) {
      return std::make_tuple(std::move(F), std::move(dF));
    } else if constexpr (K == 2) {
      auto d2F =
        std::apply(std::bind_front(std::mem_fn(&std::decay_t<decltype(f)>::hessian), f), x);
      return std::make_tuple(F, dF, d2F);
    }
  } else if constexpr (D == Type::Default) {
    return dr<K, DefaultType>(std::forward<F>(f), std::forward<Wrt>(x));
  }
}

/**
 * @brief Differentiation in tangent space using default method
 *
 * @tparam K differentiation order
 *
 * @param f function to differentiate
 * @param x reference tuple of function arguments
 * @return \p std::pair containing value and right derivative: \f$(f(x), \mathrm{d}^r f_x)\f$
 *
 * @note All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<std::size_t K = 1>
auto dr(auto && f, auto && x)
{
  return dr<K, Type::Default>(std::forward<decltype(f)>(f), std::forward<decltype(x)>(x));
}

}  // namespace diff
}  // namespace smooth

#endif  // SMOOTH__DIFF_HPP_


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>
#include <boost/simd/detail/constant/maxexponent.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

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

    template < typename A0, typename sA0>
    struct expm1_kernel;

    template < typename A0 >
    struct expm1_kernel < A0, float >
    {
      // computes expm1 for float or float vectors
      static BOOST_FORCEINLINE A0 expm1(const A0& a0) BOOST_NOEXCEPT
      {
        using i_t = bd::as_integer_t<A0>;
        using s_t = bd::scalar_of_t<A0>;
        A0 k  = nearbyint(Invlog_2<A0>()*a0);
        A0 x = fnms(k, Log_2hi<A0>(), a0);
        x =  fnms(k, Log_2lo<A0>(), x);
        A0 hx  = x*Half<A0>();
        A0 hxs = x*hx;
        A0 r1 = horn<A0,
                     0X3F800000UL,// 1
                     0XBD08887FUL, // -3.3333298E-02
                     0X3ACF6DB4UL  // 1.5825541E-03
                     > (hxs);
        A0 t  = fnms(r1, hx, A0(3));
        A0 e  = hxs*((r1-t)/(A0(6) - x*t));
        e  = fms(x, e, hxs);
        i_t ik =  toint(k);
        A0 two2mk = bitwise_cast<A0>(shift_left(Maxexponent<A0>()-ik,Nbmantissabits<s_t>()));
        A0 y = oneminus(two2mk)-(e-x);
        return ldexp(y, ik);
      }
    };

    template < typename A0 >
    struct expm1_kernel < A0, double >
    {
      // computes expm1 for  double or double vectors
      static  BOOST_FORCEINLINE A0 expm1(const A0& a0) BOOST_NOEXCEPT
      {
        using i_t = bd::as_integer_t<A0>;
        using s_t = bd::scalar_of_t<A0>;
        A0 k  = nearbyint(Invlog_2<A0>()*a0);
        A0 hi = fnms(k, Log_2hi<A0>(), a0);
        A0 lo = k*Log_2lo<A0>();
        A0 x  = hi-lo;
        A0 hxs = sqr(x)*Half<A0>();
        A0 r1 = horn<A0,
                     0X3FF0000000000000ULL,
                     0XBFA11111111110F4ULL,
                     0X3F5A01A019FE5585ULL,
                     0XBF14CE199EAADBB7ULL,
                     0X3ED0CFCA86E65239ULL,
                     0XBE8AFDB76E09C32DULL
                     > (hxs);
        A0 t  = A0(3)-r1*Half<A0>()*x;
        A0 e  = hxs*((r1-t)/(A0(6) - x*t));
        A0 c = (hi-x)-lo;
        e  = (x*(e-c)-c)-hxs;
        i_t ik =  toint(k);
        A0 two2mk = bitwise_cast<A0>(shift_left(Maxexponent<A0>()-ik,Nbmantissabits<s_t>()));
        A0 ct1= oneminus(two2mk)-(e-x);
        A0 ct2= inc((x-(e+two2mk)));
        A0 y = if_else((k < A0(20)),ct1,ct2);
        return ldexp(y, ik);
      }
    };
  }
} }
#endif


#ifndef MOCHIMOCHI_ADAGRAD_RDA_HPP_
#define MOCHIMOCHI_ADAGRAD_RDA_HPP_

#include <Eigen/Dense>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include "../../functions/enumerate.hpp"
#include "../factory/binary_oml.hpp"

class ADAGRAD_RDA : public BinaryOML {
private :
  const std::size_t kDim;
  const double kEta;
  const double kLambda;

private :
  std::size_t _timestep;
  Eigen::VectorXd _w;
  Eigen::VectorXd _h;
  Eigen::VectorXd _g;

public :
  ADAGRAD_RDA(const std::size_t dim, const double eta, const double lambda)
    : kDim(dim),
      kEta(eta),
      kLambda(lambda),
      _timestep(0),
      _w(Eigen::VectorXd::Zero(kDim)),
      _h(Eigen::VectorXd::Zero(kDim)),
      _g(Eigen::VectorXd::Zero(kDim)) {
    static_assert(std::numeric_limits<decltype(dim)>::max() > 0, "Dimension Error. (Dimension > 0)");
    static_assert(std::numeric_limits<decltype(eta)>::max() > 0, "Hyper Parameter Error. (eta > 0)");
    static_assert(std::numeric_limits<decltype(lambda)>::max() > 0, "Hyper Parameter Error. (lambda > 0)");
    assert(dim > 0);
    assert(eta > 0);
    assert(lambda > 0);
  }

  virtual ~ADAGRAD_RDA() { }

private :

  double calculate_margin(const Eigen::VectorXd& x) const {
    return _w.dot(x);
  }

  double suffer_loss(const Eigen::VectorXd& x, const int y) const {
    return std::max(0.0, 1.0 - y * _w.dot(x));
  }

public :

  std::string name() const override {
    return std::string("ADAGRAD_RDA");
  }

  bool update(const Eigen::VectorXd& feature, const int label) override {
    if (suffer_loss(feature, label) <= 0.0) { return false; }

    _timestep++;
    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                       [&](const int index, const double value) {
                         const auto gradiant = -label * value;
                         _g[index] += gradiant;
                         _h[index] += gradiant * gradiant;

                         const auto sign = _g[index] >= 0 ? 1 : -1;
                         const auto eta = kEta / std::sqrt(_h[index]);
                         const auto u = std::abs(_g[index]) / _timestep;

                         _w[index] = (u <= kLambda) ? 0.0 : -sign * eta * _timestep * (u - kLambda);
                       });
    return true;
  }

  int predict(const Eigen::VectorXd& x) const override {
    return calculate_margin(x) > 0.0 ? 1 : -1;
  }

  void save(const std::string& filename) override {
    std::ofstream ofs(filename);
    assert(ofs);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
    ofs.close();
  }

  void load(const std::string& filename) override {
    std::ifstream ifs(filename);
    assert(ifs);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
    ifs.close();
  }

private :
  friend class boost::serialization::access;
  BOOST_SERIALIZATION_SPLIT_MEMBER();
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    std::vector<double> w_vector(_w.data(), _w.data() + _w.size());
    std::vector<double> h_vector(_h.data(), _h.data() + _h.size());
    std::vector<double> g_vector(_g.data(), _g.data() + _g.size());

    ar & boost::serialization::make_nvp("w", w_vector);
    ar & boost::serialization::make_nvp("h", h_vector);
    ar & boost::serialization::make_nvp("g", g_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("eta", const_cast<double&>(kEta));
    ar & boost::serialization::make_nvp("lambda", const_cast<double&>(kLambda));
    ar & boost::serialization::make_nvp("timestep", _timestep);
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int version) {
    std::vector<double> w_vector;
    std::vector<double> h_vector;
    std::vector<double> g_vector;

    ar & boost::serialization::make_nvp("w", w_vector);
    ar & boost::serialization::make_nvp("h", h_vector);
    ar & boost::serialization::make_nvp("g", g_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("eta", const_cast<double&>(kEta));
    ar & boost::serialization::make_nvp("lambda", const_cast<double&>(kLambda));
    ar & boost::serialization::make_nvp("timestep", _timestep);

    _w = Eigen::Map<Eigen::VectorXd>(&w_vector[0], w_vector.size());
    _h = Eigen::Map<Eigen::VectorXd>(&h_vector[0], h_vector.size());
    _g = Eigen::Map<Eigen::VectorXd>(&g_vector[0], g_vector.size());
  }

};

#endif //MOCHIMOCHI_ADAGRAD_RDA_HPP_


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2015-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef MG_PROLONGATION_HH
#define MG_PROLONGATION_HH

#include <boost/fusion/include/vector.hpp>
#include <boost/timer/timer.hpp>

#include "fem/barycentric.hh"
#include "fem/spaces.hh"
#include "linalg/dynamicMatrix.hh"
#include "linalg/localMatrices.hh"
#include "linalg/threadedMatrix.hh"
#include "utilities/threading.hh"
#include "utilities/timing.hh"

namespace Kaskade
{
  /**
   * \ingroup iterative
   * \brief Computes an interpolation-based prolongation matrix from a (supposedly) coarser space to a finer space.
   *
   * \tparam CoarseSpace a FEFunctionSpace type
   * \tparam FineSpace a FEFunctionSpace type
   *
   * \param coarseSpace the (supposedly) coarser space (domain)
   * \param fineSpace   the (supposedly) finer space (image)
   *
   * Both function spaces have to be defined on the very same grid view.
   */
  template <class CoarseSpace, class FineSpace>
  NumaBCRSMatrix<Dune::FieldMatrix<double,1,1>> prolongation(CoarseSpace const& coarseSpace, FineSpace const& fineSpace)
  {
    namespace bf = boost::fusion;

    Timings& timer = Timings::instance();
    timer.start("space prolongation");

    std::vector<bf::vector<std::vector<size_t>,std::vector<size_t>,
                           DynamicMatrix<Dune::FieldMatrix<typename FineSpace::Scalar,1,1>>>> interpolationData(fineSpace.gridView().size(0));

    // Intermediate shape function values, declared here to prevent frequent reallocations
    typename CoarseSpace::Mapper::ShapeFunctionSet::SfValueArray afValues;

    // Step through all the cells and perform local interpolation on each cell.
    // TODO: do this in parallel
    for (auto const& cell: elements(fineSpace.gridView()))
    {
      auto index = fineSpace.indexSet().index(cell);

      auto const& fineIndices = fineSpace.mapper().globalIndices(index);
      auto const& coarseIndices = coarseSpace.mapper().globalIndices(index);

      std::copy(fineIndices.begin(),  fineIndices.end(),  std::back_inserter(bf::at_c<0>(interpolationData[index])));
      std::copy(coarseIndices.begin(),coarseIndices.end(),std::back_inserter(bf::at_c<1>(interpolationData[index])));

      auto const& fineSfs = fineSpace.mapper().shapefunctions(cell);
      auto const& coarseSfs = coarseSpace.mapper().shapefunctions(cell);

      // Obtain interpolation nodes of target on this cell
      auto const& iNodes(fineSpace.mapper().shapefunctions(cell).interpolationNodes());

      // Evaluate coarse space global shape functions
      evaluateGlobalShapeFunctions(coarseSpace,cell,iNodes,afValues,coarseSfs);

      // Interpolate fine space global values
      approximateGlobalValues(fineSpace,cell,afValues,bf::at_c<2>(interpolationData[index]),fineSfs);
    }
    timer.stop("space prolongation");

    timer.start("matrix creation");
    // Create a sparse prolongation matrix sparsity pattern. Note that the local prolongation matrices
    // are often sparse (e.g. for Lagrangian elements). We filter out the zero entries in order not to
    // create too densely populated prolongations, which in turn lead to very expensive conjugation
    // computations.
    NumaCRSPatternCreator<size_t> creator(fineSpace.degreesOfFreedom(),coarseSpace.degreesOfFreedom());
    for (auto const& block: interpolationData)
      for (int i=0; i<bf::at_c<0>(block).size(); ++i)
        for (int j=0; j<bf::at_c<1>(block).size(); ++j)
        {
          auto entry = bf::at_c<2>(block)[i][j];
          if (std::abs(entry) > 1e-12)
            creator.addElement(bf::at_c<0>(block)[i],bf::at_c<1>(block)[j]);
        }

    // Now create and fill the matrix itself. Overwrite already existing entries
    // (this means the weighing is just "last one wins"
    NumaBCRSMatrix<Dune::FieldMatrix<double,1,1>,size_t> p(creator);
    for (auto const& block: interpolationData)
      for (int i=0; i<bf::at_c<0>(block).size(); ++i)
        for (int j=0; j<bf::at_c<1>(block).size(); ++j)
        {
          auto entry = bf::at_c<2>(block)[i][j];
          if (std::abs(entry) > 1e-12)
            p[bf::at_c<0>(block)[i]][bf::at_c<1>(block)[j]] = entry;
        }

    timer.stop("matrix creation");
    return p;
  }

  // ---------------------------------------------------------------------------------------------------------

  /**
   * \ingroup iterative
   * \brief Computes a stack of prolongation matrices for higher order finite element spaces.
   *
   * This computes a stack of prolongation matrices for a scale of finite element spaces with
   * increasing polynomial ansatz order. From level to level, the ansatz order is doubled (maybe +1
   * for odd degrees).
   *
   * \tparam Mapper a finite element local to global mapper with scalar shape functions.
   */
  template <class Mapper>
  std::vector<NumaBCRSMatrix<Dune::FieldMatrix<double,1,1>>> prolongationStack(FEFunctionSpace<Mapper> const& space)
  {
    std::vector<NumaBCRSMatrix<Dune::FieldMatrix<double,1,1>>> stack;
    int p = space.mapper().maxOrder();

    if (p > 1) // there is some need for prolongation
    {
      H1Space<typename Mapper::Grid> coarseSpace(space.gridManager(),space.gridView(),p/2);  // a coarser space (lower order)
      stack = prolongationStack(coarseSpace);
      stack.push_back(prolongation(coarseSpace,space));
    }

    return stack;
  }

  // ---------------------------------------------------------------------------------------------------------

  /// \cond internals
  namespace ProlongationDetail
  {
    struct Node
    {
      size_t p, q; // global (fine grid) indices of parent vertices
      int level;   // level of the vertex
    };
  }
  /// \endcond

  /**
   * \ingroup iterative
   * \brief A prolongation operator for P1 finite elements from a coarser grid level to the next finer level.
   *
   * For P1 finite elements on simplicial grids, the prolongation from a coarser level to the next finer is a matrix \f$ P \f$
   * in which each row contains either one entry of value 1 (if the vertex is already contained in the coarser level grid)
   * or two entries that sum up to 1 (if the vertex is created on the finer level by bisecting the edge between two parent
   * nodes). In the latter case, the two entries default to 0.5 each.
   *
   * Storing this as a sparse matrix data structure is quite inefficient.
   *
   * This class is a specialized implementation of such prolongation matrices.
   */
  class MGProlongation
  {
  public:
    /**
     * \brief Constructor.
     * \param parents a sequence of node information. The parent indices for fine grid nodes shall refer to the fine grid numbering.
     * \param indexInCoarse a vector of length of fine grid vertices. For each coarse grid node, it contains the index of that node
     *                      in the coarse grid.
     * \param nc the number of coarse grid nodes (i.e. number of columns in P)
     * \param fineLevel the level of fine grid nodes
     */
    MGProlongation(std::vector<ProlongationDetail::Node> const& parents, std::vector<size_t> const& indexInCoarse, size_t nc, int fineLevel);

    /**
     * \brief Returns the parent nodes indices.
     *
     * For a fine grid node, this returns the parent node indices in the coarse grid. For a coarse
     * grid node, the "parent indices" coincide and refer to the node index in the coarse grid.
     */
    std::array<size_t,2> const& parents(size_t i) const
    {
      return entries[i];
    }

    /**
     * \brief Matrix vector multiplcation (update mode).
     *
     * This computes \f$ f \leftarrow f + Pc \f$.
     *
     * \param f the fine grid target vector. It has to have the correct size.
     */
    template <class Vector>
    void umv(Vector const& c, Vector& f) const
    {
      assert(f.size()==entries.size());
      assert(c.size()==nc);

      for (size_t row=0; row<entries.size(); ++row)
      {
        auto e = entries[row];
        f[row] += 0.5*(c[e[0]] + c[e[1]]);
      }
    }

    /**
     * \brief Matrix vector multiplcation.
     *
     * This computes \f$ f \leftarrow Pc \f$, and is faster than but equivalent to
     * \code
     * f = 0;
     * umv(c,f);
     * \endcode
     *
     * \param f the fine grid target vector. It has to have the correct size.
     */
    template <class Vector>
    void mv(Vector const& c, Vector& f) const
    {
      assert(f.size()==entries.size());
      assert(c.size()==nc);

      for (size_t row=0; row<entries.size(); ++row)
      {
        auto e = entries[row];
        f[row] = 0.5*(c[e[0]] + c[e[1]]);
      }
    }

    /**
     * \brief Transpose matrix vector multiplication
     *
     * This computes \f$ c \leftarrow P^T f \f$.
     *
     * \param c the coarse grid target vector. It is resized to contain the result.
     */
    template <class Vector>
    void mtv(Vector const& f, Vector& c) const
    {
      assert(f.size()==entries.size());
      if (c.size()!=nc)
        c.resize(nc);
      for (size_t i=0; i<c.N(); ++i)
        c[i] = 0;

      for (size_t row=0; row<entries.size(); ++row)
      {
        c[entries[row][0]] += 0.5*f[row];
        c[entries[row][1]] += 0.5*f[row];
      }
    }

    /**
     * \brief The number of rows.
     */
    size_t N() const
    {
      return entries.size();
    }

    /**
     * \brief The number of columns.
     */
    size_t M() const
    {
      return nc;
    }

    /**
     * \brief Galerkin projection of fine grid matrices.
     * This computes \f$ P^T A P \f$.
     *
     * \tparam Entry the entry type of Galerkin matrix to be projected, in general a quadratic Dune::FieldMatrix type
     * \tparam Index the Index type of the matrix to be projected
     *
     * \param A the quadratic fine level Galerkin matrix of size \f$ N\times N \f$ to be projected
     * \param onlyLowerTriangle if true, only the lower triangular part of symmetric A is touched, and only the lower triangular part of \f$ P^T A P\f$ is created
     */
    template <class Entry, class Index>
    NumaBCRSMatrix<Entry,Index> galerkinProjection(NumaBCRSMatrix<Entry,Index> const& A, bool onlyLowerTriangle = false) const
    {
      assert(A.N()==N() && A.M()==N());
      Timings& timer = Timings::instance();

      // First, create the sparsity pattern of the projected matrix.
      NumaCRSPatternCreator<Index> creator(M(),M(),onlyLowerTriangle);

      // If C = P^T A P, we have that C_{ij} = \sum_{k,l} P_{ki} P_{lj} A_{kl}. Hence, the entry A_{kl} contributes to
      // all C_{ij} for which there are nonzero entries P_{ki} and P_{lj} in the rows k and l of P. Thus we can simply
      // run through all nonzeros A_{kl} of A, look up the column indices i,j of rows k and l of P, and flag C_{ij}
      // as nonzero.

      // Step through all entries of A
      timer.start("h conjugation pattern");
      for (Index k=0; k<A.N(); ++k)
      {
        auto const& is = entries[k];                        // indices i for which Pki != 0
        auto row = A[k];
        for (auto ca=row.begin(); ca!=row.end(); ++ca)
        {
          Index const l = ca.index();
          auto const& js = entries[l];                      // indices j for which Plj != 0
          // add all combinations i,j
          if (is[0]==is[1] && js[0]==js[1])
            creator.addElement(is[0],js[0]);
          else
            creator.addElements(std::begin(is),std::end(is),std::begin(js),std::end(js));

          if (onlyLowerTriangle && k>l)                     // subdiagonal entry (k,l) of A -> entry (l,k) must be treated implicitly:
            creator.addElements(std::begin(js),std::end(js),std::begin(is),std::end(is)); // add all combinations (j,i)
        }
      }
      timer.stop("h conjugation pattern");

      // An alternative way of computing the sparsity pattern would be to use that the nonzero entries j in column i
      // of C are exactly those for which there is k with (nonzero P_{jk} and there is l with (nonzero P_{il} and A_{lk})).
      // Hence we can obtain the column index set J directly by the following steps:
      // (i) find all l with P_{li} nonzero -> L  [requires to access columns of P - compute the transpose patterns once]
      // (ii) find all k with A_{lk} nonzero for some l in L -> K  [probably sorting K and removing doubled entries would be a good idea here]
      // (iii) find all j with P_{kj} nonzero for some k in K -> J
      // Compared to the above implementation this would have the following (dis)advantages
      // + easy to do in parallel (since write operations are separated)
      // + fewer scattered write accesses to memory
      // - more complex implementation
      // - requires the transpose pattern of P

      // Create the sparse matrix.
      timer.start("matrix creation");
      NumaBCRSMatrix<Entry,Index> pap(creator);
      timer.stop("matrix creation");

      // Fill the sparse matrix PAP. This is done as before by stepping through all Akl entries and scatter
      // Pki*Plj*Akl into PAPij.
      timer.start("matrix conjugation");
      for (Index k=0; k<A.N(); ++k)
      {
        auto const& is = entries[k];
        auto row = A[k];
        for (auto ca=row.begin(); ca!=row.end(); ++ca)
        {
          Index const l = ca.index();
          auto const& js = entries[l];

          if (is[0]==is[1] && js[0]==js[1])               // a coarse grid node - this means four identical contributions
            pap[is[0]][js[0]] += *ca;                     // with factor 1/4. Substitute with one contribution with factor 1.
          else
          {
            auto pap0 = pap[is[0]];
            pap0[js[0]] += 0.25 * *ca;
            pap0[js[1]] += 0.25 * *ca;

            auto pap1 = pap[is[1]];
            pap1[js[0]] += 0.25 * *ca;
            pap1[js[1]] += 0.25 * *ca;
          }

          if (onlyLowerTriangle)
            abort();  // not yet implemented
        }
      }
      timer.stop("matrix conjugation");

      return pap;
    }

  private:
    // Internally we treat all rows equally: A row with one entry of value one is represented as
    // two entries which sum up to 1 (that happen to reference the same column index...). This allows a
    // very simple and uniform implementation. The vector entries contains the column indices of
    // the two entries in each row.
    std::vector<std::array<size_t,2>> entries;

    // Number of columns (i.e. coarse grid nodes).
    size_t nc;
  };

  std::ostream& operator<<(std::ostream& out, MGProlongation const& p);

  /**
   * \ingroup multigrid
   * \brief Creates a Galerkin projected Matrix \f$ P^T A P \f$ from a prolongation \f$ P \f$ and a
   *        symmetric matrix \f$ A \f$.
   * \param onlyLowerTriangle if true, A contains onl the lower triangular part of the symmetric matrix.
   */
  template <class Entry, class Index>
  auto conjugation(MGProlongation const& p, NumaBCRSMatrix<Entry,Index> const& a, bool onlyLowerTriangle)
  {
    return p.template galerkinProjection(a,onlyLowerTriangle);
  }

  // ---------------------------------------------------------------------------------------------------------

  /// \cond internals
  namespace ProlongationDetail
  {
    // For each corner of the given cell, if there are any of them which are not corners of the father cell
    // (i.e. created by bisection of an edge), enter the parent vertices into the parents vector.
    template <class LeafView, class Cell, class Parents>
    void computeParents(LeafView const& leafView, Cell const& cell, Parents& parents)
    {
      // If this cell is a coarse grid cell, none of its vertices have parents, and we're done.
      if (!cell.hasFather())
        return;

      int const dim = LeafView::dimension;

      // For each corner, check its position in the parent. If it has barycentric coordinates with one
      // entry approximately one (all others zero), it is a corner of the father cell and will be treated
      // later (or has already been treated). Otherwise there will be two entries 0.5 (all others zero),
      // and these entries denote father corners which are the parents.
      assert(cell.type().isSimplex());
      auto const& geo = cell.geometryInFather();
      int nCorners = geo.corners();
      for (int i=0; i<nCorners; ++i)
      {
        // TODO: We could first obtain the index of the corner and check whether its parents have
        //       already been determined, and skip the geometric considerations in that case.
        //       This should save some time.
        //       On the other hand, obtaining indices can also be expensive, and in the current
        //       implementation we only have to get them for corners that actually are no corners
        //       in the father cell. This saves some time, too.
        //       We should check which option is faster, and whether it makes a big difference in
        //       the first place.

        // barycentric coordinates of corner in the father cell
        auto b = barycentric(geo.corner(i));

        // find potential parent vertices (those with barycentric coordinates 0.5)
        int pcount = 0;
        int pc[2];
        for (int k=0; k<b.N(); ++k)                         // check all barycentric coordinates
          if (std::abs(b[k]-0.5) < 0.01)                    // if close to 0.5 accept
          {
            pc[pcount] = (k+1) % (dim+1);                   // map barycentric coordinate number to Dune corner number
            ++pcount;                                       // note down that we've found one more parent vertex
          }

        if (pcount == 2)                                    //  corner i is a father edge midpoint
        {
          auto const& is = leafView.indexSet();
          parents.push_back(std::make_pair(is.index(cell.template subEntity<dim>(i)),
                                           Node{is.index(cell.father().template subEntity<dim>(pc[0])),
                                                is.index(cell.father().template subEntity<dim>(pc[1])),
                                                cell.level()}));
        }
      }
    }

    /**
     * \ingroup multigrid
     * \brief Creates a stack of prolongations from parent-child relationships in grids
     * \param level
     */
    std::vector<MGProlongation> makeProlongationStack(std::vector<Node> parents, int level, size_t minNodes);
  };
  /// \endcond

  // ---------------------------------------------------------------------------------------------------------

  /**
   * \ingroup multigrid
   * \brief Computes a sequence of prolongation matrices for P1 finite elements in hierarchical grids.
   *
   * We assume that each vertex not contained in the coarse grid has been created by bisecting an edge,
   * such that there are exactly two "parent vertices".
   * \tparam Grid the Dune grid type on which the P1 space is defined. The grid has to be a simplicial grid.
   * \param grid the grid itself
   * \param minNodes minimum number of nodes to keep as coarse grid
   */
  template <class GridMan>
  std::vector<MGProlongation> prolongationStack(GridMan const& gridman, size_t minNodes=0)
  {
    Timings& timer = Timings::instance();

    auto const& grid = gridman.grid();
    auto const& leafView = grid.leafGridView();
    auto const& cellRanges = gridman.cellRanges(grid.levelGridView(0));

    // In parallel compute the parent nodes for edge midpoints cell by cell
    timer.start("parent computation");
    std::vector<std::vector<std::pair<size_t,ProlongationDetail::Node>>> myParents(cellRanges.maxRanges());
    parallelFor([&](int k, int n)
    {
      for (auto const& coarseCell: cellRanges.range(n,k))
        for (auto const& cell: descendantElements(coarseCell,grid.maxLevel()))
          ProlongationDetail::computeParents(leafView,cell,myParents[k]);
    },myParents.size());

    // Now that all parent nodes have been identified, consolidate them in an easily
    // indexable array.
    std::vector<ProlongationDetail::Node> parents(leafView.size(leafView.dimension),ProlongationDetail::Node{0,0,-1});
    for (auto const& ps: myParents)
      for (auto const& p: ps)
        parents[p.first] = p.second;
    timer.stop("parent computation");

    // Now construct the prolongation matrices for all levels.
    timer.start("made stack parents");
    auto ps = ProlongationDetail::makeProlongationStack(std::move(parents),grid.maxLevel(),minNodes);
    timer.stop("made stack parents");
    return ps;
  }

  // ---------------------------------------------------------------------------------------------------------

  /**
   * \ingroup multigrid
   * \brief Class for multigrid stacks.
   *
   * This provides the storage of and access to prolongations and projected Galerkin matrices,
   * but leaves the construction of these to derived classes.
   */
  template <class Prolongation, class Entry, class Index>
  class MultiGridStack
  {
  public:

    /**
     * \brief Constructor
     *
     * This takes both a stack of prolongations and a matching stack of projected
     */
    MultiGridStack(std::vector<Prolongation>&& ps, std::vector<NumaBCRSMatrix<Entry,Index>>&& as)
    : prolongations(std::move(ps)), galerkinMatrices(std::move(as))
    {
      assert(prolongations.size()+1==galerkinMatrices.size());
    }

    /**
     * \brief Constructor
     *
     * This takes a stack of prolongations and creates the stack of projected Galerkin matrices from the given
     * fine grid matrix. Most useful for geometric multigrid, where the prolongations are defined solely in terms of the grid.
     *
     * \see makeMultiGridStack
     */
    MultiGridStack(std::vector<Prolongation>&& ps, NumaBCRSMatrix<Entry,Index>&& A, bool onlyLowerTriangle)
    {
      Timings& timer = Timings::instance();

      prolongations = std::move(ps);
      galerkinMatrices.push_back(std::move(A));

      assert((prolongations.size() == 0) || (prolongations.back().N() == galerkinMatrices[0].N()));
      assert(galerkinMatrices[0].N() == galerkinMatrices[0].M());

      // Step through the prolongations from top level to bottom
      timer.start("matrix projection");
      for (auto pi=prolongations.crbegin(); pi!=prolongations.crend(); ++pi)
        galerkinMatrices.insert(galerkinMatrices.begin(),conjugation(*pi,galerkinMatrices.front(),onlyLowerTriangle));
      timer.stop("matrix projection");
    }

    MultiGridStack(MultiGridStack&& other) = default;

    /**
     * \brief The number of grid levels
     */
    int levels() const
    {
      return galerkinMatrices.size();
    }

    /**
     * \brief Returns the prolongation from given level to next higher one.
     * \param level precondition 0 <= level < levels()-1
     */
    Prolongation const& p(int level) const
    {
      return prolongations[level];
    }

    /**
     * \brief Returns the projected Galerkin matrix on the given level.
     * \param level precondition 1 <= level < levels()
     */
    NumaBCRSMatrix<Entry,Index> const& a(int level) const
    {
      return galerkinMatrices[level];
    }

    /**
     * \brief Returns the projected Galerkin matrix on the coarsest level.
     *
     * This is explicitly a mutable reference, such that the coarse grid matrix can be moved
     * from. This is useful, as in the multigrid, the coarsest level matrix is not referenced
     * (the coarse grid preconditioner has its own copy, maybe obtained by moving from here...).
     */
    NumaBCRSMatrix<Entry,Index>& coarseGridMatrix()
    {
      return galerkinMatrices[0];
    }

    void report(std::ostream& out) const
    {
      for (auto const& p: prolongations)
        out << "Prolongation:\n" << p;

      for (auto const& a: galerkinMatrices)
        out << "GalerkinMatrix: \n" << a;
    }

  private:
    std::vector<Prolongation>                 prolongations;    // prolongations grid i -> i+1, i=0,...,n-1
    std::vector<NumaBCRSMatrix<Entry,Index>>  galerkinMatrices; // Galerkin matrices on grid i, i=0,...,n
  };

  template <typename Prolongations, typename Entry, typename Index>
  std::ostream& operator<<(std::ostream& out, MultiGridStack<Prolongations,Entry,Index> const& mgStack) { mgStack.report(out); return out; }

  /**
   * \ingroup multigrid
   * \brief Convenience routine for creating multigrid stacks
   *
   * Given a stack of prolongations and the top level Galerkin matrix, this creates the complete stack
   * including all projected Galerkin matrices.
   *
   * \param ps a vector of prolongation matrices
   * \param A the top level (finest) Galerkin matrix to be projected down
   * \param onlyLowerTriangle if true, only the lower triangular part of A will be referenced
   */
  template <class Prolongation, class Entry, class Index>
  MultiGridStack<Prolongation,Entry,Index> makeMultiGridStack(std::vector<Prolongation>&& ps, NumaBCRSMatrix<Entry,Index>&& A,
                                                              bool onlyLowerTriangle)
  {
    return MultiGridStack<Prolongation,Entry,Index>(std::move(ps),std::move(A),onlyLowerTriangle);
  }

  /**
   * \ingroup multigrid
   * \brief convenience routine for creating multigrid stacks based on geometric coarsening for P1 elements
   */
  template <class GridMan, class Entry, class Index>
  MultiGridStack<MGProlongation,Entry,Index> makeGeometricMultiGridStack(GridMan const& gridManager, NumaBCRSMatrix<Entry,Index>&& A,
                                                                         size_t minNodes=10000, bool onlyLowerTriangle=false)
  {
    return MultiGridStack<MGProlongation,Entry,Index>(prolongationStack(gridManager,minNodes),std::move(A),onlyLowerTriangle);
  }

  /**
   * \ingroup multigrid
   * \brief Creates stack of prolongations and projected Galerkin matrices.
   *
   * \param A the symmetric sparse matrix
   * \param n stop the coarsening if the number of rows/cols drops below this number
   * \param onlyLowerTriangle if true, only the lower triangular part of symmetric A is accessed
   */
  template <class Entry, class Index>
  MultiGridStack<MGProlongation,Entry,Index> makeAlgebraicMultigridStack(NumaBCRSMatrix<Entry,Index>&& A,
                                                                         Index n=0, bool onlyLowerTriangle=false);

  /**
   * \ingroup multigrid
   * \brief convenience routine for creating multigrid stacks based on coarsening by reducing the ansatz order from P to P1
   */
  template <typename FineSpace, typename Matrix>
  auto makePMultiGridStack(FineSpace const& space, Matrix&& A, bool onlyLowerTriangle)
  {
    std::vector<NumaBCRSMatrix<Dune::FieldMatrix<double,1,1>>> prolongations;

    if (space.mapper().maxOrder() > 1)
    {
      H1Space<typename FineSpace::Grid> coarseSpace(space.gridManager(),space.gridView(),1);  // a coarser space (lower order)
      prolongations.push_back(prolongation(coarseSpace,space));
    }

    return makeMultiGridStack(std::move(prolongations),std::move(A),onlyLowerTriangle);
  }

  /**
   * \ingroup multigrid
   * \brief convenience routine for creating multigrid stacks between two spaces.
   *
   * An approximation of the projected Galerkin matrix is provided. Often, this can be assembled much more
   * efficiently than projected.
   */
  template <typename FineSpace, typename CoarseSpace, typename Matrix>
  auto makePMultiGridStack(FineSpace const& fineSpace, Matrix&& fA, CoarseSpace const& coarseSpace, Matrix&& cA)
  {
    using Prolongation = NumaBCRSMatrix<Dune::FieldMatrix<double,1,1>>;
    std::vector<Prolongation> prolongations;
    prolongations.push_back(prolongation(coarseSpace,fineSpace));

    std::vector<Matrix> matrices{std::move(cA),std::move(fA)};

    return MultiGridStack<Prolongation,typename Matrix::block_type,
                                       typename Matrix::size_type>(std::move(prolongations),std::move(matrices));
  }

}

#endif


#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// path for left and right images
string left_file = "./left.png";
string right_file = "./right.png";

// Already written, just need to declare
void showPointCloud(
    const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

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

    // camera intrinsics
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // baseline
    double b = 0.573;

    // read the images
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    // use SGBM (semi-global batch matching) in opencv to calculate the parallex
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
        0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);    // magic numbers
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    // the result point cloud
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

    // If it runs slowly, change u,v into u += 2, v += 2
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // (x, y, z, color)

            // (Important) Use the stereo model to calculate the depth and stereo coordinates of the point
            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u));
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point);
        }

    cv::imshow("disparity", disparity / 96.0);
    cv::waitKey(0);
    // draw the point cloud
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.

#ifndef poplibs_test_Rnn_hpp
#define poplibs_test_Rnn_hpp

/**
 * Functions to compute forward, backward and weight update phases of a Vanilla
 * RNN
 */

#include <boost/multi_array.hpp>
#include <popnn/NonLinearityDef.hpp>

namespace poplibs_test {
namespace rnn {

/**
 * Computes the forward non-recursive part of the RNN
 *
 * Dimensions:
 *  x:
 *  [sequence][batch][input channel]
 * weights:
 *  [input channel][output channel]
 * y:
 *  [sequence][batch][output channel]
 *
 */
void forwardWeightInput(const boost::multi_array_ref<double, 3> x,
                        const boost::multi_array_ref<double, 2> weights,
                        const boost::multi_array_ref<double, 3> y);

/**
 * Computes the recursive part of a RNN. The sequence length is derived from the
 * input x. The initial value of the output isis in yInit.
 *
 * sequence_length = x.shape()[0]
 *
 * for (s = 0; s != sequence_length; ++s) {
 *   yPrev = s == 0 ? yInit : y(s - 1, :);
 *   y(s, :) = NonLinearity(weights * yPrev + x(s, :) + bias)
 * }
 *
 * Dimensions:
 *  x:
 *  [sequence][batch][output channel]
 *  yInit:
 *  [batch][output channel]
 *  weights:
 *  [input channel][output channel]
 *  bias:
 *  [output channel]
 *  y:[sequence][batch][output channel]
 */
void forwardIterate(const boost::multi_array_ref<double, 3> x,
                    const boost::multi_array_ref<double, 2> yInit,
                    const boost::multi_array_ref<double, 2> weights,
                    const boost::multi_array_ref<double, 1> bias,
                    boost::multi_array_ref<double, 3> y,
                    popnn::NonLinearityType nonLinearityType);

/**
 * Computes the backward pass of a RNN sequence.
 *
 * Loss gradients are computed at the summer and at the input in case these need
 * to be backpropagated to previous layers.
 *
 * Dimensions:
 * acts:
 * [sequence][batch][output channel]
 * nextLayerGrads:
 * [sequence][batch][output channel]
 * weightsInput:
 * [input channel][output channel]
 * weightsOutput:
 * [output channel][output channel]
 * prevLayerGrads:
 * [sequence][batch][input channel]
 * gradientSum:
 * [sequence][batch][output channel]
 */
void backward(const boost::multi_array_ref<double, 3> acts,
              const boost::multi_array_ref<double, 3> nextLayerGrads,
              const boost::multi_array_ref<double, 2> weightsInput,
              const boost::multi_array_ref<double, 2> weightsFeedback,
              boost::multi_array_ref<double, 3> prevLayerGrads,
              boost::multi_array_ref<double, 3> gradientSum,
              popnn::NonLinearityType nonLinearityType);

/**
 * Compute the parameter deltas for the whole sequence of an RNN
 *
 * The deltas are computed for all the parameters across sequence steps and
 * batch elements.
 *
 * Dimensions:
 * actsIn:
 * [sequence][batch][input channel]
 * initState:
 * [batch][output channel]
 * actsOut:
 * [sequence][batch][output channel]
 * gradientSum:
 * [sequence][batch][output channel]
 * weightsInputDeltas
 * [input channel][output channel]
 * weightsFeedbackDeltas
 * [output channel][output channel]
 * biasesDeltas
 * [output channel]
 */
void paramUpdate(const boost::multi_array_ref<double, 3> actsIn,
                 const boost::multi_array_ref<double, 2> initState,
                 const boost::multi_array_ref<double, 3> actsOut,
                 const boost::multi_array_ref<double, 3> gradientSum,
                 boost::multi_array_ref<double, 2> weightsInputDeltas,
                 boost::multi_array_ref<double, 2> weightsFeedbackDeltas,
                 boost::multi_array_ref<double, 1> biasesDeltas);

} // End namespace rnn.
} // End namespace poplibs_test.

#endif // poplibs_test_Rnn_hpp


#include <armadillo>
#include <complex>
#include <fftw3.h>

struct FilterWorker
{
    //Members
    const int nwin;       //Window size for data
    const int npad;       //Zero padding for the FFTs

    //Filter parameters
    const float alpha;      //Power of spectral filter
    const float beta;       //Strength of spectral filter
    const float lowpass;    //Wavelength of low pass filter

    //Private arrays
    arma::cx_fmat chip;      //Chip of data
    arma::cx_fmat SP;        //Spectral filter matrix
    arma::cx_fmat LP;        //Low pass filter matrix
    arma::cx_fmat  GW;        //Gaussian smoothing window
    arma::cx_fmat work;       //Temporary arrays
    arma::cx_fmat workpad;

    //Private plans
    fftwf_plan fwdplan;         //Stores forward plan
    fftwf_plan invplan;         //Stores inverse plan

    fftwf_plan fwdpadplan;      //FFT with padding
    fftwf_plan invpadplan;      //IFFT with padding

    //Functions
    inline int nfft()     //Sum of data window + padding
    {
        return npad + nwin;
    };

    inline int nfftpad()    //Sum of data window + padding + gaussian padding
    {
        return nfft() + 8;
    };

    inline void setupPlans();     //Set up FFT plans
    inline void destroyPlans();   //Destroy FFT plans
    inline void setupLowPass();   //Setup Butterwortjh filter
    inline void setupGaussian();  //Setup Gaussian window
    inline void filter();         //Do the actual filtering

    inline void zeros()
    {
        SP.zeros();
        chip.zeros();
    };

    //Constructor
    FilterWorker(int nwin_,int npad_, float a_, float b_, float l_):
        nwin(nwin_),
        npad(npad_),
        alpha(a_),
        beta(b_),
        lowpass(l_),
        chip(npad_ + nwin_, npad_ + nwin_),
        SP(npad_ + nwin_, npad_ + nwin_),
        LP(npad_ + nwin_, npad_ + nwin_),
        GW(npad_ + nwin_ + 8, npad_ + nwin_ + 8),
        work(npad_ + nwin_, npad_ + nwin_),
        workpad(npad_ + nwin_ + 8, npad_ + nwin_ + 8)
    {
        setupPlans();
        setupLowPass();
        setupGaussian();
    };

    //Destructor
    ~FilterWorker()
    {
        destroyPlans();
    };
};

//In memory setting of imaginary part to zero
inline void setImagPartToZero(arma::cx_fmat &mat)
{
    arma::fmat spread  = arma::fmat( (float*) mat.memptr(), 2 * mat.n_rows,
                                        mat.n_cols, false);
    spread.rows(arma::regspace<arma::uvec>(1,2,spread.n_rows-1)).zeros();
}

//See if any element is non-zero
inline bool hasNonZero(arma::cx_fmat &mat)
{
    arma::fvec spread = arma::fvec( (float*) mat.memptr(), 2*mat.n_rows*mat.n_cols, false);
    return arma::any(spread);
}

//Find median of complex matrix
inline std::complex<float> findMedian(arma::cx_fmat &mat)
{
    arma::cx_fvec spread = arma::cx_fvec( (std::complex<float>*) mat.memptr(), mat.n_rows*mat.n_cols, false);
    return arma::median(spread);
}

//FFTshift to handline padding. Not to confuse with actual FFTshift.
inline void FFTrepack(arma::cx_fmat &src, arma::cx_fmat &dst, int nsize)
{
    dst.zeros();
    int halfsize = nsize/2;

    //Quad BR to TL
    dst.submat(0,0, halfsize - 1, halfsize-1) = src.submat(halfsize, halfsize, nsize-1, nsize-1);

    //Quad TL to BR
    dst.submat(halfsize, halfsize, nsize-1, nsize-1) = src.submat(0,0,halfsize-1, halfsize-1);

    //Quad TR to BL
    dst.submat(halfsize, 0, nsize-1, halfsize-1) = src.submat(0, halfsize, halfsize-1, nsize-1);

    //Quad BL to TR
    dst.submat(0, halfsize, halfsize-1, nsize-1) = src.submat(halfsize, 0, nsize-1, halfsize-1);
}

//Create in place FFT plans
inline void FilterWorker::setupPlans()
{
    int nsize = nfft();

    int nlarge = nfftpad();

    //Forward FFT plan
    fwdplan = fftwf_plan_dft_2d(nsize, nsize, (fftwf_complex*) chip.memptr(), (fftwf_complex*) chip.memptr(), FFTW_FORWARD, FFTW_MEASURE);

    //Inverse FFT plan
    invplan = fftwf_plan_dft_2d(nsize, nsize, (fftwf_complex*) chip.memptr(), (fftwf_complex*) chip.memptr(), FFTW_BACKWARD, FFTW_MEASURE);

    //Forward FFT plan for gaussian filtering
    fwdpadplan = fftwf_plan_dft_2d(nlarge, nlarge, (fftwf_complex*) workpad.memptr(), (fftwf_complex*) workpad.memptr(), FFTW_FORWARD, FFTW_MEASURE);

    //Inverse FFT plan
    invpadplan = fftwf_plan_dft_2d(nlarge, nlarge, (fftwf_complex*) workpad.memptr(), (fftwf_complex*) workpad.memptr(), FFTW_BACKWARD, FFTW_MEASURE);

}

//Destroy FFT plans
inline void FilterWorker::destroyPlans()
{
    fftwf_destroy_plan(fwdplan);
    fftwf_destroy_plan(invplan);
    fftwf_destroy_plan(fwdpadplan);
    fftwf_destroy_plan(invpadplan);
}

inline void FilterWorker::setupLowPass()
{
    int nsize = nfft();

    if (lowpass <= 0.0f)
    {
        LP.zeros();
    }
    else
    {
        LP.ones();
        float freq0 = 1.0f / lowpass;
        for(int ii=0; ii< nsize; ii++)
        {
            float ff = (ii >= (nsize/2))? (ii-nsize) : ii;
            ff /= (1.0f*nsize);

            float butter = 1.0f / (1.0f + std::pow(ff/freq0, 10.0f));

//            std::cout << "Butter: " << ff << " " << std::pow(ff/lowpass, 10.0f) << "\n";
            LP.row(ii) *= std::complex<float>(butter,0.0f);
            LP.col(ii) *= std::complex<float>(butter,0.0f);
        }
    }
}

inline void FilterWorker::setupGaussian()
{
    //gausswin(7)
    arma::fvec gwin = {0.04393693f,0.24935221f,0.70664828f,1.0f,0.70664828f,0.24935221f,0.04393693f};

    int nsize = nfftpad();
    arma::cx_fvec padded(nsize);
    padded.zeros();

    //Wrap around
    for (int ii=0; ii<4; ii++)
        padded[ii].real(gwin[3+ii]);

    for (int ii=0; ii<3;ii++)
        padded[nsize-3+ii].real(gwin[ii]);

    //Symmetric 2D from 1D array
    workpad.ones();
    for (int ii=0; ii<nsize; ii++)
    {
        workpad.row(ii) *= padded[ii];
        workpad.col(ii) *= padded[ii];
    }

    //chip.submat(0,0,3,3).print();

    //Convert to spectra and store
    fftwf_execute(fwdpadplan);
    GW = workpad/(1.0f * nsize * nsize);
    workpad.zeros();

}

//Assumes that data has already been copied into the chip
inline void FilterWorker::filter()
{
    int nsize = nfft();
    int nlarge = nfftpad();

    //Zero padding
    chip.tail_cols(npad).zeros();
    chip.tail_rows(npad).zeros();

    //If chip is empty, skip all processing
    bool status = hasNonZero(chip);
    if (!status)
    {
        chip.zeros();
        return;
    }

    //If spectral component is desired.
    if (beta > 0.0f)
    {

        //Execute FFT
        fftwf_execute(fwdplan);

        //Copy to temporary array
        work.zeros();
        work.set_real(arma::abs(chip));

        //Execute FFTshift
        FFTrepack(work, workpad, work.n_rows);

        //convert abs value of spectra to time domain
        fftwf_execute(fwdpadplan);

        //multiply with gaussian spectra
        workpad %= GW;

        //Inverse FFT to complete convolution
        fftwf_execute(invpadplan);

        //Execute IFFTshift
        FFTrepack(workpad, work, work.n_rows);

        //Setting imaginary part to zero
        setImagPartToZero(work);

        //Compute median
        std::complex<float> median = findMedian(work);
//        std::cout << "Median = " << median << "\n";

        //Scale by median if needed
        if (median != 0.0f)
        {
            work /= median;
        }

        //Raise to power
        work = arma::pow(work, alpha);
        work -= std::complex<float>(1.0f,0.0f);

        for(int ii=0; ii< work.n_elem; ii++)
        {
            float val = work[ii].real();
            val = (val > 0.0f)? val: 0.0f;
            work[ii].real(val);
        }

        work *= beta;
    }
    else
    {
        work.zeros();
    }

    //Combine low pass and spectral component here
    chip %= (work + LP)/(1.0f * nsize * nsize);

    /*std::cout << "Start \n";
    chip.submat(0,0,4,4).print();
    std::cout << "Mid \n";
    chip.submat(13,13,17,17).print();
    std::cout << "End \n";
    chip.submat(27,27,31,31).print();*/

    //Perform inverse FFT
    fftwf_execute(invplan);

    return;
}


#include <iostream>

#define _USE_MATH_DEFINES
#include <math.h>

#include <string>
#include <boost/algorithm/string.hpp>

#include "projectrtpsoundfile.h"
#include "projectrtptonegen.h"

/*!md
We need to be able to generate tones. This is following the standard: https://www.itu.int/dms_pub/itu-t/opb/sp/T-SP-E.180-2010-PDF-E.pdf. Looping will be handled by soundsoup. So this section only needs to handle one cycle of the tone. We allocate memory required to generate the tone at the sample rate but not completley!

Our goal is to be efficient, so we do not generate tis on the fly - most tones will be generated into wav files and played when required.

If we want to play a tone continuously we should find a nicely looped file (e.g 1S will mean all frequencies in the file will hit zero at the end of the file). This would simplify our generation.

In the standard we have definitions such as:

United Kingdom of Great Britain
and Northern Ireland
Busy tone - 400 0.375 on 0.375 off
Congestion tone - 400 0.4 on 0.35 off 0.225 on 0.525 off
Dial tone - 50//350+440 continuous
Number unobtainable tone - 400 continuous
Pay tone - 400 0.125 on 0.125 off
Payphone recognition tone - 1200/800 0.2 on 0.2 off 0.2 on 2.0 off
Ringing tone - 400+450//400x25//400x16 2/3 0.4 on 0.2 off 0.4 on 2.0 off

i.e. Tone - Frequency - Cadence

The frequency is

Frequency in Hz:
f1×f2 f1 is modulated by f2
f1+f2 the juxtaposition of two frequencies f1 and f2 without modulation
f1/f2 f1 is followed by f2
f1//f2 in some exchanges frequency f1 is used and in others frequency f2 is used.
Cadence in seconds: ON – OFF

Try to keep our definitions as close to the standard. We also have to introduce some other items:

* Amplitude
* Change (in frequency or amplitude) - frequency can be handled by modulated

Take ringing tone:

400+450//400x25//400x16 2/3 0.4 on 0.2 off 0.4 on 2.0 off

We can ignore the // in our definition as we can simply choose the most common one.
So either 400+450 or 400x25
Three does not appear ot be anything in the standard relating to the 2/3?

Amplitude can be introduced by *
so

400+450 becomes 400+450*0.75 (every frequency will have its amplitude reduced).
400x25*0.75 is then also suported.

Increasing tones such as:
950/1400/1800

Cadence
950/1400/1800/0:333/333/333/1000
Note, we have introduced a final /0 to indicate silence. The cadences will iterated through for every / in the frequency list and is in mS (the standard lists in seconds). We don't need to support loops as soundsoup supports loops.
For:
950/1400/1800/0:333
Means each section will be 333mS.

Change
400+450*0.75~0 will reduce the amplitude from 0.75 to 0 during that cadence period
400~450 will increase the frequency during that cadence period

Note 400+450x300 is not supported.

*/

static void gentone( int16_t *outbuffer, int sizeofblock, double startfrequency, double endfrequency, double startamp, double endamp, int samplerate )
{
  if( 0 == startfrequency && 0 == endfrequency ) return;

  double angle;
  double ampatpos = startamp;
  double amppersample = ( endamp - startamp ) / sizeofblock;
  double freqpersample = ( endfrequency - startfrequency ) / sizeofblock;
  double frequencyatpos = startfrequency;
  if( 0 == startfrequency )
  {
    angle = 0;
  }
  else
  {
    angle = ( 2 * M_PI ) / ( double ) samplerate * startfrequency;
  }

  for( int i = 0; i < sizeofblock; i++ )
  {
    *outbuffer += static_cast< int16_t >( ( sin( angle * i ) * SHRT_MAX * ampatpos ) );
    outbuffer++;

    ampatpos += amppersample;
    frequencyatpos += freqpersample;

    if( 0 == frequencyatpos )
    {
      angle = 0;
    }
    else
    {
      angle = ( 2 * M_PI ) / ( double ) samplerate * frequencyatpos;
    }
  }
}

static void gen( std::string tone, std::string filename )
{
  vectorofstrings freqscadence;
  boost::split( freqscadence, tone, boost::is_any_of( ":" ) );

  if( 2 != freqscadence.size() )
  {
    std::cerr << "You must supply a cadence, eg. 400:1000" << std::endl;
    return;
  }

  vectorofstrings frequencies;
  vectorofstrings cadences;
  boost::split( frequencies, freqscadence[ 0 ], boost::is_any_of( "/" ) );
  boost::split( cadences, freqscadence[ 1 ], boost::is_any_of( "/" ) );

  uint8_t *readbuffer = nullptr;
  wavheader outwavheader;
  initwav( &outwavheader );

  /* 1. Calculate total time */
  vectorofstrings::iterator it;
  vectorofstrings cadenceparts;
  int cadencepos = 0;
  int cadencetotal = 0;
  for( it = frequencies.begin(); it != frequencies.end(); it++ )
  {
    cadencetotal += std::atoi( cadences[ cadencepos ].c_str() );
    cadencepos = ( cadencepos + 1 ) % cadences.size();
  }

  std::cout << "Total time is " << cadencetotal << "mS" << std::endl;

  /* 2. Allocate enough memory */
  outwavheader.subchunksize = cadencetotal * outwavheader.sample_rate / 1000 * 2 /* 2 bytes per sample */;
  outwavheader.chunksize = outwavheader.subchunksize + 36;

  readbuffer = new uint8_t[ outwavheader.chunksize ];
  memset( readbuffer, 0, outwavheader.chunksize );

  /* 3. Generate tones */
  int pos = 0;
  cadencepos = 0;
  for( it = frequencies.begin(); it != frequencies.end(); it++ )
  {
    /* Current cadence. */
    int cadence = std::atoi( cadences[ cadencepos ].c_str() );
    cadencepos = ( cadencepos + 1 ) % cadences.size();

    int16_t *outbuffer = ( int16_t * ) &readbuffer[ pos * 2 ];
    int sizeofblock = outwavheader.sample_rate * cadence / 1000 /*mS*/;

    /* current frequency */
    /* *it could be 400+450 or or 400x25 450+450*0.75 or 450~480 or 450+450*0.75~1 */
    vectorofstrings freqamp;
    boost::split( freqamp, *it, boost::is_any_of( "*" ) );
    double startamp = 1;
    double endamp = 1;
    if( freqamp.size() > 1 )
    {
      vectorofstrings ampfromto;
      boost::split( ampfromto, freqamp[ 1 ], boost::is_any_of( "~" ) );
      startamp = std::atof( ampfromto[ 0 ].c_str() );
      endamp = startamp;
      if( ampfromto.size() > 1 )
      {
        endamp = std::atof( ampfromto[ 1 ].c_str() );
      }
    }
    vectorofstrings freqparts;
    boost::split( freqparts, freqamp[ 0 ], boost::is_any_of( "+x~" ) );

    double startfreq = -1;
    double endfreq = -1;

    std::size_t found = freqamp[ 0 ].find_first_of( "+x~" );
    if( std::string::npos == found )
    {
      startfreq = std::atof( freqparts[ 0 ].c_str() );
      gentone( outbuffer, sizeofblock, startfreq, startfreq, startamp, endamp, outwavheader.sample_rate );
      goto continueloop;
    }
    else
    {
      switch( freqamp[ 0 ][ found ] )
      {
        case '+':
        {
          for( auto freqit = freqparts.begin(); freqit != freqparts.end(); freqit++ )
          {
            startfreq = std::atof( freqit->c_str() );
            gentone( outbuffer, sizeofblock, startfreq, startfreq, startamp, endamp, outwavheader.sample_rate );
          }
          break;
        }
        case '~':
        {
          startfreq = std::atof( freqparts[ 0 ].c_str() );
          endfreq = std::atof( freqparts[ 1 ].c_str() );
          gentone( outbuffer, sizeofblock, startfreq, endfreq, startamp, endamp, outwavheader.sample_rate );
          break;
        }
      }
    }

continueloop:
    pos += sizeofblock;
  }

  /* Write */
  int file = open( filename.c_str(), O_RDWR | O_CREAT, S_IRUSR | S_IWUSR );
  __off_t position = lseek( file, 0, SEEK_END );
  if( 0 == position )
  {
    write( file, &outwavheader, sizeof( wavheader ) );
    write( file, readbuffer, outwavheader.chunksize );
  }
  else
  {
    wavheader currentheader;
    lseek( file, 0, SEEK_SET );
    read( file, &currentheader, sizeof( wavheader ) );
    lseek( file, 0, SEEK_END );

    if( currentheader.audio_format == outwavheader.audio_format &&
          currentheader.sample_rate == outwavheader.sample_rate )
    {
      /* Ok, good enough! */
      currentheader.chunksize += outwavheader.chunksize;
      currentheader.subchunksize += outwavheader.subchunksize;

      lseek( file, 0, SEEK_SET );
      write( file, &currentheader, sizeof( wavheader ) );
      lseek( file, 0, SEEK_END );
      write( file, readbuffer, outwavheader.chunksize );
    }
    else
    {
      std::cerr << "File format to append should be the same" << std::endl;
    }
  }

  /* Clean up */
  if( nullptr != readbuffer )
  {
    delete[] readbuffer;
  }

  close( file );
}

/*!md
## gentone
For test purposes only. Generate a tone into a wav file base on the 2 params.
*/
void gentone( const char *tone, const char *file )
{
  gen( tone, file );
}