/**
 *  Image GraphCut 3D Segmentation
 *
 *  Copyright (c) 2016, Zurich University of Applied Sciences, School of Engineering, T. Fitze, Y. Pauchard
 *
 *  Licensed under GNU General Public License 3.0 or later.
 *  Some rights reserved.
 */

 #include <gtest/gtest.h>

// boost
#include <boost/assign/list_of.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>

//
#include "MaxFlowGraphBoost.hxx"
#include "MaxFlowGraphKolmogorov.hxx"

class TestGraphLibrary : public ::testing::Test {
protected:
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
            boost::no_property,
            boost::property<boost::edge_index_t, std::size_t> > GraphType;

    typedef boost::graph_traits<GraphType>::vertex_descriptor VertexDescriptor;
    typedef boost::graph_traits<GraphType>::edge_descriptor EdgeDescriptor;
    void addBidirectionalEdge(GraphType& graph, VertexDescriptor source, VertexDescriptor target, float weight,
            float reverseWeight, std::vector<EdgeDescriptor> &reverseEdges, std::vector<float>& capacity){
        int nextEdgeId = num_edges(graph);

        // create both edges
        EdgeDescriptor edge = boost::add_edge(source, target, nextEdgeId, graph).first;
        EdgeDescriptor reverseEdge = boost::add_edge(target, source, nextEdgeId + 1, graph).first;

        // add them to out property maps
        reverseEdges.push_back(reverseEdge);
        reverseEdges.push_back(edge);
        capacity.push_back(weight);
        capacity.push_back(weight);
    }

    virtual void SetUp() {

    }

    virtual void TearDown() {

    }

};

TEST_F(TestGraphLibrary, ComputeMaxFlow){
    /*
     *                input              expected segmentation
     *
     *          F = F = x - B = B         F = F = F - B = B
     *          ||  ||  |   ||  ||        ||  ||  |   ||  ||
     *          x = F - x = B = x    ->   F = F - B = B = B
     *          ||  ||  |   ||  ||        ||  ||  |   ||  ||
     *          F = F = x - B = B         F = F = F - B = B
     *
     * F is initialized as foreground (source)
     * B is initialized as background (sink)
     * - and | indicate small weight (low throughput),
     * = and || indicate larger weights -> desirable flow.
     *
     */

    // create graph with 4 vertices
    int numberOfVertices = 3*5 + 2;
    float smallWeight = 1;
    float largeWeight = 1000;

    // containers
    GraphType graph(numberOfVertices);
    std::vector<EdgeDescriptor> reverseEdges;
    std::vector<float> capacity;
    std::vector<int> groups(numberOfVertices);

    // get all the descriptors
    VertexDescriptor vSource = boost::vertex(0, graph);
    VertexDescriptor v0 = boost::vertex(1, graph);
    VertexDescriptor v1 = boost::vertex(2, graph);
    VertexDescriptor v2 = boost::vertex(3, graph);
    VertexDescriptor v3 = boost::vertex(4, graph);
    VertexDescriptor v4 = boost::vertex(5, graph);
    VertexDescriptor v5 = boost::vertex(6, graph);
    VertexDescriptor v6 = boost::vertex(7, graph);
    VertexDescriptor v7 = boost::vertex(8, graph);
    VertexDescriptor v8 = boost::vertex(9, graph);
    VertexDescriptor v9 = boost::vertex(10, graph);
    VertexDescriptor v10 = boost::vertex(11, graph);
    VertexDescriptor v11 = boost::vertex(12, graph);
    VertexDescriptor v12 = boost::vertex(13, graph);
    VertexDescriptor v13 = boost::vertex(14, graph);
    VertexDescriptor v14 = boost::vertex(15, graph);
    VertexDescriptor vSink = boost::vertex(16, graph);

    // add horizontal edges
    addBidirectionalEdge(graph, v0, v1, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v1, v2, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v2, v3, smallWeight, smallWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v3, v4, largeWeight, largeWeight, reverseEdges, capacity);

    addBidirectionalEdge(graph, v5, v6, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v6, v7, smallWeight, smallWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v7, v8, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v8, v9, largeWeight, largeWeight, reverseEdges, capacity);

    addBidirectionalEdge(graph, v10, v11, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v11, v12, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v12, v13, smallWeight, smallWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v13, v14, largeWeight, largeWeight, reverseEdges, capacity);

    // vertical edges
    addBidirectionalEdge(graph, v0, v5, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v1, v6, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v2, v7, smallWeight, smallWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v3, v8, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v4, v9, largeWeight, largeWeight, reverseEdges, capacity);

    addBidirectionalEdge(graph, v5, v10, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v6, v11, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v7, v12, smallWeight, smallWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v8, v13, largeWeight, largeWeight, reverseEdges, capacity);
    addBidirectionalEdge(graph, v9, v14, largeWeight, largeWeight, reverseEdges, capacity);

    // connect the sources
    std::vector<VertexDescriptor> sourceNodes = boost::assign::list_of(v0)(v1)(v6)(v10)(v11);
    for(int i = 0; i < sourceNodes.size(); ++i){
        addBidirectionalEdge(graph, sourceNodes[i], vSource, largeWeight, largeWeight, reverseEdges, capacity);
        addBidirectionalEdge(graph, sourceNodes[i], vSink, smallWeight, smallWeight, reverseEdges, capacity);
    }

    // connect the sinks
    std::vector<VertexDescriptor> sinkNodes = boost::assign::list_of(v3)(v4)(v8)(v13)(v14);
    for(int i = 0; i < sinkNodes.size(); ++i){
        addBidirectionalEdge(graph, sinkNodes[i], vSink, largeWeight, largeWeight, reverseEdges, capacity);
        addBidirectionalEdge(graph, sinkNodes[i], vSource, smallWeight, smallWeight, reverseEdges, capacity);
    }

    // probably also need to connect the uncertain nodes?

    std::vector<float> residualCapacity(boost::num_edges(graph), 0);

    // check if the data structure looks as expected
    EXPECT_EQ(numberOfVertices, boost::num_vertices(graph));
    EXPECT_EQ(22 * 2 + sourceNodes.size()*4 + sinkNodes.size()*4, boost::num_edges(graph));

    // max flow
    boost::boykov_kolmogorov_max_flow(graph
            , boost::make_iterator_property_map(&capacity.front(), boost::get(boost::edge_index, graph))
            , boost::make_iterator_property_map(&residualCapacity.front(), boost::get(boost::edge_index, graph))
            , boost::make_iterator_property_map(&reverseEdges.front(), boost::get(boost::edge_index, graph))
            , boost::make_iterator_property_map(&groups.front(), boost::get(boost::vertex_index, graph))
            , boost::get(boost::vertex_index, graph)
            , vSource
            , vSink);

    // cexpected segmentation
    std::set<int> expectedForeground = boost::assign::list_of(0)(1)(2)(3)(6)(7)(11)(12)(13);
    std::set<int> expectedBackground = boost::assign::list_of(4)(5)(8)(9)(10)(14)(15)(16);

    // check the group of each vertex with the expected results
    for(size_t index=0; index < numberOfVertices; ++index){
        if(groups[index] == groups[vSource]){
            if(expectedForeground.find(index) != expectedForeground.end()){
                SUCCEED();
                expectedForeground.erase(index);
            } else{
                FAIL() << "missing "<<index << " in foreground results";
            }
        }
        else if(groups[index] == groups[vSink]){
            if(expectedBackground.find(index) != expectedBackground.end()){
                SUCCEED();
                expectedBackground.erase(index);
            } else{
                FAIL() << "missing "<<index << " in background results";
            }
        }
        else{
            FAIL() << "Vertex is neither foreground nor background, something went wrong.";
        }
    }

    // both containers should now be empty
    EXPECT_EQ(0, expectedForeground.size());
    EXPECT_EQ(0, expectedBackground.size());
}

TEST_F(TestGraphLibrary, MaxFlowGraphBoost){
    // same exmaple as in ComputeMaxFlow, but this time with the wrapper

    // create graph with 4 vertices
    int numberOfVertices = 3*5;
    float smallWeight = 1;
    float largeWeight = 1000;

    MaxFlowGraphBoost graph(3, 5, 1);

    // add horizontal edges
    graph.addBidirectionalEdge(0, 1, largeWeight, largeWeight);
    graph.addBidirectionalEdge(1, 2, largeWeight, largeWeight);
    graph.addBidirectionalEdge(2, 3, smallWeight, smallWeight);
    graph.addBidirectionalEdge(3, 4, largeWeight, largeWeight);
    graph.addBidirectionalEdge(5, 6, largeWeight, largeWeight);
    graph.addBidirectionalEdge(6, 7, smallWeight, smallWeight);
    graph.addBidirectionalEdge(7, 8, largeWeight, largeWeight);
    graph.addBidirectionalEdge(8, 9, largeWeight, largeWeight);
    graph.addBidirectionalEdge(10, 11, largeWeight, largeWeight);
    graph.addBidirectionalEdge(11, 12, largeWeight, largeWeight);
    graph.addBidirectionalEdge(12, 13, smallWeight, smallWeight);
    graph.addBidirectionalEdge(13, 14, largeWeight, largeWeight);

    // vertical edges
    graph.addBidirectionalEdge(0, 5, largeWeight, largeWeight);
    graph.addBidirectionalEdge(1, 6, largeWeight, largeWeight);
    graph.addBidirectionalEdge(2, 7, smallWeight, smallWeight);
    graph.addBidirectionalEdge(3, 8, largeWeight, largeWeight);
    graph.addBidirectionalEdge(4, 9, largeWeight, largeWeight);
    graph.addBidirectionalEdge(5, 10, largeWeight, largeWeight);
    graph.addBidirectionalEdge(6, 11, largeWeight, largeWeight);
    graph.addBidirectionalEdge(7, 12, smallWeight, smallWeight);
    graph.addBidirectionalEdge(8, 13, largeWeight, largeWeight);
    graph.addBidirectionalEdge(9, 14, largeWeight, largeWeight);

    // connect the sources
    std::vector<unsigned int> sourceNodes = boost::assign::list_of(0)(1)(6)(10)(11);
    for(int i = 0; i < sourceNodes.size(); ++i){
        graph.addTerminalEdges(sourceNodes[i], largeWeight, smallWeight);
    }

    // connect the sinks
    std::vector<unsigned int> sinkNodes = boost::assign::list_of(3)(4)(8)(13)(14);
    for(int i = 0; i < sinkNodes.size(); ++i){
        graph.addTerminalEdges(sinkNodes[i], smallWeight, largeWeight);
    }

    // check if the data structure looks as expected
    EXPECT_EQ(numberOfVertices, graph.getNumberOfVertices()); // +2 because a sink + source should've been added
    EXPECT_EQ(22 * 2 + sourceNodes.size()*4 + sinkNodes.size()*4, graph.getNumberOfEdges());

    // max flow
    graph.calculateMaxFlow();

    // cexpected segmentation
    std::set<unsigned int> expectedForeground = boost::assign::list_of(0)(1)(2)(5)(6)(10)(11)(12);
    std::set<unsigned int> expectedBackground = boost::assign::list_of(3)(4)(7)(8)(9)(13)(14);

    // check the group of each vertex with the expected results
    for(size_t index=0; index < graph.getNumberOfVertices(); ++index){
        if(graph.groupOf(index) == graph.groupOfSource()){
            if(expectedForeground.find(index) != expectedForeground.end()){
                SUCCEED();
                expectedForeground.erase(index);
            } else{
                FAIL() << "missing "<<index << " in foreground results";
            }
        }
        else if(graph.groupOf(index) == graph.groupOfSink()){
            if(expectedBackground.find(index) != expectedBackground.end()){
                SUCCEED();
                expectedBackground.erase(index);
            } else{
                FAIL() << "missing "<<index << " in background results";
            }
        }
        else{
            FAIL() << "Vertex " << index << " is neither foreground nor background, something went wrong.";
        }
    }

    // both containers should now be empty
    EXPECT_EQ(0, expectedForeground.size());
    EXPECT_EQ(0, expectedBackground.size());
}

TEST_F(TestGraphLibrary, MaxFlowGraphKolmogorov){
    // same example as in MaxFlowGraphBoost

    // create graph with 4 vertices
    int numberOfVertices = 3*5;
    float smallWeight = 1;
    float largeWeight = 1000;

    MaxFlowGraphKolmogorov graph(3, 5, 1);

    // add horizontal edges
    graph.addBidirectionalEdge(0, 1, largeWeight, largeWeight);
    graph.addBidirectionalEdge(1, 2, largeWeight, largeWeight);
    graph.addBidirectionalEdge(2, 3, smallWeight, smallWeight);
    graph.addBidirectionalEdge(3, 4, largeWeight, largeWeight);
    graph.addBidirectionalEdge(5, 6, largeWeight, largeWeight);
    graph.addBidirectionalEdge(6, 7, smallWeight, smallWeight);
    graph.addBidirectionalEdge(7, 8, largeWeight, largeWeight);
    graph.addBidirectionalEdge(8, 9, largeWeight, largeWeight);
    graph.addBidirectionalEdge(10, 11, largeWeight, largeWeight);
    graph.addBidirectionalEdge(11, 12, largeWeight, largeWeight);
    graph.addBidirectionalEdge(12, 13, smallWeight, smallWeight);
    graph.addBidirectionalEdge(13, 14, largeWeight, largeWeight);

    // vertical edges
    graph.addBidirectionalEdge(0, 5, largeWeight, largeWeight);
    graph.addBidirectionalEdge(1, 6, largeWeight, largeWeight);
    graph.addBidirectionalEdge(2, 7, smallWeight, smallWeight);
    graph.addBidirectionalEdge(3, 8, largeWeight, largeWeight);
    graph.addBidirectionalEdge(4, 9, largeWeight, largeWeight);
    graph.addBidirectionalEdge(5, 10, largeWeight, largeWeight);
    graph.addBidirectionalEdge(6, 11, largeWeight, largeWeight);
    graph.addBidirectionalEdge(7, 12, smallWeight, smallWeight);
    graph.addBidirectionalEdge(8, 13, largeWeight, largeWeight);
    graph.addBidirectionalEdge(9, 14, largeWeight, largeWeight);

    // connect the sources
    std::vector<unsigned int> sourceNodes = boost::assign::list_of(0)(1)(6)(10)(11);
    for(int i = 0; i < sourceNodes.size(); ++i){
        graph.addTerminalEdges(sourceNodes[i], largeWeight, smallWeight);
    }

    // connect the sinks
    std::vector<unsigned int> sinkNodes = boost::assign::list_of(3)(4)(8)(13)(14);
    for(int i = 0; i < sinkNodes.size(); ++i){
        graph.addTerminalEdges(sinkNodes[i], smallWeight, largeWeight);
    }

    // check if the data structure looks as expected
    EXPECT_EQ(numberOfVertices, graph.getNumberOfVertices()); // +2 because a sink + source should've been added

    // max flow
    graph.calculateMaxFlow();

    // cexpected segmentation
    std::set<unsigned int> expectedForeground = boost::assign::list_of(0)(1)(2)(5)(6)(10)(11)(12);
    std::set<unsigned int> expectedBackground = boost::assign::list_of(3)(4)(7)(8)(9)(13)(14);

    // check the group of each vertex with the expected results
    for(size_t index=0; index < graph.getNumberOfVertices(); ++index){
        if(graph.groupOf(index) == graph.groupOfSource()){
            if(expectedForeground.find(index) != expectedForeground.end()){
                SUCCEED();
                expectedForeground.erase(index);
            } else{
                FAIL() << "missing "<<index << " in foreground results";
            }
        }
        else if(graph.groupOf(index) == graph.groupOfSink()){
            if(expectedBackground.find(index) != expectedBackground.end()){
                SUCCEED();
                expectedBackground.erase(index);
            } else{
                FAIL() << "missing "<<index << " in background results";
            }
        }
        else{
            FAIL() << "Vertex " << index << " is neither foreground nor background, something went wrong.";
        }
    }

    // both containers should now be empty
    EXPECT_EQ(0, expectedForeground.size());
    EXPECT_EQ(0, expectedBackground.size());
}


#include <iostream>
#include <stdio.h>

#include <boost/numeric/mtl/utility/complexity.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/less.hpp>

using namespace std;
using namespace mtl::complexity_classes;
namespace mpl = boost::mpl;

struct wahr {
  void operator() () {
    cout << "vrai\n"; }
};

struct falsch {
  void operator() () {
    cout << "faute\n"; }
};

void schreib(infinite) {
  cout << "unendlich\n"; }

void schreib(polynomial) {
    cout << "polynomial\n"; }

void schreib(quadratic) {
    cout << "quadratisch\n"; }

void schreib(n_polylog_n) {
    cout << "n log^k n\n"; }

void schreib(n_log_n) {
    cout << "n log n\n"; }

void schreib(linear) {
    cout << "linear\n"; }

void schreib(linear_cached) {
    cout << "linear cached\n"; }

void schreib(polylog_n) {
    cout << "polynomial log\n"; }

void schreib(log_n) {
    cout << "log\n"; }

void schreib(constant) {
    cout << "constant\n"; }

void schreib(cached) {
    cout << "cached\n"; }

template <typename X, typename Y> void write_less(X, Y) {
  typedef mpl::less<X, Y> less_res;
  typename mpl::if_<less_res, wahr, falsch>::type()();
}

template <typename X, typename Y> void write_plus(X, Y) {
  schreib(typename mtl::complexity_classes::plus<X, Y>::type());
}

template <typename X, typename Y> void write_mal(X, Y) {
  schreib(typename mtl::complexity_classes::times<X, Y>::type());
}

int main (int, char**) {

  write_less(quadratic(), infinite());
  write_less(quadratic(), linear());

  write_plus(quadratic(), infinite());
  write_plus(quadratic(), linear());
  write_plus(linear(), quadratic());
  write_plus(n_log_n(), linear());

  write_mal(quadratic(), infinite());
  write_mal(infinite(), quadratic());
  write_mal(quadratic(), quadratic());
  write_mal(linear(), quadratic());
  write_mal(linear(), log_n());
  write_mal(linear(), polylog_n());
  write_mal(n_log_n(), log_n());
  write_mal(n_log_n(), polylog_n());
  write_mal(cached(), log_n());
  write_mal(log_n(), log_n());
  write_mal(cached(), constant());

  return 0;
}


// [[Rcpp::plugins(openmp)]]
#ifdef _OPENMP
#include <omp.h>
#endif
#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Core>
#include <random>
#include "distributions.h"
#include "concurrentqueue.h"

// [[Rcpp::depends(RcppEigen)]]
using namespace Rcpp;
using namespace RcppEigen;
using namespace Eigen;
inline void initialize_file( std::ofstream& outFile,int M,int N){
  bool queueFull;
  queueFull=0;

  outFile<< "iteration,"<<"mu,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "beta[" << (i+1) << "],";

  }
  outFile<<"sigmaE,"<<"sigmaG,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "comp[" << (i+1) << "],";
  }
  unsigned int i;
  for(i = 0; i < (N-1);i++){
    outFile << "epsilon[" << (i+1) << "],";
  }

  outFile << "epsilon[" << (i+1) << "]";
  outFile<<"\n";
}

//' BayesR sampler
//'
//' @param outputFile The file in which the samples aftare burnin will be stored
//' @param seed random seed
//' @param max_iterations total of number of samples taken
//' @param burn_in integer leq than max_iterations, number of samples used for burn in, after which, al samples will  be stored in the outputFile
//' @param thinning thinning regime, not implemented
//' @param X matrix of snp markers, or covariates of interest
//' @param Y vector of response variates, must have the same number of rows as X
//' @param sigma0 variance of the zero-centered normal prior over the intercept
//' @param v0E degrees of  freedom of the prior inverse scaled chi-squared distribution over residues variance
//' @param s02E  scale parameter of the prior inverse scaled chi-squared distribution over residues variance
//' @param v0G degrees of freedom of the prior inverse scaled chi-squared distribution over genetic effects variance
//' @param s02G  scale parameter of the prior inverse scaled chi-squared distribution over residues variance
//' @param cva Vector of mixture components variances.
//'
//' @return (max_iteration - burn_in) modulo thinning samples saved in outputFile.
//' @export
//'
//' @examples
// [[Rcpp::export]]
void BayesRSamplerV2(std::string outputFile, int seed, int max_iterations, int burn_in, int thinning, Eigen::MatrixXd X, Eigen::VectorXd Y,double sigma0, double v0E, double s02E, double v0G, double s02G,Eigen::VectorXd cva) {
  int flag;
  moodycamel::ConcurrentQueue<Eigen::VectorXd> q;
  flag=0;
  int N(Y.size());
  int M(X.cols());
  VectorXd components(M);
  std::ofstream outFile;

  outFile.open(outputFile);
  initialize_file(outFile,M,N);
  VectorXd sampleq(2*M+4+N);
  IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", "", "");
  int K(cva.size()+1);
  ////////////validate inputs

  if(max_iterations < burn_in || max_iterations<1 || burn_in<1) //validations related to mcmc burnin and iterations
  {
    Rcpp::Rcerr<<"error: burn_in has to be a positive integer and smaller than the maximum number of iterations ";
    return;
  }
  if(sigma0 < 0 || v0E < 0 || s02E < 0 || v0G < 0||  s02G < 0 )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: hyper parameters have to be positive";
    //return;
  }
  if((cva.array()==0).any() )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: the zero component is already included in the model by default";
    //return;
  }
  if((cva.array()<0).any() )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: the variance of the components should be positive";
    //return;
  }
  /////end of declarations//////

//  Eigen::initParallel();
 // Eigen::setNbThreads(10);

#ifdef _OPENMP
 omp_set_num_threads(2);
#endif
#pragma omp parallel num_threads(2) shared(flag,q,M,N)
{
#pragma omp sections
{
  //begin producer
  {

    //mean and residual variables
    double mu; // mean or intercept
    double sigmaG; //genetic variance
    double sigmaE; // residuals variance

    //component variables
    VectorXd priorPi(K); // prior probabilities for each component
    VectorXd pi(K); // mixture probabilities
    VectorXd cVa(K); //component-specific variance
    VectorXd logL(K); // log likelihood of component
    VectorXd muk(K); // mean of k-th component marker effect size
    VectorXd denom(K-1); // temporal variable for computing the inflation of the effect variance for a given non-zero componnet
    double num;//storing dot product
    int m0; // total num ber of markes in model
    VectorXd v(K); //variable storing the component assignment
    VectorXd cVaI(K);// inverse of the component variances

    //linear model variables
    MatrixXd beta(M,1); // effect sizes
    VectorXd y_tilde(N); // variable containing the adjusted residuals to exclude the effects of a given marker
    VectorXd epsilon(N); // variable containing the residuals
    VectorXd xsquared(M); //variable containing the squared norm of the X columns

    //sampler variables
    VectorXd sample(2*M+4+N); // varible containg a sambple of all variables in the model, M marker effects, M component assigned to markers, sigmaE, sigmaG, mu, iteration number and Explained variance
    std::vector<int> markerI;
    for (int i=0; i<M; ++i) {
      markerI.push_back(i);
    }

    int marker;
    double acum;

    priorPi[0]=0.5;

    priorPi.segment(1,(K-1))=priorPi[0]*cVa.segment(1,(K-1)).segment(1,(K-1)).array()/cVa.segment(1,(K-1)).segment(1,(K-1)).sum();
    y_tilde.setZero();
    cVa[0] = 0;
    cVa.segment(1,(K-1))=cva;

    cVaI[0] = 0;
    cVaI.segment(1,(K-1))=cVa.segment(1,(K-1)).cwiseInverse();

    beta.setZero();

    mu=0;

    sigmaG=beta_rng(1,1);

    pi=priorPi;

    components.setZero();
    std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
    epsilon= Y.array() - mu - (X*beta).array();
    sigmaE=epsilon.squaredNorm()/N*0.5;
    xsquared=X.colwise().squaredNorm();
    for(int iteration=0; iteration < max_iterations; iteration++){

      if(iteration>0)
        if( iteration % (int)std::ceil(max_iterations/10) ==0)
          Rcpp::Rcout << "iteration: "<<iteration <<"\n";

      epsilon= epsilon.array()+mu;//  we substract previous value
      mu = norm_rng(epsilon.sum()/(double)N, sigmaE/(double)N); //update mu
      epsilon= epsilon.array()-mu;// we substract again now epsilon =Y-mu-X*beta

      std::random_shuffle(markerI.begin(), markerI.end());

      m0=0;
      v.setZero();
      for(int j=0; j < M; j++){

        marker= markerI[j];

        y_tilde= epsilon.array()+(X.col(marker)*beta(marker,0)).array();//now y_tilde= Y-mu-X*beta+ X.col(marker)*beta(marker)_old

        muk[0]=0.0;//muk for the zeroth component=0

       // std::cout<< muk;
        //we compute the denominator in the variance expression to save computations
        denom=xsquared(marker)+(sigmaE/sigmaG)*cVaI.segment(1,(K-1)).array();
        //we compute the dot product to save computations
        num=(X.col(marker).cwiseProduct(y_tilde)).sum();
        //muk for the other components is computed according to equaitons
        muk.segment(1,(K-1))= num/denom.array();

        logL= pi.array().log();//first component probabilities remain unchanged

        //update the log likelihood for each component
        logL.segment(1,(K-1))=logL.segment(1,(K-1)).array() - 0.5*((((sigmaG/sigmaE)*(xsquared(marker)))*cVa.segment(1,(K-1)).array() + 1).array().log()) + 0.5*( muk.segment(1,(K-1)).array()*num)/sigmaE;

        double p(beta_rng(1,1));//I use beta(1,1) because I cant be bothered in using the std::random or create my own uniform distribution, I will change it later

        if(((logL.segment(1,(K-1)).array()-logL[0]).abs().array() >700 ).any() ){
         acum=0;
        }else{
          acum=1.0/((logL.array()-logL[0]).exp().sum());
        }

        for(int k=0;k<K;k++){
          if(p<=acum){
            //if zeroth component
            if(k==0){
              beta(marker,0)=0;
            }else{
              beta(marker,0)=norm_rng(muk[k],sigmaE/denom[k-1]);
            }
            v[k]+=1.0;
            components[marker]=k;
            break;
          }else{
            //if too big or too small
            if(((logL.segment(1,(K-1)).array()-logL[k+1]).abs().array() >700 ).any() ){
              acum+=0;
            }
            else{
              acum+=1.0/((logL.array()-logL[k+1]).exp().sum());
            }
          }
        }
       epsilon=y_tilde-X.col(marker)*beta(marker,0);//now epsilon contains Y-mu - X*beta+ X.col(marker)*beta(marker)_old- X.col(marker)*beta(marker)_new

      }

      m0=M-v[0];
      sigmaG=inv_scaled_chisq_rng(v0G+m0,(beta.squaredNorm()*m0+v0G*s02G)/(v0G+m0));

      sigmaE=inv_scaled_chisq_rng(v0E+N,((epsilon).squaredNorm()+v0E*s02E)/(v0E+N));

      pi=dirichilet_rng(v.array() + 1.0);

      if(iteration >= burn_in)
      {
        if(iteration % thinning == 0){
          sample<< iteration,mu,beta,sigmaE,sigmaG,components,epsilon;
          q.enqueue(sample);
#ifdef _OPENMP

#else
          if(q.try_dequeue(sampleq))
            outFile<< sampleq.transpose().format(CommaInitFmt) << "\n";
#endif
           //here we have the consumer

        }

      }

    }

    std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>( t2 - t1 ).count();
    Rcpp::Rcout << "duration: "<<duration << "s\n";
    flag=1;
  }//end producer
#ifdef _OPENMP
#pragma omp section
      {

        while(!flag ){
          if(q.try_dequeue(sampleq))
            outFile<< sampleq.transpose().format(CommaInitFmt) << "\n";
        }
      }//end consumer
#endif
}
}

}

/*** R
M=200 #non zero marker effects
N=2000 #observations
MT=2000 #number of markers
B=matrix(rnorm(MT,sd=sqrt(0.5)),ncol=1) #marker effects, M marquers explain approx 50% of the variance
B[sample(1:MT,MT-M),1]=0 #we set MT-M marker effects to zero
#B=-abs(B)
X <- matrix(rnorm(MT*N), N, MT); var(X[,1])
G <- X%*%B; var(G)
Y=X%*%B+rnorm(N,sd=sqrt(0.4)); var(Y)
Y=Y
X=scale(X)
P=0.5 #prior probability of a marker being excluded from the model
sigma0=0.01# prior  variance of a zero mean gaussian prior over the mean mu NOT IMPLEMENTED
v0E=0.01 # degrees of freedom over the inv scaled chi square prior over residuals variance
s02E=0.01 #scale of the inv scaled chi square prior over residuals variance
v0G=0.01 #degrees of freedom of the inv bla bla prior over snp effects
s02G=0.01 # scale for the same
BayesRSamplerV2("./test2.csv",2, 5000, 2000,10,X, Y,sigma0,v0E,s02E,v0G,s02G,P)
library(readr)
tmp <- read_csv("./test2.csv")
#names(tmp)
#plot(tmp$sigmaG); mean(tmp$sigmaG)
plot(B,colMeans(tmp[,grep("beta",names(tmp))]))
lines(B,B)
abline(h=0)
var(G)
mean(tmp$EV)
1-var(G)
mean(tmp$sigmaE)
plot(tmp$mu)
plot(tmp$sigmaE)
plot(tmp$sigmaG)
hist(as.matrix(tmp[,grep("comp",names(tmp))])) #histogram of components, component 0= variance 0, component 1= variance 0.0001 and so long so forth
*/


#ifndef SDE_HPP_
#define SDE_HPP_

#include <boost/numeric/odeint.hpp>

#include "aux.hpp"

/** EM implementation taking std::vector.
 * Works with non-diagonal noise.
 */
class EulerMaruyamaStepper {
public:
  EulerMaruyamaStepper(Rng & rng) : rng(rng) { /* empty */ }
  typedef std::vector<double> state_type;
  typedef std::vector<double> deriv_type;
  typedef double value_type;
  typedef double time_type;
  typedef double order_type;
  typedef boost::numeric::odeint::stepper_tag stepper_category;
  static order_type order() {
    return 0.5;
  }
  template<class System>
  void do_step(System system, state_type & x, time_type t, time_type dt) const {
    deriv_type F(x.size()), G(x.size());
    system(x, F, t);
    // first query dimension of the Wiener process
    int wdim = system.get().wiener_dim();
    // sample a step of the Wiener process
    std::vector<double> Zi(wdim);
    std::generate(Zi.begin(), Zi.end(), [&](){return rng.normal(0, 1);});
    // now query for the dimension of the SDEs to allow for OD
    int cdim = system.get().continuous_dim();
    // sample a step of the Wiener process
    std::vector<double> Zo(cdim);
    std::generate(Zo.begin(), Zo.end(), [&](){return rng.normal(0, 1);});
    // use the volatility_vec_prod method from VectorField
    system.get().volatility_vec_prod(x, G, t, Zi, Zo);
    for ( size_t i = 0; i < x.size(); ++i ) {
      x[i] += dt * F[i] + sqrt(dt) * G[i]; // G = sigma * Z
    }
  }
private:
  /** ref to an RNG. Used to create normal deviates
   * @todo: construct actual RNG instead?
   */
  Rng & rng;
};

/** EM implementation taking std::vector.
 * Only works with diagonal noise.
 */
class EulerMaruyamaStepperDiag {
public:
  EulerMaruyamaStepperDiag(Rng & rng) : rng(rng) { /* empty */ }
  typedef std::vector<double> state_type;
  typedef std::vector<double> deriv_type;
  typedef double value_type;
  typedef double time_type;
  typedef double order_type;
  typedef boost::numeric::odeint::stepper_tag stepper_category;
  static order_type order() {
    return 0.5;
  }
  template<class System>
  void do_step(System system, state_type & x, time_type t, time_type dt) const {
    deriv_type F(x.size()), G(x.size());
    system(x, F, t);
    system.get().diffusion(x, G, t);
    for ( size_t i = 0; i < x.size(); ++i ) {
      double Z = rng.normal(0, 1);
      x[i] += dt * F[i] + sqrt(dt) * G[i] * Z;
    }
  }
private:
  /** ref to an RNG. Used to create normal deviates
   * @todo: construct actual RNG instead?
   */
  Rng & rng;
};

/** Runge-Kutta discretization of the Milstein scheme.
 * Only works for diagonal noise.
 *
 * @todo: check restrictions: we need that sigma_ii is
 * only dependent of x_i.
 */
 class RungeKuttaMilsteinStepper {
 public:
   RungeKuttaMilsteinStepper(Rng & rng) : rng(rng) { /* empty */ }
   typedef std::vector<double> state_type;
   typedef std::vector<double> deriv_type;
   typedef double value_type;
   typedef double time_type;
   typedef double order_type;
   typedef boost::numeric::odeint::stepper_tag stepper_category;
   static order_type order() {
     return 1.0;
   }
   /** The Millstein method takes steps as follows:
    * \f[
    *  Y_{n+1} = Y_n + f_n h + g_n \Delta W_n + \frac12 g_n g_n' [\Delta W_n^2 - h]
    * \f]
    * with \f$ f_n = f(Y_n) \f$, \f$ g_n = g(Y_n) \f$ and
    * \f$ g_n' = \frac{\partial g}{\partial x}(Y_n) \f$, and
    * \f$ \Delta W_n \sim \sqrt{h} \mathcal{N}(0,1) \f$.
    * The Kunge-Kutta approximation removes the need for a derivative,
    * and uses an intermediate step \f$ \bar{Y}_n = Y_n + f_n h + g_n \sqrt{h} \f$.
    * The next value is then calculated as
    * \f[
    *  Y_{n+1} = Y_n + f_n h + g_n \Delta W_n +
    *    \frac12 g_n \frac{1}{\sqrt{h}}(\bar{g}_n-g_n)[\Delta W_n^2 - h]
    * \f]
    * where \f$ \bar{g}_n = g(\bar{Y}_n) \f$.
    */
   template<class System>
   void do_step(System system, state_type & x, time_type t, time_type dt) const {
     deriv_type F(x.size()), G(x.size()), Gtilde(x.size());
     state_type xtilde(x.size());
     time_type sqdt = sqrt(dt);
     system(x, F, t);
     system.get().diffusion(x, G, t);
     // define intermediate step
     for ( size_t i = 0; i < x.size(); ++i ) {
       xtilde[i] = x[i] + dt*F[i] + sqdt*G[i];
     }
     // define Gtilde
     system.get().diffusion(xtilde, Gtilde, t);
     // update x
     for ( size_t i = 0; i < x.size(); ++i ) {
       double dW = rng.normal(0, 1) * sqdt;
       x[i] += dt*F[i] + G[i]*dW + 0.5*(Gtilde[i] - G[i]) * (dW*dW - dt) / sqdt;
     }
   }
 private:
   Rng & rng; // ref to an RNG
 };

/** @todo Implementation of an adaptive Stochastic Runge Kutta (SRK) method
 * using Rejection Sampling with Memory (RSwM)
 */

#endif


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * \file
 * \brief Test of blurred sine function.
 */
#include "num_prob_collect/regularization/blur_sine.h"

#include <cmath>
#include <random>

#include <Eigen/Core>
#include <celero/Celero.h>

#include "log_error_udm.h"
#include "log_param_udm.h"
#include "num_collect/regularization/explicit_gcv.h"
#include "num_collect/regularization/explicit_l_curve.h"
#include "num_collect/regularization/full_gen_tikhonov.h"
#include "num_collect/regularization/tikhonov.h"
#include "num_prob_collect/regularization/dense_diff_matrix.h"

// NOLINTNEXTLINE: external library
CELERO_MAIN

class blur_sine_fixture : public celero::TestFixture {
public:
    blur_sine_fixture() = default;

    [[nodiscard]] auto getExperimentValues() const
        -> std::vector<celero::TestFixture::ExperimentValue> override {
        std::vector<celero::TestFixture::ExperimentValue> problem_space;
        problem_space.emplace_back(-100);  // NOLINT
        problem_space.emplace_back(-4);    // NOLINT
        problem_space.emplace_back(-2);    // NOLINT
        problem_space.emplace_back(0);     // NOLINT
        return problem_space;
    }

    void setUp(
        const celero::TestFixture::ExperimentValue& experiment_value) override {
        error_rate_ = std::pow(10.0,  // NOLINT
            static_cast<int>(experiment_value.Value));
        std::mt19937 engine;  // NOLINT
        std::normal_distribution<double> dist{0.0,
            std::sqrt(prob_.data().squaredNorm() /
                static_cast<double>(prob_.data().size()) * error_rate_)};
        data_with_error_ = prob_.data();
        for (num_collect::index_type i = 0; i < data_with_error_.size(); ++i) {
            data_with_error_(i) += dist(engine);
        }
    }

    void set_error(const Eigen::VectorXd& solution) {
        log_error_->addValue(
            std::log10((solution - prob_.solution()).squaredNorm() /
                prob_.solution().squaredNorm()));
    }

    void set_param(double val) { log_param_->addValue(std::log10(val)); }

    [[nodiscard]] auto getUserDefinedMeasurements() const -> std::vector<
        std::shared_ptr<celero::UserDefinedMeasurement>> override {
        return {log_error_, log_param_};
    }

    [[nodiscard]] auto prob() const
        -> const num_prob_collect::regularization::blur_sine& {
        return prob_;
    }

    [[nodiscard]] auto data_with_error() const -> const Eigen::VectorXd& {
        return data_with_error_;
    }

    [[nodiscard]] auto dense_diff_matrix() const -> const Eigen::MatrixXd& {
        return dense_diff_matrix_;
    }

private:
    double error_rate_{};

#ifndef NDEBUG
    static constexpr num_collect::index_type solution_size = 30;
    static constexpr num_collect::index_type data_size = solution_size;
#else
    static constexpr num_collect::index_type solution_size = 60;
    static constexpr num_collect::index_type data_size = solution_size;
#endif
    const num_prob_collect::regularization::blur_sine prob_{
        data_size, solution_size};

    Eigen::VectorXd data_with_error_{};

    const Eigen::MatrixXd dense_diff_matrix_{
        num_prob_collect::regularization::dense_diff_matrix<Eigen::MatrixXd>(
            solution_size)};

    std::shared_ptr<log_error_udm> log_error_{
        std::make_shared<log_error_udm>()};
    std::shared_ptr<log_param_udm> log_param_{
        std::make_shared<log_param_udm>()};
};

constexpr std::int64_t samples = 30;
#ifndef NDEBUG
constexpr std::int64_t iterations = 1;
#else
constexpr std::int64_t iterations = 10;
#endif

using coeff_type =
    typename num_prob_collect::regularization::blur_sine::coeff_type;
using data_type =
    typename num_prob_collect::regularization::blur_sine::data_type;

// NOLINTNEXTLINE: external library
BASELINE_F(
    reg_blur_sine, tikhonov_l_curve, blur_sine_fixture, samples, iterations) {
    using solver_type =
        num_collect::regularization::tikhonov<coeff_type, data_type>;
    using searcher_type =
        num_collect::regularization::explicit_l_curve<solver_type>;

    solver_type solver;
    solver.compute(prob().coeff(), data_with_error());

    searcher_type searcher{solver};
    searcher.search();
    Eigen::VectorXd solution;
    searcher.solve(solution);

    set_error(solution);
    set_param(searcher.opt_param());
}

// NOLINTNEXTLINE: external library
BENCHMARK_F(
    reg_blur_sine, tikhonov_gcv, blur_sine_fixture, samples, iterations) {
    using solver_type =
        num_collect::regularization::tikhonov<coeff_type, data_type>;
    using searcher_type =
        num_collect::regularization::explicit_gcv<solver_type>;

    solver_type solver;
    solver.compute(prob().coeff(), data_with_error());

    searcher_type searcher{solver};
    searcher.search();
    Eigen::VectorXd solution;
    searcher.solve(solution);

    set_error(solution);
    set_param(searcher.opt_param());
}

// NOLINTNEXTLINE: external library
BENCHMARK_F(reg_blur_sine, full_gen_tik_l_curve, blur_sine_fixture, samples,
    iterations) {
    using solver_type =
        num_collect::regularization::full_gen_tikhonov<coeff_type, data_type>;
    using searcher_type =
        num_collect::regularization::explicit_l_curve<solver_type>;

    solver_type solver;
    solver.compute(prob().coeff(), data_with_error(), dense_diff_matrix());

    searcher_type searcher{solver};
    searcher.search();
    Eigen::VectorXd solution;
    searcher.solve(solution);

    set_error(solution);
    set_param(searcher.opt_param());
}

// NOLINTNEXTLINE: external library
BENCHMARK_F(
    reg_blur_sine, full_gen_tik_gcv, blur_sine_fixture, samples, iterations) {
    using solver_type =
        num_collect::regularization::full_gen_tikhonov<coeff_type, data_type>;
    using searcher_type =
        num_collect::regularization::explicit_gcv<solver_type>;

    solver_type solver;
    solver.compute(prob().coeff(), data_with_error(), dense_diff_matrix());

    searcher_type searcher{solver};
    searcher.search();
    Eigen::VectorXd solution;
    searcher.solve(solution);

    set_error(solution);
    set_param(searcher.opt_param());
}


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ROUND_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ROUND_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/maxflint.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/ceil.hpp>
#include <boost/simd/function/scalar/copysign.hpp>
#include <boost/simd/function/scalar/is_ltz.hpp>
#include <boost/simd/function/scalar/seldec.hpp>
#include <boost/simd/function/scalar/tenpower.hpp>
#include <boost/simd/detail/math.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( round_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( round_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
    #ifdef BOOST_SIMD_HAS_ROUNDF
      return ::roundf(a0);
    #else
      const A0 v = simd::abs(a0);
      if (!(v <=  Maxflint<A0>()))
        return a0;
      A0 c =  boost::simd::ceil(v);
       return copysign(seldec(c-Half<A0>() > v, c), a0);
    #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( round_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
    #ifdef BOOST_SIMD_HAS_ROUND
      return ::round(a0);
    #else
      const A0 v = simd::abs(a0);
      if (!(v <=  Maxflint<A0>()))
        return a0;
      A0 c =  boost::simd::ceil(v);
      return copysign(seldec(c-Half<A0>() > v, c), a0);
    #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( round_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0>;
      A0 fac = tenpower(i_t(a1));
      A0 tmp = round(a0*fac)/fac;
      return is_ltz(a1) ? round(tmp) : tmp;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( round_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bs::std_tag
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( A0  a0, std_tag const&) const BOOST_NOEXCEPT
    {
      return std::round(a0);
    }
  };
} } }

#endif


#include <iostream>
#include <Eigen/Dense>
#include "util.h"
#include "cppcnn.h"

using Eigen::MatrixXd;

int main() {
    using namespace std;

	cout.sync_with_stdio(false);
	cout << "Threads:" << Eigen::nbThreads() << endl;

	/* inputs matrix: [n * m], m is the pixels number of one image, n is the number of examples.
	 * labels matrix: [m * 1], m is the number of labels.
	 */
    auto train_inputs = util::read_mnist("../../data/mnist/train-images.idx3-ubyte",
										 "../../data/mnist/train-labels.idx1-ubyte");
	auto test_inputs = util::read_mnist("../../data/mnist/t10k-images.idx3-ubyte",
										"../../data/mnist/t10k-labels.idx1-ubyte");

	auto cnn = make_shared<CppCNN>();
	cnn->set_enable_momentum(false);
	cnn->set_enable_adam(false);
	cnn->set_learn_rate_decay(false);
	cnn->set_output_size(10);
	cnn->set_learn_rate(0.005);
	cnn->set_max_epoch(3);
	cnn->set_minibatch_size(1);

	cnn->add_conv_layer(3, 8);
	cnn->add_max_pool_layer(2, 2);
	cnn->set_hidden_layers({32, 16});

	cnn->train(train_inputs.first, train_inputs.second);

	cnn->test(test_inputs.first, test_inputs.second);

    return 0;
}


/*=========================================================================

 medInria

 Copyright (c) INRIA 2013. All rights reserved.
 See LICENSE.txt for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

=========================================================================*/

#include <vector>
#include <complex>
#include <cmath>

#include <vtkSphericalHarmonicSource.h>

#include <vtkPolyDataNormals.h>
#include <vtkCellArray.h>
#include <vtkFloatArray.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkMath.h>
#include <vtkObjectFactory.h>
#include <vtkPointData.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkMatrix4x4.h>
#include <vtkStreamingDemandDrivenPipeline.h>

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Compute spherical associated Legendre function
#if (defined __APPLE__ || defined WIN32 || defined CLANG)
#include <boost/math/special_functions/legendre.hpp>
#else
#include <tr1/cmath>
#endif //WIN32

#if (defined __APPLE__ || defined WIN32 || defined CLANG)
double sphLegendre(int _l, int _m, double theta) {
  double factor = sqrt(((double)(2*_l+1) / (4.0*vtkMath::DoublePi()))*(boost::math::factorial<double>((unsigned int)(_l - _m))
                                                        / boost::math::factorial<double>((unsigned int)(_l + _m))))*boost::math::legendre_p (_l, _m, cos(theta));
    return factor;
}
#else
double sphLegendre(int _l, int _m, double theta) {
    double factor =std::tr1::sph_legendre(_l,_m,theta);
    return factor;
}
#endif //WIN32
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

static itk::Vector<double, 3> Cartesian2Spherical(const itk::Vector<double, 3> vITK );

static itk::VariableSizeMatrix<double>
ComputeSHMatrixMaxThesis(const int rank,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                         const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections);

static itk::VariableSizeMatrix<double>
ComputeSHMatrixTournier(const int rank,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                        const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections);

static itk::VariableSizeMatrix<double>
ComputeSHMatrixRshBasis(const int rank,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                        const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections);

static itk::VariableSizeMatrix<double>
ComputeSHMatrix (const int rank,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                 const bool FlipZ, itk::VariableSizeMatrix<double>& PhiThetaDirections);

static void
TranslateAndDeformShell(vtkPolyData* shell,vtkPoints* outPts,double center[3],
                        bool deform,vtkMatrix4x4* transform=0);

vtkCxxRevisionMacro(vtkSphericalHarmonicSource,"$Revision: 0 $");
vtkStandardNewMacro(vtkSphericalHarmonicSource);

vtkSphericalHarmonicSource::vtkSphericalHarmonicSource(int tess)
{
  this->Radius = 0.5;
  this->Center[0] = 0.0;
  this->Center[1] = 0.0;
  this->Center[2] = 0.0;
  this->RotationMatrix = 0;

  this->SetNumberOfInputPorts(0);

  this->DeformOn();
  this->NormalizeOff();
  this->FlipXOff();
  this->FlipYOff();
  // By Default we flip the z-axis, the internal x,y,z have z flipped with respect to visu
  this->FlipZOn();
  this->MaxThesisFuncOff();

  this->TesselationType = Icosahedron;
  this->TesselationBasis = SHMatrix;
  this->Tesselation = tess;
  this->sphereT = vtkTessellatedSphereSource::New();
 this-> SphericalHarmonics = 0;
 this-> Order = 4;
  this->SetNumberOfSphericalHarmonics (15);
}

vtkSphericalHarmonicSource::~vtkSphericalHarmonicSource()
{
  if (this->sphereT)
    this->sphereT->Delete();

  if(this->RotationMatrix)
    this->RotationMatrix->Delete();

  SphericalHarmonics = 0;
}

void vtkSphericalHarmonicSource::SetNumberOfSphericalHarmonics(const int number)
{
  NumberOfSphericalHarmonics = number;
  if (SphericalHarmonics)
    delete[] SphericalHarmonics;
  SphericalHarmonics = new double[NumberOfSphericalHarmonics];
  SphericalHarmonics[0] = 1.0;
  for(int i=1;i<NumberOfSphericalHarmonics;++i)
    SphericalHarmonics[i] = 0.0;

  this->UpdateSphericalHarmonicSource();
  this->Modified();
}

int* vtkSphericalHarmonicSource::GetTesselationRange()
{
  int* range = new int[2];
  GetTesselationRange(range);
  return range;
}

void vtkSphericalHarmonicSource::GetTesselationRange(int *range)
{
  range[0] = 2;
  range[1] = 5;
}

void vtkSphericalHarmonicSource::SetSphericalHarmonicComponent(int i,double v)
{
  if (SphericalHarmonics[i]!=v)
  {
    vtkDebugMacro(<< this->GetClassName() << " (" << this
                  <<"): setting Spherical Harmonics coefficient "<< i <<" to "<< v);
    this->Modified();
    SphericalHarmonics[i] = v;
  }
}

int
vtkSphericalHarmonicSource::
RequestData(vtkInformation *vtkNotUsed(request),vtkInformationVector **vtkNotUsed(inputVector),
            vtkInformationVector *outputVector)
{
  // Get the info and output objects.
  vtkInformation* outInfo = outputVector->GetInformationObject(0);
  vtkPolyData*    output  = vtkPolyData::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()));
  int N = this->sphereT->GetOutput()->GetNumberOfPoints();
  // SH Source stuff.
  vtkFloatArray* sValues = vtkFloatArray::New();
  sValues->SetNumberOfTuples(N);
  sValues->SetNumberOfComponents(1);

  /* Project data on the sphere:
    In matrix form:
    S := spherical function (discrete data on the sphere)
    C := Spherical harmonic coefficients
    B := Basis function
    S = CB
    */

  itk::VariableSizeMatrix<double> C(1,NumberOfSphericalHarmonics);

  for (int i=0;i<NumberOfSphericalHarmonics;++i){
    C(0,i) = SphericalHarmonics[i];
  }

  itk::VariableSizeMatrix<double> S/*(1,BasisFunction.GetVnlMatrix().size())*/;
  S = C*BasisFunction;

  if (Normalize) {

    double min = S.GetVnlMatrix().min_value();
    double max = S.GetVnlMatrix().max_value();

//    std::cout << "Normalizing by min/max" << min << "/" << max << "\n";

    if (max!=min) {
      for (unsigned i=0; i<S.Rows(); ++i)
        for (unsigned j=0; j<S.Cols(); ++j)
          S(i,j) -= min;
      S /= (max-min);
    } else {
      for (unsigned i=0; i<S.Rows(); ++i)
        for (unsigned j=0; j<S.Cols(); ++j)
          S(i,j) = 1.0;
    }
  }
  S *= this->Radius;

  for(int i=0; i<N; ++i) {
    if  (i%10000==0) {
      this->UpdateProgress ((vtkFloatingPointType)i/N);
      if (this->GetAbortExecute())
        break;
    }
    sValues->SetTuple1(i,S(0,i));
  }

  this->sphereT->GetOutput()->GetPointData()->SetScalars(sValues);
  sValues->Delete();

  // Don't know how to copy everything but the points.
  output->DeepCopy(this->sphereT->GetOutput());
  output->GetPoints()->Reset();

  TranslateAndDeformShell(this->sphereT->GetOutput(),output->GetPoints(),this->Center,Deform,RotationMatrix);

  return 1;
}

void vtkSphericalHarmonicSource::PrintSelf(ostream& os,vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);

  os << indent << "Tessellation Order: " << Tesselation << std::endl;
  os << indent << "Tessellation Type: " << TesselationType << std::endl;
  os << indent << "Tessellation Basis: " << TesselationBasis << std::endl;

  os << indent << "SH Basis Order: " << Order << std::endl;
  os << indent << "Length of SH coefficient vector: "
     << NumberOfSphericalHarmonics << std::endl;
  os << indent << "SH Basis: " << NumberOfSphericalHarmonics << "x"
     << this->sphereT->GetOutput()->GetNumberOfPoints() << std::endl;
  os << indent << "SH Coefficients:" << std::endl << '[';
  for (int i=0;i<NumberOfSphericalHarmonics;++i)
    os << indent << SphericalHarmonics[i] << " ";
  os << indent << ']' << std::endl;
}

int vtkSphericalHarmonicSource::RequestInformation(vtkInformation*,vtkInformationVector**,
                                                   vtkInformationVector *outputVector)
{
  // Get the info object
  vtkInformation* outInfo = outputVector->GetInformationObject(0);

  outInfo->Set(vtkStreamingDemandDrivenPipeline::MAXIMUM_NUMBER_OF_PIECES(),-1);
  outInfo->Set(vtkStreamingDemandDrivenPipeline::WHOLE_BOUNDING_BOX(),
               this->Center[0]-this->Radius,this->Center[0]+this->Radius,
               this->Center[1]-this->Radius,this->Center[1]+this->Radius,
               this->Center[2]-this->Radius,this->Center[2]+this->Radius);

  return 1;
}

itk::Vector<double, 3> Cartesian2Spherical(const itk::Vector<double, 3> vITK)
{
  const double r =vITK.GetNorm();
  if (r==0) {
    std::cerr << "Cannot have a 0 radius in spherical coordinates!\n" << std::endl;
    exit(1);
  }
  // r, phi [0, PI), theta [0, PI) computation
  const double spherical [3] = { vITK.GetNorm(), atan2(vITK[1],vITK[0]), acos(vITK[2]/r)};
  const itk::Vector<double, 3> s (spherical);

  return s;
}

/*
  Compute SH matrix for discrete samplings on the sphere
  rank := number of spherical harmonics (rank of HOT in Reseach report 5681)
  n_s  := number of spherical values on the sphere (Research report 5681)
*/
itk::VariableSizeMatrix<double>
ComputeSHMatrix(const int order,vtkPolyData* shell,const bool FlipX,const bool FlipY,
                const bool FlipZ, itk::VariableSizeMatrix<double>& PhiThetaDirections)
{
  const int n_s   = shell->GetNumberOfPoints();
  const int rank =  (order+1)*(order+2)/2;
  itk::VariableSizeMatrix<double> B(rank,n_s);
  vtkPoints* vertices = shell->GetPoints();

  for (int i=0;i<n_s;++i) {
    // Get spherical component of the point direction and transform them in spherical coordinates.
    double p[3];
    vertices->GetPoint(i,p);

    itk::Vector<double, 3> dITK;
    dITK[0] = (FlipX) ? -p[0] : p[0];
    dITK[1] = (FlipY) ? -p[1] : p[1];
    dITK[2] = (FlipZ) ? -p[2] : p[2];

    const itk::Vector<double, 3> v = Cartesian2Spherical(dITK);

    const double phi   = v[1];
    const double theta = v[2];
    double temp=0;
    PhiThetaDirections(i,0) = phi;
    PhiThetaDirections(i,1) = theta;

    for (int l=0,j=0;l<=order;l+=2) {

      //  Handle the case m=0
      B(j,i) = sphLegendre(l,0,theta);
      j = j+1;

      for(int m=1,s=-1;m<=l;++m,++j,s=-s) {
        temp = sphLegendre(l, m,theta)*std::sqrt(2.0);

        //-m Real like t3 at hardi.cpp but math simplified and with tr1
        B(j,i) = s*temp*(cos(m*phi));

        //+m Imag like t3 at hardi.cpp but math simplified and with tr1
        B(++j,i) = temp*(sin(s*m*phi));
      }
    }
  }

  return B;
}

itk::VariableSizeMatrix<double>
ComputeSHMatrixMaxThesis(const int order,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                         const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections)
{
  const int n_s   = shell->GetNumberOfPoints();
  const int rank =  (order+1)*(order+2)/2;
  itk::VariableSizeMatrix<double> B(rank,n_s);
  vtkPoints* vertices = shell->GetPoints();
  for (int i=0;i<n_s;++i) {

    // Get spherical component of the point direction and transform them in spherical coordinates.
    double p[3];
    vertices->GetPoint(i,p);

    itk::Vector<double, 3> dITK;
    dITK[0] = (FlipX) ? -p[0] : p[0];
    dITK[1] = (FlipY) ? -p[1] : p[1];
    dITK[2] = (FlipZ) ? -p[2] : p[2];

    const itk::Vector<double, 3> v = Cartesian2Spherical(dITK);
    const double phi   = v[1];// v.y;
    const double theta = v[2];//v.z;

    PhiThetaDirections(i,0) = phi;
    PhiThetaDirections(i,1) = theta;

    //  It is even nicer to compute the SH once (for m>0 and for m<0).
    //  The central term is given by the suite u_n
    //  TO
    double temp=0;
    for (int l=0,j=0;l<=order;l+=2,j+=2*l-1) {
      B(j,i) = sphLegendre(l,0,theta);

      for(int m=1,j1=j-1,j2=j+1;m<=l;++m,--j1,++j2) {
        temp = std::sqrt(2.0)*sphLegendre(l,m,theta);
        B(j1,i) = temp*cos(m*phi);
        B(j2,i) = temp*sin(m*phi);
      }
    }
  }

  return B;
}

itk::VariableSizeMatrix<double>
ComputeSHMatrixTournier(const int order,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                        const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections)
{
  const int n_s   = shell->GetNumberOfPoints();
  const int rank =  (order+1)*(order+2)/2;
  /*
      We declare the Bmatrix of size n_s x n_b.
    */
  itk::VariableSizeMatrix<double> B(rank,n_s);

  vtkPoints* vertices = shell->GetPoints();
  std::complex<float>  cplx_1;

  for(int i = 0; i < n_s; i++) {
    // Get spherical component of the point direction and transform them in spherical coordinates.
    double p[3];
    vertices->GetPoint(i,p);

    itk::Vector<double, 3> dITK;
    dITK[0] = (FlipX) ? -p[0] : p[0];
    dITK[1] = (FlipY) ? -p[1] : p[1];
    dITK[2] = (FlipZ) ? -p[2] : p[2];

    const itk::Vector<double, 3> v = Cartesian2Spherical(dITK);
    const double phi   = v[1]; //v.y;
    const double theta = v[2]; //v.z;

    PhiThetaDirections(i,0) = phi;
    PhiThetaDirections(i,1) = theta;
    int j = 0;
    //counter for the j dimension of B
    //get spherical component of the direction vector
    double temp=0;

    for(int l = 0; l <= order; l+=2)
      for(int m = -l,s=1; m <= l; m++,s=-s) {
        temp = sphLegendre(l,std::abs(m),theta);
        if(m >= 0) { /* positive "m" SH */
          B(j,i) =  temp*cos(m*phi);;
        }
        else { /* negative "m" SH  */
          B(j,i) = s*temp*sin(m*phi);
        }
        j++;
      }
  }

  return B;
}

itk::VariableSizeMatrix<double>
ComputeSHMatrixRshBasis(const int order,vtkPolyData* shell,const bool FlipX,const bool FlipY,
                        const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections)
{
  const int n_s   = shell->GetNumberOfPoints();
  const int rank =  (order+1)*(order+2)/2;

  itk::VariableSizeMatrix<double> B(rank,n_s);
  vtkPoints* vertices = shell->GetPoints();

  for (int i=0;i<n_s;++i) {

    // Get spherical component of the point direction and transform them in spherical coordinates.
    double p[3];
    vertices->GetPoint(i,p);

    itk::Vector<double, 3> dITK;
    dITK[0] = (FlipX) ? -p[0] : p[0];
    dITK[1] = (FlipY) ? -p[1] : p[1];
    dITK[2] = (FlipZ) ? -p[2] : p[2];

    const itk::Vector<double, 3> v = Cartesian2Spherical(dITK);
    const double phi   = v[1]; //v.y;
    const double theta = v[2]; // v.z;
    PhiThetaDirections(i,0) = phi;
    PhiThetaDirections(i,1) = theta;
    double temp=0;

    for (int l=0,j=0;l<=order;l+=2) {
      B(j,i)=sphLegendre(l,0,theta);
      j=j+1;
      for(int m=1,s=-1;m<=l;++m,++j,s=-s) {
        temp = sphLegendre(l, m,theta)*std::sqrt(2.0);
        //-m Real like RshBasis.pdf Luke Bloy eq 1.2 but math simplified and with tr1
        B(j,i)   = temp*(cos(m*phi));
        //+m Imag
        B(++j,i) = temp*(sin(m*phi));
      }
    }
  }
  return B;
}

void vtkSphericalHarmonicSource::SetSphericalHarmonics(double* _arg)
{
  vtkDebugMacro(<< this->GetClassName() << " (" << this
                << "): setting Spherical Harmonics to " << _arg);
  this->SphericalHarmonics = _arg;
  this->Modified();
}

void vtkSphericalHarmonicSource::UpdateSphericalHarmonicSource()
{
  this->sphereT->SetPolyhedraType(TesselationType);
  this->sphereT->SetResolution(Tesselation);
  this->sphereT->Update();

  itk::VariableSizeMatrix<double>  PhiThetaDirection(this->sphereT->GetOutput()->GetNumberOfPoints(),2);

  switch (TesselationBasis) {
  case SHMatrix:
  {
    BasisFunction = ComputeSHMatrix(Order,this->sphereT->GetOutput(),FlipX,FlipY,FlipZ,PhiThetaDirection);
    break;
  }
  case SHMatrixMaxThesis:
  {
    BasisFunction = ComputeSHMatrixMaxThesis(Order,this->sphereT->GetOutput(),FlipX,FlipY,FlipZ,PhiThetaDirection);
    break;
  }
  case SHMatrixTournier:
  {
    BasisFunction = ComputeSHMatrixTournier(Order,this->sphereT->GetOutput(),FlipX,FlipY,FlipZ,PhiThetaDirection);
    break;
  }
  case SHMatrixRshBasis:
  {
    BasisFunction = ComputeSHMatrixRshBasis(Order,this->sphereT->GetOutput(),FlipX,FlipY,FlipZ,PhiThetaDirection);
    break;
  }
  }
  PhiThetaShellDirections = PhiThetaDirection;

}

void TranslateAndDeformShell(vtkPolyData *shell,vtkPoints* outPts,double center[3],
                             bool deform,vtkMatrix4x4* transform)
{
  vtkPoints* inPts = shell->GetPoints();
  const int  n     = inPts->GetNumberOfPoints();

//  double range[2];
//  shell->GetPointData()->GetScalars()->GetRange(range);
//  const double rangeDiff = (range[0]!=range[1]) ? range[1]-range[0] : 1.;

  vtkDataArray* sValues  = shell->GetPointData()->GetScalars();

  for (int i=0;i<n;++i) {
    double point[4];
    inPts->GetPoint(i,point);

    if (deform) {
      const double val = sValues->GetTuple1(i);
      point[0] = (val)*point[0];// ((val-range[0])/rangeDiff)*point[0];
      point[1] = (val)*point[1];//((val-range[0])/rangeDiff)*point[1];
      point[2] =(val)*point[2];// ((val-range[0])/rangeDiff)*point[2];
    }
    point[3] = 1.0;
    const double* pointOut = (transform!=0) ? transform->MultiplyDoublePoint(point) : &point[0];
    outPts->InsertNextPoint(pointOut[0]+center[0],pointOut[1]+center[1],pointOut[2]+center[2]);
  }
}


#define BOOST_TEST_MODULE interface
#include <boost/test/included/unit_test.hpp>

#include <cmath>
#include <limits>
#include <map>
#include <stdexcept>
#include <string>

#include "matheval.hpp"

BOOST_AUTO_TEST_CASE(integration1) {
    std::string expr = "pow(x/2 + sqrt(x**2/4 + y**3/24), 1/3)";
    double x = 2.0, y = -1.0;

    matheval::Parser parser;
    BOOST_CHECK_NO_THROW(parser.parse(expr));

    std::map<std::string, double> symbol_table;
    symbol_table.insert(std::make_pair("x", x));
    symbol_table.insert(std::make_pair("y", y));

    double result = 0;
    BOOST_CHECK_NO_THROW(result = parser.evaluate(symbol_table));

    double expected = std::pow(
        x / 2. + std::sqrt(std::pow(x, 2.) / 4. + std::pow(y, 3.) / 24.),
        1. / 3.);
    BOOST_CHECK_CLOSE_FRACTION(result, expected,
                               std::numeric_limits<double>::epsilon());
}

BOOST_AUTO_TEST_CASE(integration2) {
    std::string expr = "(";

    matheval::Parser parser;

    // Parsing should fail
    BOOST_CHECK_THROW(parser.parse(expr), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(integration3) {
    std::string expr = "x";

    matheval::Parser parser;

    BOOST_CHECK_NO_THROW(parser.parse(expr));

    // Evaluating should fail
    BOOST_CHECK_THROW(parser.evaluate(),
                      std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(integration4) {
    std::string expr = "1 + 1";

    matheval::Parser parser;

    BOOST_CHECK_NO_THROW(parser.parse(expr));

    BOOST_CHECK_NO_THROW(parser.optimize());
}


#pragma once

#include <Eigen/Core>
#include <complex>
#include <cstddef>
#include <memory>
#include <utility>
#include <vector>
#include "ear/fft.hpp"

namespace ear {
  namespace dsp {

    namespace block_convolver_impl {
      /// Type for real data (float).
      using real_t = float;
      /// Type for complex data.
      using complex_t = std::complex<real_t>;

      using TDVector = Eigen::Matrix<real_t, Eigen::Dynamic, 1>;
      using FDVector = Eigen::Matrix<complex_t, Eigen::Dynamic, 1>;

      /** Static data required to perform convolution of a particular block
       * size; may be shared between any number of BlockConvolver and
       * BlockConvolver::Filter instances. */
      class Context {
       public:
        /** Create a Context with a given block size.
         * @param block_size Block size in samples.
         * @param fft_impl FFT implementation to use.
         */
        Context(size_t block_size, FFTImpl<real_t> &fft_impl);

       private:
        // number of samples in a block
        const size_t block_size;

        // fft of block_size * 2
        std::shared_ptr<FFTPlan<real_t>> fft;

        // time domain fft block size
        const size_t td_size;
        // frequency domain fft block size
        const size_t fd_size;

        friend class BlockConvolver;
        friend class Filter;
      };

      /** A filter response which may be shared between many BlockConvolver
       * instances.
       *
       * This stores the pre-transformed filter blocks. */
      class Filter {
       public:
        Filter(const std::shared_ptr<Context> &ctx, const TDVector &filter);
        Filter(const std::shared_ptr<Context> &ctx, size_t n,
               const real_t *filter);

        /** The number of blocks in the filter. */
        size_t num_blocks() const { return blocks.size(); }

       private:
        std::vector<FDVector> blocks;

        friend class BlockConvolver;
      };

      /** BlockConvolver implements partitioned overlap-add convolution with a
       * fixed block size, with efficient fading between filters.
       */
      class BlockConvolver {
       public:
        /** Create a BlockConvolver given the block size and number of blocks.
         * @param ctx Context required for transformations.
         * @param num_blocks Maximum number of blocks of any filter used.
         */
        BlockConvolver(const std::shared_ptr<Context> &ctx, size_t num_blocks);

        /** Create a BlockConvolver given the block size and number of blocks.
         *  If filter == nullptr, num_blocks must be specified.
         * @param ctx Context required for transformations.
         * @param filter Initial filter to be used, or nullptr for no filter.
         * @param num_blocks Maximum number of blocks of any filter used; using
         * 0 will take the number of blocks from the passed filter.
         */
        BlockConvolver(const std::shared_ptr<Context> &ctx,
                       const std::shared_ptr<const Filter> &filter,
                       size_t num_blocks = 0);

        /** Pass a block of audio through the filter.
         * @param in Input samples of length block_size
         * @param out Output samples of length block_size
         */
        void process(const Eigen::Ref<const TDVector> &in,
                     Eigen::Ref<TDVector> out);

        void process(const float *in, float *out);

        /** Crossfade to a new filter during the next block.
         *
         * This is equivalent to:
         * - Creating a new convolver.
         * - Passing the next block of samples through the old and new
         * convolvers, with the input to the old faded down across the block,
         * and the input to the new faded up across the block. All subsequent
         * blocks are passed through the new filter.
         * - Mixing the output of the old and new filters for the next
         * num_blocks blocks.
         *
         * @param filter Filter to crossfade to; should be alive for as long as
         * it is active. Pass nullptr for no filter.
         */
        void crossfade_filter(const std::shared_ptr<const Filter> &filter);

        /** Switch to a different filter at the start of the next block.
         * @param filter Filter to switch to; should be alive for as long as it
         * is active. Pass nullptr for no filter.
         */
        void set_filter(const std::shared_ptr<const Filter> &filter);

       private:
        // wrapper around an eigen type which knows if it contains only zeros
        template <typename T>
        struct ZeroTrack {
          template <typename... Args>
          ZeroTrack(Args &&...args) : data(std::forward<Args>(args)...) {
            clear();
          }
          T data;
          bool zero = false;
          /** Get access to the data for reading. */
          const T &read() { return data; }
          /** Get access to the data for writing; clears the zero flag. */
          T &write() {
            zero = false;
            return data;
          }
          /** Zero the buffer and set the zero flag. */
          void clear() {
            if (!zero) {
              zero = true;
              data.setZero();
            }
          }
        };

        std::shared_ptr<Context> ctx;
        std::unique_ptr<FFTWorkBuf> fft_work_buf;
        const size_t num_blocks;

        // check that a filter is valid for use with this convolver
        void check_filter(const std::shared_ptr<const Filter> &filter);

        // filters(i) accesses a circular buffer of filters, length num_blocks
        // + 1. on each frame, the input is crossfaded up and down, and passed
        // through
        // - filters[1:] (old)
        // - filters[:-1] (new)
        // After process, filters has been shifted one along, with filters(0)
        // left at filters(1); filters(0) is then set to filters(1), such that
        // the next filter is the same as the previous by default. set_filter
        // simply writes to filters(0).
        std::shared_ptr<const Filter> &filters(size_t i);
        // filter_queue and filter_ofs implement the above circular buffer.
        std::vector<std::shared_ptr<const Filter>> filter_queue;
        size_t filter_ofs;

        // a queue of spectra of the input, after zero padding on the right hand
        // side. If the filter is changed before the input block i frames ago,
        // spectra_old(i) contains the input faded down (to convolve with the
        // old filter) and spectra_new(i) contains the input faded up (to
        // convolve with the new filter). If the filter is not changed, only
        // spectra_new(i) contains data. num_blocks in length, each of size n+1.
        ZeroTrack<FDVector> &spectra_old(size_t i);
        ZeroTrack<FDVector> &spectra_new(size_t i);
        // implementation the above circular buffer
        std::vector<ZeroTrack<FDVector>> spectra_queue_old;
        std::vector<ZeroTrack<FDVector>> spectra_queue_new;
        size_t spectra_ofs;

        // Rotate the filter and spectra queues.
        void rotate_queues();

        // The second half of the ifft output for the last block, added to the
        // first half before output; size n.
        ZeroTrack<TDVector> last_tail;

        // Temporaries used by process to store the padded and faded inputs;
        // size 2n, the second half should always be zeros.
        ZeroTrack<TDVector> current_td_old;
        ZeroTrack<TDVector> current_td_new;
        // Temporary used by process to store the multiplied spectrum; size n+1.
        ZeroTrack<FDVector> multiply_out;
        // Temporary used to store the time domain output, size 2n.
        ZeroTrack<TDVector> out_td;
      };

    }  // namespace block_convolver_impl
  }  // namespace dsp
}  // namespace ear


//
// Created by krematas on 3/9/18.
//

//
// Created by krematas on 3/9/18.
//
#include <Eigen/Sparse>
#include "core.h"

typedef Eigen::SparseMatrix<var_t> SpMat;
typedef Eigen::Triplet<var_t> T;

void getPixelNeighbors(int height, int width, std::vector<std::vector<int>>& neighborId){

    for(int i=0; i<height; i++){
        for(int j=0; j<width; j++){

            if(i == 0){
                neighborId[i*width+j].push_back((i+1)*width+j);
            }else if(i == height-1){
                neighborId[i*width+j].push_back((i-1)*width+j);
            }else{
                neighborId[i*width+j].push_back((i+1)*width+j);
                neighborId[i*width+j].push_back((i-1)*width+j);
            }

            if(j == 0){
                neighborId[i*width+j].push_back(i*width+j+1);
            }else if(j == width-1){
                neighborId[i*width+j].push_back(i*width+j-1);
            }else{
                neighborId[i*width+j].push_back(i*width+j+1);
                neighborId[i*width+j].push_back(i*width+j-1);
            }

        }
    }

}

void getLabelPosition(var_t *img, int h, int w, std::map<int, std::vector<int>>& ht){
    for(int i=0; i<h; i++) {
        for (int j = 0; j < w; j++) {

            if(img[i*w+j] >= 1.0){
                int lbl = int(img[i*w+j]-1);
                ht[lbl].push_back(i*w+j);
            }

        }
    }
}

SpMat setU(int N, std::map<int, std::vector<int>>& ht, Eigen::VectorXf& y){
    std::vector<T> tripletList;

    for(std::map<int,std::vector<int>>::iterator it = ht.begin(); it != ht.end(); ++it) {
        std::vector<int> pixLocation = it->second;
        for(int i=0; i<pixLocation.size();i++){
            tripletList.push_back(T(pixLocation[i], pixLocation[i], 1.));
            y[pixLocation[i]] = float(it->first);
        }
    }

    SpMat U(N,N);
    U.setFromTriplets(tripletList.begin(), tripletList.end());
    return U;
}

void setDW(var_t* image, var_t* edges, int h, int w, std::vector<SpMat>& out, float sigma1, float sigma2){

    int N = h * w;
    std::vector<std::vector<int>> neighborId(N);
    getPixelNeighbors(h, w, neighborId);

    std::vector<T> tripletListD;
    std::vector<T> tripletListW;
    int M = 0;

    for(int i=0; i<neighborId.size(); i++){
        int x, y;
        y = i/w;
        x = i%w;
        var_t r1 = image[y*w*3+x*3+0];
        var_t g1 = image[y*w*3+x*3+1];
        var_t b1 = image[y*w*3+x*3+2];
        var_t e1 = edges[y*w+x];

        for(int j=0; j<neighborId[i].size(); j++){

            y = neighborId[i][j]/w;
            x = neighborId[i][j]%w;
            var_t r2 = image[y*w*3+x*3+0];
            var_t g2 = image[y*w*3+x*3+1];
            var_t b2 = image[y*w*3+x*3+2];

            var_t weight0 = exp(-((r1-r2)*(r1-r2) + (g1-g2)*(g1-g2)+ (b1-b2)*(b1-b2))/sigma1);
            var_t weight1 = exp(-(e1*e1)/sigma2);

            tripletListD.push_back(T(M, i, 1.));
            tripletListD.push_back(T(M, neighborId[i][j], -1.));
            tripletListW.push_back(T(M, M, weight0*weight1));

            M++;

        }
    }

    SpMat D(M, N);
    D.setFromTriplets(tripletListD.begin(), tripletListD.end());

    SpMat W(M, M);
    W.setFromTriplets(tripletListW.begin(), tripletListW.end());
    out.push_back(D);
    out.push_back(W);
}

var_t* segmentFromPoses(var_t *img, var_t *edges, var_t *poseData, int height, int width, float sigma1, float sigma2){
    std::map<int, std::vector<int>> ht;
    getLabelPosition(poseData, height, width, ht);
    Eigen::VectorXf y(height*width);
    SpMat U = setU(height*width, ht, y);
    std::vector<SpMat> DW;
    setDW(img, edges, height, width, DW, sigma1, sigma2);
    SpMat D = DW[0];
    SpMat W = DW[1];

    Eigen::VectorXf b = U*y;

    SpMat A = U + D.transpose()*W*D;

    Eigen::SimplicialCholesky <SpMat> solver(A);
    Eigen::VectorXf x = solver.solve(b);

    var_t *output = new var_t[height*width];
    for(int i=0; i<height*width; i++)
        output[i] = x[i];

    return output;
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "Utils/Math/QuaternionFit.h"
#include "Utils/Geometry/ElementInfo.h"
#include "Utils/Typenames.h"
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <utility>

using namespace Eigen;

namespace Scine {
namespace Utils {

Eigen::VectorXd QuaternionFit::makeWeightsVector(const ElementTypeCollection& elements) {
  Eigen::VectorXd vec(elements.size());
  for (int i = 0; i < elements.size(); ++i) {
    vec(i) = ElementInfo::mass(elements[i]);
  }
  return vec;
}

void QuaternionFit::align() {
  /*
   * Translation into origin
   */
  // multiply positions with weights colwise: mat.array().colwise()*weights.array()
  // sum into vector and divide: (...).sum() / weights.sum()
  refCenter_ = (refMat_.array().colwise() * weights_.array()).colwise().sum() / weights_.sum();
  fitCenter_ = (fitMat_.array().colwise() * weights_.array()).colwise().sum() / weights_.sum();
  fittedMat_ = fitMat_.rowwise() - fitCenter_.transpose();

  /*
   * Rotation
   */
  Eigen::Matrix4d b = Eigen::Matrix4d::Zero();
  // generate decomposable matrix per atom and add them
  for (int i = 0; i < fitMat_.rows(); i++) {
    Eigen::Vector3d fitPos = fittedMat_.row(i);
    Eigen::Vector3d refPos = refMat_.row(i) - refCenter_.transpose();
    Eigen::Matrix4d a = Eigen::Matrix4d::Zero();
    a.block(0, 1, 1, 3) = (fitPos - refPos).transpose();
    a.block(1, 0, 3, 1) = refPos - fitPos;
    a.block(1, 1, 3, 3) = Eigen::Matrix3d::Identity().rowwise().cross(refPos + fitPos);
    b += a.transpose() * a * weights_[i];
  }

  // Decompose b
  SelfAdjointEigenSolver<Matrix4d> eigensolver(b);

  // Apply rotation
  //   If the eigenvalue of the last eigenvector is larger (absolute) than that of the first one
  //   it is beneficial to allow a rotation including inversion, as this will lead to the better
  //   fit.
  if (improperRotationIsAllowed_ && fabs(eigensolver.eigenvalues()[0]) < fabs(eigensolver.eigenvalues()[3])) {
    const Eigen::Vector4d& q = eigensolver.eigenvectors().col(3);
    rotMat_ = -Quaterniond(q[0], q[1], q[2], q[3]).toRotationMatrix();
    fittedMat_ = (rotMat_ * fittedMat_.transpose()).transpose();
    maxEigenvalue_ = eigensolver.eigenvalues()[3];
  }
  else {
    const Eigen::Vector4d& q = eigensolver.eigenvectors().col(0);
    rotMat_ = Quaterniond(q[0], q[1], q[2], q[3]).toRotationMatrix();
    fittedMat_ = (rotMat_ * fittedMat_.transpose()).transpose();
    maxEigenvalue_ = eigensolver.eigenvalues()[0];
  }

  /*
   * Translation onto reference center
   */
  fittedMat_ = fittedMat_.rowwise() + refCenter_.transpose();
}

double QuaternionFit::getRMSD() const {
  // collect squared norms: ((refMat_-fittedMat_).rowwise().squaredNorm()
  // sum, divide and sqrt: std::sqrt( (...).sum() / refMat_.rows() )
  return std::sqrt((refMat_ - fittedMat_).rowwise().squaredNorm().sum() / refMat_.rows());
}

double QuaternionFit::getWeightedRMSD(const Eigen::VectorXd& weights) const {
  // collect sqared norms: ((refMat_-fittedMat_).rowwise().squaredNorm()
  // multiply with corresponding weight: (...).array()*weights.array()
  // sum, divide and sqrt: std::sqrt( (...).sum() / refMat_.rows() )
  return std::sqrt(((refMat_ - fittedMat_).rowwise().squaredNorm().array() * weights.array()).sum() / refMat_.rows());
}

double QuaternionFit::getWeightedRMSD() const {
  return std::move(getWeightedRMSD(weights_));
}

Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> QuaternionFit::getFittedData() const {
  return fittedMat_;
}

Eigen::Matrix3d QuaternionFit::getRotationMatrix() const {
  return rotMat_.transpose();
}

Eigen::Vector3d QuaternionFit::getTransVector() const {
  return fitCenter_ - refCenter_;
}

double QuaternionFit::getRotRMSD() const {
  double rotRMSD = (fitMat_.rowwise() - fitCenter_.transpose()).rowwise().squaredNorm().sum();
  rotRMSD += (refMat_.rowwise() - refCenter_.transpose()).rowwise().squaredNorm().sum();
  rotRMSD -= 2.0 * abs(maxEigenvalue_);
  if (rotRMSD > 0)
    return sqrt(rotRMSD / refMat_.rows());
  return 0.0;
}

} /* namespace Utils */
} /* namespace Scine */


#include "IntegratorGL.hh"

#include <Eigen/Dense>
#include <map>
#include <iterator>

#include "GSLSamplerGL.hh"
#include "TypesFunctions.hh"

using namespace Eigen;
using namespace std;

IntegratorGL::IntegratorGL(int orders) : IntegratorBase(orders)
{
  init_sampler();
}

IntegratorGL::IntegratorGL(size_t bins, int orders, double* edges) : IntegratorBase(bins, orders, edges)
{
  init_sampler();
}

IntegratorGL::IntegratorGL(size_t bins, int* orders, double* edges) : IntegratorBase(bins, orders, edges)
{
  init_sampler();
}

void IntegratorGL::sample(FunctionArgs& fargs){
  auto& rets=fargs.rets;
  GSLSamplerGL sampler;
  sampler.fill_bins(m_orders.size(), m_orders.data(), m_edges.data(), rets[0].buffer, m_weights.data());
  rets[1].x = m_edges.cast<double>();
  rets[2].x = 0.0;
  auto npoints=m_edges.size()-1;
  rets[3].x = 0.5*(m_edges.tail(npoints)+m_edges.head(npoints));
  rets.untaint();
  rets.freeze();
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <Eigen/LU>
#include <algorithm>

template<typename MatrixType> void inverse_permutation_4x4()
{
  typedef typename MatrixType::Scalar Scalar;
  typedef typename MatrixType::RealScalar RealScalar;
  Vector4i indices(0,1,2,3);
  for(int i = 0; i < 24; ++i)
  {
    MatrixType m = PermutationMatrix<4>(indices);
    MatrixType inv = m.inverse();
    double error = double( (m*inv-MatrixType::Identity()).norm() / NumTraits<Scalar>::epsilon() );
    EIGEN_DEBUG_VAR(error)
    VERIFY(error == 0.0);
    std::next_permutation(indices.data(),indices.data()+4);
  }
}

template<typename MatrixType> void inverse_general_4x4(int repeat)
{
  typedef typename MatrixType::Scalar Scalar;
  typedef typename MatrixType::RealScalar RealScalar;
  double error_sum = 0., error_max = 0.;
  for(int i = 0; i < repeat; ++i)
  {
    MatrixType m;
    RealScalar absdet;
    do {
      m = MatrixType::Random();
      absdet = ei_abs(m.determinant());
    } while(absdet < NumTraits<Scalar>::epsilon());
    MatrixType inv = m.inverse();
    double error = double( (m*inv-MatrixType::Identity()).norm() * absdet / NumTraits<Scalar>::epsilon() );
    error_sum += error;
    error_max = std::max(error_max, error);
  }
  std::cerr << "inverse_general_4x4, Scalar = " << type_name<Scalar>() << std::endl;
  double error_avg = error_sum / repeat;
  EIGEN_DEBUG_VAR(error_avg);
  EIGEN_DEBUG_VAR(error_max);
   // FIXME that 1.25 used to be a 1.0 until the NumTraits changes on 28 April 2010, what's going wrong??
   // FIXME that 1.25 used to be 1.2 until we tested gcc 4.1 on 30 June 2010 and got 1.21.
  VERIFY(error_avg < (NumTraits<Scalar>::IsComplex ? 8.0 : 1.25));
  VERIFY(error_max < (NumTraits<Scalar>::IsComplex ? 64.0 : 20.0));
}

void test_prec_inverse_4x4()
{
  CALL_SUBTEST_1((inverse_permutation_4x4<Matrix4f>()));
  CALL_SUBTEST_1(( inverse_general_4x4<Matrix4f>(200000 * g_repeat) ));

  CALL_SUBTEST_2((inverse_permutation_4x4<Matrix<double,4,4,RowMajor> >()));
  CALL_SUBTEST_2(( inverse_general_4x4<Matrix<double,4,4,RowMajor> >(200000 * g_repeat) ));

  CALL_SUBTEST_3((inverse_permutation_4x4<Matrix4cf>()));
  CALL_SUBTEST_3((inverse_general_4x4<Matrix4cf>(50000 * g_repeat)));
}


#include <boost/test/unit_test.hpp>
#include <complex>
#include "../../JeanBaptiste/include/AlgorithmFactory.h"
#include "../include/AlgorithmFixture.h"
#include <string>

namespace ut = boost::unit_test;
namespace jb = jeanbaptiste;
namespace jbo = jeanbaptiste::options;

class Radix4Fixture
    : public AlgorithmFixture
{
protected:
    bool initialized_;

public:
    Radix4Fixture()
        : AlgorithmFixture()
    {
        BOOST_TEST_MESSAGE("Setup fixture: square pulse of 64 samples.");
        BOOST_TEST((initialized_ = algorithmResult_.initialize("../../test cases/square pulse (n=64).xml", "fft.in", workingSet_, expectedOutIFFT_, "fft.out", expectedOutFFT_)), "Loading test data failed.");
    }

    ~Radix4Fixture()
    {}
};

BOOST_FIXTURE_TEST_SUITE(Radix4TestSuite, Radix4Fixture)

    BOOST_AUTO_TEST_CASE(fft_radix_4_dif)
    {
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Running radix 4 DIF FFT and IFFT.");

        // Create Radix-4 DIF FFT algorithms for sample counts 4 ... 1024.
        jb::AlgorithmFactory<1, 5, jbo::Radix_4, jbo::Decimation_In_Frequency, jbo::Direction_Forward, jbo::Window_None,
            jbo::Normalization_Square_Root, std::complex<double>> fftFactory;

        // Create Radix-4 DIF IFFT algorithms for sample counts 4 ... 1024.
        jb::AlgorithmFactory<1, 5, jbo::Radix_4, jbo::Decimation_In_Frequency, jbo::Direction_Backward, jbo::Window_None,
            jbo::Normalization_Square_Root, std::complex<double>> ifftFactory;

        runAlgorithms(fftFactory.getAlgorithm(3), ifftFactory.getAlgorithm(3));
    }

    BOOST_AUTO_TEST_CASE(fft_radix_4_dit)
    {
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Running radix 4 DIT FFT and IFFT.");

        // Create Radix-4 DIT FFT algorithms for sample counts 4 ... 1024.
        jb::AlgorithmFactory<1, 5, jbo::Radix_4, jbo::Decimation_In_Time, jbo::Direction_Forward, jbo::Window_None,
            jbo::Normalization_Square_Root, std::complex<double>> fftFactory;

        // Create Radix-4 DIT IFFT algorithms for sample counts 4 ... 1024.
        jb::AlgorithmFactory<1, 5, jbo::Radix_4, jbo::Decimation_In_Time, jbo::Direction_Backward, jbo::Window_None,
            jbo::Normalization_Square_Root, std::complex<double>> ifftFactory;

        runAlgorithms(fftFactory.getAlgorithm(3), ifftFactory.getAlgorithm(3));
    }

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/atand.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>

STF_CASE_TPL (" atandreal",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::atand;

  using r_t = decltype(atand(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(atand(bs::Inf<T>()), 90, 0.5);
  STF_ULP_EQUAL(atand(bs::Minf<T>()), -90, 0.5);
  STF_ULP_EQUAL(atand(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(atand(bs::Half<T>()), T(2.656505117707799e+01), 0.5);
  STF_ULP_EQUAL(atand(bs::Mhalf<T>()), T(-2.656505117707799e+01), 0.5);
  STF_ULP_EQUAL(atand(bs::Mone<T>()), T(-45), 0.5);
  STF_ULP_EQUAL(atand(bs::One<T>()), T(45), 0.5);
  STF_ULP_EQUAL(atand(bs::Zero<T>()), bs::Zero<r_t>(), 0.5);
}


/** @file
 *
 * This file is part of Boost, not part of Chaste per se.
 *
 * We use the <autogenerated> tag to ignore it from our Doxygen checker.
 *
 * This file is provided to users of Boost up to 1.63 inclusive and gives forward compatibility with
 * Boost 1.64-1.65 and hopefully beyond.
 * (An optimisation in exponential distributions was applied to boost in version 1.64,
 * which also affects normal distribution tails.)
 *
 * Minimal changes were made to the file to include it here, simply giving the class a unique name
 * and making it use the Chaste copy of exponential_distribution rather than the boost one.
 *
 */

/* boost random/gamma_distribution.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 */

#ifndef BOOST_165_RANDOM_GAMMA_DISTRIBUTION_HPP
#define BOOST_165_RANDOM_GAMMA_DISTRIBUTION_HPP

#include <boost/assert.hpp>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/limits.hpp>
#include <boost/random/detail/config.hpp>
//#include <boost/random/exponential_distribution.hpp> // swapped for chaste copy of exponential v1.65
#include <boost/static_assert.hpp>
#include <iosfwd>
#include <istream>
#include "Boost165ExponentialDistribution.hpp"

namespace boost
{
namespace random
{

    // The algorithm is taken from Knuth

    /**
 * The gamma distribution is a continuous distribution with two
 * parameters alpha and beta.  It produces values > 0.
 *
 * It has
 * \f$\displaystyle p(x) = x^{\alpha-1}\frac{e^{-x/\beta}}{\beta^\alpha\Gamma(\alpha)}\f$.
 */
    template <class RealType = double>
    class gamma_distribution_v165
    {
    public:
        typedef RealType input_type;
        typedef RealType result_type;

        class param_type
        {
        public:
            typedef gamma_distribution_v165 distribution_type;

            /**
         * Constructs a @c param_type object from the "alpha" and "beta"
         * parameters.
         *
         * Requires: alpha > 0 && beta > 0
         */
            param_type(const RealType& alpha_arg = RealType(1.0),
                       const RealType& beta_arg = RealType(1.0))
                    : _alpha(alpha_arg), _beta(beta_arg)
            {
            }

            /** Returns the "alpha" parameter of the distribution. */
            RealType alpha() const { return _alpha; }
            /** Returns the "beta" parameter of the distribution. */
            RealType beta() const { return _beta; }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
            /** Writes the parameters to a @c std::ostream. */
            template <class CharT, class Traits>
            friend std::basic_ostream<CharT, Traits>&
            operator<<(std::basic_ostream<CharT, Traits>& os,
                       const param_type& parm)
            {
                os << parm._alpha << ' ' << parm._beta;
                return os;
            }

            /** Reads the parameters from a @c std::istream. */
            template <class CharT, class Traits>
            friend std::basic_istream<CharT, Traits>&
            operator>>(std::basic_istream<CharT, Traits>& is, param_type& parm)
            {
                is >> parm._alpha >> std::ws >> parm._beta;
                return is;
            }
#endif

            /** Returns true if the two sets of parameters are the same. */
            friend bool operator==(const param_type& lhs, const param_type& rhs)
            {
                return lhs._alpha == rhs._alpha && lhs._beta == rhs._beta;
            }
            /** Returns true if the two sets fo parameters are different. */
            friend bool operator!=(const param_type& lhs, const param_type& rhs)
            {
                return !(lhs == rhs);
            }

        private:
            RealType _alpha;
            RealType _beta;
        };

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
        BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

        /**
     * Creates a new gamma_distribution with parameters "alpha" and "beta".
     *
     * Requires: alpha > 0 && beta > 0
     */
        explicit gamma_distribution_v165(const result_type& alpha_arg = result_type(1.0),
                                         const result_type& beta_arg = result_type(1.0))
                : _exp(result_type(1)), _alpha(alpha_arg), _beta(beta_arg)
        {
            BOOST_ASSERT(_alpha > result_type(0));
            BOOST_ASSERT(_beta > result_type(0));
            init();
        }

        /** Constructs a @c gamma_distribution from its parameters. */
        explicit gamma_distribution_v165(const param_type& parm)
                : _exp(result_type(1)), _alpha(parm.alpha()), _beta(parm.beta())
        {
            init();
        }

        // compiler-generated copy ctor and assignment operator are fine

        /** Returns the "alpha" paramter of the distribution. */
        RealType alpha() const { return _alpha; }
        /** Returns the "beta" parameter of the distribution. */
        RealType beta() const { return _beta; }
        /** Returns the smallest value that the distribution can produce. */
        RealType min BOOST_PREVENT_MACRO_SUBSTITUTION() const { return 0; }
        /* Returns the largest value that the distribution can produce. */
        RealType max BOOST_PREVENT_MACRO_SUBSTITUTION() const
        {
            return (std::numeric_limits<RealType>::infinity)();
        }

        /** Returns the parameters of the distribution. */
        param_type param() const { return param_type(_alpha, _beta); }
        /** Sets the parameters of the distribution. */
        void param(const param_type& parm)
        {
            _alpha = parm.alpha();
            _beta = parm.beta();
            init();
        }

        /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
        void reset() { _exp.reset(); }

        /**
     * Returns a random variate distributed according to
     * the gamma distribution.
     */
        template <class Engine>
        result_type operator()(Engine& eng)
        {
#ifndef BOOST_NO_STDC_NAMESPACE
            // allow for Koenig lookup
            using std::tan;
            using std::sqrt;
            using std::exp;
            using std::log;
            using std::pow;
#endif
            if (_alpha == result_type(1))
            {
                return _exp(eng) * _beta;
            }
            else if (_alpha > result_type(1))
            {
                // Can we have a boost::mathconst please?
                const result_type pi = result_type(3.14159265358979323846);
                for (;;)
                {
                    result_type y = tan(pi * uniform_01<RealType>()(eng));
                    result_type x = sqrt(result_type(2) * _alpha - result_type(1)) * y
                        + _alpha - result_type(1);
                    if (x <= result_type(0))
                        continue;
                    if (uniform_01<RealType>()(eng) > (result_type(1) + y * y) * exp((_alpha - result_type(1))
                                                                                * log(x / (_alpha - result_type(1)))
                                                                                - sqrt(result_type(2) * _alpha
                                                                                - result_type(1))
                                                                                * y))
                        continue;
                    return x * _beta;
                }
            }
            else /* alpha < 1.0 */
            {
                for (;;)
                {
                    result_type u = uniform_01<RealType>()(eng);
                    result_type y = _exp(eng);
                    result_type x, q;
                    if (u < _p)
                    {
                        x = exp(-y / _alpha);
                        q = _p * exp(-x);
                    }
                    else
                    {
                        x = result_type(1) + y;
                        q = _p + (result_type(1) - _p) * pow(x, _alpha - result_type(1));
                    }
                    if (u >= q)
                        continue;
                    return x * _beta;
                }
            }
        }

        template <class URNG>
        RealType operator()(URNG& urng, const param_type& parm) const
        {
            return gamma_distribution_v165(parm)(urng);
        }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
        /** Writes a @c gamma_distribution_v165 to a @c std::ostream. */
        template <class CharT, class Traits>
        friend std::basic_ostream<CharT, Traits>&
        operator<<(std::basic_ostream<CharT, Traits>& os,
                   const gamma_distribution_v165& gd)
        {
            os << gd.param();
            return os;
        }

        /** Reads a @c gamma_distribution from a @c std::istream. */
        template <class CharT, class Traits>
        friend std::basic_istream<CharT, Traits>&
        operator>>(std::basic_istream<CharT, Traits>& is, gamma_distribution_v165& gd)
        {
            gd.read(is);
            return is;
        }
#endif

        /**
     * Returns true if the two distributions will produce identical
     * sequences of random variates given equal generators.
     */
        friend bool operator==(const gamma_distribution_v165& lhs,
                               const gamma_distribution_v165& rhs)
        {
            return lhs._alpha == rhs._alpha
                && lhs._beta == rhs._beta
                && lhs._exp == rhs._exp;
        }

        /**
     * Returns true if the two distributions can produce different
     * sequences of random variates, given equal generators.
     */
        friend bool operator!=(const gamma_distribution_v165& lhs,
                               const gamma_distribution_v165& rhs)
        {
            return !(lhs == rhs);
        }

    private:
        /// \cond hide_private_members

        template <class CharT, class Traits>
        void read(std::basic_istream<CharT, Traits>& is)
        {
            param_type parm;
            if (is >> parm)
            {
                param(parm);
            }
        }

        void init()
        {
#ifndef BOOST_NO_STDC_NAMESPACE
            // allow for Koenig lookup
            using std::exp;
#endif
            _p = exp(result_type(1)) / (_alpha + exp(result_type(1)));
        }
        /// \endcond

        exponential_distribution_v165<RealType> _exp;
        result_type _alpha;
        result_type _beta;
        // some data precomputed from the parameters
        result_type _p;
    };

} // namespace random

} // namespace boost

#endif // BOOST_165_RANDOM_GAMMA_DISTRIBUTION_HPP