#include <iostream>
#include <type.hpp>

#include <Eigen/Eigen>

#include "Geometry/ParameterDesign/Bezier.hpp"
#include "imgui/implot.h"
#include "Visualization/Visualizer.h"
#include "Geometry/ParameterDesign/ParameterCurve.h"
#include "Geometry/ParameterDesign/Rational.h"

class BezierVisualizer :public Visualizer
{
protected:
	void AddPoint();
	void DragPoint();

	void draw(bool* p_open) override;

	std::vector<Point2d> points;
	std::vector<Float> weights;
	bool updated = true;

	void evaluate_bezier()
	{
		using V = Eigen::Vector2d;
		auto temp_points = std::vector<Eigen::Vector2d>(3);

		points[0] = V(points[0].x(), 0);
		points[1] = V(0, points[1].y());
		temp_points[0] = points[0];
		temp_points[1] = V(points[0].x(), points[1].y());
		temp_points[2] = V(2 * points[0].x(), 2 * points[1].y());

		weights = { 1,1,0 };
		RationalBSpline<3> RBS(temp_points, weights);
		RBS.evaluate();

		for (int i = 0; i < Length; ++i)
		{
			xs[i] = RBS(ctr_points[i]).x();
			ys[i] = RBS(ctr_points[i]).y();
		}
		weights = { 1,-1,0 };
		temp_points[1] *= -1;
		RBS = RationalBSpline<3>(temp_points, weights);

		RBS.evaluate();
		for (int i = 0; i < Length; ++i)
		{
			xs[i + Length] = RBS(1.0 - ctr_points[i]).x();
			ys[i + Length] = RBS(1.0 - ctr_points[i]).y();
		}

		ctr_xs.resize(points.size());
		ctr_ys.resize(points.size());

		for (int i = 0; i < points.size(); ++i)
		{
			ctr_xs[i] = points[i].x();
			ctr_ys[i] = points[i].y();
		}
	}

public:
	BezierVisualizer() {
		for (int i = 0; i < Length; ++i)
		{
			ctr_points[i] = Float(i) / (Length - 1);
			//ctr_points[i + Length] = Float(i) / (Length - 1);
		}

		points.resize(2);
		points[0] = Eigen::Vector2d(2, 0);
		points[1] = Eigen::Vector2d(0, 1);
	}
	const float Length = 1001;

	std::vector<float> ctr_points = std::vector<float>(Length);
	std::vector<float> xs = std::vector<float>(2 * Length);
	std::vector<float> ys = std::vector<float>(2 * Length);

	std::vector<float> ctr_xs = std::vector<float>(0);
	std::vector<float> ctr_ys = std::vector<float>(0);
};

void BezierVisualizer::AddPoint()
{
	//if (ImGui::IsMouseClicked(ImGuiMouseButton_Right))
	//{
	//	auto pos = ImPlot::GetPlotMousePos();
	//	points.emplace_back(pos.x, pos.y);
	//	updated = true;
	//}
}

void BezierVisualizer::DragPoint()
{
	for (int i = 0; i < points.size(); ++i)
	{
		auto& point = points[i];
		updated |= ImPlot::DragPoint(("Point " + std::to_string(i)).c_str(), &point.x(), &point.y());
	}
}

void BezierVisualizer::draw(bool* p_open)
{
	if (updated)
	{
		evaluate_bezier();
		updated = false;
	}
	if (ImGui::BeginTabBar("Homework 2")) {
		if (ImGui::BeginTabItem("Interpolation"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail(), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				int delta = 667;

				ImPlot::PlotLine("Bezier", &xs[0], &ys[0], Length + delta);

				ImPlot::PlotLine("Bezier", &xs[Length + delta], &ys[Length + delta], Length - delta);
				//if (!ctr_xs.empty())
				//{
				//	ImPlot::PlotLine("ControlBox", &ctr_xs[0], &ctr_ys[0], ctr_xs.size());
				//}
				AddPoint();
				DragPoint();

				ImPlot::EndPlot();
			}

			ImGui::EndTabItem();
		}

		ImGui::EndTabBar();
	}
	ImGui::End();
}

int main()
{
	BezierVisualizer visualizer;
	visualizer.RenderLoop();
}


#ifndef POLICY_HPP
#define POLICY_HPP

#include <limits>
#include "helpers.hpp"
#include "stats.hpp"
#include <boost/math/distributions/geometric.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/shared_ptr.hpp>

using boost::shared_ptr;

template<class PT>
class PolicyBase
{
public:
  PolicyBase(unsigned long horizon, const int arms) :
    totalrewards(arms, 0), sample_index(arms, 0), horizon(horizon), totalsampled(arms,0), totalscore(0), arms(arms) {}

  void simulate(const Table& rewards, const unsigned long t, const double mustar,
    Stats& stats)
  {
    int arm = (static_cast<PT*>(this))->choosearm(t);
    totalsampled[arm] += 1;
    double outcome = rewards[arm][sample_index[arm]];
    totalrewards[arm] += outcome;
    sample_index[arm]++;
    totalscore += outcome;
    const double instantregret = (t*mustar) - totalscore;
    #pragma omp critical
    {
      stats(instantregret, t);
    }
  }
//protected:
  vector<double> totalrewards;
  vector<int> sample_index;
  unsigned long horizon;
  vector<double> totalsampled;
  double totalscore;
  const int arms;
};

template<class PT>
class PolicyCollection
{
public:

  void simulate(Table& rewards, const unsigned long t, const double mustar,
    vector<Stats>& totalregret)
  {
    for (unsigned int i = 0; i < pcs.size(); ++i)
    {
      pcs[i]->simulate(rewards, t, mustar, totalregret[i]);
    }
  }

  vector<shared_ptr<PT> > pcs;
};

class ThompsonPolicy : public PolicyBase<ThompsonPolicy>
{
public:
  ThompsonPolicy(RandomSampleHelper& bh, unsigned long horizon, int arms, bool gaussian) :
    PolicyBase<ThompsonPolicy>(horizon, arms), bh(bh), gaussian(gaussian) {}
  int choosearm(int t)
  {
    int result = -1;
    double max = -std::numeric_limits<double>::max();
    for(int a = 0; a < arms; ++a)
    {
      if (totalsampled[a] == totalrewards[a])
        return a;
      const double samp = gaussian ? bh.sampleGaussian(totalrewards[a]/(1.0+totalsampled[a]), sqrt(1.0/(1.0+totalsampled[a]))) :
        bh.sampleBeta(totalrewards[a] + 1, totalsampled[a] - totalrewards[a] + 1);
      if(samp > max)
      {
        result = a;
        max = samp;
      }
    }
    return result;
  }

private:
  RandomSampleHelper& bh;
  bool gaussian;
};

class UCB1Policy : public PolicyBase<UCB1Policy>
{
public:
  UCB1Policy(unsigned long horizon, int arms) :
    PolicyBase<UCB1Policy>(horizon, arms) {}

  int choosearm(int t)
  {
    int result = -1;
    double max = -std::numeric_limits<double>::max();
    if(t <= arms)
      return t-1;
    for(int a = 0; a < arms; ++a)
    {
      const double avg = double(totalrewards[a])/double(totalsampled[a]);
      const double ucb = avg + sqrt(2*log(double(t))/double(totalsampled[a]));
      if(ucb > max)
      {
        result = a;
        max = ucb;
      }
    }
    return result;
  }
};

class BayesUCBPolicy : public PolicyBase<BayesUCBPolicy>
{
public:
  BayesUCBPolicy(unsigned long horizon, int arms, bool gaussian) :
    PolicyBase<BayesUCBPolicy>(horizon, arms), gaussian(gaussian) {}

  int choosearm(int t)
  {
    int result = -1;
    double max = -std::numeric_limits<double>::max();
    if(t <= arms)
      return t-1;
    for(int a = 0; a < arms; ++a)
    {
      const double gamma = 1.0 - 1.0 / double(t);
      double ucb = 0;
      if (gaussian)
      {
        double mun = totalrewards[a]/(1.0+totalsampled[a]);
        double sigman = sqrt(1.0/(1.0+totalsampled[a]));
        boost::math::normal_distribution<double, policies::policy<> > normdist(mun,sigman);
        ucb = boost::math::quantile(normdist, gamma);
      }
      else
      {
        ucb = ibeta_inv(totalrewards[a]+1, totalsampled[a]-totalrewards[a] + 1, gamma);
      }
      if(ucb > max)
      {
        result = a;
        max = ucb;
      }
    }
    return result;
  }
private:
  bool gaussian;
};

class NaivePolicy : public PolicyBase<NaivePolicy>
{
public:
  NaivePolicy(unsigned long horizon, int arms) :
    PolicyBase<NaivePolicy>(horizon, arms) {}

  int choosearm(int t)
  {
    int result = -1;
    double max = -numeric_limits<double>::max();
    if(t <= arms)
      return t-1;
    #ifdef PRINT_POLICY
    cout << "round [" << t << "]. ";
    #endif
    for(int a = 0; a < arms; ++a)
    {
      const double avg = double(totalrewards[a])/double(totalsampled[a]);
      #ifdef PRINT_POLICY
      cout << "avg for arm [" << a << "] is [" << avg << "]. ";
      #endif
      if(avg > max)
      {
        result = a;
        max = avg;
      }
    }
    #ifdef PRINT_POLICY
    cout << "picked arm [" << result << "]" << endl;
    #endif
    return result;
  }
};

class GittinsPolicy : public PolicyBase<GittinsPolicy>
{
public:
  GittinsPolicy(const Table3D& indices, unsigned long horizon, int arms, double b_param, bool gaussian) :
    PolicyBase<GittinsPolicy>(horizon, arms), indices(indices), gaussian(gaussian), b_param(b_param)
  {
  }

  int choosearm(int it)
  {
    int result = -1;
    double max = -1;
    int offset = (int) b_param;
    for(int a = 0; a < arms; ++a)
    {
      double gindex = 0;
      double g = 1.0 - 1.0/(b_param + double(it));
      if (indices.size() == 0)
      {
        if (gaussian)
          gindex = approximategi2(g, totalrewards[a]/(1.0+totalsampled[a]), 1.0/sqrt(1+totalsampled[a]));
        else
          gindex = approximategi(g, totalrewards[a]+1, totalsampled[a]-totalrewards[a]+1);
      }
      else {
        if (gaussian)
          gindex = approximategi2(g, totalrewards[a]/(1.0+totalsampled[a]), 1.0/sqrt(1+totalsampled[a]));
        else
          gindex = indices[it + offset -1][totalrewards[a]][totalsampled[a]-totalrewards[a]];
      }
      if(gindex > max)
      {
        result = a;
        max = gindex;
      }
      #ifdef PRINT_POLICY
      cout << "a" << a << " = " << long(totalrewards[a]) << ". b = " << long(totalsampled[a]-totalrewards[a]) << ". gindex = " << gindex << ". ";
      #endif
    }
    #ifdef PRINT_POLICY
    cout << endl;
    #endif
    return result;
  }
private:
  const Table3D& indices;

  double approximategi(double g, double a, double b)
  {
    const double mu = a/(a+b);
    const double var = a*b/((a+b)*(a+b)*(a+b+1));
    const static double c = -log(g);
    return mu + sqrt(var)*psi(var/c);
  }

  double psi(const double s)
  {
    if(s<=0.2)
      return sqrt(s/2.0);
    if(s <= 1)
      return 0.49 - 0.11/sqrt(s);
    if (s <= 5)
      return 0.63 - 0.26/sqrt(s);
    if (s <= 15)
      return 0.77 - 0.58/sqrt(s);
    return sqrt(2*log(s) - log(log(s)) - log(16*M_PI));
  }

  double approximategi2(double g, double mun, double sigman)
  {
    return g == 0.0 ? 0.0 : mun + geefunc(sigman, g);
  }

  double geefunc(double s, double gamma)
  {
    return sqrt(-log(gamma))*bfunc(-s*s/log(gamma));
  }

  double bfunc(double s)
  {
    if (s <= 1.0/7.0)
      return s/sqrt(2);
    if (s <= 100)
      return exp(-0.02645*(log(s)*log(s)) + 0.89106*log(s) - 0.4873);
    return sqrt(s)*sqrt(2*log(s) - log(log(s)) - log(16*PI));
  }

  bool gaussian;
  double b_param;
};

class IDSMinFunc
{
public:
  IDSMinFunc(double di, double dj, double gi, double gj)
    : gi(gi), gj(gj), di(di), dj(dj)
  {}

  double operator() (double q)
  {
    return (q*di + (1-q)*dj)*(q*di + (1-q)*dj)/(q*gi + (1-q)*gj);
  }

  void print()
  {
    cout << "gi:" << gi << ". gj:" << gj;
    cout << "di:" << di << ". dj:" << dj;
    cout << endl;
  }
private:
  double gi, gj, di, dj;
};

class IDSPolicy : public PolicyBase<IDSPolicy>
{
  const vector<double>& uniforms;
  int POINTS;
  double RANGE;
  double STEPSIZE;
public:
  IDSPolicy(unsigned long horizon, const vector<double>& uniforms, int arms, bool gaussian_, int bpoints) :
    PolicyBase<IDSPolicy>(horizon, arms),
    uniforms(uniforms),
    POINTS(gaussian_ ? 8000 : bpoints),
    fi(arms, vector<double>(POINTS, 0)),
    Fi(arms, vector<double>(POINTS, 0)),
    Qi(arms, vector<double>(POINTS, 0)),
    F(POINTS, 0),
    gaussian(gaussian_),
    lastret(-1)
  {
    RANGE = 40.0;
    STEPSIZE = gaussian ? RANGE/double(POINTS) : 1.0/double(POINTS);
    boost::math::normal_distribution<> normdist(0,1);
    for(int i = 0; i < POINTS; ++i)
    {
      double Fprod = 1;
      for(int a =0; a < arms; ++a)
      {
        double x = (gaussian ? -RANGE/2.0 : 0) + STEPSIZE*(i+0.5);
        Fi[a][i] = gaussian ? cdf(normdist, x) : ibeta(1.0,1.0,x);
        Fprod *= Fi[a][i];
        fi[a][i] = gaussian ? pdf(normdist, x) : ibeta_derivative(1.0,1.0,x);
        Qi[a][i] = gaussian ? 0 : 0.5*ibeta(1.0+1.0,1.0,x);
      }
      F[i] = Fprod;
    }
  }

  int choosearm(int t)
  {

    vector<double> delta(arms, 0);
    vector<double> gee(arms, 0);
    computeDeltasAndGees(delta, gee);

    int istar=-1,jstar=-1;
    double qstar=-1.0;
    setupRandomization(gee, delta, qstar, istar, jstar);
    int ret= uniforms[t-1] < qstar ? istar : jstar;
    assert (ret >= 0 && ret < arms);

    //remember the last arm that was pulled, so we can update relevant values of fi, Fi, etc. on the next iteration
    lastret = ret;
    return ret;
  }

  double probOptimal(int arm)
  {
    double result = 0;
    for(int i = 0; i < POINTS; ++i)
    {
      double fprod = 1;
      for (int a = 0; a < arms; ++a)
      {
        fprod *= a == arm ? 1.0 : Fi[a][i];
      }
      result += fi[arm][i]*fprod;
    }
    return STEPSIZE*result;
  }

  static void setupRandomization(const vector<double>& gee,
                          const vector<double>& delta,
                          double& qstar,
                          int& istar,
                          int& jstar)
  {
    int arms = gee.size();
    double opt = numeric_limits<double>::max();
    for(int j = 0; j < arms-1; ++j)
    {
      for(int i = j+1; i < arms; ++i)
      {
        IDSMinFunc minfun(delta[i], delta[j], gee[i], gee[j]);
        std::pair<double,double> res = brent_find_minima(minfun, 0.0, 1.0, 20);
        assert((res.second < opt) || !(i == 1 && j == 0));
        if(res.second < opt)
        {
          opt = res.second;
          istar = i; jstar = j;
          qstar = res.first;
        }
      }
    }
  }

  void computeDeltasAndGees(vector<double>& delta, vector<double>& gee)
  {
    vector<boost::math::normal_distribution<> > normdists;
    vector<double> muns;
    vector<double> sigmans;
    for (int a = 0; a < arms; ++a)
    {
      double mu = totalrewards[a]/(1.0 + totalsampled[a]);
      double sigma = sqrt(1.0/(1.0 + totalsampled[a]));
      assert(!gaussian || abs(mu) < RANGE/2.0 - sigma);
      muns.push_back(mu);
      sigmans.push_back(sigma);
      normdists.push_back(boost::math::normal_distribution<>(mu, sigma));
    }
    vector<double> as(arms, 0);
    vector<double> bs(arms, 0);
    vector<double> proboptimal(arms, 0);
    for(int i=0; i < arms;++i)
    {
      as[i] = totalrewards[i]+1;
      bs[i] = totalsampled[i]-totalrewards[i]+1;
    }

    if (lastret >= 0)
    {
      const boost::math::normal_distribution<>& newnormdist = normdists[lastret];
      const int newa = totalrewards[lastret]+1;
      const int newb = totalsampled[lastret] - totalrewards[lastret]+1;
      for(int i = 0; i < POINTS; ++i)
      {
        double x =  (gaussian ? -RANGE/2.0 : 0.0) + (i+0.5)*STEPSIZE;

        fi[lastret][i] = gaussian ? pdf(newnormdist, x) : ibeta_derivative(newa,newb,x);
        Fi[lastret][i] = gaussian ? cdf(newnormdist, x) : ibeta(newa,newb,x);
        Qi[lastret][i] = gaussian ? 0 : double(newa)/double(newa+newb)*ibeta(newa+1,newb,x);
        F[i] = 1;
        for(int a = 0; a < arms; ++a)
        {
          F[i] *= Fi[a][i];
        }
      }
    }

    for(int a =0; a < arms; ++a)
      proboptimal[a] = probOptimal(a);
    #ifndef NDEBUG
    double totalprob = 0;
    for (int a = 0; a < arms; ++a)
    {
     totalprob += proboptimal[a];
    }
    if (abs(totalprob - 1.0) >=  1e-1)
    {
      cerr << "probabilities must sum to one when in fact totalprob = " << totalprob << endl;
      for (int a = 0; a < arms; ++a)
      {
        cerr << "totalsampled[" << a << "] = " << totalsampled[a] << ". proboptimal[" << a << "] = " << proboptimal[a] << endl;
        cerr << "totalrewards[" << a << "] = " << totalrewards[a] << endl;
      }
      exit(1);
    }
    #endif
    Table Ms(arms,vector<double>(arms,0));
    for(int i = 0; i < arms;++i)
    {
      for(int j = 0; j < arms; ++j)
      {
        double sum=0;
        if(i==j)
        {
          for(int p=0;p < POINTS; ++p)
          {
            double x = (gaussian ? - RANGE/2.0 : 0) + (p+0.5)*STEPSIZE;
            sum += x*fi[i][p]*F[p]/max(Fi[i][p],0.0000001);
          }
          sum*=STEPSIZE;
          Ms[i][j] = sum/proboptimal[i];
          assert(gaussian || (Ms[i][j] < 1 && Ms[i][j] > 0));
          continue;
        }
        for(int p =0; p < POINTS;++p)
        {
          sum +=gaussian ? fi[i][p]*F[p]*fi[j][p]/max(Fi[i][p]*Fi[j][p],0.000001) : fi[i][p]*F[p]*Qi[j][p]/max(Fi[i][p]*Fi[j][p],0.000001);
          if(isnan(sum))
          {
            cerr << "sum is nan! probably cuz Fi["<<i<<"]["<<p<<"]=" << Fi[i][p] << ". ";
            cerr << "and Fi["<<j<<"]["<<p<<"]=" << Fi[j][p] << endl;
            cerr << "fi[" << i << "][" << p <<"]="<<fi[i][p] << endl;
            cerr << "Qi[" << j << "][" << p <<"]="<<Qi[j][p] << endl;
            cerr << "F[" << p <<"]="<< F[p] << endl;
            cerr << "the result is=" << fi[i][p]*F[p]*Qi[j][p]/max(Fi[i][p]*Fi[j][p],0.000001) << endl;
            cerr << "the denominator is=" << max(Fi[i][p]*Fi[j][p],0.000001) << endl;
            cerr << "the numerator is " << fi[i][p]*F[p]*Qi[j][p] << endl;
            exit(1);
          }
        }
        sum*=STEPSIZE;
        Ms[i][j] = gaussian ? muns[j] - sum * sigmans[j]*sigmans[j]/proboptimal[i] :  sum/proboptimal[i];
        assert(!isnan(Ms[i][j]));
      }
    }
    double rhostar=0;
    for(int i = 0; i < arms;++i)
    {
      rhostar += proboptimal[i]*Ms[i][i];
    }
    for(int i = 0; i < arms; ++i)
    {
      delta[i] = gaussian ? rhostar - muns[i] : rhostar - as[i]/(as[i]+bs[i]);
      double geeval=0;
      for(int j = 0; j < arms;++j)
      {
        geeval += gaussian ? proboptimal[j] * (Ms[j][i] - muns[i])*(Ms[j][i] - muns[i]) :  proboptimal[j]*kl(Ms[j][i], as[i]/(as[i]+bs[i]));
        assert(!isnan(geeval));
      }
      gee[i] = geeval;
    }
  }

  Table fi;
  Table Fi;
  Table Qi;
  vector<double> F;
  bool gaussian;
  int lastret;
};

class GaussianIDSPolicy : public PolicyBase<GaussianIDSPolicy>
{

public:
  GaussianIDSPolicy(unsigned long horizon, RandUnif& rng, int arms) :
    PolicyBase<GaussianIDSPolicy>(horizon, arms), unif(rng), horizon(horizon)
  {
  }

private:
  RandUnif& unif;
  unsigned long horizon;
};

class InterestingPolicy : public PolicyBase<InterestingPolicy>
{
public:
  InterestingPolicy(unsigned long horizon, int arms, double a_param, double b_param, bool gaussian, int numSteps = 1) :
    PolicyBase<InterestingPolicy>(horizon, arms), a_param(a_param), b_param(b_param), gaussian(gaussian), numSteps(numSteps)
  {
  }

  int choosearm(int it)
  {
    double t = double(it);
    int result = -1;
    double max = -numeric_limits<double>::max();

    double  gamma_t = 1.0 - a_param/(b_param + t);

    for(int a = 0; a < arms; ++a)
    {
      #ifdef PRINT_POLICY
      cout << "discount for " << a << " is " << gamma << " ";
      #endif
      const double index = gaussian ? ::computeAGIGaussian(gamma_t, totalrewards[a]/(1+totalsampled[a]), 1.0/sqrt(1+totalsampled[a]))
         : ::computeApproxGI(gamma_t, totalrewards[a] + 1, (long)(totalsampled[a] - totalrewards[a]) + 1, numSteps);
      if(index > max)
      {
        result = a;
        max = index;
      }
      #ifdef PRINT_POLICY
      cout << "a" << a << " = " << long(totalrewards[a]) << ". b = " << long(totalsampled[a]-totalrewards[a]) << ". gindex = " << index << ". ";
      #endif
    }
    #ifdef PRINT_POLICY
    cout << endl;
    #endif
    return result;
  }
private:
  const double a_param, b_param;
  bool gaussian;
  int numSteps;
};

#endif


#include "timer.hh"
#include <Eigen/Dense>
#include <cstdlib>
#include <iostream>
#include <vector>

using namespace std;
using namespace Eigen;

typedef MatrixXf Matrix;
typedef RowVectorXf RowVector;
typedef VectorXf ColVector;

class NeuralNetwork
{
  vector<ColVector*> neuronLayers; // different layers of output network
  vector<ColVector*> cacheLayers;  // values of layers before activation
  vector<ColVector*> deltas;       // stores the error contribution of each neurons
  vector<MatrixXf*> weights;       // the weights of connections between layers
  vector<uint> topology;
  double learningRate;
  // enum { NeedsToAlign = (sizeof(neuronLayers)%16)==0 };

public:
  // EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)

  NeuralNetwork( vector<uint> topology, double learningRate = 0.005f );

  ColVector forward_propagation( ColVector& input );
  void backward_propagation( ColVector& output );
  void error_calculation( ColVector& output );
  void update_weights();

  double activationFunction( double x ) { return max( x, 0.0f ); }

  ColVector activationFunction( ColVector& x ) { return x.cwiseMax( 0 ); }

  double activationFunctionDerivative( double x )
  {
    if ( x >= 0 )
      return 1;
    else
      return 0;
  }

  void print()
  {
    const IOFormat CleanFmt( 4, 0, ", ", "\n", "[", "]" );
    for ( uint i = 1; i < topology.size(); i++ ) {
      cout << "Weights Layer " << i << ":\n"
           << ( *weights[i - 1] ).format( CleanFmt ) << "\n\n"
           << "cacheLayers: Layer " << i << ":\n"
           << ( *cacheLayers[i] ).format( CleanFmt ) << "\n\n"
           << "neuronLayers Layer " << i << ":\n"
           << ( *neuronLayers[i] ).format( CleanFmt ) << "\n\n";
    }
  }
};

NeuralNetwork::NeuralNetwork( vector<uint> topology_, double learningRate_ )
  : neuronLayers()
  , cacheLayers()
  , deltas()
  , weights()
  , topology( topology_ )
  , learningRate( learningRate_ )
{
  topology = topology_;
  learningRate = learningRate_;
  for ( uint i = 0; i < topology.size(); i++ ) {
    if ( i != topology.size() - 1 )
      neuronLayers.push_back( new ColVector( topology[i] + 1 ) );
    // +1 for biases
    else
      neuronLayers.push_back( new ColVector( topology[i] ) );

    // initialize cache and delta vectors
    cacheLayers.push_back( new ColVector( neuronLayers[i]->rows() ) );
    deltas.push_back( new ColVector( neuronLayers[i]->rows() ) );

    // setting up biases nodes
    if ( i != topology.size() - 1 ) {
      neuronLayers.back()->coeffRef( topology[i] ) = 1.0;
      cacheLayers.back()->coeffRef( topology[i] ) = 1.0;
    }

    // initialize weights matrix
    if ( i > 0 ) {
      if ( i != topology.size() - 1 ) {
        weights.push_back( new MatrixXf( topology[i] + 1, topology[i - 1] + 1 ) );

        // random weights initialisation
        weights.back()->setRandom();

        // no "normal" weights into biases
        weights.back()->row( topology[i] ).setZero();
        weights.back()->coeffRef( topology[i], topology[i - 1] ) = 1.0;
      } else {
        weights.push_back( new MatrixXf( topology[i], topology[i - 1] + 1 ) );
        weights.back()->setRandom();
      }
    }
  }
}

ColVector NeuralNetwork::forward_propagation( ColVector& input )
{
  // setting the input layer of the NN
  // block takes 4 arguments : startRow, startCol, blockRows, blockCols

  neuronLayers.front()->block( 0, 0, neuronLayers.front()->size() - 1, 1 ) = input;

  // forward propagation
  for ( uint i = 1; i < topology.size() - 1; i++ ) {
    ( *cacheLayers[i] ) = ( *weights[i - 1] ) * ( *neuronLayers[i - 1] );
    ( *neuronLayers[i] ) = activationFunction( *cacheLayers[i] );
    // resetting back the bias NN node
    neuronLayers[i]->coeffRef( topology[i] ) = 1.0;
  }
  // no activation at last layer
  uint i = topology.size() - 1;
  ( *cacheLayers[i] ) = ( *weights[i - 1] ) * ( *neuronLayers[i - 1] );
  ( *neuronLayers[i] ) = *cacheLayers[i];

  return *neuronLayers[topology.size() - 1];
}

void program_body()
{
  vector<uint> TOPOLOGY = { 5, 3, 2, 1 };
  ColVector input( 5 );
  input << 1.0f, -2.0f, 3.0f, -2.0f, 0.0f;
  NeuralNetwork nn( TOPOLOGY );
  uint64_t start = Timer::timestamp_ns();
  ColVector output = nn.forward_propagation( input );
  uint64_t end = Timer::timestamp_ns();
  cout << end - start << endl;

  const IOFormat CleanFmt( 4, 0, ", ", "\n", "[", "]" );

  cout << "input:\n"
       << input.format( CleanFmt ) << "\n\n"
       << "output:\n"
       << output.format( CleanFmt ) << "\n\n";
  nn.print();
}

int main()
{
  try {
    ios::sync_with_stdio( false );
    program_body();
    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}


#ifndef BART_SRC_CONVERGENCE_MOMENTS_CONVERGENCE_CHECKER_L_INFINITY_NORM_HPP_
#define BART_SRC_CONVERGENCE_MOMENTS_CONVERGENCE_CHECKER_L_INFINITY_NORM_HPP_

#include "convergence/convergence_checker.hpp"
#include <deal.II/lac/vector.h>

namespace bart::convergence::moments {

/*! \brief Checks for convergence between flux moments using the percentage
 * change in the \f$L_{\infty}\f$ norm, compared to the current iteration (\f$i\f$):
 *
 * \f[
 *
 * \Delta_i = \frac{|\phi_i - \phi_{i-1}|_{\infty}}{|\phi_{i}|_{\infty}}
 *
 * \f]
 *
 * Convergence is achieved if \f$\Delta_i \leq \Delta_{\text{max}}\f$.
 * */
class ConvergenceCheckerLInfinityNorm : public ConvergenceChecker<dealii::Vector<double>> {
 public:
  using Vector = dealii::Vector<double>;
  explicit ConvergenceCheckerLInfinityNorm(const double max_delta = 1e-6) { max_delta_ = CheckNonNegative(max_delta); };

  auto SetMaxDelta(const double& to_set) -> void override { max_delta_ = CheckNonNegative(to_set); };
  auto IsConverged(const Vector &current_iteration, const Vector &previous_iteration) -> bool override {
    Vector difference(current_iteration);
    difference.add(-1, previous_iteration);
    delta_ = difference.linfty_norm()/current_iteration.linfty_norm();
    is_converged_ = delta_ <= max_delta_;
    return is_converged_;
  }

 private:
  auto CheckNonNegative(const double to_check) -> double {
    AssertThrow(to_check > 0, dealii::ExcMessage("Error in ConvergenceCheckerLInfinityNorm, max delta value to set must "
                                                 "be greater than 0"))
    return to_check;
  }
};

} // namespace bart::convergence::moments

#endif //BART_SRC_CONVERGENCE_MOMENTS_CONVERGENCE_CHECKER_L_INFINITY_NORM_HPP_


#include <boost/math/constants/constants.hpp>
#include "EnergyResolution.hh"
#include "TypesFunctions.hh"
#include <fmt/format.h>
#include <string.h>

constexpr double pi = boost::math::constants::pi<double>();

EnergyResolution::EnergyResolution(bool propagate_matrix) :
EnergyResolution({"Eres_a" , "Eres_b" , "Eres_c"}, propagate_matrix){

}

EnergyResolution::EnergyResolution(const std::vector<std::string>& pars, bool propagate_matrix) :
HistSmearSparse(propagate_matrix)
{
  if(pars.size()!=3u){
    throw std::runtime_error("Energy resolution should have exactly 3 parameters");
  }
  variable_(&m_a, pars[0]);
  variable_(&m_b, pars[1]);
  variable_(&m_c, pars[2]);

  transformation_("matrix")
      .input("Edges", /*inactive*/true)
      .output("FakeMatrix")
      .types(TypesFunctions::ifHist<0>, TypesFunctions::if1d<0>, TypesFunctions::toMatrix<0,0,0>)
      .types(&EnergyResolution::getEdges)
      .func(&EnergyResolution::calcMatrix);

  add_transformation();
  add_input();
  set_open_input();
}

void EnergyResolution::getEdges(TypesFunctionArgs& fargs) {
  m_edges = fargs.args[0].edges.data();
}

double EnergyResolution::relativeSigma(double Etrue) const noexcept {
  return sqrt(pow(m_a, 2)+ pow(m_b, 2)/Etrue + pow(m_c/Etrue, 2));
}

double EnergyResolution::resolution(double Etrue, double Erec) const noexcept {
  static const double twopisqr = std::sqrt(2*pi);
  const double sigma = Etrue * relativeSigma(Etrue);
  const double reldiff = (Etrue - Erec)/sigma;

  return std::exp(-0.5*pow(reldiff, 2))/(twopisqr*sigma);
}

void EnergyResolution::calcMatrix(FunctionArgs& fargs) {
  m_sparse_cache.setZero();

  auto& ret = fargs.rets[0];
  auto* edges = m_edges;
  auto bins = ret.type.shape[0];
  m_sparse_cache.resize(bins, bins);

  /* fill the cache matrix with probalilities for number of events to leak to other bins */
  /* colums corressponds to reconstrucred energy and rows to true energy */
  auto bin_center = [edges](size_t index){ return (edges[index+1] + edges[index])/2; };
  for (size_t etrue = 0; etrue < bins; ++etrue) {
    double Etrue = bin_center(etrue);
    double dEtrue = edges[etrue+1] - edges[etrue];

    bool right_edge_reached{false};
    /* precalculating probabilities for events in given bin to leak to
     * neighbor bins  */
    for (size_t erec = 0; erec < bins; ++erec) {
      double Erec = bin_center(erec);
      double rEvents = dEtrue*resolution(Etrue, Erec);

      if (rEvents < 1E-10) {
        if (right_edge_reached) {
           break;
        }
        continue;
      }
      m_sparse_cache.insert(erec, etrue) = rEvents;
      if (!right_edge_reached) {
        right_edge_reached = true;
      }
    }
  }
  m_sparse_cache.makeCompressed();

  if ( m_propagate_matrix )
    fargs.rets[0].mat = m_sparse_cache;
}


#include <blitz/array.h>
#include <random/uniform.h>
#include <bp_util.h>

using namespace blitz;
using namespace ranlib;

template <typename T>
Array<T, 1> cnd(Array<T, 1> & x)
{
    int samples = x.numElements();
    Array<T, 1> l(samples), k(samples), w(samples), res(samples);
    Array<bool, 1> mask(samples);
    T a1 = 0.31938153,
      a2 =-0.356563782,
      a3 = 1.781477937,
      a4 =-1.821255978,
      a5 = 1.330274429,
      pp = 2.5066282746310002; // sqrt(2.0*PI)

    l = abs(x);
    k = 1.0 / (1.0 + 0.2316419 * l);
    w = 1.0 - 1.0 / pp * exp(-1.0*l*l/2.0) * \
        (a1*k + \
         a2*(pow(k,(T)2)) + \
         a3*(pow(k,(T)3)) + \
         a4*(pow(k,(T)4)) + \
         a5*(pow(k,(T)5)));

    mask    = x < 0.0;
    res     = (w * cast<T>(!mask) + (1.0-w)* cast<T>(mask));
    return res;
}

template <typename T>
T* pricing(size_t samples, size_t iterations, char flag, T x, T d_t, T r, T v)
{
    Array<T, 1> d1(samples), d2(samples), res(samples);
    T* p    = (T*)malloc(sizeof(T)*samples);    // Intermediate results
    T t     = d_t;                              // Initial delta

    Array<T, 1> s(samples);                     // Initial uniform sampling
    Uniform<T> rand;                            // values between 58-62
    rand.seed((unsigned int)time(0));
    s = rand.random() *4.0 +58.0;

    for(size_t i=0; i<iterations; i++) {
        d1 = (log(s/x) + (r+v*v/2.0)*t) / (v*sqrt(t));
        d2 = d1-v*sqrt(t);
        if (flag == 'c') {
            res = s * cnd(d1) - x * exp(-r * t) * cnd(d2);
        } else {
            Array<T, 1> tmp1(samples), tmp2(samples);
            tmp1 = -1.0*d2;
            tmp2 = -1.0*d1;

            res = x * exp(-r*t) * cnd(tmp1) - s*cnd(tmp2);
        }
        t += d_t;                               // Increment delta
        p[i] = sum(res) / (T)samples;           // Result from timestep
    }

    return p;
}

//FLOP count: 2*s+i*(s*8+2*s*23) where s is samples and i is iterations

int main(int argc, char* argv[])
{
    bp_util_type bp = bp_util_create(argc, argv, 2);
    if (bp.args.has_error) {
        return 1;
    }
    const size_t samples    = bp.args.sizes[0];
    const size_t iterations = bp.args.sizes[1];

    bp.timer_start();
    double* prices = pricing(
        samples, iterations,
        'c', 65.0, 1.0 / 365.0,
        0.08, 0.3
    );
    bp.timer_stop();

    bp.print("black_scholes(cpp11_blitz)");
    if (bp.args.verbose) {
        cout << ", \"output\": [";
        for(size_t i=0; i<iterations; i++) {
            cout << prices[i];
            if (iterations-1!=i) {
                cout << ", ";
            }
        }
        cout << "]" << endl;
    }

    free(prices);
    return 0;
}


// find_root_example.cpp

// Copyright Paul A. Bristow 2007.

// 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)

// Example of using root finding.

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[root_find1
/*`
First we need some includes to access the normal distribution
(and some std output of course).
*/

#include <boost/math/tools/roots.hpp> // root finding.

#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.

#include <iostream>
  using std::cout; using std::endl; using std::left; using std::showpoint; using std::noshowpoint;
#include <iomanip>
  using std::setw; using std::setprecision;
#include <limits>
  using std::numeric_limits;
//] //[/root_find1]

namespace boost{ namespace math { namespace tools
{

template <class F, class T, class Tol>
inline std::pair<T, T> bracket_and_solve_root(F f, // functor
                                              const T& guess,
                                              const T& factor,
                                              bool rising,
                                              Tol tol, // binary functor specifying termination when tol(min, max) becomes true.
                                              // eps_tolerance most suitable for this continuous function
                                              boost::uintmax_t& max_iter); // explicit (rather than default) max iterations.
// return interval as a pair containing result.

namespace detail
{

  // Functor for finding standard deviation:
  template <class RealType, class Policy>
  struct standard_deviation_functor
  {
     standard_deviation_functor(RealType m, RealType s, RealType d)
        : mean(m), standard_deviation(s)
     {
     }
     RealType operator()(const RealType& sd)
     { // Unary functor - the function whose root is to be found.
        if(sd <= tools::min_value<RealType>())
        { //
           return 1;
        }
        normal_distribution<RealType, Policy> t(mean, sd);
        RealType qa = quantile(complement(t, alpha));
        RealType qb = quantile(complement(t, beta));
        qa += qb;
        qa *= qa;
        qa *= ratio;
        qa -= (df + 1);
        return qa;
     } // operator()
     RealType mean;
     RealType standard_deviation;
  }; // struct standard_deviation_functor
} // namespace detail

template <class RealType, class Policy>
RealType normal_distribution<RealType, Policy>::find_standard_deviation(
      RealType difference_from_mean,
      RealType mean,
      RealType sd,
      RealType hint) // Best guess available - current sd if none better?
{
   static const char* function = "boost::math::normal_distribution<%1%>::find_standard_deviation";

   // Check for domain errors:
   RealType error_result;
   if(false == detail::check_probability(
      function, sd, &error_result, Policy())
      )
      return error_result;

   if(hint <= 0)
   { // standard deviation can never be negative.
      hint = 1;
   }

   detail::standard_deviation_functor<RealType, Policy> f(mean, sd, difference_from_mean);
   tools::eps_tolerance<RealType> tol(policies::digits<RealType, Policy>());
   boost::uintmax_t max_iter = 100;
   std::pair<RealType, RealType> r = tools::bracket_and_solve_root(f, hint, RealType(2), false, tol, max_iter, Policy());
   RealType result = r.first + (r.second - r.first) / 2;
   if(max_iter == 100)
   {
      policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
         " either there is no answer to how many degrees of freedom are required"
         " or the answer is infinite.  Current best guess is %1%", result, Policy());
   }
   return result;
} // find_standard_deviation
} // namespace tools
} // namespace math
} // namespace boost

int main()
{
  cout << "Example: Normal distribution, root finding.";
  try
  {

//[root_find2

/*`A machine is set to pack 3 kg of ground beef per pack.
Over a long period of time it is found that the average packed was 3 kg
with a standard deviation of 0.1 kg.
Assuming the packing is normally distributed,
we can find the fraction (or %) of packages that weigh more than 3.1 kg.
*/

double mean = 3.; // kg
double standard_deviation = 0.1; // kg
normal packs(mean, standard_deviation);

double max_weight = 3.1; // kg
cout << "Percentage of packs > " << max_weight << " is "
<< cdf(complement(packs, max_weight)) << endl; // P(X > 3.1)

double under_weight = 2.9;
cout <<"fraction of packs <= " << under_weight << " with a mean of " << mean
  << " is " << cdf(complement(packs, under_weight)) << endl;
// fraction of packs <= 2.9 with a mean of 3 is 0.841345
// This is 0.84 - more than the target 0.95
// Want 95% to be over this weight, so what should we set the mean weight to be?
// KK StatCalc says:
double over_mean = 3.0664;
normal xpacks(over_mean, standard_deviation);
cout << "fraction of packs >= " << under_weight
<< " with a mean of " << xpacks.mean()
  << " is " << cdf(complement(xpacks, under_weight)) << endl;
// fraction of packs >= 2.9 with a mean of 3.06449 is 0.950005
double under_fraction = 0.05;  // so 95% are above the minimum weight mean - sd = 2.9
double low_limit = standard_deviation;
double offset = mean - low_limit - quantile(packs, under_fraction);
double nominal_mean = mean + offset;

normal nominal_packs(nominal_mean, standard_deviation);
cout << "Setting the packer to " << nominal_mean << " will mean that "
  << "fraction of packs >= " << under_weight
  << " is " << cdf(complement(nominal_packs, under_weight)) << endl;

/*`
Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95.

Setting the packer to 3.13263 will mean that fraction of packs >= 2.9 is 0.99,
but will more than double the mean loss from 0.0644 to 0.133.

Alternatively, we could invest in a better (more precise) packer with a lower standard deviation.

To estimate how much better (how much smaller standard deviation) it would have to be,
we need to get the 5% quantile to be located at the under_weight limit, 2.9
*/
double p = 0.05; // wanted p th quantile.
cout << "Quantile of " << p << " = " << quantile(packs, p)
  << ", mean = " << packs.mean() << ", sd = " << packs.standard_deviation() << endl; //
/*`
Quantile of 0.05 = 2.83551, mean = 3, sd = 0.1

With the current packer (mean = 3, sd = 0.1), the 5% quantile is at 2.8551 kg,
a little below our target of 2.9 kg.
So we know that the standard deviation is going to have to be smaller.

Let's start by guessing that it (now 0.1) needs to be halved, to a standard deviation of 0.05
*/
normal pack05(mean, 0.05);
cout << "Quantile of " << p << " = " << quantile(pack05, p)
  << ", mean = " << pack05.mean() << ", sd = " << pack05.standard_deviation() << endl;

cout <<"Fraction of packs >= " << under_weight << " with a mean of " << mean
  << " and standard deviation of " << pack05.standard_deviation()
  << " is " << cdf(complement(pack05, under_weight)) << endl;
//
/*`
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.05 is 0.9772

So 0.05 was quite a good guess, but we are a little over the 2.9 target,
so the standard deviation could be a tiny bit more. So we could do some
more guessing to get closer, say by increasing to 0.06
*/

normal pack06(mean, 0.06);
cout << "Quantile of " << p << " = " << quantile(pack06, p)
  << ", mean = " << pack06.mean() << ", sd = " << pack06.standard_deviation() << endl;

cout <<"Fraction of packs >= " << under_weight << " with a mean of " << mean
  << " and standard deviation of " << pack06.standard_deviation()
  << " is " << cdf(complement(pack06, under_weight)) << endl;
/*`
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.06 is 0.9522

Now we are getting really close, but to do the job properly,
we could use root finding method, for example the tools provided, and used elsewhere,
in the Math Toolkit, see
[link math_toolkit.toolkit.internals1.roots2  Root Finding Without Derivatives].

But in this normal distribution case, we could be even smarter and make a direct calculation.
*/
//] [/root_find2]

  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}  // int main()

/*

Output is:

*/


#pragma once

#include <cmath>

#ifdef __POPC__
#undef USE_EIGEN
#else
#ifdef USE_EIGEN
#include <Eigen/Dense>
#endif
#include <iostream>
#endif

namespace light {

#ifdef USE_EIGEN
using Vector = Eigen::Vector3f;
#else
  struct Vector {
  float x, y, z;
  Vector() {}
  Vector(float x0, float y0, float z0) : x(x0), y(y0), z(z0) {}
  Vector operator + (const Vector &b) const {
    return Vector(x + b.x, y + b.y, z + b.z);
  }
  Vector& operator += (const Vector &b) {
    x += b.x;
    y += b.y;
    z += b.z;
    return *this;
  }
  Vector operator * (float b) const { return Vector(x*b, y*b, z*b); }
  Vector& operator *= (float s) {
    x *= s;
    y *= s;
    z *= s;
    return *this;
  }
  Vector operator - (const Vector &b) const {
    return Vector(x-b.x, y-b.y, z-b.z);
  }
  Vector operator - () const { return Vector(-x, -y, -z); }
  Vector operator / (float b) const { return Vector(x/b, y/b, z/b); }
  Vector cwiseProduct(const Vector &b) const {
    return Vector(x*b.x, y*b.y, z*b.z);
  }
  Vector normalized() const { return *this * (1.f/sqrtf(x*x + y*y + z*z)); }
  float squaredNorm() const { return x*x + y*y + z*z; }
  float norm() const { return sqrtf(squaredNorm()); }
  float dot(const Vector &b) const { return x*b.x + y*b.y + z*b.z; }
  Vector cross(const Vector &b) const {
    return Vector(y*b.z - z*b.y, z*b.x - x*b.z, x*b.y - y*b.x);
  }
  const float& operator () (size_t i) const { return i == 0 ? x : (i == 1 ? y : z); }
  Vector abs() const {
    return Vector(fabsf(x), fabsf(y), fabsf(z));
  }
  const Vector& array() const { return *this; } // For Eigen compatibility only
  bool isZero() const { return x == 0.f && y == 0.f && z == 0.f; }
  bool isNonZero() const { return x != 0.f || y != 0.f || z != 0.f; }
};

#endif

} // end namespace light

#ifndef __POPC__
#ifndef USE_EIGEN
inline
std::ostream& operator << (std::ostream &os, const light::Vector &v) {
  os << v.x << " " << v.y << " " << v.z;
  return os;
}
#endif
#endif


#include <boost/math/constants/constants.hpp>
#include "EnergyResolutionInput.hh"
#include "TypeClasses.hh"
#include "TypesFunctions.hh"
#include <fmt/format.h>
#include <string.h>

constexpr double pi = boost::math::constants::pi<double>();

using namespace TypeClasses;

EnergyResolutionInput::EnergyResolutionInput(bool propagate_matrix) :
HistSmearSparse(propagate_matrix)
{
  transformation_("matrix")
      .input("Edges", /*inactive*/true)     // Input bin edges [N]
      .input("RelSigma")                    // Relative Sigma value for each bin center [N-1]
      .output("FakeMatrix")
      .types(new CheckKindT<double>(DataKind::Hist, 0), new CheckKindT<double>(DataKind::Points, 1))
      .types(new CheckNdimT<double>(1), new CheckSameTypesT<double>({0,1}, "shape"))
      .types(TypesFunctions::toMatrix<0,0,0>)
      .func(&EnergyResolutionInput::calcMatrix);

  add_transformation();
  add_input();
  set_open_input();
}

double EnergyResolutionInput::resolution(double Etrue, double Erec, double RelSigma) const noexcept {
  static const double twopisqr = std::sqrt(2*pi);
  const double sigma = Etrue * RelSigma;
  const double reldiff = (Etrue - Erec)/sigma;

  return std::exp(-0.5*pow(reldiff, 2))/(twopisqr*sigma);
}

void EnergyResolutionInput::calcMatrix(FunctionArgs& fargs) {
  m_sparse_cache.setZero();

  auto& args = fargs.args;
  auto* edges = args[0].type.edges.data();
  auto& relsigmas = args[1].x;
  auto& ret = fargs.rets[0];
  size_t nbins = ret.type.shape[0];

  m_sparse_cache.resize(nbins, nbins);

  /* fill the cache matrix with probalilities for number of events to leak to other bins */
  /* colums corressponds to reconstrucred energy and rows to true energy */
  auto bin_center = [edges](size_t index){ return (edges[index+1] + edges[index])/2; };
  for (size_t etrue = 0; etrue < nbins; ++etrue) {
    auto Etrue   = bin_center(etrue);
    auto dEtrue  = edges[etrue+1] - edges[etrue];
    auto relsigma=relsigmas[etrue];

    bool right_edge_reached{false};
    /* precalculating probabilities for events in given bin to leak to
     * neighbor bins  */
    for (size_t erec = 0; erec < nbins; ++erec) {
      auto Erec = bin_center(erec);
      auto rEvents = dEtrue*resolution(Etrue, Erec, relsigma);

      if (rEvents < 1E-10) {
        if (right_edge_reached) {
           break;
        }
        continue;
      }
      m_sparse_cache.insert(erec, etrue) = rEvents;
      if (!right_edge_reached) {
        right_edge_reached = true;
      }
    }
  }
  m_sparse_cache.makeCompressed();

  if ( m_propagate_matrix )
    fargs.rets[0].mat = m_sparse_cache;
}


/*
compare the distribution from sampling result
with the original probabilistic distribution
by using Earth Mover's Distance(EMD).

Given two probability density function a and b, the EMD is calculated as follows:

  EMD(a, b) := integral_{x=-inf}^{+inf} { | A(x) - B(x) | }
where A and B is cumulative distribution function of a and b.

*/

#include <map>
#include <string>
#include <iostream>
#include <iomanip>
#include <cmath>
#include <numeric>
#include <algorithm>
#include <tuple>
#include <random>

#include <EigenRand/EigenRand>

template<typename ArrTy, typename PdfFn>
double calc_emd_with_pdf(ArrTy&& arr, const std::tuple<PdfFn, double, double>& cdf_range, size_t step)
{
	double ret = 0;
	auto arr_begin = arr.data();
	auto arr_end = arr_begin + arr.size();
	std::sort(arr_begin, arr_end);
	const auto& pdf = std::get<0>(cdf_range);
	const auto& lb = std::get<1>(cdf_range), &rb = std::get<2>(cdf_range);

	Eigen::ArrayXd cdf{ step };
	double acc = 0;
	for (size_t i = 0; i < step; ++i)
	{
		const double x = lb + (rb - lb) * i / step;
		cdf[i] = acc;
		acc += pdf(x);
	}
	cdf /= acc;

	size_t arr_p = 0;
	for (size_t i = 0; i < step; ++i)
	{
		const double x = lb + (rb - lb) * i / step;
		arr_p = std::find_if(arr_begin + arr_p, arr_end, [&](double a) { return a > x; }) - arr_begin;
		double arr_cum = (double)arr_p / arr.size();
		ret += std::abs(cdf[i] - arr_cum);
	}
	return ret * (rb - lb) / step;
}

template<typename ArrTy, typename CdfFn>
double calc_emd_with_cdf(ArrTy&& arr, const std::tuple<CdfFn, double, double>& cdf_range, size_t step)
{
	double ret = 0;
	auto arr_begin = arr.data();
	auto arr_end = arr_begin + arr.size();
	std::sort(arr_begin, arr_end);
	const auto& cdf = std::get<0>(cdf_range);
	const auto& lb = std::get<1>(cdf_range), &rb = std::get<2>(cdf_range);

	size_t arr_p = 0;
	for (size_t i = 0; i < step; ++i)
	{
		const double x = lb + (rb - lb) * i / step;
		arr_p = std::find_if(arr_begin + arr_p, arr_end, [&](double a) { return a > x; }) - arr_begin;
		double arr_cum = (double)arr_p / arr.size();
		ret += std::abs(cdf(x) - arr_cum);
	}
	return ret * (rb - lb) / step;
}

template<typename ArrTy, typename PmfFn>
double calc_kldiv(ArrTy&& arr, const std::tuple<PmfFn, int, int>& pmf_range)
{
	double ret = 0;
	const auto& pmf = std::get<0>(pmf_range);
	const auto& b = std::get<1>(pmf_range), e = std::get<2>(pmf_range);
	Eigen::ArrayXd p{ e - b + 1 }, q{ e - b + 1};
	p.setZero();
	for (int i = 0; i < arr.size(); ++i)
	{
		if (arr[i] < b || arr[i] > e) continue;
		p[arr[i] - b] += 1;
	}
	p /= arr.size();

	for (int i = b; i <= e; ++i) q[i - b] = pmf(i);
	q /= q.sum();

	return (((p / q) + 1e-12).log() * p).sum();
}

double gamma_dist(double x, double alpha, double beta)
{
	if (x <= 0) return 0;
	return std::exp(-x / beta)
		/ (std::tgamma(alpha) * std::pow(beta, alpha))
		* std::pow(x, alpha - 1);
}

double chisquared_dist(double x, double n)
{
	if (x <= 0) return 0;
	return std::pow(x, n / 2 - 1) * std::exp(-x / 2)
		/ std::pow(2, n / 2) / std::tgamma(n / 2);
}

double student_t_dist(double x, double n)
{
	return std::pow(1 + x * x /n, -(n + 1) / 2);
}

double fisher_f_dist(double x, double m, double n)
{
	if (x <= 0) return 0;
	return std::sqrt((std::pow(m * x, m) * std::pow(n, n)) / std::pow(m * x + n, m + n)) / x;
}

double combination(int n, int c)
{
	if (n == 0 || n == 1 || c == 0 || c == n) return 1;
	c = std::min(c, n - c);

	double ret = 1;
	for (int i = 0; i < c; ++i)
	{
		ret *= (n - i);
		ret /= i + 1;
	}
	return ret;
}

double binomial_dist(int x, int n, double p)
{
	return combination(n, x) * std::pow(p, x) * std::pow(1 - p, n - x);
}

double negative_binomial_dist(int x, int n, double p)
{
	return combination(n + x - 1, x) * std::pow(1 - p, x);
}

auto balanced_cdf = std::make_tuple([](double x) { return (x + 1) / 2; }, -1., 1.);
auto ur_cdf = std::make_tuple([](double x) { return x; }, 0., 1.);
auto normal_cdf = std::make_tuple([](double x) { return (1 + std::erf(x / std::sqrt(2))) / 2; }, -8., 8.);
auto lognormal_cdf = std::make_tuple([](double x) { return (1 + std::erf(std::log(x) / std::sqrt(2))) / 2; }, 0., 4.);
auto exp_cdf = std::make_tuple([](double x) { return 1 - std::exp(-x); }, 0., 32.);
auto weibull_cdf = std::make_tuple([](double x) { return 1 - std::exp(-std::pow(x, 2)); }, 0., 32.);
auto extreme_value_cdf = std::make_tuple([](double x) { return std::exp(-std::exp(1 - x)); }, -8., 24.);
auto gamma11_pdf = std::make_tuple([](double x) { return gamma_dist(x, 1, 1); }, 0., 16.);
auto gamma51_pdf = std::make_tuple([](double x) { return gamma_dist(x, 5, 1); }, 0., 16.);
auto gamma21_pdf = std::make_tuple([](double x) { return gamma_dist(x, 0.2, 1); }, 0., 16.);
auto chisquared_pdf = std::make_tuple([](double x) { return chisquared_dist(x, 7); }, 0., 64.);
auto cauchy_cdf = std::make_tuple([](double x) { return std::atan(x) / Eigen::Rand::constant::pi + 0.5; }, -16., 16.);
auto student5_pdf = std::make_tuple([](double x) { return student_t_dist(x, 5); }, -8., 8.);
auto student20_pdf = std::make_tuple([](double x) { return student_t_dist(x, 20); }, -8., 8.);
auto fisher11_cdf = std::make_tuple([](double x) { return 2 * std::atan(std::sqrt(x)) / Eigen::Rand::constant::pi; }, 0., 256.);
auto fisher55_pdf = std::make_tuple([](double x) { return fisher_f_dist(x, 5, 5); }, 0., 16.);

auto uniform10_pmf = std::make_tuple([](int x) { return 1; }, 0, 9);
auto discrete_pmf = std::make_tuple([](int x) { return x + 1; }, 0, 5);
auto poisson1_pmf = std::make_tuple([](int x) { return std::pow(1, x) / std::tgamma(x + 1); }, 0, 127);
auto poisson16_pmf = std::make_tuple([](int x) { return std::pow(16, x) / std::tgamma(x + 1); }, 0, 127);
auto binomial1_pmf = std::make_tuple([](int x) { return binomial_dist(x, 10, 0.5); }, 0, 10);
auto binomial2_pmf = std::make_tuple([](int x) { return binomial_dist(x, 30, 0.75); }, 0, 30);
auto binomial3_pmf = std::make_tuple([](int x) { return binomial_dist(x, 50, 0.25); }, 0, 50);
auto negbinomial1_pmf = std::make_tuple([](int x) { return negative_binomial_dist(x, 10, 0.5); }, 0, 127);
auto negbinomial2_pmf = std::make_tuple([](int x) { return negative_binomial_dist(x, 20, 0.25); }, 0, 127);
auto negbinomial3_pmf = std::make_tuple([](int x) { return negative_binomial_dist(x, 30, 0.75); }, 0, 127);
auto geometric25_pmf = std::make_tuple([](int x) { return std::pow(1 - 0.25, x); }, 0, 127);
auto geometric75_pmf = std::make_tuple([](int x) { return std::pow(1 - 0.75, x); }, 0, 127);

template<typename Rng>
std::map<std::string, double> test_eigenrand_cont(size_t size, size_t step, size_t seed)
{
	std::map<std::string, double> ret;
	Eigen::ArrayXf arr{ size };
	Rng urng{ seed };

	arr = Eigen::Rand::balancedLike(arr, urng);
	ret["balanced"] = calc_emd_with_cdf(arr, balanced_cdf, step);

	arr = Eigen::Rand::uniformRealLike(arr, urng);
	ret["uniformReal"] = calc_emd_with_cdf(arr, ur_cdf, step);

	arr = Eigen::Rand::normalLike(arr, urng);
	ret["normal"] = calc_emd_with_cdf(arr, normal_cdf, step);

	arr = Eigen::Rand::lognormalLike(arr, urng);
	ret["lognormal"] = calc_emd_with_cdf(arr, lognormal_cdf, step);

	arr = Eigen::Rand::gammaLike(arr, urng, 1, 1);
	ret["gamma(1,1)"] = calc_emd_with_pdf(arr, gamma11_pdf, step);

	arr = Eigen::Rand::gammaLike(arr, urng, 5, 1);
	ret["gamma(5,1)"] = calc_emd_with_pdf(arr, gamma51_pdf, step);

	arr = Eigen::Rand::gammaLike(arr, urng, 0.2, 1);
	ret["gamma(0.2,1)"] = calc_emd_with_pdf(arr, gamma21_pdf, step);

	arr = Eigen::Rand::exponentialLike(arr, urng);
	ret["exponential"] = calc_emd_with_cdf(arr, exp_cdf, step);

	arr = Eigen::Rand::weibullLike(arr, urng, 2);
	ret["weibull(2,1)"] = calc_emd_with_cdf(arr, weibull_cdf, step);

	arr = Eigen::Rand::extremeValueLike(arr, urng, 1, 1);
	ret["extremeValue(1,1)"] = calc_emd_with_cdf(arr, extreme_value_cdf, step);

	arr = Eigen::Rand::chiSquaredLike(arr, urng, 7);
	ret["chiSquared(7)"] = calc_emd_with_pdf(arr, chisquared_pdf, step);

	arr = Eigen::Rand::cauchyLike(arr, urng);
	ret["cauchy"] = calc_emd_with_cdf(arr, cauchy_cdf, step);

	arr = Eigen::Rand::studentTLike(arr, urng, 1);
	ret["studentT(1)"] = calc_emd_with_cdf(arr, cauchy_cdf, step);

	arr = Eigen::Rand::studentTLike(arr, urng, 5);
	ret["studentT(5)"] = calc_emd_with_pdf(arr, student5_pdf, step);

	arr = Eigen::Rand::studentTLike(arr, urng, 20);
	ret["studentT(20)"] = calc_emd_with_pdf(arr, student20_pdf, step);

	arr = Eigen::Rand::fisherFLike(arr, urng, 1, 1);
	ret["fisherF(1,1)"] = calc_emd_with_cdf(arr, fisher11_cdf, step);

	arr = Eigen::Rand::fisherFLike(arr, urng, 5, 5);
	ret["fisherF(5,5)"] = calc_emd_with_pdf(arr, fisher55_pdf, step);

#ifdef TEST_DOUBLE
	Eigen::ArrayXd arrd{ size };
	arrd = Eigen::Rand::uniformRealLike(arrd, urng);
	ret["uniformReal/double"] = calc_emd_with_cdf(arrd, ur_cdf, step);

	arrd = Eigen::Rand::normalLike(arrd, urng);
	ret["normal/double"] = calc_emd_with_cdf(arrd, normal_cdf, step);

	arrd = Eigen::Rand::lognormalLike(arrd, urng);
	ret["lognormal/double"] = calc_emd_with_cdf(arrd, lognormal_cdf, step);

	arrd = Eigen::Rand::gammaLike(arrd, urng, 1, 1);
	ret["gamma(1,1)/double"] = calc_emd_with_pdf(arrd, gamma11_pdf, step);

	arrd = Eigen::Rand::gammaLike(arrd, urng, 5, 1);
	ret["gamma(5,1)/double"] = calc_emd_with_pdf(arrd, gamma51_pdf, step);

	arrd = Eigen::Rand::gammaLike(arrd, urng, 0.2, 1);
	ret["gamma(0.2,1)/double"] = calc_emd_with_pdf(arrd, gamma21_pdf, step);

	arrd = Eigen::Rand::exponentialLike(arrd, urng);
	ret["exponential/double"] = calc_emd_with_cdf(arrd, exp_cdf, step);

	arrd = Eigen::Rand::weibullLike(arrd, urng, 2);
	ret["weibull(2,1)/double"] = calc_emd_with_cdf(arrd, weibull_cdf, step);

	arrd = Eigen::Rand::extremeValueLike(arrd, urng, 1, 1);
	ret["extremeValue(1,1)/double"] = calc_emd_with_cdf(arrd, extreme_value_cdf, step);

	arrd = Eigen::Rand::chiSquaredLike(arrd, urng, 7);
	ret["chiSquared(7)/double"] = calc_emd_with_pdf(arrd, chisquared_pdf, step);

	arrd = Eigen::Rand::cauchyLike(arrd, urng);
	ret["cauchy/double"] = calc_emd_with_cdf(arrd, cauchy_cdf, step);

	arrd = Eigen::Rand::studentTLike(arrd, urng, 1);
	ret["studentT(1)/double"] = calc_emd_with_cdf(arrd, cauchy_cdf, step);

	arrd = Eigen::Rand::studentTLike(arrd, urng, 5);
	ret["studentT(5)/double"] = calc_emd_with_pdf(arrd, student5_pdf, step);

	arrd = Eigen::Rand::studentTLike(arrd, urng, 20);
	ret["studentT(20)/double"] = calc_emd_with_pdf(arrd, student20_pdf, step);

	arrd = Eigen::Rand::fisherFLike(arrd, urng, 1, 1);
	ret["fisherF(1,1)/double"] = calc_emd_with_cdf(arrd, fisher11_cdf, step);

	arrd = Eigen::Rand::fisherFLike(arrd, urng, 5, 5);
	ret["fisherF(5,5)/double"] = calc_emd_with_pdf(arrd, fisher55_pdf, step);
#endif
	return ret;
}

template<typename Rng>
std::map<std::string, double> test_eigenrand_disc(size_t size, size_t step, size_t seed)
{
	std::map<std::string, double> ret;
	Eigen::ArrayXi arri{ size };
	Rng urng{ seed };

	arri = Eigen::Rand::uniformIntLike(arri, urng, 0, 9);
	ret["uniformInt(0,9)"] = calc_kldiv(arri, uniform10_pmf);

	arri = Eigen::Rand::discreteLike(arri, urng, { 1, 2, 3, 4, 5, 6 });
	ret["discrete(1,2,3,4,5,6)"] = calc_kldiv(arri, discrete_pmf);

	arri = Eigen::Rand::poissonLike(arri, urng, 1);
	ret["poisson(1)"] = calc_kldiv(arri, poisson1_pmf);

	arri = Eigen::Rand::poissonLike(arri, urng, 16);
	ret["poisson(16)"] = calc_kldiv(arri, poisson16_pmf);

	arri = Eigen::Rand::binomialLike(arri, urng, 10, 0.5);
	ret["binomial(10,0.5)"] = calc_kldiv(arri, binomial1_pmf);

	arri = Eigen::Rand::binomialLike(arri, urng, 30, 0.75);
	ret["binomial(30,0.75)"] = calc_kldiv(arri, binomial2_pmf);

	arri = Eigen::Rand::binomialLike(arri, urng, 50, 0.25);
	ret["binomial(50,0.25)"] = calc_kldiv(arri, binomial3_pmf);

	arri = Eigen::Rand::negativeBinomialLike(arri, urng, 10, 0.5);
	ret["negativeBinomial(10,0.5)"] = calc_kldiv(arri, negbinomial1_pmf);

	arri = Eigen::Rand::negativeBinomialLike(arri, urng, 20, 0.25);
	ret["negativeBinomial(20,0.25)"] = calc_kldiv(arri, negbinomial2_pmf);

	arri = Eigen::Rand::negativeBinomialLike(arri, urng, 30, 0.75);
	ret["negativeBinomial(30,0.75)"] = calc_kldiv(arri, negbinomial3_pmf);

	arri = Eigen::Rand::geometricLike(arri, urng, 0.25);
	ret["geometric(0.25)"] = calc_kldiv(arri, geometric25_pmf);

	arri = Eigen::Rand::geometricLike(arri, urng, 0.75);
	ret["geometric(0.75)"] = calc_kldiv(arri, geometric75_pmf);
	return ret;
}

std::map<std::string, double> test_old(size_t size, size_t step, size_t seed)
{
	std::map<std::string, double> ret;
	Eigen::ArrayXf arr{ size };

	arr = Eigen::ArrayXf::Random(size);
	ret["balanced"] = calc_emd_with_cdf(arr, balanced_cdf, step);
	return ret;
}

std::map<std::string, double> test_cpp11_cont(size_t size, size_t step, size_t seed)
{
	std::map<std::string, double> ret;
	Eigen::ArrayXf arr{ size };
	Eigen::ArrayXd arrd{ size };
	std::mt19937_64 urng{ seed };

	{
		std::uniform_real_distribution<float> dist;
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["uniformReal"] = calc_emd_with_cdf(arr, ur_cdf, step);

	{
		std::uniform_real_distribution<double> dist;
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["uniformReal/double"] = calc_emd_with_cdf(arrd, ur_cdf, step);

	{
		std::normal_distribution<> dist;
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["normal"] = calc_emd_with_cdf(arr, normal_cdf, step);

	{
		std::normal_distribution<double> dist;
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["normal/double"] = calc_emd_with_cdf(arrd, normal_cdf, step);

	{
		std::lognormal_distribution<> dist;
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["lognormal"] = calc_emd_with_cdf(arr, lognormal_cdf, step);

	{
		std::lognormal_distribution<double> dist;
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["lognormal/double"] = calc_emd_with_cdf(arrd, lognormal_cdf, step);

	{
		std::gamma_distribution<> dist{ 1, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(1,1)"] = calc_emd_with_pdf(arr, gamma11_pdf, step);

	{
		std::gamma_distribution<double> dist{ 1, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(1,1)/double"] = calc_emd_with_pdf(arrd, gamma11_pdf, step);

	{
		std::gamma_distribution<> dist{ 5, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(5,1)"] = calc_emd_with_pdf(arr, gamma51_pdf, step);

	{
		std::gamma_distribution<double> dist{ 5, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(5,1)/double"] = calc_emd_with_pdf(arrd, gamma51_pdf, step);

	{
		std::gamma_distribution<> dist{ 0.2, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(0.2,1)"] = calc_emd_with_pdf(arr, gamma21_pdf, step);

	{
		std::gamma_distribution<double> dist{ 0.2, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["gamma(0.2,1)/double"] = calc_emd_with_pdf(arrd, gamma21_pdf, step);

	{
		std::exponential_distribution<> dist;
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["exponential"] = calc_emd_with_cdf(arr, exp_cdf, step);

	{
		std::exponential_distribution<double> dist;
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["exponential/double"] = calc_emd_with_cdf(arrd, exp_cdf, step);

	{
		std::weibull_distribution<> dist{ 2, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["weibull(2,1)"] = calc_emd_with_cdf(arr, weibull_cdf, step);

	{
		std::weibull_distribution<double> dist{ 2, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["weibull(2,1)/double"] = calc_emd_with_cdf(arrd, weibull_cdf, step);

	{
		std::extreme_value_distribution<> dist{ 1, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["extremeValue(1,1)"] = calc_emd_with_cdf(arr, extreme_value_cdf, step);

	{
		std::extreme_value_distribution<double> dist{ 1, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["extremeValue(1,1)/double"] = calc_emd_with_cdf(arrd, extreme_value_cdf, step);

	{
		std::chi_squared_distribution<> dist{ 7 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["chiSquared(7)"] = calc_emd_with_pdf(arr, chisquared_pdf, step);

	{
		std::chi_squared_distribution<double> dist{ 7 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["chiSquared(7)/double"] = calc_emd_with_pdf(arrd, chisquared_pdf, step);

	{
		std::cauchy_distribution<> dist{ 0, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["cauchy"] = calc_emd_with_cdf(arr, cauchy_cdf, step);

	{
		std::cauchy_distribution<double> dist{ 0, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["cauchy/double"] = calc_emd_with_cdf(arrd, cauchy_cdf, step);

	{
		std::student_t_distribution<> dist{ 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(1)"] = calc_emd_with_cdf(arr, cauchy_cdf, step);

	{
		std::student_t_distribution<double> dist{ 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(1)/double"] = calc_emd_with_cdf(arrd, cauchy_cdf, step);

	{
		std::student_t_distribution<> dist{ 5 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(5)"] = calc_emd_with_pdf(arr, student5_pdf, step);

	{
		std::student_t_distribution<double> dist{ 5 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(5)/double"] = calc_emd_with_pdf(arrd, student5_pdf, step);

	{
		std::student_t_distribution<> dist{ 20 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(20)"] = calc_emd_with_pdf(arr, student20_pdf, step);

	{
		std::student_t_distribution<double> dist{ 20 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["studentT(20)/double"] = calc_emd_with_pdf(arrd, student20_pdf, step);

	{
		std::fisher_f_distribution<> dist{ 1, 1 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["fisherF(1,1)"] = calc_emd_with_cdf(arr, fisher11_cdf, step);

	{
		std::fisher_f_distribution<double> dist{ 1, 1 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["fisherF(1,1)/double"] = calc_emd_with_cdf(arrd, fisher11_cdf, step);

	{
		std::fisher_f_distribution<> dist{ 5, 5 };
		arr = Eigen::ArrayXf::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["fisherF(5,5)"] = calc_emd_with_pdf(arr, fisher55_pdf, step);

	{
		std::fisher_f_distribution<double> dist{ 5, 5 };
		arrd = Eigen::ArrayXd::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["fisherF(5,5)/double"] = calc_emd_with_pdf(arrd, fisher55_pdf, step);
	return ret;
}

std::map<std::string, double> test_cpp11_disc(size_t size, size_t step, size_t seed)
{
	std::map<std::string, double> ret;
	Eigen::ArrayXi arri{ size };
	std::mt19937_64 urng{ seed };

	{
		std::uniform_int_distribution<> dist{ 0, 9 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["uniformInt(0,9)"] = calc_kldiv(arri, uniform10_pmf);

	{
		std::discrete_distribution<> dist{ 1, 2, 3, 4, 5, 6 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["discrete(1,2,3,4,5,6)"] = calc_kldiv(arri, discrete_pmf);

	{
		std::poisson_distribution<> dist{ 1 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["poisson(1)"] = calc_kldiv(arri, poisson1_pmf);

	{
		std::poisson_distribution<> dist{ 16 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["poisson(16)"] = calc_kldiv(arri, poisson16_pmf);

	{
		std::binomial_distribution<> dist{ 10, 0.5 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["binomial(10,0.5)"] = calc_kldiv(arri, binomial1_pmf);

	{
		std::binomial_distribution<> dist{ 30, 0.75 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["binomial(30,0.75)"] = calc_kldiv(arri, binomial2_pmf);

	{
		std::binomial_distribution<> dist{ 50, 0.25 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["binomial(50,0.25)"] = calc_kldiv(arri, binomial3_pmf);

	{
		std::negative_binomial_distribution<> dist{ 10, 0.5 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["negativeBinomial(10,0.5)"] = calc_kldiv(arri, negbinomial1_pmf);

	{
		std::negative_binomial_distribution<> dist{ 20, 0.25 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["negativeBinomial(20,0.25)"] = calc_kldiv(arri, negbinomial2_pmf);

	{
		std::negative_binomial_distribution<> dist{ 30, 0.75 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["negativeBinomial(30,0.75)"] = calc_kldiv(arri, negbinomial3_pmf);

	{
		std::geometric_distribution<> dist{ 0.25 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["geometric(0.25)"] = calc_kldiv(arri, geometric25_pmf);

	{
		std::geometric_distribution<> dist{ 0.75 };
		arri = Eigen::ArrayXi::NullaryExpr(size, [&]() { return dist(urng); });
	}
	ret["geometric(0.75)"] = calc_kldiv(arri, geometric75_pmf);
	return ret;
}

int main(int argc, char** argv)
{
	size_t size = 32768, step = size * 8, repeat = 50;

	if (argc > 1) size = std::stoi(argv[1]);
	if (argc > 2) step = std::stoi(argv[2]);

	std::map<std::string, double> err, errSq, kl, klSq;

	for (size_t i = 0; i < repeat; ++i)
	{
		std::cout << "Repeat " << i << " ..." << std::endl;
		for (auto& p : test_eigenrand_cont<std::mt19937_64>(size, step, 42 * i))
		{
			err[p.first + "\t:EigenRand"] += p.second;
			errSq[p.first + "\t:EigenRand"] += p.second * p.second;
		}

		for (auto& p : test_eigenrand_disc<std::mt19937_64>(size, step, 42 * i))
		{
			kl[p.first + "\t:EigenRand"] += p.second;
			klSq[p.first + "\t:EigenRand"] += p.second * p.second;
		}

#if defined(EIGEN_VECTORIZE_SSE2) || defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_NEON)
		for (auto& p : test_eigenrand_cont<Eigen::Rand::Vmt19937_64>(size, step, 42 * i))
		{
			err[p.first + "\t:ERand+Vrng"] += p.second;
			errSq[p.first + "\t:ERand+Vrng"] += p.second * p.second;
		}

		for (auto& p : test_eigenrand_disc<Eigen::Rand::Vmt19937_64>(size, step, 42 * i))
		{
			kl[p.first + "\t:ERand+Vrng"] += p.second;
			klSq[p.first + "\t:ERand+Vrng"] += p.second * p.second;
		}
#endif
#ifndef SKIP_REFERENCE
		for (auto& p : test_cpp11_cont(size, step, 42 * i))
		{
			err[p.first + "\t:C++11"] += p.second;
			errSq[p.first + "\t:C++11"] += p.second * p.second;
		}

		for (auto& p : test_cpp11_disc(size, step, 42 * i))
		{
			kl[p.first + "\t:C++11"] += p.second;
			klSq[p.first + "\t:C++11"] += p.second * p.second;
		}

		for (auto& p : test_old(size, step, 42 * i))
		{
			err[p.first + "\t:Old"] += p.second;
			errSq[p.first + "\t:Old"] += p.second * p.second;
		}
#endif
	}

	std::cout << "[Earth Mover's Distance] Mean (Stdev)" << std::endl;
	for (auto& p : err)
	{
		double mean = p.second / repeat;
		double var = (errSq[p.first] / repeat) - mean * mean;
		size_t sp = p.first.find('\t');
		std::cout << std::left << std::setw(28) << p.first.substr(0, sp);
		std::cout << std::setw(14) << p.first.substr(sp + 1);
		std::cout << ": " << mean << " (" << std::sqrt(var) << ")" << std::endl;
	}
	std::cout << std::endl;
	std::cout << "[KL Divergence] Mean (Stdev)" << std::endl;
	for (auto& p : kl)
	{
		double mean = p.second / repeat;
		double var = (klSq[p.first] / repeat) - mean * mean;
		size_t sp = p.first.find('\t');
		std::cout << std::left << std::setw(28) << p.first.substr(0, sp);
		std::cout << std::setw(14) << p.first.substr(sp + 1);
		std::cout << ": " << mean << " (" << std::sqrt(var) << ")" << std::endl;
	}
	std::cout << std::endl;
	return 0;
}


#ifndef _ECO_UTIL_HPP_
#define _ECO_UTIL_HPP_

#include <iostream>
#include <string>
#include <math.h>
#include <algorithm>
#include <complex>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "ffttools.hpp"
#include "matrix_operator.hpp"

namespace eco_tracker {

typedef std::vector<std::vector<Eigen::MatrixXcf> > EcoFeats;

EcoFeats runFFt(const EcoFeats &xlw);

void genGaussianYf(
    const float& sigma_y,
    const float& T_size,
    const std::vector<Eigen::MatrixXcf>& k_x,
    const std::vector<Eigen::MatrixXcf>& k_y,
    std::vector<Eigen::MatrixXcf>& y_f);

void genCosWindow(
    const int& row,
    const int& col,
    Eigen::MatrixXcf& cos_window);

EcoFeats initInterpolateFFt(
    const EcoFeats& xlf,
    const std::vector<Eigen::MatrixXcf>& interp1_fs,
    const std::vector<Eigen::MatrixXcf>& interp2_fs);

EcoFeats interpolateDFT(
    const EcoFeats& xlf,
    const std::vector<Eigen::MatrixXcf>& interp1_fs,
    const std::vector<Eigen::MatrixXcf>& interp2_fs);

EcoFeats computeFeautrePower(const EcoFeats &feats);

EcoFeats compactFourierCoeff(const EcoFeats &xf);

EcoFeats fullFourierCoeff(const EcoFeats &xf);

std::vector<Eigen::MatrixXcf> vectorFeature(const EcoFeats &x);

void initProjectionMatrix(
    const EcoFeats& init_sample,
	const std::vector<int>& compressed_dim,
    std::vector<Eigen::MatrixXcf>& project_matrix);

EcoFeats projectFeature(
    const EcoFeats& x,
    const std::vector<Eigen::MatrixXcf>& project_matrix);

EcoFeats projectFeatureMultScale(
    const EcoFeats& x,
    const std::vector<Eigen::MatrixXcf>& project_matrix);

EcoFeats EcoFeatureDotDivide(
    const EcoFeats &a,
    const EcoFeats &b);

std::complex<float> EcoFeatInnerProduct(
    const EcoFeats& f1,
    const EcoFeats& f2);

EcoFeats shiftSample(EcoFeats &xf,
                     float x,
                     float y,
                     std::vector<Eigen::MatrixXcf> kx,
                     std::vector<Eigen::MatrixXcf> ky);

std::vector<Eigen::MatrixXcf> getProjectMatEnergy(
    const std::vector<Eigen::MatrixXcf> project_mat,
    const std::vector<int>& feature_dim,
	const std::vector<Eigen::MatrixXcf>& yf);

} // namespace eco_tracker
#endif


#include <armadillo>
#include <iostream>

using namespace arma;

int main(int argc, char *argv[]) {
    if (argc < 2) {
        std::cerr << "# error: no file specified" << std::endl;
        return 1;
    }
    mat A;
    A.load(argv[1], raw_ascii);
    A.print("A:");
    std::cout << "det(A) = " << det(A) << std::endl;
    return 0;
}


#include <iostream>
#include <cmath>

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

#include <sophus/so3.h>
#include <sophus/se3.h>

int main(int argc, char** argv) {
  Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();

  Sophus::SO3 SO3_R(R);

  Sophus::SO3 SO3_V(0, 0, M_PI / 2);

  Eigen::Quaterniond Q(R);
  Sophus::SO3 SO3_Q(Q);

  std::cout << "SO3 from matrix: " << SO3_R << std::endl;
  std::cout << "SO3 from vector: " << SO3_V << std::endl;
  std::cout << "SO3 from quaternion: " << SO3_Q << std::endl;

  Eigen::Vector3d so3 = SO3_R.log();
  std::cout << "so3 = " << so3.transpose() << std::endl;

  std::cout << "so3 hat = \n" << Sophus::SO3::hat(so3) << std::endl;

  std::cout << "so3 hat vee = " << Sophus::SO3::vee(Sophus::SO3::hat(so3)) << std::endl;

  Eigen::Vector3d update_so3(1e-4, 0, 0);
  Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3) * SO3_R;
  std::cout << "SO3 updated = " << SO3_updated << std::endl;

  std::cout << "op SE3" << std::endl;

  Eigen::Vector3d t(1, 0, 0);
  Sophus::SE3 SE3_Rt(R, t);
  Sophus::SE3 SE3_Qt(Q, t);

  std::cout << "SE3 from R, t = \n" << SE3_Rt << std::endl;
  std::cout << "SE3 from Q, t = \n" << SE3_Qt << std::endl;

  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();

  std::cout << "se3 = \n" << se3.transpose() << std::endl;

  std::cout << "se3 hat = \n" << Sophus::SE3::hat(se3) << std::endl;
  std::cout << "se3 hat vee = \n" <<
            Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose() << std::endl;

  Vector6d update_se3;
  update_se3.setZero();
  update_se3(0, 0) = 1e-4d;
  Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3) * SE3_Rt;

  std::cout << "SE3 updated = \n" << SE3_updated.matrix() << std::endl;

  return 0;
}


///////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2020 - 2021.                 //
//  Distributed under the Boost Software License,                //
//  Version 1.0. (See accompanying file LICENSE_1_0.txt          //
//  or copy at http://www.boost.org/LICENSE_1_0.txt)             //
///////////////////////////////////////////////////////////////////

#include <cmath>

#if defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

#if defined(__clang__) && !defined(__APPLE__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/bindings/decwide_t.hpp>

#include <math/wide_decimal/decwide_t_examples.h>

namespace
{
  constexpr std::uint32_t wide_decimal_digits10 = UINT32_C(1001);

  using dec1001_t = math::wide_decimal::decwide_t<wide_decimal_digits10>;
}

bool math::wide_decimal::example009_boost_math_standalone()
{
  const dec1001_t x = dec1001_t(UINT32_C(123456789)) / 100U;

  using std::cbrt;
  using std::fabs;

  // Compare wide-decimal's cube root function with that of Boost.Math.
  // Also exercise several different interpretations of the constant pi.

  const dec1001_t c       = cbrt(x / math::wide_decimal::pi<wide_decimal_digits10, typename dec1001_t::limb_type, std::allocator<void>, double>());

  const dec1001_t control = boost::math::cbrt(x / boost::math::constants::pi<dec1001_t>());

  const dec1001_t closeness = fabs(1 - (c / control));

  const bool result_is_ok = closeness < (std::numeric_limits<dec1001_t>::epsilon() * 10);

  return result_is_ok;
}

// Enable this if you would like to activate this main() as a standalone example.
#if 0

#include <iomanip>
#include <iostream>

int main()
{
  const bool result_is_ok = math::wide_decimal::example009_boost_math_standalone();

  std::cout << "result_is_ok: " << std::boolalpha << result_is_ok << std::endl;
}

#endif

#if defined(__clang__) && !defined(__APPLE__)
#pragma GCC diagnostic pop
#endif

#if defined(__GNUC__)
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#endif


#include <Eigen/Dense>
#include <Eigen/LU>

#include <iostream>
#include <iomanip>

#include "timer.h"

// Models a resistence between node i,j with i < j (could, in principle, handle different resistences)
typedef std::pair< int, int>            resistor;
// Vector containing all resistences in the circuit (excluded the variable one, which is modelled afterwards)
typedef std::vector< resistor >         resistor_topology;
// Models a ground or source of voltage, indexes store the node connected to the ground/source through a resistence
typedef std::tuple<int, int, double>    voltage;
// Vector containing a voltage object for each node connected to sink or source.
typedef std::vector< voltage >          voltage_topology;

//! \brief Class implementing the topology of the circuit (cf. Figure)
//! Computes impedance of the entire circuit (between node 16 and 17) exploiting
//! the SMW formula for the inversion of low rank perturbations of a matrix A0,
//! whose factorization in known in advance
template <class Matrix>
class ImpedanceMap {
    static const std::size_t nnodes = 15; //< Compile time hard-coded size of circuit
public:
    //! \brief Constructor: Builds system matrix and rhs and performs lu decomposition
    //! The lu decomposition is stored in lu and can be reused in the SMW formula
    //! to avoid expensive matrix solves for repeated usages of the operator()
    //! \param R_ resistence R
    //! \param W_ source voltage W at node 16, gound is 0 at node 17
    ImpedanceMap(double R_, double W_) : R(R_), W(W_) {
        // In the following, instead of specifying directly the entries of A\_0 and of rhs
        // we define some auxiliary structure and automatically generate the right entries for
        // the structures we specified.
        // This way: we avoid silly mistakes, avoid a very long list of matrix entreis specifications,
        // have a flexible way (we may easily change the topology) and automatically take care of shifting the indices
        // Of course, if you want, you can manually put each entry in the matrix and rhs. The result (should) be the same)

        // We implement the topology of the resistances by pushing each
        // Resistance between node i < j in a std::vector of pair (i,j) \in \mathbb{N}^2
        // This way we can automatically build a symmetric matrix, take care of indexing
        // and avoid dumb mistakes during the matrix filling
        resistor_topology T;
        T.reserve(23);
        T.push_back(resistor(1,2));
        T.push_back(resistor(1,5));
        T.push_back(resistor(2,5));
        T.push_back(resistor(2,3));
        T.push_back(resistor(2,14));
        T.push_back(resistor(3,4));
        T.push_back(resistor(3,15));
        T.push_back(resistor(4,6));
        T.push_back(resistor(4,15));
        T.push_back(resistor(5,7));
        T.push_back(resistor(5,14));
        T.push_back(resistor(6,9));
        T.push_back(resistor(6,15));
        T.push_back(resistor(7,10));
        T.push_back(resistor(7,11));
        T.push_back(resistor(8,9));
        T.push_back(resistor(8,12));
        T.push_back(resistor(8,13));
        T.push_back(resistor(8,15));
        T.push_back(resistor(9,13));
        T.push_back(resistor(10,11));
        T.push_back(resistor(11,12));
        T.push_back(resistor(12,13));
        // Default matrix entries sert to one (varistor)
        T.push_back(resistor(14,15));

        // We implement voltage gound and source topology by specifiying which node is connected to ground/source
        // trough a resistance. This will be also part of rhs
        voltage_topology S;
        S.reserve(4);
        S.push_back(voltage(6, 16, W));
        S.push_back(voltage(7, 17, 0));
        S.push_back(voltage(11,17, 0));
        S.push_back(voltage(14,17, 0));

        // Automatically build A0 filling from topology
        Matrix A0 = Matrix::Zero(nnodes, nnodes);
        for(auto it: T) {
            // Shift indeces down by 1
            auto i = it.first - 1;
            auto j = it.second - 1;
            // Fill diagonal: each resistance contributes += R into the diagonal of both nodes
            A0(i,i) += 1;
            A0(j,j) += 1;
            // Fill the off diagonal (negative part in \Delta W_{i,j}, the matrix is kept symmetric
            A0(i,j) -= 1;
            A0(j,i) -= 1;
        }

        // Fill in the rest (source and ground), i.e. components with $\Delta W_{i,j}$ with j > 15
        // Each node i connected to ground or source contributes to the rhs with R * W (R resistence between node i and ground/source
        // node, W is voltage at sink or source) and to its own diagonal with R
        rhs = Matrix::Zero(nnodes,1);
        for(auto it: S) {
            // Shift index down by 1 and get voltage in W2
            int i = std::get<0>(it) - 1;
            auto W2 = std::get<2>(it);
            // Add voltage in rhs (resistance assumed to be R)
            rhs(i) += W2;
            // Add resistance to matrix diagonal: contribution of source current
            A0(i,i) += 1;
        }

        // Precompute lu factorizaion of A0
        lu = A0.lu();
    };

    //! \brief Compute the impedance given the resistence variable R_x
    //! Use SMW formula for low rank perturbations to avoid expensive inversion
    //! if factorization of the base system matrix is already known
    //! \param Rx resistence R_x > 0 of the varistor between node 14 and 15
    //! \return impedance = W  * I of the system A(R_x)
    double operator()(double Rx) {
        // Store the scaled factor for convenience
        double fac = R/Rx;

        // There are many ways to create the same matrix u*v
        // Create U, the 15x2 matrix in (A+UV)
        Matrix U = Matrix::Zero(nnodes, 1);
        U(13) = -1;
        U(14) = 1;
        // V will be U tranps

        //// Use SMW formula to compute (A + UU')^{-1} rhs
        // Formula is A^{-1} rhs - A^{-1} * u * (I + V*A^{-1}*U)^{-1} V * A^{-1}

        // Start by precomputing A^{-1} rhs (needed twice), column vector of length 15
        auto Ainvrhs = lu.solve(rhs);
        // The precompute A^{-1} U (15x2 matrix), needed twice
        auto Ainvu = lu.solve(U);
        // The compute alpha, 2x2 matrix whose inverse is cheap
        auto alpha = (Matrix::Identity(2,2) + U.transpose() / -1 * (1-fac) * Ainvu).inverse();
        // Put the formula toghether, x is a 15x1 column vector containing voltages at each node (except 16,17, prescribed)
        auto x = Ainvrhs - Ainvu * alpha * U.transpose() / -1 * (1-fac) * Ainvrhs;

        // Compute the current I = \Delta W_{16,5} / R and then impedance = W / I
        // \Delta W_{16,5} = (W - x_5)
        return W * R / (W - x(5));
    };
private:
    Eigen::PartialPivLU< Matrix > lu; //< Store lu decomposition of a for efficiency
    double R, W; //< Resistance R and source voltage W
    Matrix rhs; //< Store rhs vector prescribing sink and source voltages
};

int main(void) {
    ImpedanceMap<Eigen::MatrixXd> IM = ImpedanceMap<Eigen::MatrixXd>(1, 1);
    std::cout << "Impedance [R = 1]: " << IM(1) << std::endl;

    std::cout << std::setw(30) << "Impedance [Ohm]" << std::setw(30) << "R_x [Ohm]" << std::endl;
    std::cout << std::setw(30) << IM(0)             << std::setw(30) << " " << 0 << std::endl;
    std::cout << std::setw(30) << IM(0.1)           << std::setw(30) << " " << 0.1 << std::endl;
    for(auto Rx = 1; Rx <= 1024; Rx *= 2) {
        std::cout << std::setw(30) << IM(Rx)        << std::setw(30) << " " << Rx << std::endl;
    }

    timer<> tmr;
    tmr.start();
    ImpedanceMap<Eigen::MatrixXd> IM2 = ImpedanceMap<Eigen::MatrixXd>(1, 1);
    IM2(1);
    tmr.stop();
    std::cout << "Took:             " << tmr.min().count() / 1000000. << " ms" << std::endl;

}

Nummerical Methods for CSE/PS3/solutions_ps3/impedancemap.cpp


#include <iostream>
#include <string>
#include <vector>
#include <Eigen/Core>
#include <QApplication>
#include <visoptslider/visoptslider.hpp>
#include <optimization-test-functions.hpp>

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

    // Define the target function and bound
    constexpr int num_dimensions = 3;
    constexpr auto target_function = [](const Eigen::VectorXd& x)
    {
        return otf::GetValue(x, otf::FunctionType::Rosenbrock);
    };
    const Eigen::Vector3d upper_bound(+ 2.0, + 2.0, + 2.0);
    const Eigen::Vector3d lower_bound(- 2.0, - 2.0, - 2.0);
    constexpr double maximum_value = 200.0;
    constexpr double minimum_value = 0.0;

    // Optional settings
    const std::vector<std::string> labels = { "x1", "x2", "x3" };
    constexpr bool show_values = true;
    constexpr int resolution = 200;

    // Instantiate and initialize the widget
    visopt::SlidersWidget sliders_widget;
    sliders_widget.initialize(num_dimensions,
                              target_function,
                              upper_bound,
                              lower_bound,
                              maximum_value,
                              minimum_value,
                              labels,
                              show_values,
                              resolution);

    // Set a callback function
    sliders_widget.setCallback([&sliders_widget]()
    {
        std::cout << sliders_widget.getArgument().transpose() << std::endl;
    });

    // Show the widget
    sliders_widget.show();
    return app.exec();
}


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

#ifndef BOOST_MATH_INTERPOLATORS_PCHIP_HPP
#define BOOST_MATH_INTERPOLATORS_PCHIP_HPP
#include <memory>
#include <boost/math/interpolators/detail/cubic_hermite_detail.hpp>

namespace boost::math::interpolators {

template<class RandomAccessContainer>
class pchip {
public:
    using Real = typename RandomAccessContainer::value_type;

    pchip(RandomAccessContainer && x, RandomAccessContainer && y,
          Real left_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN(),
          Real right_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN())
    {
        if (x.size() < 4)
        {
            throw std::domain_error("Must be at least four data points.");
        }
        RandomAccessContainer s(x.size(), std::numeric_limits<Real>::quiet_NaN());
        if (isnan(left_endpoint_derivative))
        {
            // O(h) finite difference derivative:
            // This, I believe, is the only derivative guaranteed to be monotonic:
            s[0] = (y[1]-y[0])/(x[1]-x[0]);
        }
        else
        {
            s[0] = left_endpoint_derivative;
        }

        for (decltype(s.size()) k = 1; k < s.size()-1; ++k) {
            Real hkm1 = x[k] - x[k-1];
            Real dkm1 = (y[k] - y[k-1])/hkm1;

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

        }
        // Quadratic extrapolation at the other end:
        auto n = s.size();
        if (isnan(right_endpoint_derivative))
        {
            s[n-1] = (y[n-1]-y[n-2])/(x[n-1] - x[n-2]);
        }
        else
        {
            s[n-1] = right_endpoint_derivative;
        }
        impl_ = std::make_shared<detail::cubic_hermite_detail<RandomAccessContainer>>(std::move(x), std::move(y), std::move(s));
    }

    Real operator()(Real x) const {
        return impl_->operator()(x);
    }

    Real prime(Real x) const {
        return impl_->prime(x);
    }

    friend std::ostream& operator<<(std::ostream & os, const pchip & m)
    {
        os << *m.impl_;
        return os;
    }

    void push_back(Real x, Real y) {
        using std::abs;
        using std::isnan;
        if (x <= impl_->x_.back()) {
             throw std::domain_error("Calling push_back must preserve the monotonicity of the x's");
        }
        impl_->x_.push_back(x);
        impl_->y_.push_back(y);
        impl_->dydx_.push_back(std::numeric_limits<Real>::quiet_NaN());
        auto n = impl_->size();
        impl_->dydx_[n-1] = (impl_->y_[n-1]-impl_->y_[n-2])/(impl_->x_[n-1] - impl_->x_[n-2]);
        // Now fix s_[n-2]:
        auto k = n-2;
        Real hkm1 = impl_->x_[k] - impl_->x_[k-1];
        Real dkm1 = (impl_->y_[k] - impl_->y_[k-1])/hkm1;

        Real hk = impl_->x_[k+1] - impl_->x_[k];
        Real dk = (impl_->y_[k+1] - impl_->y_[k])/hk;
        Real w1 = 2*hk + hkm1;
        Real w2 = hk + 2*hkm1;
        if ( (dk > 0 && dkm1 < 0) || (dk < 0 && dkm1 > 0) || dk == 0 || dkm1 == 0)
        {
            impl_->dydx_[k] = 0;
        }
        else
        {
            impl_->dydx_[k] = (w1+w2)/(w1/dkm1 + w2/dk);
        }
    }

private:
    std::shared_ptr<detail::cubic_hermite_detail<RandomAccessContainer>> impl_;
};

}
#endif


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <functional>
#include <mutex>
#include <Eigen/Geometry>

#ifndef SIMPLE_DTW_HPP
#define SIMPLE_DTW_HPP

namespace simple_dtw
{
    template<typename FirstDatatype, typename SecondDatatype, typename DistanceFn,
             typename FirstAllocator = std::allocator<FirstDatatype>,
             typename SecondAllocator = std::allocator<SecondDatatype>>
    class SimpleDTW
    {
    private:

        void InitializeMatrix(const size_t first_sequence_size, const size_t second_sequence_size)
        {
            const ssize_t rows = (ssize_t)first_sequence_size + 1;
            const ssize_t cols = (ssize_t)second_sequence_size + 1;
            if (dtw_matrix_.rows() < rows || dtw_matrix_.cols() < cols)
            {
                dtw_matrix_ = Eigen::MatrixXd::Zero(rows, cols);
                if (rows > 1 && cols > 1)
                {
                    for (ssize_t row = 1; row < rows; row++)
                    {
                        dtw_matrix_(row, 0) = std::numeric_limits<double>::infinity();
                    }
                    for (ssize_t col = 1; col < cols; col++)
                    {
                        dtw_matrix_(0, col) = std::numeric_limits<double>::infinity();
                    }
                }
            }

        }

        Eigen::MatrixXd dtw_matrix_;

    public:

        SimpleDTW()
        {
            InitializeMatrix(0, 0);
        }

        SimpleDTW(const size_t first_sequence_size, const size_t second_sequence_size)
        {
            InitializeMatrix(first_sequence_size, second_sequence_size);
        }

        double EvaluateWarpingCost(
                const std::vector<FirstDatatype, FirstAllocator>& first_sequence,
                const std::vector<SecondDatatype, SecondAllocator>& second_sequence,
                const DistanceFn& distance_fn)
        {
            InitializeMatrix(first_sequence.size(), second_sequence.size());
            //Compute DTW cost for the two sequences
            for (ssize_t i = 1; i <= (ssize_t)first_sequence.size(); i++)
            {
                const FirstDatatype& first_item = first_sequence[(size_t)i - 1];
                for (ssize_t j = 1; j <= (ssize_t)second_sequence.size(); j++)
                {
                    const SecondDatatype& second_item = second_sequence[(size_t)j - 1];
                    const double index_cost = distance_fn(first_item, second_item);
                    double prev_cost = 0.0;
                    // Get the three neighboring values from the matrix to use for the update
                    double im1j = dtw_matrix_(i - 1, j);
                    double im1jm1 = dtw_matrix_(i - 1, j - 1);
                    double ijm1 = dtw_matrix_(i, j - 1);
                    // Start the update step
                    if (im1j < im1jm1 && im1j < ijm1)
                    {
                        prev_cost = im1j;
                    }
                    else if (ijm1 < im1j && ijm1 < im1jm1)
                    {
                        prev_cost = ijm1;
                    }
                    else
                    {
                        prev_cost = im1jm1;
                    }
                    // Update the value in the matrix
                    const double new_cost = index_cost + prev_cost;
                    dtw_matrix_(i, j) = new_cost;
                }
            }
            //Return total path cost
            const double warping_cost = dtw_matrix_((ssize_t)first_sequence.size(), (ssize_t)second_sequence.size());
            return warping_cost;
        }
    };

    // DistanceFn must match the prototype std::function<double(const FirstDataype&, const SecondDatatype&)>
    template<typename FirstDatatype, typename SecondDatatype, typename DistanceFn,
             typename FirstAllocator = std::allocator<FirstDatatype>,
             typename SecondAllocator = std::allocator<SecondDatatype>>
    inline double ComputeDTWDistance(
            const std::vector<FirstDatatype, FirstAllocator>& first_sequence,
            const std::vector<SecondDatatype, SecondAllocator>& second_sequence,
            const DistanceFn& distance_fn)
    {
        SimpleDTW<FirstDatatype, SecondDatatype, DistanceFn, FirstAllocator, SecondAllocator> dtw_evaluator;
        return dtw_evaluator.EvaluateWarpingCost(first_sequence, second_sequence, distance_fn);
    }
}

#endif // SIMPLE_DTW_HPP


#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>
#include <boost/numeric/bindings/traits/ublas_sparse.hpp>
#include <boost/numeric/bindings/mumps/mumps_driver.hpp>
#include <iostream>
#include <fstream>
#include <complex>

template <typename T>
int test() {
  namespace ublas = ::boost::numeric::ublas ;
  namespace mumps = ::boost::numeric::bindings::mumps ;

  int const n = 10 ;

  typedef ublas::coordinate_matrix<T, ublas::column_major, 1, ublas::unbounded_array<int> > coo_type ;

  coo_type coo( n, n, n + 6 ) ;

  for (int i=0; i<n; ++i) coo(i,i) = i+1.0 ;
  coo(2,3) = T(1.0) ;
  coo(2,4) = T(1.0) ;
  coo(5,6) = T(-1.0) ;
  coo(2,6) = T(1.0) ;
  coo(9,0) = T(1.0) ;
  coo(2,7) = T(-1.0) ;

  coo.sort() ;
  std::cout << "matrix " << coo << std::endl ;

  ublas::vector<T> v( 10 ) ;
  ublas::vector<T> w( 10 ) ;

  std::fill( w.begin(), w.end(), 1.0 ) ;

  for (int i=1; i<n; ++i) {
    w(i) += w(i-1) ;
  }

  for (int i=0; i<n; ++i) {
    v[i] = T(coo(i,i)) * w[i] ;
  }
  v[2] += T(coo(2,3)) * w[3] ;
  v[2] += T(coo(2,4)) * w[4] ;
  v[5] += T(coo(5,6)) * w[6] ;
  v[2] += T(coo(2,6)) * w[6] ;
  v[9] += T(coo(9,0)) * w[0] ;
  v[2] += T(coo(2,7)) * w[7] ;
  std::cout << "rhs : " << v << std::endl ;

  mumps::mumps< coo_type > mumps_coo ;

  mumps_coo.icntl[2]=mumps_coo.icntl[3] = 0 ;

  // Analysis
  mumps_coo.job = 1 ;
  matrix_integer_data( mumps_coo, coo ) ;
  driver( mumps_coo ) ;

  // Factorization
  mumps_coo.job = 2 ;
  matrix_value_data( mumps_coo, coo ) ;
  driver( mumps_coo ) ;

  // Solve
  mumps_coo.job = 3 ;
  rhs_sol_value_data( mumps_coo, v ) ;
  driver( mumps_coo ) ;

  std::cout << "w : " << w << std::endl ;
  std::cout << "v : " << v << std::endl ;

  if ( norm_2( v - w ) > 1.e-10 * norm_2( v ) ) return 1 ;

  return 0 ;
}

int main() {
  if ( test<float>() ) return 1 ;
  if ( test<double>() ) return 2 ;
  if ( test< std::complex<float> >() ) return 3 ;
  if ( test< std::complex<double> >() ) return 4 ;
  return 0 ;
}