/* -*- c++ -*- */
/*
 * Copyright 2010,2012 Free Software Foundation, Inc.
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

// Calculate the taps for the CPM phase responses

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <cmath>
#include <cfloat>
#include <gnuradio/analog/cpm.h>

//gives us erf on compilers without it
#include <boost/math/special_functions/erf.hpp>
namespace bm = boost::math;

namespace gr {
  namespace analog {

#ifndef M_TWOPI
#  define M_TWOPI (2*M_PI)
#endif

    //! Normalised sinc function, sinc(x)=sin(pi*x)/pi*x
    inline double
    sinc(double x)
    {
      if(x == 0) {
	return 1.0;
      }
      return sin(M_PI * x) / (M_PI * x);
    }

    //! Taps for L-RC CPM (Raised cosine of length L symbols)
    std::vector<float>
    generate_cpm_lrc_taps(unsigned samples_per_sym, unsigned L)
    {
      std::vector<float> taps(samples_per_sym * L, 1.0/L/samples_per_sym);
      for(unsigned i = 0; i < samples_per_sym * L; i++) {
	taps[i] *= 1 - cos(M_TWOPI * i / L / samples_per_sym);
      }

      return taps;
    }

    /*! Taps for L-SRC CPM (Spectral raised cosine of length L symbols).
     *
     * L-SRC has a time-continuous phase response function of
     *
     * g(t) = 1/LT * sinc(2t/LT) * cos(beta * 2pi t / LT) / (1 - (4beta / LT * t)^2)
     *
     * which is the Fourier transform of a cos-rolloff function with rolloff
     * beta, and looks like a sinc-function, multiplied with a rolloff term.
     * We return the main lobe of the sinc, i.e., everything between the
     * zero crossings.
     * The time-discrete IR is thus
     *
     * g(k) = 1/Ls * sinc(2k/Ls) * cos(beta * pi k / Ls) / (1 - (4beta / Ls * k)^2)
     * where k = 0...Ls-1
     * and s = samples per symbol.
     */
    std::vector<float>
    generate_cpm_lsrc_taps(unsigned samples_per_sym, unsigned L, double beta)
    {
      double Ls = (double) L * samples_per_sym;
      std::vector<double> taps_d(L * samples_per_sym, 0.0);
      std::vector<float> taps(L * samples_per_sym, 0.0);

      double sum = 0;
      for(unsigned i = 0; i < samples_per_sym * L; i++) {
	double k =  i - Ls/2; // Causal to acausal

	taps_d[i] = 1.0 / Ls * sinc(2.0 * k / Ls);

	// For k = +/-Ls/4*beta, the rolloff term's cos-function becomes zero
	// and the whole thing converges to PI/4 (to prove this, use de
	// l'hopital's rule).
	if(fabs(fabs(k) - Ls/4/beta) < 2*DBL_EPSILON) {
	  taps_d[i] *= M_PI_4;
	}
	else {
	  double tmp = 4.0 * beta * k / Ls;
	  taps_d[i] *= cos(beta * M_TWOPI * k / Ls) / (1 - tmp * tmp);
	}
	sum += taps_d[i];
      }

      for(unsigned i = 0; i < samples_per_sym * L; i++) {
	taps[i] = (float) taps_d[i] / sum;
      }

      return taps;
    }

    //! Taps for L-REC CPM (Rectangular pulse shape of length L symbols)
    std::vector<float>
    generate_cpm_lrec_taps(unsigned samples_per_sym, unsigned L)
    {
      return std::vector<float>(samples_per_sym * L, 1.0/L/samples_per_sym);
    }

    //! Helper function for TFM
    double tfm_g0(double k, double sps)
    {
      if(fabs(k) < 2 * DBL_EPSILON) {
	return 1.145393004159143; // 1 + pi^2/48 / sqrt(2)
      }

      const double pi2_24 = 0.411233516712057; // pi^2/24
      double f = M_PI * k / sps;
      return sinc(k/sps) - pi2_24 * (2 * sin(f) - 2*f*cos(f) - f*f*sin(f)) / (f*f*f);
    }

    //! Taps for TFM CPM (Tamed frequency modulation)
    //
    // See [2, Chapter 2.7.2].
    //
    // [2]: Anderson, Aulin and Sundberg; Digital Phase Modulation
    std::vector<float>
    generate_cpm_tfm_taps(unsigned sps, unsigned L)
    {
      unsigned causal_shift = sps * L / 2;
      std::vector<double> taps_d(sps * L, 0.0);
      std::vector<float> taps(sps * L, 0.0);

      double sum = 0;
      for(unsigned i = 0; i < sps * L; i++) {
	double k = (double)(((int)i) - ((int)causal_shift)); // Causal to acausal

	taps_d[i] = tfm_g0(k - sps, sps) +
	  2 * tfm_g0(k,       sps) +
	  tfm_g0(k + sps, sps);
	sum += taps_d[i];
      }

      for(unsigned i = 0; i < sps * L; i++) {
	taps[i] = (float) taps_d[i] / sum;
      }

      return taps;
    }

    //! Taps for Gaussian CPM. Phase response is truncated after \p L symbols.
    //  \p bt sets the 3dB-time-bandwidth product.
    //
    // Note: for h = 0.5, this is the phase response for GMSK.
    //
    // This C99-compatible formula for the taps is taken straight
    // from [1, Chapter 9.2.3].
    // A version in Q-notation can be found in [2, Chapter 2.7.2].
    //
    // [1]: Karl-Dirk Kammeyer; Nachrichtenübertragung, 4th Edition.
    // [2]: Anderson, Aulin and Sundberg; Digital Phase Modulation
    //
    std::vector<float>
    generate_cpm_gaussian_taps(unsigned samples_per_sym, unsigned L, double bt)
    {
      double Ls = (double) L * samples_per_sym;
      std::vector<double> taps_d(L * samples_per_sym, 0.0);
      std::vector<float> taps(L * samples_per_sym, 0.0);

      // alpha = sqrt(2/ln(2)) * pi * BT
      double alpha = 5.336446256636997 * bt;
      for(unsigned i = 0; i < samples_per_sym * L; i++) {
	double k =  i - Ls/2; // Causal to acausal
	taps_d[i] = (bm::erf(alpha * (k / samples_per_sym + 0.5)) -
		     bm::erf(alpha * (k / samples_per_sym - 0.5)))
	  * 0.5 / samples_per_sym;
	taps[i] = (float) taps_d[i];
      }

      return taps;
    }

    std::vector<float>
    cpm::phase_response(cpm_type type, unsigned samples_per_sym, unsigned L, double beta)
    {
      switch(type) {
      case LRC:
	return generate_cpm_lrc_taps(samples_per_sym, L);

      case LSRC:
	return generate_cpm_lsrc_taps(samples_per_sym, L, beta);

      case LREC:
	return generate_cpm_lrec_taps(samples_per_sym, L);

      case TFM:
	return generate_cpm_tfm_taps(samples_per_sym, L);

      case GAUSSIAN:
	return generate_cpm_gaussian_taps(samples_per_sym, L, beta);

      default:
	return generate_cpm_lrec_taps(samples_per_sym, 1);
      }
    }

  } // namespace analog
} // namespace gr


#ifndef qlex_doubleexponentialcalibration_hpp
#define qlex_doubleexponentialcalibration_hpp

#include <ql/math/optimization/endcriteria.hpp>
#include <ql/math/optimization/projectedcostfunction.hpp>
#include <ql/math/array.hpp>
#include <ql/quote.hpp>
#include <boost/shared_ptr.hpp>
#include <vector>

using namespace QuantLib;

namespace QLExtension {

    class QuantLib::Quote;
    class QuantLib::OptimizationMethod;
    class QuantLib::ParametersTransformation;

	// Time homogeneous vol term structure
	// pp. 104 Valuation and Risk Management in Energy Market
	// sigma^2(T-t) = simga^2[exp(-2b_1(T-1)+lambda*exp(-2b_2(T-t)]
    class DoubleExponentialCalibration {

    private:

		class DoubleExponentialError : public CostFunction {
          public:
            DoubleExponentialError(DoubleExponentialCalibration* dblexp) : dblexp_(dblexp) {}

            Real value(const Array& x) const {
				dblexp_->sigma_ = x[0];
				dblexp_->b1_ = x[1];
				dblexp_->b2_ = x[2];
				dblexp_->lambda_ = x[3];
				return dblexp_->error();
            }
            Disposable<Array> values(const Array& x) const {
				dblexp_->sigma_ = x[0];
				dblexp_->b1_ = x[1];
				dblexp_->b2_ = x[2];
				dblexp_->lambda_ = x[3];
				return dblexp_->errors();
            }
          private:
            DoubleExponentialCalibration* dblexp_;
        };

      public:
        DoubleExponentialCalibration() {};
		DoubleExponentialCalibration(
             const std::vector<Real>& t,
             const std::vector<Real>& blackVols,
             Real sigmaGuess = 0.10,
             Real b1Guess =  0.25,
             Real b2Guess =  0.60,
             Real lambdaGuess =  0.5,
             bool sigmaIsFixed = false,
             bool b1IsFixed = false,
             bool b2IsFixed = false,
             bool lambdaIsFixed = false,
             bool vegaWeighted = false,
             const boost::shared_ptr<EndCriteria>& endCriteria
                      = boost::shared_ptr<EndCriteria>(),
             const boost::shared_ptr<OptimizationMethod>& method
                      = boost::shared_ptr<OptimizationMethod>());

        //! adjustment factors needed to match Black vols
		std::vector<Real> k() const;		// big T approach
        std::vector<Real> k(const std::vector<Real>& t,
                            const std::vector<Real>& blackVols) const;
        void compute();
        //calibration results
		// obsolute
        Real value(Real x, Real T) const;
		Real value(Real t1, Real t2, Real T) const;

		// max abs(modelvol - marketvol)
        Real maxError() const;
		// vector (modelvol-marketvol)*sqrt(weight)
        Disposable<Array> errors() const;
		// square sum of errors()
		Real error() const;

        EndCriteria::Type endCriteria() const;

        Real sigma() const;
        Real b1() const;
        Real b2() const;
        Real lambda() const;

        bool sigmaIsFixed_, b1IsFixed_, b2IsFixed_, lambdaIsFixed_;
        Real sigma_, b1_, b2_, lambda_;

      private:

        // optimization method used for fitting
        mutable EndCriteria::Type dblexpEndCriteria_;
        boost::shared_ptr<EndCriteria> endCriteria_;
        boost::shared_ptr<OptimizationMethod> optMethod_;
        mutable std::vector<Real> weights_;
        bool vegaWeighted_;
        //! Parameters
        std::vector<Real> times_, blackVols_;

		inline void validateDoubleExponentialParameters(Real sigma,
			Real b1, // no condition on b
			Real b2,
			Real lambda) {
			QL_REQUIRE(sigma > 0,
				"sigma (" << sigma << ") must be positive");
			QL_REQUIRE(b1 > 0,
				"b1 (" << b1 << ") must be positive");
			QL_REQUIRE(b2 > 0,
				"b2 (" << b2 << ") must be positive");
			QL_REQUIRE(lambda > 0,
				"lambda (" << lambda << ") must be positive");
		}
    };
}

#endif


// Author: Daisuke Kanaizumi
// Affiliation: Department of Applied Mathematics, Waseda University

// verification program for the q-Hypergeometric function

#ifndef QHYPERGEOMETRIC_HPP
#define QHYPERGEOMETRIC_HPP

#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/complex.hpp>
#include <kv/qAiry.hpp>
#include <kv/Heine.hpp>
#include <kv/Pochhammer.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
namespace ub = boost::numeric::ublas;
namespace kv{
template <class T> interval<T> qPochhammer(const ub::vector<interval<T> >& a,const interval<T>& q,int n){
interval<T> res,pro;
int r;
r=a.size();
pro=1.;
for(int i=0;i<=r-1;i++){
pro=pro*qPochhammer(interval<T>(a(i)),interval<T>(q),int (n));
}
res=pro;
return res;
}
template <class T> complex<interval<T> >qPochhammer(const ub::vector<complex<interval<T> > >& a,const interval<T>& q,int n){
complex<interval<T> >res,pro;
int r;
r=a.size();
pro=1.;
for(int i=0;i<=r-1;i++){
  pro=pro*qPochhammer(complex<interval<T> >(a(i)),interval<T>(q),int (n));
}
res=pro;
return res;
}

template <class T> interval<T> QHypergeom(const ub::vector<interval<T> >& a,const ub::vector<interval<T> >& b,const interval<T>& q,const interval<T>& z){
  interval<T>res,mid,first,ratio,pro1,pro2;
  T rad;
  int r,s;
  r=a.size();
  s=b.size();
  mid=1.;
  pro1=1.;
  pro2=1.;
  int N;
  N=1000;
  for(int i=0;i<=s-1;i++){
    while(abs(b(i))>pow(1/q,N)){
      N=N+500;
    }
  }
  if (q>=1){
    throw std::domain_error("value of q must be under 1");
  }
  if (q<=0){
    throw std::domain_error("q must be positive");
  }
  if (r>s+1){
    throw std::domain_error("r>s+1 is not implemented");
  }
  if(r==s+1){
    if (abs(z)>=1){
      throw std::domain_error("absolute value of z must be under 1");
    }
    if(r==2&&s==1){
      // Heine hypergeometric function
      res=Heine(interval<T>(a(0)),interval<T>(a(1)),interval<T>(b(0)),interval<T>(q),interval<T>(z));
    }
    else{
      for(int n=1;n<=N-1;n++){
	mid=mid+qPochhammer(ub::vector<interval<T> >(a),interval<T>(q),int (n))*pow(z,n)
	  /qPochhammer(ub::vector<interval<T> >(b),interval<T>(q),int (n))/qPochhammer(interval<T>(q),interval<T>(q),int (n));
      }
      first=abs(qPochhammer(ub::vector<interval<T> >(a),interval<T>(q),int (N))*pow(z,N)
		/qPochhammer(ub::vector<interval<T> >(b),interval<T>(q),int (N))/qPochhammer(interval<T>(q),interval<T>(q),int (N)));
      for(int j=0;j<=s-1;j++){
	pro1=pro1*(1+abs(b(j)-a(j))*pow(q,N)/abs(1-b(j)*pow(q,N)));
      }
      ratio=abs(z)*pro1*(1+pow(q,N)*abs(q-a(r-1))/abs(1-pow(q,N+1)));
      if(ratio<1){
	rad=(first/(1-ratio)).upper();
	res=mid+rad*interval<T>(-1.,1.);
      }
      else{
	throw std::domain_error("ratio is more than 1");
      }

    }
  }
  if(r<=s){
    if(r==0&&s==1){
      res=_0phi_1(interval<T>(b(0)),interval<T>(q),interval<T>(z));
    }
    if(r==1&&s==1){
      res=_1phi_1(interval<T>(a(0)),interval<T>(b(0)),interval<T>(q),interval<T>(z));
    }

    else{
      for(int n=1;n<=N-1;n++){
	mid=mid+qPochhammer(ub::vector<interval<T> >(a),interval<T>(q),int (n))*std::pow(-1,1+s-r)*pow(q,n*(n-1)*0.5*(1+s-r))*pow(z,n)
	  /qPochhammer(ub::vector<interval<T> >(b),interval<T>(q),int (n))/qPochhammer(interval<T>(q),interval<T>(q),int (n));
      }
      first=abs(qPochhammer(ub::vector<interval<T> >(a),interval<T>(q),int (N))*std::pow(-1,1+s-r)*pow(q,N*(N-1)*0.5*(1+s-r))*pow(z,N)
		/qPochhammer(ub::vector<interval<T> >(b),interval<T>(q),int (N))/qPochhammer(interval<T>(q),interval<T>(q),int (N)));
      for(int k=0;k<=r-1;k++){
	pro1=pro1*(1+abs(b(k)-a(k))*pow(q,N)/abs(1-b(k)*pow(q,N)));
      }
      for(int h=r;h<=s-1;h++){
	pro2=pro2*pow(q,N*(1+s-r))/abs(1-b(h)*pow(q,N));
      }
      ratio=abs(z)*pro1*pro2/abs(1-pow(q,N));
      if(ratio<1){
	rad=(first/(1-ratio)).upper();
	res=mid+rad*interval<T>(-1.,1.);
      }
      else{
	throw std::domain_error("ratio is more than 1");
      }
    }
  }
return res;
}

  template <class T> complex<interval<T> >QHypergeom(const ub::vector<complex<interval<T> > >& a,const ub::vector<complex<interval<T> > >& b,const interval<T>& q,const complex<interval<T> >& z){
    complex<interval<T> >res,mid;
    interval<T>first,ratio,pro1,pro2;
    T rad;
    int r,s;
    r=a.size();
    s=b.size();
    mid=1.;
    pro1=1.;
    pro2=1.;
    int N;
    N=1000;
    for(int i=0;i<=s-1;i++){
      while(abs(b(i))>pow(1/q,N)){
	N=N+500;
      }
    }
    if (q>=1){
      throw std::domain_error("value of q must be under 1");
    }
    if (q<=0){
      throw std::domain_error("q must be positive");
    }
    if (r>s+1){
      throw std::domain_error("r>s+1 is not implemented");
    }
    if(r==s+1){
      if (abs(z)>=1){
	throw std::domain_error("absolute value of z must be under 1");
      }
      if(r==2&&s==1){
	// Heine hypergeometric function
	res=Heine(complex<interval<T> >(a(0)),complex<interval<T> >(a(1)),complex<interval<T> >(b(0)),interval<T>(q),complex<interval<T> >(z));
}
      else{
	for(int n=1;n<=N-1;n++){
	  mid=mid+qPochhammer(ub::vector<complex<interval<T> > >(a),interval<T>(q),int (n))*pow(z,n)
	    /qPochhammer(ub::vector<complex<interval<T> > >(b),interval<T>(q),int (n))/qPochhammer(interval<T>(q),interval<T>(q),int (n));
	}
	first=abs(qPochhammer(ub::vector<complex<interval<T> > >(a),interval<T>(q),int (N))*pow(z,N)
		  /qPochhammer(ub::vector<complex<interval<T> > >(b),interval<T>(q),int (N))/qPochhammer(interval<T>(q),interval<T>(q),int (N)));
	for(int j=0;j<=s-1;j++){
	  pro1=pro1*(1+abs(b(j)-a(j))*pow(q,N)/abs(1-b(j)*pow(q,N)));
	}
	ratio=abs(z)*pro1*(1+pow(q,N)*abs(q-a(r-1))/abs(1-pow(q,N+1)));
	if(ratio<1){
	  rad=(first/(1-ratio)).upper();
	  res=complex_nbd(mid,rad);
	}
	else{
	  throw std::domain_error("ratio is more than 1");
	}

      }
    }
    if(r<=s){
      if(r==0&&s==1){
	res=_0phi_1(complex<interval<T> >(b(0)),interval<T>(q),complex<interval<T> >(z));
      }
      if(r==1&&s==1){
	res=_1phi_1(complex<interval<T> >(a(0)),complex<interval<T> >(b(0)),interval<T>(q),complex<interval<T> >(z));
      }

      else{
	for(int n=1;n<=N-1;n++){
	  mid=mid+qPochhammer(ub::vector<complex<interval<T> > >(a),interval<T>(q),int (n))*std::pow(-1,1+s-r)*pow(q,n*(n-1)*0.5*(1+s-r))*pow(z,n)
	    /qPochhammer(ub::vector<complex<interval<T> > >(b),interval<T>(q),int (n))/qPochhammer(interval<T>(q),interval<T>(q),int (n));
	}
	first=abs(qPochhammer(ub::vector<complex<interval<T> > >(a),interval<T>(q),int (N))*std::pow(-1,1+s-r)*pow(q,N*(N-1)*0.5*(1+s-r))*pow(z,N)
		  /qPochhammer(ub::vector<complex<interval<T> > >(b),interval<T>(q),int (N))/qPochhammer(interval<T>(q),interval<T>(q),int (N)));
	for(int k=0;k<=r-1;k++){
	  pro1=pro1*(1+abs(b(k)-a(k))*pow(q,N)/abs(1-b(k)*pow(q,N)));
	}
	for(int h=r;h<=s-1;h++){
	  pro2=pro2*pow(q,N*(1+s-r))/abs(1-b(h)*pow(q,N));
	}
	ratio=abs(z)*pro1*pro2/abs(1-pow(q,N));
	if(ratio<1){
	  rad=(first/(1-ratio)).upper();
	  res=complex_nbd(mid,rad);
	}
	else{
	  throw std::domain_error("ratio is more than 1");
	}
      }
    }
    return res;
  }
  template <class T> complex<interval<T> >qAppell1(const complex<interval<T> >& a,const complex<interval<T> >& b,const complex<interval<T> >& bp,const complex<interval<T> >& c ,
						     const interval<T>& q,const complex<interval<T> >& x,const complex<interval<T> >& y){
    // verification program for the first q-Appell function
    // reference:DLMF http://dlmf.nist.gov/17.11 formula 17.11.1
    complex<interval<T> >res;
    ub::vector<complex<interval<T> > >v1(3),v2(2);
    v1(0)=c/a;v1(1)=x;v1(2)=y;
    v2(0)=b*x;v2(1)=bp*y;
    if (q>=1){
      throw std::domain_error("value of q must be under 1");
    }
    if (q<=0){
      throw std::domain_error("q must be positive");
    }
    if (abs(x).upper()>=1){
      throw std::domain_error("absolute value of x must be under 1");
    }
    if (abs(y).upper()>=1){
      throw std::domain_error("absolute value of y must be under 1");
    }

    res=infinite_qPochhammer(complex<interval<T> >(a),interval<T>(q))
      *infinite_qPochhammer(complex<interval<T> >(b*x),interval<T>(q))
      *infinite_qPochhammer(complex<interval<T> >(bp*y),interval<T>(q))
      /infinite_qPochhammer(complex<interval<T> >(c),interval<T>(q))
      /infinite_qPochhammer(complex<interval<T> >(x),interval<T>(q))
      /infinite_qPochhammer(complex<interval<T> >(y),interval<T>(q))
      *QHypergeom(ub::vector<complex<interval<T> > >(v1),ub::vector<complex<interval<T> > >(v2),interval<T>(q),complex<interval<T> >(a));
    return res;
  }

  template <class T> interval<T> infinite_qPochhammer(const ub::vector<interval<T> >& a,const interval<T>& q){
    interval<T> res,pro;
    int r;
    r=a.size();
    pro=1.;
    for(int i=0;i<=r-1;i++){
      pro=pro*infinite_qPochhammer(interval<T>(a(i)),interval<T>(q));
    }
    res=pro;
    return res;
  }
  template <class T> complex<interval<T> >infinite_qPochhammer(const ub::vector<complex<interval<T> > >& a,const interval<T>& q){
    complex<interval<T> >res,pro;
    int r;
    r=a.size();
    pro=1.;
    for(int i=0;i<=r-1;i++){
      pro=pro*infinite_qPochhammer(complex<interval<T> >(a(i)),interval<T>(q));
    }
    res=pro;
    return res;
  }
  template <class T> complex<interval<T> >qLauricellaD(const complex<interval<T> >& a,const ub::vector<complex<interval<T> > >& b,const complex<interval<T> >& c ,
						     const interval<T>& q,const ub::vector<complex<interval<T> > >& x){
    // verification program for the q-Lauricella function type D
    // reference: Andrews (1972), Gasper-Rahman (2004) (Page 300, Exercise 10.17)
    if (q>=1){
      throw std::domain_error("value of q must be under 1");
    }
    if (q<=0){
      throw std::domain_error("q must be positive");
    }
    if(b.size()!=x.size()){
      throw std::domain_error("size of b and x must be same");
    }
    int r;
    r=b.size();
    for(int k=0;k<=r-1;k++){
      if(abs(x(k)).upper()>=1){
      throw std::domain_error("value of x must be under 1");
      }
    }
    complex<interval<T> >res;
    ub::vector<complex<interval<T> > >v1(r),v2(r);
    for(int i=0;i<=r-1;i++){
      v1(i)=b(i)*x(i);
    }
    v2(0)=c/a;
    for(int j=1;j<=r-1;j++){
      v2(j)=x(j);
    }
    res=infinite_qPochhammer(complex<interval<T> >(a),interval<T>(q))/infinite_qPochhammer(complex<interval<T> >(c),interval<T>(q))
      *infinite_qPochhammer(ub::vector<complex<interval<T> > >(v1),interval<T>(q))/infinite_qPochhammer(ub::vector<complex<interval<T> > >(x),interval<T>(q))
      *QHypergeom(ub::vector<complex<interval<T> > >(v2),ub::vector<complex<interval<T> > >(v1),interval<T>(q),complex<interval<T> >(a));
    return res;
  }
  template <class T> complex<interval<T> >VWP(const complex<interval<T> >& a,const ub::vector<complex<interval<T> > >& b,const interval<T>& q,const complex<interval<T> >& x){
    // verification program for the very-well-poised q-hypergeometric function
    if (q>=1){
      throw std::domain_error("value of q must be under 1");
    }
    if (q<=0){
      throw std::domain_error("q must be positive");
    }
    if (abs(x)>=1){
      throw std::domain_error("absolute value of x must be under 1");
    }
    int r;
    r=b.size()+3;
    complex<interval<T> >res;
    ub::vector<complex<interval<T> > >v1(r+1),v2(r);
    v1(0)=a;v1(1)=q*sqrt(a);v1(2)=-q*sqrt(a);
    for(int i=3;i<=r;i++){
      v1(i)=b(i-3);
    }
    v2(0)=sqrt(a);v2(1)=-sqrt(a);
    for(int j=2;j<=r-1;j++){
      v2(j)=a*q/b(j-2);
    }

    res=QHypergeom(ub::vector<complex<interval<T> > >(v1),ub::vector<complex<interval<T> > >(v2),interval<T>(q),complex<interval<T> >(x));
    return res;
  }
  template <class T> interval<T> Euler_qlog(const interval<T>& q,const interval<T> & x){
    // verification program for Euler's q-logarithm
    // reference: Koelink-Van Assche
    interval<T> res;
    ub::vector<interval<T> >v1(3),v2(2);
    v1(0)=q;v1(1)=q;v1(2)=q*x;
    v2(0)=q*q;v2(1)=0;
    res=-q*(1-x)*QHypergeom(ub::vector<interval<T>  >(v1),ub::vector<interval<T>  >(v2),interval<T>(q),interval<T> (q))/(1-q);
    return res;

  }
}
#endif


#include "unmixing.h"
#include <cmath>
#include <cfloat>
#include <iostream>
#include <ctime>
#include <thread>
#include <Eigen/LU>
#include "nloptutility.h"
#include "image_processing.h"

using Eigen::Vector3d;
using Eigen::Vector4d;
using Eigen::VectorXd;
using Eigen::Vector3i;
using Eigen::Matrix3d;
using ImageProcessing::Image;
using ImageProcessing::ColorImage;

//#define SPARCITY
#define PARALLEL

namespace
{

#ifdef PARALLEL
// Perform the function in parallel for { 0, 1, ..., n - 1 }
template<typename Callable>
void perform_in_parallel(Callable function, int width, int height)
{
    const int hint      = std::thread::hardware_concurrency();
    const int n_threads = std::min(width * height, (hint == 0) ? 4 : hint);

    auto inner_loop = [width, height, n_threads, function](const int j)
    {
        const int n = width * height;

        const int start_index = j * (n / n_threads);
        const int end_index   = (j + 1 == n_threads) ? n : (j + 1) * (n / n_threads);

        for (int k = start_index; k < end_index; ++ k) function(k % width, k / width);
    };
    std::vector<std::thread> threads;
    for (int j = 0; j < n_threads; ++ j) threads.push_back(std::thread(inner_loop, j));
    for (auto& t : threads) t.join();
}
#endif

struct ColorKernel
{
    ColorKernel(const Vector3d& mu, const Matrix3d& sigma_inv, int seed_x = - 1, int seed_y = - 1) :
        mu(mu),
        sigma_inv(sigma_inv),
        seed_x(seed_x),
        seed_y(seed_y)
    {
    }

    Vector3d mu;
    Matrix3d sigma_inv;
    int seed_x;
    int seed_y;

    double calculate_squared_Mahalanobis_distance(const Vector3d& color) const
    {
        return (color - mu).transpose() * sigma_inv * (color - mu);
    }
};

struct OptimizationParameterSet
{
    Vector3d target_color;
    Vector4d lambda;
    double   lo;
    double   sigma;
    std::vector<ColorKernel> kernels;
    bool     use_sparcity;
    bool     use_target_alphas; // If true, the alternative constraint (Eq. 6) will be used instead of the unity constraint (Eq. 2).
    VectorXd target_alphas;     // This will be used when "use_target_alphas" is true.
};

Vector3d composite_color(const VectorXd& alphas, const VectorXd& colors)
{
    const int number_of_layers = alphas.rows();
    Vector3d sum_color = Vector3d::Zero();
    for (int index = 0; index < number_of_layers; ++ index)
    {
        sum_color += alphas[index] * colors.segment<3>(index * 3);
    }
    return sum_color;
}

VectorXd gradient_of_equality_constraint_terms(const VectorXd& alphas,
                                               const VectorXd& colors,
                                               const Vector3d& target_color,
                                               const Vector4d& constraint_vector,
                                               const Vector4d& lambda,
                                               double lo,
                                               bool use_target_alphas,
                                               const VectorXd& target_alphas = VectorXd())
{
    const int number_of_layers = alphas.rows();

    VectorXd grad_lagrange_term = VectorXd(number_of_layers * 4);
    VectorXd grad_penalty_term  = VectorXd(number_of_layers * 4);

    const double   sum_alpha = alphas.sum();
    const Vector3d sum_color = composite_color(alphas, colors);

    for (int index = 0; index < number_of_layers; ++ index)
    {
        const Vector3d& u = colors.segment<3>(index * 3);
        const double    a = alphas(index);

        const Vector3d partial_g_u_per_partial_a = 2.0 * u.cwiseProduct(sum_color - target_color);
        const double   partial_g_a_per_partial_a = use_target_alphas ? 2.0 * (a - target_alphas(index)) : 2.0 * (sum_alpha - 1.0);

        const double partial_lambda_transpose_g_per_partial_alpha = lambda.segment<3>(0).transpose() * partial_g_u_per_partial_a + lambda(3) * partial_g_a_per_partial_a;

        const double partial_g_u_r_per_partial_u_r = 2.0 * a * (sum_color(0) - target_color(0));
        const double partial_g_u_g_per_partial_u_g = 2.0 * a * (sum_color(1) - target_color(1));
        const double partial_g_u_b_per_partial_u_b = 2.0 * a * (sum_color(2) - target_color(2));

        const double partial_lambda_transpose_g_per_partial_u_r = lambda(0) * partial_g_u_r_per_partial_u_r;
        const double partial_lambda_transpose_g_per_partial_u_g = lambda(1) * partial_g_u_g_per_partial_u_g;
        const double partial_lambda_transpose_g_per_partial_u_b = lambda(2) * partial_g_u_b_per_partial_u_b;

        const Vector4d partial_g_per_partial_a = Vector4d(partial_g_u_per_partial_a(0), partial_g_u_per_partial_a(1), partial_g_u_per_partial_a(2), partial_g_a_per_partial_a);

        grad_lagrange_term(index)                            = partial_lambda_transpose_g_per_partial_alpha;
        grad_lagrange_term(number_of_layers + index * 3 + 0) = partial_lambda_transpose_g_per_partial_u_r;
        grad_lagrange_term(number_of_layers + index * 3 + 1) = partial_lambda_transpose_g_per_partial_u_g;
        grad_lagrange_term(number_of_layers + index * 3 + 2) = partial_lambda_transpose_g_per_partial_u_b;

        grad_penalty_term(index)                            = 0.5 * lo * 2.0 * constraint_vector.transpose() * partial_g_per_partial_a;
        grad_penalty_term(number_of_layers + index * 3 + 0) = 0.5 * lo * 2.0 * constraint_vector(0) * partial_g_u_r_per_partial_u_r;
        grad_penalty_term(number_of_layers + index * 3 + 1) = 0.5 * lo * 2.0 * constraint_vector(1) * partial_g_u_g_per_partial_u_g;
        grad_penalty_term(number_of_layers + index * 3 + 2) = 0.5 * lo * 2.0 * constraint_vector(2) * partial_g_u_b_per_partial_u_b;
    }

    return grad_lagrange_term + grad_penalty_term;
}

double calculate_equality_constraint_terms(const Vector4d& constraint_vector, const Vector4d& lambda, double lo)
{
    return lambda.transpose() * constraint_vector + 0.5 * lo * constraint_vector.squaredNorm();
}

Vector4d calculate_equality_constraint_vector(const VectorXd& alphas,
                                              const VectorXd& colors,
                                              const Vector3d& target_color,
                                              bool use_target_alphas,
                                              const VectorXd& target_alphas = VectorXd())
{
    const Vector3d sum_color = composite_color(alphas, colors);
    const Vector3d g_color   = (sum_color - target_color).cwiseProduct(sum_color - target_color);
    const double   sum_alpha = alphas.sum();
    const double   g_alpha   = use_target_alphas ? (alphas - target_alphas).squaredNorm() : (sum_alpha - 1.0) * (sum_alpha - 1.0);

    return Vector4d(g_color(0), g_color(1), g_color(2), g_alpha);
}

Vector4d calculate_equality_constraint_vector(const VectorXd& x,
                                              const Vector3d& target_color,
                                              bool use_target_alphas,
                                              const VectorXd& target_alphas = VectorXd())
{
    const int number_of_layers = x.rows() / 4;
    return calculate_equality_constraint_vector(x.segment(0, number_of_layers), x.segment(number_of_layers, number_of_layers * 3), target_color, use_target_alphas, target_alphas);
}

// Calculate the gradient of the main objective function (Eq. 4)
VectorXd gradient_of_energy_function(const VectorXd& alphas,
                                     const VectorXd& colors,
                                     const std::vector<ColorKernel>& kernels,
                                     double sigma,
                                     bool use_sparcity)
{
    const int number_of_layers = alphas.rows();

    VectorXd grad = VectorXd(number_of_layers * 4);

    // Main term
    for (int index = 0; index < number_of_layers; ++ index)
    {
        const ColorKernel& k = kernels[index];
        const Vector3d&    u = colors.segment<3>(index * 3);
        grad(index) = k.calculate_squared_Mahalanobis_distance(u);
        grad.segment<3>(number_of_layers + index * 3) = 2.0 * alphas(index) * k.sigma_inv * (u - k.mu);
    }

    // Sparcity term
    if (use_sparcity)
    {
        double alpha_sum         = alphas.sum();
        double alpha_squared_sum = alphas.squaredNorm();
        for (int index = 0; index < number_of_layers; ++ index)
        {
            grad(index) += sigma * (alpha_squared_sum - 2.0 * alphas(index) * alpha_sum) / (alpha_squared_sum * alpha_squared_sum);
        }
    }

    return grad;
}

// Calculate the main objective function (Eq. 4)
double energy_function(const VectorXd& alphas,
                       const VectorXd& colors,
                       const std::vector<ColorKernel>& kernels,
                       double sigma,
                       bool use_sparcity)
{
    const int number_of_layers = alphas.rows();

    // Main term
    double energy = 0.0;
    for (int index = 0; index < number_of_layers; ++ index)
    {
        energy += alphas[index] * kernels[index].calculate_squared_Mahalanobis_distance(colors.segment<3>(index * 3));
    }

    // Sparcity term
    if (use_sparcity) energy += sigma * ((alphas.sum() / alphas.squaredNorm()) - 1.0);

    return energy;
}

double objective_function(const std::vector<double> &x, std::vector<double>& grad, void* data)
{
    const int number_of_layers = x.size() / 4;

    const OptimizationParameterSet& set = *static_cast<const OptimizationParameterSet*>(data);

    const VectorXd alphas = Eigen::Map<const VectorXd>(&x[0], number_of_layers);
    const VectorXd colors = Eigen::Map<const VectorXd>(&x[number_of_layers], number_of_layers * 3);

    const Vector4d constraint_vector = calculate_equality_constraint_vector(alphas, colors, set.target_color, set.use_target_alphas, set.target_alphas);

    if (!grad.empty())
    {
        const VectorXd gradient_energy     = gradient_of_energy_function(alphas, colors, set.kernels, set.sigma, set.use_sparcity);
        const VectorXd gradient_constraint = gradient_of_equality_constraint_terms(alphas, colors, set.target_color, constraint_vector, set.lambda, set.lo, set.use_target_alphas, set.target_alphas);
        Eigen::Map<VectorXd>(&grad[0], grad.size()) = gradient_energy + gradient_constraint;
    }

    return energy_function(alphas, colors, set.kernels, set.sigma, set.use_sparcity) + calculate_equality_constraint_terms(constraint_vector, set.lambda, set.lo);
}

VectorXd solve_per_pixel_optimization(const Vector3d& target_color,
                                      const std::vector<ColorKernel>& kernels,
                                      bool for_refinement = false,
                                      const VectorXd& initial_colors = VectorXd(),
                                      const VectorXd& target_alphas = VectorXd())
{
    const int number_of_layers = kernels.size();

    const VectorXd upper = VectorXd::Constant(number_of_layers * 4, 1.0);
    const VectorXd lower = VectorXd::Constant(number_of_layers * 4, 0.0);

    constexpr double gamma   = 0.25;
    constexpr double epsilon = 1e-08;
    constexpr double beta    = 10.0;

    OptimizationParameterSet set;
    set.kernels           = kernels;
    set.lambda            = Vector4d::Constant(0.1);
    set.lo                = 0.1;
    set.target_color      = target_color;
    set.sigma             = 10.0;
    set.target_alphas     = target_alphas;
    if (!for_refinement)
    {
#ifdef SPARCITY
        set.use_sparcity      = true;
#else
        set.use_sparcity      = false;
#endif
        set.use_target_alphas = false;
    }
    else
    {
        set.use_sparcity      = false;
        set.use_target_alphas = true;
    }

    // Find an initial solution
    VectorXd x_initial = VectorXd::Zero(number_of_layers * 4);
    if (!for_refinement)
    {
        double min_distance  = DBL_MAX;
        int    closest_index = - 1;
        for (int index = 0; index < number_of_layers; ++ index)
        {
            double distance = kernels[index].calculate_squared_Mahalanobis_distance(set.target_color);
            if (min_distance > distance)
            {
                min_distance  = distance;
                closest_index = index;
            }
        }
        x_initial(closest_index) = 1.0;
        for (int index = 0; index < number_of_layers; ++ index)
        {
            x_initial.segment<3>(number_of_layers + index * 3) = (index == closest_index) ? set.target_color : kernels[index].mu;
            for (int i : { 0, 1, 2}) x_initial(number_of_layers + index * 3 + i) = std::max(std::min(x_initial(number_of_layers + index * 3 + i), 1.0), 0.0);
        }
    }
    else
    {
        x_initial.segment(0, number_of_layers) = target_alphas;
        x_initial.segment(number_of_layers, number_of_layers * 3) = initial_colors;
    }

    VectorXd x = x_initial;

    int count = 0;
    constexpr int max_count = 100;
    while (true)
    {
        const VectorXd x_new = nloptUtility::compute(x, upper, lower, objective_function, &set, nlopt::LD_MMA, 100, epsilon);
        const Vector4d g     = calculate_equality_constraint_vector(x    , set.target_color, for_refinement, target_alphas);
        const Vector4d g_new = calculate_equality_constraint_vector(x_new, set.target_color, for_refinement, target_alphas);

        set.lambda += set.lo * g_new;
        if (g_new.norm() > gamma * g.norm()) set.lo *= beta;

        const bool is_unchanged = (x_new - x).squaredNorm() < epsilon;
        const bool is_satisfied = g_new.norm() < epsilon;

        x = x_new;

        if ((is_unchanged && is_satisfied) || count > max_count) break;

        ++ count;
    }
    return x;
}

void print_kernel(const ColorKernel& kernel)
{
    std::cout << "mu: " << std::endl;
    std::cout << kernel.mu.transpose() << std::endl;
    std::cout << "sigma: " << std::endl;
    std::cout << kernel.sigma_inv.inverse() << std::endl;
    std::cout << "seed: " << std::endl;
    std::cout << "(" << kernel.seed_x << ", " << kernel.seed_y << ")" << std::endl;
}

void print_kernels(const std::vector<ColorKernel>& kernels)
{
    for (const ColorKernel& kernel : kernels)
    {
        std::cout << "---------------------" << std::endl;
        print_kernel(kernel);
    }
    std::cout << "---------------------" << std::endl;
}

void compute_normal_distribution(const ColorImage& original_image, const Image& weight_map, Vector3d& mu, Matrix3d& sigma)
{
    const int width  = original_image.width();
    const int height = original_image.height();

    mu = Vector3d::Zero();
    for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y)
    {
        const Vector3d I = original_image.get_rgb(x, y);
        mu += weight_map.get_pixel(x, y) * I;
    }
    sigma = Matrix3d::Zero();
    for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y)
    {
        const Vector3d I = original_image.get_rgb(x, y);
        sigma += weight_map.get_pixel(x, y) * (I - mu) * (I - mu).transpose();
    }

    // For avoiding singularity (note: this process is not used in the original paper)
    constexpr double epsilon = 1e-03; // This value is empirically set
    sigma += epsilon * Matrix3d::Identity();
}

std::vector<ColorImage> perform_matte_refinement(const ColorImage& original_image, const std::vector<ColorImage>& layers, const std::vector<ColorKernel>& kernels)
{
    assert(layers.size() == kernels.size());

    const int number = layers.size();
    const int width  = original_image.width();
    const int height = original_image.height();
    const int radius = 60 * std::min(width, height) / 1000;
    constexpr double epsilon = 1e-04;

    // Apply guided filter
    std::vector<Image> refined_alphas;
    for (const ColorImage& layer : layers)
    {
        const Image alpha = layer.get_a();
        const Image refined_alpha = ImageProcessing::apply_guided_filter(alpha, original_image, radius, epsilon);

        refined_alphas.push_back(refined_alpha);
    }

    // Regularize alphas such that the sum equals to one for each pixel
    for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y)
    {
        double sum = 0.0;
        for (int i = 0; i < number; ++ i)
        {
            refined_alphas[i].set_pixel(x, y, std::max(std::min(refined_alphas[i].get_pixel(x, y), 1.0), 0.0));
            sum += refined_alphas[i].get_pixel(x, y);
        }
        assert(sum > 0.0);
        for (int i = 0; i < number; ++ i)
        {
            refined_alphas[i].set_pixel(x, y, refined_alphas[i].get_pixel(x, y) / sum);
        }
    }

    // Perform optimization
    std::vector<ColorImage> refined_layers(number, ColorImage(width, height));
    auto per_pixel_process = [&](int x, int y)
    {
        VectorXd initial_colors(number * 3);
        VectorXd target_alphas(number);
        for (int i = 0; i < number; ++ i)
        {
            initial_colors.segment<3>(i * 3) = layers[i].get_rgb(x, y);
            target_alphas(i) = refined_alphas[i].get_pixel(x, y);
        }

        const Vector3d pixel_color = original_image.get_rgb(x, y);
        const VectorXd solution = solve_per_pixel_optimization(pixel_color, kernels, true, initial_colors, target_alphas);

        const VectorXd alphas = Eigen::Map<const VectorXd>(&solution[0], number);
        const VectorXd colors = Eigen::Map<const VectorXd>(&solution[number], number * 3);

        for (int index = 0; index < number; ++ index)
        {
            refined_layers[index].set_rgba(x, y, colors.segment<3>(index * 3), alphas(index));
        }
    };
#ifdef PARALLEL
    perform_in_parallel(per_pixel_process, width, height);
#else
    for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y) per_pixel_process(x, y);
#endif

    return refined_layers;
}

std::vector<ColorImage> convert_alpha_add_to_overlay(const std::vector<ColorImage>& layers)
{
    const int number = layers.size();
    const int width  = layers.front().width();
    const int height = layers.front().height();

    std::vector<ColorImage> overlay_layers(number, ColorImage(width, height));

    for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y)
    {
        for (int index = 0; index < number; ++ index)
        {
            VectorXd overlay_alphas = VectorXd(number);
            double sum_current_alpha = 0.0;
            for (int i = 0; i <= index; ++ i)
            {
                sum_current_alpha += layers[i].get_a().get_pixel(x, y);
            }
            constexpr double epsilon = 1e-16;
            overlay_alphas(index) = (sum_current_alpha < epsilon) ? 1.0 : layers[index].get_a().get_pixel(x, y) / sum_current_alpha;
            overlay_layers[index].set_rgba(x, y, layers[index].get_rgb(x, y), overlay_alphas(index));
        }
    }
    return overlay_layers;
}

}

void ColorUnmixing::compute_color_unmixing(const std::string &image_file_path, const std::string &output_directory_path)
{
    clock_t start_time = clock();

    // Import the target image
    const ColorImage original_image(image_file_path);
    const int width  = original_image.width();
    const int height = original_image.height();

    // Calculate intermediate images
    const Image gray_image         = original_image.get_luminance();
    const Image gradient_magnitude = ImageProcessing::calculate_gradient_magnitude(gray_image);

    // Compute color models
    std::vector<ColorKernel> kernels;
    std::vector<std::vector<bool>> well_represented(width, std::vector<bool>(height, false));

    constexpr double tau                 = 5.0; // The value used in the original paper is 5.
    constexpr int    neighborhood_radius = 10;  // In the paper, this value is fixed to 10 (i.e., 20 x 20 neighborhood) for any input image. This may need to be modified so that it adapts to the image size.
    constexpr int    number_of_bins      = 10;

    while (true)
    {
        // Initialize bins
        double bins[number_of_bins][number_of_bins][number_of_bins];
        std::fill(bins[0][0], bins[number_of_bins][0], 0.0);

        auto get_bin = [number_of_bins, &original_image](int x, int y)
        {
            const Vector3d pixel_color = original_image.get_rgb(x, y);
            const int bin_r = std::min(static_cast<int>(std::floor(pixel_color(0) * number_of_bins)), number_of_bins - 1);
            const int bin_g = std::min(static_cast<int>(std::floor(pixel_color(1) * number_of_bins)), number_of_bins - 1);
            const int bin_b = std::min(static_cast<int>(std::floor(pixel_color(2) * number_of_bins)), number_of_bins - 1);
            return Vector3i(bin_r, bin_g, bin_b);
        };

        // Poll bins
        auto per_pixel_polling_process = [&](int x, int y)
        {
            // Skip if the pixel is already well represented
            if (well_represented[x][y]) return;

            // Calculate bin
            const Vector3i bin = get_bin(x, y);

            // Calculate per-pixel representation score
            double representation_score = DBL_MAX;
            if (!kernels.empty())
            {
                const Vector3d pixel_color = original_image.get_rgb(x, y);
                const VectorXd solution = solve_per_pixel_optimization(pixel_color, kernels);
                const VectorXd alphas = Eigen::Map<const VectorXd>(&solution[0], kernels.size());
                const VectorXd colors = Eigen::Map<const VectorXd>(&solution[kernels.size()], kernels.size() * 3);

                representation_score = energy_function(alphas, colors, kernels, 0.0, false);
            }

            // Reject if it is already well represented
            if (representation_score < tau * tau)
            {
                well_represented[x][y] = true;
                return;
            }

            // Calculate vote values
            const double vote_weight = std::exp(- gradient_magnitude.get_pixel(x, y)) * (1.0 - std::exp(- representation_score));

            // Vote to bin
            bins[bin(0)][bin(1)][bin(2)] += vote_weight;
        };
#ifdef PARALLEL
        perform_in_parallel(per_pixel_polling_process, width, height);
#else
        for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y) per_pixel_polling_process(x, y);
#endif

        // Export the current mask
        Image well_represented_map(width, height, 0.0);
        for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y)
        {
            well_represented_map.set_pixel(x, y, (well_represented[x][y] ? 1.0 : 0.0));
        }
        well_represented_map.save(output_directory_path + "/rep" + std::to_string(kernels.size()) + ".png");

        // Break the loop if *almost* all the pixels are already well represented
        int count = 0;
        for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y)
        {
            if (well_represented[x][y]) ++ count;
        }
        std::cout << count << " / " << width * height << std::endl;
        constexpr double almost_threshold = 0.995;
        const bool done = (almost_threshold < static_cast<double>(count) / static_cast<double>(width * height));
        if (done) break;

        // Select the most popular bin
        Vector3i max_bin = Vector3i::Constant(- 1);
        double max_bin_vote = 0.0;
        for (int r = 0; r < number_of_bins; ++ r)
        {
            for (int g = 0; g < number_of_bins; ++ g)
            {
                for (int b = 0; b < number_of_bins; ++ b)
                {
                    if (max_bin_vote <= bins[r][g][b])
                    {
                        max_bin = Vector3i(r, g, b);
                        max_bin_vote = bins[r][g][b];
                    }
                }
            }
        }

        // Select seed pixel
        int seed_x = - 1;
        int seed_y = - 1;
        double max_score = 0.0;
        for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y)
        {
            // Ignore if it is well represented already
            if (well_represented[x][y]) continue;

            // Calculate bin
            const Vector3i bin = get_bin(x, y);

            // Ignore if the pixel does not belong to the selected bin
            if (bin != max_bin) continue;

            // Calculate score
            int neighborhood_count = 0;
            for (int offset_x = - neighborhood_radius; offset_x <= neighborhood_radius; ++ offset_x)
            {
                for (int offset_y = - neighborhood_radius; offset_y <= neighborhood_radius; ++ offset_y)
                {
                    if (x + offset_x < 0 || x + offset_x >= width)  continue;
                    if (y + offset_y < 0 || y + offset_y >= height) continue;
                    if (well_represented[x + offset_x][y + offset_y]) continue;

                    const Vector3i neighbor_bin = get_bin(x + offset_x, y + offset_y);

                    if (neighbor_bin == max_bin) ++ neighborhood_count;
                }
            }
            assert(neighborhood_count > 0);
            const double score = static_cast<double>(neighborhood_count) * std::exp(- gradient_magnitude.get_pixel(x, y));

            // Update the seed pixel candidate
            if (max_score < score)
            {
                max_score = score;
                seed_x = x;
                seed_y = y;
            }
        }
        assert(seed_x >= 0 && seed_y >= 0);

        // Compute guided filter weights
        // Note: preventing the values from being negative is necessary to ensure the validity of the obtained normal distribution
        const Image weight_map = ImageProcessing::calculate_guided_filter_kernel(gray_image, seed_x, seed_y, neighborhood_radius);

        // Export the weight map
        Image temporary_weight_map = weight_map;
        temporary_weight_map.scale_to_unit();
        temporary_weight_map.save(output_directory_path + "/weight" + std::to_string(kernels.size()) + ".png");

        // Calculate the color distribution
        Vector3d mu;
        Matrix3d sigma;
        compute_normal_distribution(original_image, weight_map, mu, sigma);

        // Add a new color kernel
        const ColorKernel kernel = ColorKernel(mu, sigma.inverse(), seed_x, seed_y);
        print_kernel(kernel);
        kernels.push_back(kernel);
    }

    const int number_of_layers = kernels.size();

    std::vector<ColorImage> layers(number_of_layers, ColorImage(width, height));

    auto per_pixel_process = [&](int x, int y)
    {
        const Vector3d pixel_color = original_image.get_rgb(x, y);
        const VectorXd solution = solve_per_pixel_optimization(pixel_color, kernels);

        const VectorXd alphas = Eigen::Map<const VectorXd>(&solution[0], number_of_layers);
        const VectorXd colors = Eigen::Map<const VectorXd>(&solution[number_of_layers], number_of_layers * 3);

        for (int index = 0; index < number_of_layers; ++ index)
        {
            layers[index].set_rgba(x, y, colors.segment<3>(index * 3), alphas(index));
        }
    };

#ifdef PARALLEL
    perform_in_parallel(per_pixel_process, width, height);
#else
    for (int x = 0; x < width; ++ x) for (int y = 0; y < height; ++ y) per_pixel_process(x, y);
#endif

    const std::vector<ColorImage> overlay_layers = convert_alpha_add_to_overlay(layers);
    const std::vector<ColorImage> refined_layers = perform_matte_refinement(original_image, layers, kernels);
    const std::vector<ColorImage> refined_overlay_layers = convert_alpha_add_to_overlay(refined_layers);

    // Export layers
    for (int index = 0; index < number_of_layers; ++ index)
    {
        layers[index].save(output_directory_path + "/layer" + std::to_string(index) + ".png");
        layers[index].get_a().save(output_directory_path + "/layer_alpha" + std::to_string(index) + ".png");
        overlay_layers[index].save(output_directory_path + "/overlay_layer" + std::to_string(index) + ".png");
        refined_layers[index].save(output_directory_path + "/refined_layer" + std::to_string(index) + ".png");
        refined_layers[index].get_a().save(output_directory_path + "/refined_layer_alpha" + std::to_string(index) + ".png");
        refined_overlay_layers[index].save(output_directory_path + "/refined_overlay_layer" + std::to_string(index) + ".png");
    }

    // Export the original image
    original_image.save(output_directory_path + "/original.png");

    // Print kernel info
    print_kernels(kernels);

    clock_t finish_time = clock();
    double duration = (double)(finish_time - start_time) / CLOCKS_PER_SEC;
    std::cout << "\nTotal Time: " << duration << "s" << std::endl;
}


#pragma once

#include <iostream>

#include <Eigen/Dense>

//The geometry of the grid
enum Geometry
{
    Triangular = 0,
    Orthogonal = 1,
    Hexagonal = 2
};

std::ostream& operator<<(std::ostream& os, const Geometry& g);

//The orientation of the grid
enum Orientation
{
    Horizontal = 0,
    Vertical = 1
};

std::ostream& operator<<(std::ostream& os, const Orientation& o);

//this is a flat mesh
//i will principaly use it to set lens array and checkerboard (if needed ;) )
template<int Dimension>
struct PolygonMesh
{
    Geometry geometry;
    Orientation orientation;
    std::array<int,2> dimensions;
    double edge_length;  //the mean distance between two vertices

    PolygonMesh(const Geometry g = Triangular
              , const Orientation o = Horizontal
              , const std::array<int,2>& d = std::array<int,2>{0,0}
              , const double e = 0.0
        )
    : geometry(g), dimensions(d), edge_length(e)
    {}

    ~PolygonMesh(){};

    Eigen::Matrix<double,Dimension,1> vertex(const int col, const int row) const;
    // Eigen::Matrix<double,Dimension,1> vertex(const int i) const;

    // return edge parameters linking two nodes (note by indices)
    // edge(const int i1, const int i2) const;
};

using PolygonMesh2D = PolygonMesh<2>;
using PolygonMesh3D = PolygonMesh<3>;

template<int Dimension>
Eigen::Matrix<double,Dimension,1> PolygonMesh<Dimension>::vertex(const int col, const int row) const
{
    // if ( col < 0 or col >= width )
    //     throw(std::out_of_range( boost::str(boost::format("PolygonMesh<Dimension>::vertex: wrong col index (%1%)")%col)) );
    // if ( row < 0 or row >= height )
    //     throw(std::out_of_range( boost::str(boost::format("PolygonMesh<Dimension>::vertex: wrong row index (%1%)")%row)) );

    Eigen::Matrix<double,Dimension,1> p = Eigen::Matrix<double,Dimension,1>::Zero();

    if (geometry == Triangular)
    {
        std::cerr << "Error: PolygonMesh::vertex: Not implemented." << std::endl;
        if (orientation == Horizontal)
        {
            return p;
        }
        else if (orientation == Vertical)
        {
            return p;
        }
        else
            std::cerr << "Error: PolygonMesh::vertex: wrong orientation value (" << orientation << ")."<< std::endl;

    }
    else if (geometry == Orthogonal)
    {
        p.x() = edge_length * (double(col) + double(row));
        p.y() = edge_length * (double(row) + double(col));
    }
    else if (geometry == Hexagonal)
    {
        double val = 0.0;
        if (orientation == Horizontal)
        {
            if (row%2 == 0)
                val = 0.5;

            p.x() =  (double(col) + val) * edge_length;
            p.y() =  double(row) * edge_length * std::sin(M_PI / 3.0);

            return p;

        }
        if (orientation == Vertical)
        {
            if (col%2 == 0)
                val = 0.5;

            p.x() = double(col) * edge_length * std::sin(M_PI / 3.0);
            p.y() = (double(row) + val) * edge_length;

            return p;
        }
        else
            std::cerr << "Error: PolygonMesh::vertex: wrong orientation value (" << orientation << ")."<< std::endl;
    }
    else
        std::cerr << "Error: PolygonMesh::vertex: wrong geometry value (" << geometry << ")."<< std::endl;

    return {};
}

template<int Dimension>
std::ostream& operator<<(std::ostream& os, const PolygonMesh<Dimension>& m)
{
    os << "geometry: " << m.geometry << "\n";
    os << "orientation: " << m.orientation << "\n";
    os << "dimensions: [" << m.dimensions[0] << ", " << m.dimensions[1] << "]\n";
    os << "edges length: " << m.edge_length;

    return os;
}


/*
 * A header-only version of RedSVD
 *
 * Copyright (c) 2014 Nicolas Tessore
 *
 * based on RedSVD
 *
 * Copyright (c) 2010 Daisuke Okanohara
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above Copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * 3. Neither the name of the authors nor the names of its contributors
 *    may be uses to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 *    Minor modifications by Luca Formaggia.
 */

#ifndef REDSVD_MODULE_H
#define REDSVD_MODULE_H

#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include <cstdlib>
#include <cmath>
#include <random>

namespace RedSVD
{
  //! It generates a (pseudo) x and y extracted from a (pseudo) random Gaussian distribution
  /*!
   * It uses the Box-Muller transform, see
   * [this Wikipedia page](https://en.wikipedia.org/wiki/Box%E2%80%93Muller_transform),
   * to generate a couple of variables from a normally distribution
   * with zero mean and unitary variance.
   *
   * This version uses rand() to generate the uniform distribution from
   * which the Gaussian one is derived.  It may be changed to implement
   * the new random distribution.
   *
   * \note Compared with the original version, it is no more used to generate
   * random Gaussian matrices, since we use the random number generator
   * of the standard library.
   */
  template<typename Scalar>
  inline void sample_gaussian(Scalar& x, Scalar& y)
  {
    using std::sqrt;
    using std::log;
    using std::cos;
    using std::sin;

    constexpr Scalar PI(3.1415926535897932384626433832795028841971693993751);

    Scalar v1 = (Scalar)(std::rand() + Scalar(1)) / ((Scalar)RAND_MAX+Scalar(2));
    Scalar v2 = (Scalar)(std::rand() + Scalar(1)) / ((Scalar)RAND_MAX+Scalar(2));
    Scalar len = sqrt(Scalar(-2) * log(v1));
    x = len * cos(Scalar(2) * PI * v2);
    y = len * sin(Scalar(2) * PI * v2);
  }

  //! Generates a random Gaussian matrix
  /*! A matrix whose elements are drawn from Normal distribution with
   * zero mean and unitary variance.
   *
   * \note Modified from the original code by Luca Formaggia: using standard library random number distribution.
  */
  template<typename MatrixType>
  inline void sample_gaussian(MatrixType& mat)
  {
    // generate random seed
    std::random_device rd{};
    std::mt19937 gen{rd()};
    // define the distribution
    typedef typename MatrixType::Scalar Scalar;
    std::normal_distribution<Scalar> d;

    typedef typename MatrixType::Index Index;

    for(Index i = 0; i < mat.rows(); ++i)
      for(Index j = 0; j < mat.cols(); ++j)
        mat(i, j)=d(gen);
  }
  //! Performs Gram Schmidt on the matrix columns
  /*!
   * \tparam MatrixType An Eigen Dense Matrix
   * \param mat The matrix on which to operate. The orthonormalisation is performed in-place
   * \param EPS tolerance to discard small columns. Defaulted to 1e-6
   */
  template<typename MatrixType>
  inline void gram_schmidt(MatrixType& mat,
                           typename MatrixType::Scalar const EPS=1.E-6)
  {
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::Index Index;

    for(Index i = 0; i < mat.cols(); ++i)
      {
        // c_i = c_i - sum_{j<i} (c_i * c_j)c_j
        for(Index j = 0; j = i.
        if(norm < EPS)
          {
            for(Index k = i; k < mat.cols(); ++k)
              mat.col(k).setZero();
            return;
          }
        // Orhonormalization
        mat.col(i) /= norm;
      }
  }

  //! Performs reduced Singular Value Decomposition
  /*!
   * Given a nxm matrix \f$A\f$ it computes matrices \f$U\f$ (nxk, orthogonal), \f$\Sigma\f$ (kxk, diagonal) and
   * \f$V\f$ (mxk, orthogonal), so that \f$A^*=U\Sigma V^T\f$ is a rank-k matrix approximation of \f$A\f$.
   * Matrix \f$\Sigma\f$ is returned as vector of size k containing the diagonal elements, which are an approximation
   * of the first singular values of \f$A\f$.
   *
   * This algorithm uses is a probabilistic algorithm. More details on this type of algorithms may be found in
   * Finding structures with randomness:..., Halko, N. and Martinsson, P. G. and Tropp, J. A. (2009)
   */
  template<typename My_MatrixType>
  class RedSVD
  {
  public:
    typedef My_MatrixType MatrixType;
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::Index Index;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> DenseMatrix;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ScalarVector;
    //! Default constructor
    RedSVD()=default;

    //! A may be a dense or sparse Eigen matrix.
    /*!
      Here rank is taken as min between n. rows and n. columns
      No great memory savings.
    */
    RedSVD(const MatrixType& A)
    {
      int r = (A.rows() < A.cols()) ? A.rows() : A.cols();
      compute(A, r);
    }
    //! Specifying the rank
    /*!
      \param A a sparse or dense Eigen matrix
      \param rank the desired rank for the reduced decomposition
    */
    RedSVD(const MatrixType& A, const Index rank)
    {
      compute(A, rank);
    }
    /*!
      \brief Computes the actual reduced SVD
      It is automatically called by constructors that take a matrix in input.
    */
    void compute(const MatrixType& A, const Index rank)
    {
      if(A.cols() == 0 || A.rows() == 0)
        return;

      Index r = (rank < A.cols()) ? rank : A.cols();

      r = (r < A.rows()) ? r : A.rows();

      // Gaussian Random Matrix for A^T
      DenseMatrix O(A.rows(), r);
      sample_gaussian(O);

      // Compute Sample Matrix of A^T
      DenseMatrix Y = A.transpose() * O;

      // Orthonormalize Y
      gram_schmidt(Y);

      // Range(B) = Range(A^T)
      DenseMatrix B = A * Y;

      // Gaussian Random Matrix
      DenseMatrix P(B.cols(), r);
      sample_gaussian(P);

      // Compute Sample Matrix of B
      DenseMatrix Z = B * P;

      // Orthonormalize Z
      gram_schmidt(Z);

      // Range(C) = Range(B)
      DenseMatrix C = Z.transpose() * B;
      // Thin SVD.
      Eigen::JacobiSVD<DenseMatrix> svdOfC(C, Eigen::ComputeThinU | Eigen::ComputeThinV);

      // C = USV^T
      // A^* = (Z * U) * S * (Y * V)^T (low rank approx of A).
      m_matrixU = Z * svdOfC.matrixU();
      m_vectorS = svdOfC.singularValues();
      m_matrixV = Y * svdOfC.matrixV();
    }

    //! Returns matrix U
    DenseMatrix matrixU() const
    {
      return m_matrixU;
    }
    //! Returns vector with first k singular values
    ScalarVector singularValues() const
    {
      return m_vectorS;
    }
    //! Returns matrix V
    DenseMatrix matrixV() const
    {
      return m_matrixV;
    }

  private:
    DenseMatrix m_matrixU;
    ScalarVector m_vectorS;
    DenseMatrix m_matrixV;
  };

  //! Like RedSVD but for symmetric matrices
  /*!
   * In this case, \f$U=V\f$ is the matrix with the first (approximate) eigenvectors of \f$A\f$ and \f$\Sigma\f$ contains
   * the (approximate) first k eigenvalues.
   */
  template<typename _MatrixType>
  class RedSymEigen
  {
  public:
    typedef _MatrixType MatrixType;
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::Index Index;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> DenseMatrix;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ScalarVector;

    RedSymEigen()=default;

    RedSymEigen(const MatrixType& A)
    {
      int r = (A.rows() < A.cols()) ? A.rows() : A.cols();
      compute(A, r);
    }

    RedSymEigen(const MatrixType& A, const Index rank)
    {
      compute(A, rank);
    }

    void compute(const MatrixType& A, const Index rank)
    {
      if(A.cols() == 0 || A.rows() == 0)
        return;

      Index r = (rank < A.cols()) ? rank : A.cols();

      r = (r < A.rows()) ? r : A.rows();

      // Gaussian Random Matrix
      DenseMatrix O(A.rows(), r);
      sample_gaussian(O);

      // Compute Sample Matrix of A
      DenseMatrix Y = A.transpose() * O;

      // Orthonormalize Y
      gram_schmidt(Y);

      DenseMatrix B = Y.transpose() * A * Y;
      Eigen::SelfAdjointEigenSolver<DenseMatrix> eigenOfB(B);

      m_eigenvalues = eigenOfB.eigenvalues();
      m_eigenvectors = Y * eigenOfB.eigenvectors();
    }
    //! Returns a vector with the first k approximate eigenvalues
    /*!
      They are the diagonal elements of \f$\Sigma\f$.
     */
    ScalarVector eigenvalues() const
    {
      return m_eigenvalues;
    }
    //! Returns a nxk orthogonal matrix whose columns are the first k approximate eigenvectors
    DenseMatrix eigenvectors() const
    {
      return m_eigenvectors;
    }

  private:
    ScalarVector m_eigenvalues;
    DenseMatrix m_eigenvectors;
  };

  //! Performs the principal component analysis
  /*!
   * It works by contructing a RedSVD object to perform an low-rank SVD decomposition and then
   * it returns the nxk matrix \f$V\f$ that represent the components of the PCA and the nxk matrix
   * \f$U\Sigma\f$, which are the scores.
   */
  template<typename _MatrixType>
  class RedPCA
  {
  public:
    typedef _MatrixType MatrixType;
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::Index Index;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> DenseMatrix;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ScalarVector;

    RedPCA() {}

    RedPCA(const MatrixType& A)
    {
      int r = (A.rows() < A.cols()) ? A.rows() : A.cols();
      compute(A, r);
    }

    RedPCA(const MatrixType& A, const Index rank)
    {
      compute(A, rank);
    }

    void compute(const DenseMatrix& A, const Index rank)
    {
      RedSVD<MatrixType> redsvd(A, rank);

      ScalarVector S = redsvd.singularValues();

      m_components = redsvd.matrixV();
      m_scores = redsvd.matrixU() * S.asDiagonal();
    }
    //! Returns the components
    DenseMatrix components() const
    {
      return m_components;
    }
    //! Returns the scores
    DenseMatrix scores() const
    {
      return m_scores;
    }

  private:
    DenseMatrix m_components;
    DenseMatrix m_scores;
  };
}

#endif


/* =========================================================================
   Copyright (c) 2012-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
                             -----------------
               ViennaFEM - The Vienna Finite Element Method Library
                             -----------------

   Author:     Karl Rupp                          rupp@iue.tuwien.ac.at

   License:    MIT (X11), see file LICENSE in the ViennaFEM base directory
============================================================================ */

// include necessary system headers
#include <iostream>

// ViennaFEM includes:
#include "viennafem/fem.hpp"
#include "viennafem/io/vtk_writer.hpp"

// ViennaGrid includes:
#include "viennagrid/forwards.hpp"
#include "viennagrid/config/default_configs.hpp"
#include "viennagrid/io/netgen_reader.hpp"

// ViennaData includes:
#include "viennadata/api.hpp"

// ViennaMath includes:
#include "viennamath/expression.hpp"

// Boost.uBLAS includes:
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

//ViennaCL includes:
#ifndef VIENNACL_HAVE_UBLAS
 #define VIENNACL_HAVE_UBLAS
#endif

#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/prod.hpp"

/** @brief A tag class used for storing the permittivity (i.e. a cell quantity) with ViennaData */
struct permittivity_key
{
  // Operator< is required for compatibility with std::map
  bool operator<(permittivity_key const & /*other*/) const { return false; }
};

int main()
{
  typedef viennagrid::triangular_2d_mesh                                                  DomainType;
  typedef viennagrid::result_of::segmentation<DomainType>::type                           SegmentationType;
  typedef SegmentationType::iterator                                                      SegmentationIteratorType;
  typedef viennagrid::result_of::segment_handle<SegmentationType>::type                   SegmentType;
  typedef viennagrid::result_of::cell_tag<DomainType>::type                               CellTagType;
  typedef viennagrid::result_of::element<DomainType, viennagrid::vertex_tag>::type        VertexType;
  typedef viennagrid::result_of::element<DomainType, CellTagType>::type                   CellType;

  typedef viennagrid::result_of::element_range<DomainType, viennagrid::vertex_tag>::type  VertexContainerType;
  typedef viennagrid::result_of::iterator<VertexContainerType>::type                      VertexIteratorType;
  typedef viennagrid::result_of::element_range<SegmentType, CellTagType>::type            CellOnSegmentContainerType;
  typedef viennagrid::result_of::iterator<CellOnSegmentContainerType>::type               CellOnSegmentIteratorType;

  typedef boost::numeric::ublas::compressed_matrix<viennafem::numeric_type>  MatrixType;
  typedef boost::numeric::ublas::vector<viennafem::numeric_type>             VectorType;

  typedef viennamath::function_symbol   FunctionSymbol;
  typedef viennamath::equation          Equation;

  //
  // Create a domain from file
  //
  DomainType my_domain;
  SegmentationType segments(my_domain);

  //
  // Create a storage object
  //
  typedef viennadata::storage<> StorageType;
  StorageType   storage;

  try
  {
    viennagrid::io::netgen_reader my_netgen_reader;
    my_netgen_reader(my_domain, segments, "../examples/data/square224.mesh");
  }
  catch (...)
  {
    std::cerr << "File-Reader failed. Aborting program..." << std::endl;
    return EXIT_FAILURE;
  }

  //
  // Specify Poisson equation with inhomogeneous permittivity:
  //
  FunctionSymbol u(0, viennamath::unknown_tag<>());   //an unknown function used for PDE specification
  FunctionSymbol v(0, viennamath::test_tag<>());   //an unknown function used for PDE specification
  viennafem::cell_quan<CellType, viennamath::expr::interface_type>  permittivity; permittivity.wrap_constant( storage, permittivity_key() );

  //the strong form (not yet functional because of ViennaMath limitations)
  //Equation poisson_equ = viennamath::make_equation( viennamath::div(permittivity * viennamath::grad(u)), 0);

  //the weak form:
  Equation poisson_equ = viennamath::make_equation(
                          viennamath::integral(viennamath::symbolic_interval(),
                                               permittivity * (viennamath::grad(u) * viennamath::grad(v)) ),
                          0);

  MatrixType system_matrix;
  VectorType load_vector;

  //
  // Setting boundary information on domain (this should come from device specification)
  //
  //setting some boundary flags:
  VertexContainerType vertices = viennagrid::elements<VertexType>(my_domain);
  for (VertexIteratorType vit = vertices.begin();
      vit != vertices.end();
      ++vit)
  {
    // Boundary condition: 0 at left boundary, 1 at right boundary
    if ( viennagrid::point(my_domain, *vit)[0] == 0.0)
      viennafem::set_dirichlet_boundary(storage, *vit, 0.0);
    else if ( viennagrid::point(my_domain, *vit)[0] == 1.0)
      viennafem::set_dirichlet_boundary(storage, *vit, 1.0);

  }

  //
  // Create PDE solver functors: (discussion about proper interface required)
  //
  viennafem::pde_assembler<StorageType> fem_assembler(storage);

  //
  // Solve system and write solution vector to pde_result:
  // (discussion about proper interface required. Introduce a pde_result class?)
  //
  std::size_t si = 0;
  for(SegmentationIteratorType sit = segments.begin(); sit != segments.end(); sit++)
  {
    //set permittivity:
    CellOnSegmentContainerType cells = viennagrid::elements<CellType>(*sit);
    for (CellOnSegmentIteratorType cit  = cells.begin();
                                   cit != cells.end();
                                 ++cit)
    {
      if (si == 0) //Si
        viennadata::access<permittivity_key, double>(storage, permittivity_key(), *cit) = 3.9;
      else //SiO2
        viennadata::access<permittivity_key, double>(storage, permittivity_key(), *cit) = 11.9;
    }

    fem_assembler(viennafem::make_linear_pde_system(poisson_equ,
                                                    u,
                                                    viennafem::make_linear_pde_options(0,
                                                                                viennafem::lagrange_tag<1>(),
                                                                                viennafem::lagrange_tag<1>())
                                                  ),
                  *sit,
                  system_matrix,
                  load_vector
                );
  }

  VectorType pde_result = viennacl::linalg::solve(system_matrix, load_vector, viennacl::linalg::cg_tag());
  std::cout << "* solve(): Residual: " << norm_2(prod(system_matrix, pde_result) - load_vector) << std::endl;

  //
  // Writing solution back to domain (discussion about proper way of returning a solution required...)
  //
  viennafem::io::write_solution_to_VTK_file(pde_result, "poisson_cellquan_2d", my_domain, segments, storage, 0);

  std::cout << "*****************************************" << std::endl;
  std::cout << "* Poisson solver finished successfully! *" << std::endl;
  std::cout << "*****************************************" << std::endl;
  return EXIT_SUCCESS;
}


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

#ifndef BOOST_MATH_SF_BINOMIAL_HPP
#define BOOST_MATH_SF_BINOMIAL_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{ namespace math{

template <class T, class Policy>
T binomial_coefficient(unsigned n, unsigned k, const Policy& pol)
{
   BOOST_STATIC_ASSERT(!boost::is_integral<T>::value);
   BOOST_MATH_STD_USING
   static const char* function = "boost::math::binomial_coefficient<%1%>(unsigned, unsigned)";
   if(k > n)
      return policies::raise_domain_error<T>(
         function,
         "The binomial coefficient is undefined for k > n, but got k = %1%.",
         static_cast<T>(k), pol);
   T result;
   if((k == 0) || (k == n))
      return static_cast<T>(1);
   if((k == 1) || (k == n-1))
      return static_cast<T>(n);

   if(n <= max_factorial<T>::value)
   {
      // Use fast table lookup:
      result = unchecked_factorial<T>(n);
      result /= unchecked_factorial<T>(n-k);
      result /= unchecked_factorial<T>(k);
   }
   else
   {
      // Use the beta function:
      if(k < n - k)
         result = k * beta(static_cast<T>(k), static_cast<T>(n-k+1), pol);
      else
         result = (n - k) * beta(static_cast<T>(k+1), static_cast<T>(n-k), pol);
      if(result == 0)
         return policies::raise_overflow_error<T>(function, 0, pol);
      result = 1 / result;
   }
   // convert to nearest integer:
   return ceil(result - 0.5f);
}
//
// Type float can only store the first 35 factorials, in order to
// increase the chance that we can use a table driven implementation
// we'll promote to double:
//
template <>
inline float binomial_coefficient<float, policies::policy<> >(unsigned n, unsigned k, const policies::policy<>& pol)
{
   return policies::checked_narrowing_cast<float, policies::policy<> >(binomial_coefficient<double>(n, k, pol), "boost::math::binomial_coefficient<%1%>(unsigned,unsigned)");
}

template <class T>
inline T binomial_coefficient(unsigned n, unsigned k)
{
   return binomial_coefficient<T>(n, k, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SF_BINOMIAL_HPP


#include "storm/utility/numerical.h"

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

#include "storm/utility/macros.h"
#include "storm/utility/constants.h"
#include "storm/exceptions/InvalidArgumentException.h"
#include "storm/exceptions/PrecisionExceededException.h"

namespace storm {
    namespace utility {
        namespace numerical {

            template<typename ValueType>
            FoxGlynnResult<ValueType>::FoxGlynnResult() : left(0), right(0), totalWeight(storm::utility::zero<ValueType>()) {
                // Intentionally left empty.
            }

            /*!
             * The following implementation of Fox and Glynn's algorithm is taken from David Jansen's patched version
             * in MRMC, which is based on his paper:
             *
             * https://pms.cs.ru.nl/iris-diglib/src/getContent.php?id=2011-Jansen-UnderstandingFoxGlynn
             *
             * We have only adapted the code to match more of C++'s and our coding guidelines.
             */

            template<typename ValueType>
            FoxGlynnResult<ValueType> foxGlynnFinder(ValueType lambda, ValueType epsilon) {
                ValueType tau = std::numeric_limits<ValueType>::min();
                ValueType omega = std::numeric_limits<ValueType>::max();
                ValueType const sqrt_2_pi = boost::math::constants::root_two_pi<ValueType>();
                ValueType const log10_e = std::log10(boost::math::constants::e<ValueType>());

                uint64_t m = static_cast<uint64_t>(lambda);

                int64_t left = 0;
                int64_t right = 0;

                // tau is only used in underflow checks, which we are going to do in the logarithm domain.
                tau = log(tau);

                // In error bound comparisons, we always compare with epsilon*sqrt_2_pi.
                epsilon *= sqrt_2_pi;

                // Compute left truncation point.
                if (m < 25) {
                    // For lambda below 25 the exponential can be smaller than tau. If that is the case we expect
                    // underflows and warn the user.
                    if (-lambda <= tau) {
                        STORM_LOG_WARN("Fox-Glynn: 0 < lambda < 25, underflow near Poi(" << lambda << ", 0) = " << std::exp(-lambda) << ". The results are unreliable.");
                    }

                    // Zero is used as left truncation point for lambda <= 25.
                    left = 0;
                } else {
                    // Compute the left truncation point for lambda >= 25 (for lambda < 25 we use zero as left truncation point).

                    ValueType const bl = (1 + 1 / lambda) * std::exp((1/lambda) * 0.125);
                    ValueType const sqrt_lambda = std::sqrt(lambda);
                    int64_t k;

                    // Start looking for the left truncation point:
                    // * start search at k=4 (taken from original Fox-Glynn paper)
                    // * increase the left truncation point until we fulfil the error condition

                    for (k = 4;; ++k) {
                        ValueType max_err;

                        left = m - static_cast<int64_t>(std::ceil(k*sqrt_lambda + 0.5));

                        // For small lambda the above calculation can yield negative truncation points, crop them here.
                        if (left <= 0) {
                            left = 0;
                            break;
                        }

                        // Note that Propositions 2-4 in Fox--Glynn mix up notation: they write Phi where they mean
                        // 1 - Phi. (In Corollaries 1 and 2, phi is used correctly again.)
                        max_err = bl * exp(-0.5 * (k*k)) / k;
                        if (max_err * 2 <= epsilon) {
                            // If the error on the left hand side is smaller, we can be more lenient on the right hand
                            // side. To this end, we now set epsilon to the part of the error that has not yet been eaten
                            // up by the left-hand truncation.
                            epsilon -= max_err;
                            break;
                        }
                    }

                    // Finally the left truncation point is found.
                }

                // Compute right truncation point.
                {
                    ValueType lambda_max;
                    int64_t m_max, k;

                    // According to Fox-Glynn, if lambda < 400 we should take lambda = 400, otherwise use the original
                    // value. This is for computing the right truncation point.
                    if (m < 400) {
                        lambda_max = 400;
                        m_max = 400;
                        epsilon *= 0.662608824988162441697980;
                        /* i.e. al = (1+1/400) * exp(1/16) * sqrt_2; epsilon /= al; */
                    } else {
                        lambda_max = lambda;
                        m_max = m;
                        epsilon *= (1 - 1 / (lambda + 1)) * 0.664265347050632847802225;
                        /* i.e. al = (1+1/lambda) * exp(1/16) * sqrt_2; epsilon /= al; */
                    }

                    // Find right truncation point.

                    // This loop is a modification to the original Fox-Glynn paper.
                    // The search for the right truncation point is only terminated by  the error condition and not by
                    // the stop index from the FG paper. This can yield more accurate results if necessary.
                    for (k = 4;; ++k) {
                        // dkl_inv is between 1 - 1e-33 and 1 if lambda_max >= 400 and k >= 4; this will always be
                        // rounded to 1.0. We therefore leave the factor out.
                        // double dkl_inv=1 - exp(-266/401.0 * (k*sqrt(2*lambda_max) + 1.5));

                        // actually: "k * (dkl_inv*epsilon/al) >= exp(-0.5 * k^2)", but epsilon has been changed appropriately.
                        if (k * epsilon >= exp(-0.5*(k*k))) {
                            break;
                        }
                    }
                    right = m_max + static_cast<int64_t>(std::ceil(k * std::sqrt(2 * lambda_max) + 0.5));
                    if (right > m_max + static_cast<int64_t>(std::ceil((lambda_max + 1) * 0.5))) {
                        STORM_LOG_WARN("Fox-Glynn: right = " << right << " >> lambda = " << lambda_max << ", cannot bound the right tail. The results are unreliable.");
                    }
                }

                // Time to set the initial value for weights.
                FoxGlynnResult<ValueType> fgresult;
                fgresult.left = static_cast<uint64_t>(left);
                fgresult.right = static_cast<uint64_t>(right);
                fgresult.weights.resize(fgresult.right - fgresult.left + 1);

                fgresult.weights[m - left] = omega / (1.0e+10 * (right - left));

                if (m >= 25) {
                    // Perform underflow check.
                    ValueType result, log_c_m_inf;
                    int64_t i;

                    // we are going to compare with tau - log(w[m]).
                    tau -= std::log(fgresult.weights[m - left]);

                    // We take the c_m_inf = 0.14627 / sqrt( m ), as for lambda >= 25
                    // c_m = 1 / ( sqrt( 2.0 * pi * m ) ) * exp( m - lambda - 1 / ( 12.0 * m ) ) => c_m_inf.
                    // Note that m-lambda is in the interval (-1,0], and -1/(12*m) is in [-1/(12*25),0).
                    // So, exp(m-lambda - 1/(12*m)) is in (exp(-1-1/(12*25)),exp(0)).
                    // Therefore, we can improve the lower bound on c_m to exp(-1-1/(12*25)) / sqrt(2*pi) = ~0.14627.
                    // Its logarithm is -1 - 1/(12*25) - log(2*pi) * 0.5 = ~ -1.922272 (rounded towards -infinity).
                    log_c_m_inf = -1.922272 - log((double) m) * 0.5;

                    // We use FG's Proposition 6 directly (and not Corollary 4 i and ii), as k_prime may be too large
                    // if pFG->left == 0.
                    i = m - left;

                    // Equivalent to 2*i <= m, equivalent to i <= lambda/2.
                    if (i <= left) {
                        // Use Proposition 6 (i). Note that Fox--Glynn are off by one in the proof of this proposition;
                        // they sum up to i-1, but should have summed up to i. */
                        result = log_c_m_inf
                        - i * (i+1) * (0.5 + (2*i+1)/(6*lambda)) / lambda;
                    } else {
                        // Use Corollary 4 (iii). Note that k_prime <= sqrt(m+1)/m is a misprint for k_prime <= m/sqrt(m+1),
                        // which is equivalent to left >= 0, which holds trivially.
                        result = -lambda;
                        if (left != 0) {
                            // Also use Proposition 6 (ii).
                            double result_1 = log_c_m_inf + i * log(1 - i/(double) (m+1));

                            // Take the maximum.
                            if (result_1 > result) {
                                result = result_1;
                            }
                        }
                    }
                    if (result <= tau) {
                        int64_t const log10_result = static_cast<int64_t>(std::floor(result * log10_e));
                        STORM_LOG_WARN("Fox-Glynn: lambda >= 25, underflow near Poi(" << lambda << "," << left << ") <= " << std::exp(result - log10_result/log10_e) << log10_result << ". The results are unreliable.");
                    }

                    // We still have to perform an underflow check for the right truncation point when lambda >= 400.
                    if (m >= 400) {
                        // Use Proposition 5 of Fox--Glynn.
                        i = right - m;
                        result = log_c_m_inf - i * (i + 1) / (2 * lambda);
                        if (result <= tau) {
                            int64_t const log10_result = static_cast<int64_t>(std::floor(result * log10_e));
                            STORM_LOG_WARN("Fox-Glynn: lambda >= 25, underflow near Poi(" << lambda << "," << right << ") <= " << std::exp(result - log10_result/log10_e) << log10_result << ". The results are unreliable.");
                        }
                    }
                }

                return fgresult;
            }

            template<typename ValueType>
            FoxGlynnResult<ValueType> foxGlynnWeighter(ValueType lambda, ValueType epsilon) {
                ValueType tau = std::numeric_limits<ValueType>::min();

                // The magic m point.
                uint64_t m = static_cast<uint64_t>(lambda);
                int64_t j, t;

                FoxGlynnResult<ValueType> result = foxGlynnFinder(lambda, epsilon);

                // Fill the left side of the array.
                for (j = m - result.left; j > 0; --j) {
                    result.weights[j - 1] = (j + result.left) / lambda * result.weights[j];
                }

                t = result.right - result.left;

                // Fill the right side of the array, have two cases lambda < 400 & lambda >= 400.
                if (m < 400) {
                    // Perform the underflow check, according to Fox-Glynn.
                    STORM_LOG_ERROR_COND(result.right <= 600, "Fox-Glynn: " << result.right << " > 600, underflow is possible.");

                    // Compute weights.
                    for (j = m - result.left; j < t; ++j) {
                        ValueType q = lambda / (j + 1 + result.left);
                        if (result.weights[j] > tau / q) {
                            result.weights[j + 1] = q * result.weights[j];
                        } else {
                            t = j;
                            result.right = j + result.left;
                            result.weights.resize(result.right - result.left + 1);

                            // It's time to compute W.
                            break;
                        }
                    }
                } else {
                    // Compute weights.
                    for (j = m - result.left; j < t; ++j) {
                        result.weights[j + 1] = lambda / (j + 1 + result.left) * result.weights[j];
                    }
                }

                // It is time to compute the normalization weight W.
                result.totalWeight = storm::utility::zero<ValueType>();
                j = 0;

                // t was set above.
                while(j < t) {
                    if (result.weights[j] <= result.weights[t]) {
                        result.totalWeight += result.weights[j];
                        j++;
                    } else {
                        result.totalWeight += result.weights[t];
                        t--;
                    }
                }
                result.totalWeight += result.weights[j];

                STORM_LOG_TRACE("Fox-Glynn: ltp = " << result.left << ", rtp = " << result.right << ", w = " << result.totalWeight << ", " << result.weights.size() << " weights.");

                return result;
            }

            template<typename ValueType>
            FoxGlynnResult<ValueType> foxGlynn(ValueType lambda, ValueType epsilon) {
                STORM_LOG_THROW(lambda > 0, storm::exceptions::InvalidArgumentException, "Fox-Glynn requires positive lambda.");
                return foxGlynnWeighter(lambda, epsilon);
            }

            template FoxGlynnResult<double> foxGlynn(double lambda, double epsilon);

        }
    }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Andreas Gaida
 Copyright (C) 2008 Ralph Schreyer
 Copyright (C) 2008, 2019 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/math/functional.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/distributions/chisquaredistribution.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/termstructures/volatility/equityfx/localvoltermstructure.hpp>
#include <ql/methods/finitedifferences/meshers/fdmhestonvariancemesher.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <set>
#include <algorithm>

namespace QuantLib {

    namespace {
        struct interpolated_volatility {
            interpolated_volatility(const std::vector<Real>& pGrid,
                                    const std::vector<Real>& vGrid)
            : variance(pGrid.begin(), pGrid.end(), vGrid.begin()) {}
            Real operator()(Real x) const {
                return std::sqrt(variance(x, true));
            }
            LinearInterpolation variance;
        };
    }

    FdmHestonVarianceMesher::FdmHestonVarianceMesher(
        Size size,
        const ext::shared_ptr<HestonProcess> & process,
        Time maturity, Size tAvgSteps, Real epsilon,
        Real mixingFactor)
        : Fdm1dMesher(size) {

        std::vector<Real> vGrid(size, 0.0), pGrid(size, 0.0);
        const Real mixedSigma = process->sigma()*mixingFactor;
        const Real df  = 4*process->theta()*process->kappa()/squared(mixedSigma);
        try {
            std::multiset<std::pair<Real, Real> > grid;

            for (Size l=1; l<=tAvgSteps; ++l) {
                const Real t = (maturity*l)/tAvgSteps;
                const Real ncp = 4*process->kappa()*std::exp(-process->kappa()*t)/(squared(mixedSigma)
                    *(1-std::exp(-process->kappa()*t)))*process->v0();
                const Real k = squared(mixedSigma)
                    *(1-std::exp(-process->kappa()*t))/(4*process->kappa());

                const Real qMin = 0.0; // v_min = 0.0;
                const Real qMax = std::max(process->v0(),
                    k*InverseNonCentralCumulativeChiSquareDistribution(
                                            df, ncp, 100, 1e-8)(1-epsilon));

                const Real minVStep=(qMax-qMin)/(50*size);
                Real ps,p = 0.0;

                Real vTmp = qMin;
                grid.insert(std::pair<Real, Real>(qMin, epsilon));

                for (Size i=1; i < size; ++i) {
                    ps = (1 - epsilon - p)/(size-i);
                    p += ps;
                    const Real tmp = k*InverseNonCentralCumulativeChiSquareDistribution(
                        df, ncp, 100, 1e-8)(p);

                    const Real vx = std::max(vTmp+minVStep, tmp);
                    p = NonCentralCumulativeChiSquareDistribution(df, ncp)(vx/k);
                    vTmp=vx;
                    grid.insert(std::pair<Real, Real>(vx, p));
                }
            }
            QL_REQUIRE(grid.size() == size*tAvgSteps,
                       "something wrong with the grid size");

            const std::vector<std::pair<Real, Real> > tp(grid.begin(), grid.end());

            for (Size i=0; i < size; ++i) {
                const Size b = (i*tp.size())/size;
                const Size e = ((i+1)*tp.size())/size;
                for (Size j=b; j < e; ++j) {
                    vGrid[i]+=tp[j].first/(e-b);
                    pGrid[i]+=tp[j].second/(e-b);
                }
            }
        }
        catch (const Error&) {
            // use default mesh
            const Real vol = mixedSigma*
                std::sqrt(process->theta()/(2*process->kappa()));

            const Real mean = process->theta();
            const Real upperBound = std::max(process->v0()+4*vol, mean+4*vol);
            const Real lowerBound
                = std::max(0.0, std::min(process->v0()-4*vol, mean-4*vol));

            for (Size i=0; i < size; ++i) {
                pGrid[i] = i/(size-1.0);
                vGrid[i] = lowerBound + i*(upperBound-lowerBound)/(size-1.0);
            }
        }

        Real skewHint = ((process->kappa() != 0.0)
                ? std::max(1.0, mixedSigma/process->kappa()) : 1.0);

        std::sort(pGrid.begin(), pGrid.end());
        volaEstimate_ = GaussLobattoIntegral(100000, 1e-4)(
            interpolated_volatility(pGrid, vGrid),
                pGrid.front(), pGrid.back())*std::pow(skewHint, 1.5);

        const Real v0 = process->v0();
        for (Size i=1; i<vGrid.size(); ++i) {
            if (vGrid[i-1] <= v0 && vGrid[i] >= v0) {
                if (std::fabs(vGrid[i-1] - v0) < std::fabs(vGrid[i] - v0))
                    vGrid[i-1] = v0;
                else
                    vGrid[i] = v0;
            }
        }

        std::copy(vGrid.begin(), vGrid.end(), locations_.begin());

        for (Size i=0; i < size-1; ++i) {
            dminus_[i+1] = dplus_[i] = vGrid[i+1] - vGrid[i];
        }
        dplus_.back() = dminus_.front() = Null<Real>();
    }

    FdmHestonLocalVolatilityVarianceMesher::FdmHestonLocalVolatilityVarianceMesher(
        Size size,
        const ext::shared_ptr<HestonProcess>& process,
        const ext::shared_ptr<LocalVolTermStructure>& leverageFct,
        Time maturity, Size tAvgSteps, Real epsilon,
        Real mixingFactor)
     : Fdm1dMesher(size) {

        const FdmHestonVarianceMesher mesher(
            size, process, maturity, tAvgSteps, epsilon, mixingFactor);

        for (Size i=0; i < size; ++i) {
            dplus_[i] = mesher.dplus(i);
            dminus_[i] = mesher.dminus(i);
            locations_[i] = mesher.location(i);
        }

        volaEstimate_ = mesher.volaEstimate();

        if (leverageFct != nullptr) {
            typedef boost::accumulators::accumulator_set<
                Real, boost::accumulators::stats<
                    boost::accumulators::tag::mean> >
                accumulator_set;

            accumulator_set acc;

            const Real s0 = process->s0()->value();

            acc(leverageFct->localVol(0, s0, true));

            const Handle<YieldTermStructure> rTS = process->riskFreeRate();
            const Handle<YieldTermStructure> qTS = process->dividendYield();

            for (Size l=1; l <= tAvgSteps; ++l) {
                const Real t = (maturity*l)/tAvgSteps;
                const Real vol = volaEstimate_ * boost::accumulators::mean(acc);

                const Real fwd = s0*qTS->discount(t)/rTS->discount(t);

                const Size sAvgSteps = 50;

                std::vector<Real> u(sAvgSteps), sig(sAvgSteps);

                for (Size i=0; i < sAvgSteps; ++i) {
                    u[i] = epsilon + ((1-2*epsilon)/(sAvgSteps-1))*i;
                    const Real x = InverseCumulativeNormal()(u[i]);

                    const Real gf = x*vol*std::sqrt(t);
                    const Real f = fwd*std::exp(gf);

                    sig[i] = squared(leverageFct->localVol(t, f, true));
                }

                const Real leverageAvg =
                    GaussLobattoIntegral(10000, 1e-4)(
                        interpolated_volatility(u, sig), u.front(), u.back())
                    / (1-2*epsilon);

                acc(leverageAvg);
            }
            volaEstimate_ *= boost::accumulators::mean(acc);
        }
    }
}