#include <iostream>
#include <armadillo.h>
#include <digitRecognition.h>
//#include <mkl.h>

using namespace arma;

constexpr int training_size = 4000;    //m
constexpr int input_layer_size = 784;  //k
constexpr int hidden_layer_size = 500;  //n
constexpr int num_labels = 10;

constexpr double lambda = 0.1;
constexpr int max_iterations = 1000;

int main () {
//	std::cout << "Starting program with " << mkl_get_max_threads() << " threads..." << std::endl;

	//mkl_set_num_threads(4);

    mat predictions(training_size,1,fill::zeros);

    mat x_train(training_size,input_layer_size,fill::zeros);
    //x_train.load("c:\\train_x_4000.csv");
    x_train.load("./mnist_data/train_x_4000.csv");

    mat y_train(training_size,1,fill::randu);
    //y_train.load("c:\\train_y_4000.csv");
    y_train.load("./mnist_data/train_y_4000.csv");

    mat s(10000,10000,fill::randu);
    mat t(10000,10000,fill::randu);
    mat u(10000,10000,fill::zeros);

	wall_clock timer;
	timer.tic();
    u = s * t;
    double n = timer.toc();
	std::cout << "10k x 10k matrix multiplication completed in: " << n << " seconds." << endl;
    pause();

    ////calculate mean of the training data
    //double x_mean = sum(sum(x_train));
    //x_mean /= x_train.n_elem;  //33.5026  TODO: check this
    //
    ////calculate standard devication of the training data
    //mat x_train_mean(training_size,input_layer_size);
    //x_train_mean = x_train;
    //x_train_mean.transform( [x_mean](double val){return (val - x_mean);} );
    //double x_std = sum(sum(x_train_mean));
    //x_std /= (x_train_mean.n_elem);
    //x_std = std::sqrt(x_std);  //7.23942e-06  TODO: check this

	x_train = x_train / 255.0;
	//x_train.transform([](double val){return (val/255.0); });
	//std::cout << x_train.row(0) << endl;

	mat initial_theta1(hidden_layer_size, input_layer_size + 1, fill::ones);  //784 26   n k+1
	//initial_theta1.load("c:\\theta1.csv");
	initial_theta1.load("./mnist_data/theta1.csv");
	mat initial_theta2(num_labels, hidden_layer_size + 1, fill::ones);        //25 11     numlabels n+1
	//initial_theta2.load("c:\\theta2.csv");
	initial_theta2.load("./mnist_data/theta2.csv");

	initial_theta1.reshape(hidden_layer_size*(input_layer_size + 1),1);
	initial_theta2.reshape(num_labels*(hidden_layer_size + 1),1);

    mat combined_theta = join_cols(initial_theta1,initial_theta2);
    //std::cout << "combined_theta rows: " << combined_theta.n_rows << ", cols: " << combined_theta.n_cols << endl;

	//test section----------------------
	mat gradient1 = combined_theta;
	double cost=0.0;

	    //costfunction(cost, gradient1, combined_theta, input_layer_size, hidden_layer_size, num_labels, x_train, y_train, lambda);
	    fmincg(cost, max_iterations,combined_theta,input_layer_size,hidden_layer_size,num_labels,x_train,y_train,lambda);

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

	std::cout << "Final cost: " << cost << endl;

    initial_theta1 = combined_theta.submat(0, 0, initial_theta1.n_cols-1, 0);
	initial_theta1.reshape(hidden_layer_size, input_layer_size + 1);
    //std::cout << "initial_theta1 rows: " << initial_theta1.n_rows << ", cols: " << initial_theta1.n_cols << endl;

    initial_theta2 = combined_theta.submat(initial_theta1.n_cols, 0,initial_theta1.n_cols+initial_theta2.n_cols-1, 0);
	initial_theta2.reshape(num_labels, hidden_layer_size + 1);
    //std::cout << "initial_theta2 rows: " << initial_theta2.n_rows << ", cols: " << initial_theta2.n_cols << endl;

    predict(initial_theta1,initial_theta2,x_train,predictions);

    //std::cout << "predictions: " << predictions.row(0) << endl;

	//pauseJNS();

    return 0;
}

//         real function costandgradient(gradient, nn_params,input_layer_size,hidden_layer_size,num_labels, inputdata, y, lambda)

//         real, allocatable :: inputdata(:,:), X(:,:), y(:,:), y_representative(:,:), gradient(:,:), ones(:,:)
//         real, allocatable :: a_1(:,:),z_2(:,:),a_2(:,:),z_3(:,:),a_3(:,:),hofx(:,:)
//         real, allocatable :: delta_2(:,:), temp_delta_2(:,:), delta_3(:,:), gradient_1(:,:), gradient_2(:,:)
//         real, allocatable :: Theta1_grad(:,:), Theta2_grad(:,:)
//         integer :: input_layer_size,hidden_layer_size,num_labels,m,l,K,i
//         integer :: a1,a2,b1,b2,c1,c2
//         real :: lambda, temp, J
//         real, allocatable :: nn_params(:,:), Theta1(:,:), Theta2(:,:), temptheta1(:,:),temptheta2(:,:)
//         real, allocatable :: a_2_ones(:,:), summation(:,:), summation2(:,:), summation3(:,:),z_2_ones(:,:), sigmoid_z_2(:,:)

//         real :: timer1, timer2, holder1, holder2
//         double precision :: average1 = 0.0
//         double precision :: average2 = 0.0
//         double precision :: average3 = 0.0
//         double precision :: counter = 0.0

//         m = size(inputdata,1)
//         l = size(y,1)
//         K = num_labels

//         allocate(Theta1(1:hidden_layer_size,1:(input_layer_size+1)))
//         allocate(Theta2(1:num_labels,1:(hidden_layer_size+1)))
//         allocate(ones(m,1))
//         allocate(X(size(inputdata,1),(size(inputdata,2)+1)))
//         allocate(y_representative(l,num_labels))
//         allocate(z_2(m,hidden_layer_size))
//         allocate(a_1(size(X,1),size(X,2)))
//         allocate(a_2(size(z_2,1),size(z_2,2)))
//         allocate(a_2_ones(size(a_2,1),(size(a_2,2)+1)))
//         allocate(z_3(size(a_2_ones,1),size(Theta2,1)))  !a_2 * Theta2'
//         allocate(temptheta2(size(Theta2,2),size(Theta2,1)))
//         allocate(a_3(size(z_3,1),size(z_3,2)))
//         allocate(summation(size(z_3,1),size(z_3,2)))
//         allocate(summation2(size(z_3,1),size(z_3,2)))
//         allocate(summation3(size(z_3,1),size(z_3,2)))
//         allocate(delta_3(size(a_3,1),size(a_3,2)))
//         allocate(z_2_ones(size(z_2,1),(size(z_2,2)+1)))
//         allocate(sigmoid_z_2(size(z_2_ones,1),size(z_2_ones,2)))
//         allocate(temp_delta_2(size(delta_3,1),size(Theta2,2)))
//         allocate(delta_2(size(temp_delta_2,1),size(sigmoid_z_2,2)))
//         allocate(gradient_2(size(delta_3,2),size(a_2_ones,2)))
//         allocate(gradient_1(size(delta_2,2)-1,size(a_1,2)))
//         allocate(Theta2_grad(1:size(gradient_2),1))
//         allocate(Theta1_grad(1:size(gradient_1),1))

//         allocate(temptheta1(size(Theta1,2),size(Theta1,1)))

//         Theta1 = reshape(nn_params(1:hidden_layer_size*(input_layer_size + 1),1), (/ hidden_layer_size, (input_layer_size + 1) /))
//         Theta2 = reshape(nn_params((1+hidden_layer_size*(input_layer_size + 1)):,1), (/ num_labels, (hidden_layer_size + 1) /))

//         !print *,shape(Theta1)   !500 x 785
//         !print *,shape(Theta2)   !10  x 501

//         ones = 1.0

//         !timer1=secnds(0.0)

//         do i=1,m
//             temp = y(i,1)
//             do j=1,K
//                 if(temp == j)then
//                     y_representative(i,j) = 1
//                 else
//                     y_representative(i,j) = 0
//                 end if
//             end do
//         end do

//         X(:,1:1) = ones
//         X(:,2:) = inputdata
//         a_1 = X

//         z_2 = 0.0

//         call matrix_multiply(a_1,0,Theta1,1,z_2)

//         a_2 = 0.0
//         call sigmoid(z_2,a_2)

//         a_2_ones(:,1:1) = ones
//         a_2_ones(:,2:) = a_2

//         z_3 = 0.0
//         call matrix_multiply(a_2_ones,0,Theta2,1,z_3)

//         call sigmoid(z_3,a_3)

//         summation = 0.0
//         summation = -y_representative * log(a_3) - (1 - y_representative) * log(1 - a_3)

//         J = sum(summation)
//         J = J / m

//         !do concurrent (i = 1:size(delta_3,1))
//         !    delta_3(i,:) = a_3(i,:) - y(1,:)
//         !end do

//         delta_3 = a_3 - y_representative

//         z_2_ones(:,1:1) = ones
//         z_2_ones(:,2:) = z_2

//         sigmoid_z_2 = 0.0

//         call sigmoidgradient(z_2_ones,sigmoid_z_2)

//         !temp_delta_2 = matmul(delta_3,Theta2)

// !timer1=secnds(0.0)
//         !a1 = size(delta_3,1)
//         !a2 = size(delta_3,2)
//         !b1 = size(Theta2,1)
//         !b2 = size(Theta2,2)
//         !call sGEMM('N','N',a1,b2,b1,1.0,delta_3,a1,Theta2,b1,0.0,temp_delta_2,a1)
//         call matrix_multiply(delta_3,0,Theta2,0,temp_delta_2)
// !timer2=secnds(timer1)
// !average1 = average1 + timer2
// !counter = counter + 1
// !print '(a, f8.4)','matrix_multiply timer: ', average1/counter

//         delta_2 =0.0
//         !delta_2 = temp_delta_2 * sigmoid_z_2   !elemental multiplication
//         call vsMul( size(temp_delta_2), temp_delta_2, sigmoid_z_2, delta_2 )   !elemental multiplication

//         gradient_2 = 0.0
//         gradient_1 = 0.0

//         call matrix_multiply(delta_3,1,a_2_ones,0,gradient_2)

//         call matrix_multiply(delta_2(:,2:),1,a_1,0,gradient_1)

//         gradient_2 = gradient_2 / m
//         gradient_1 = gradient_1 / m

//         gradient_2(:,2:) = gradient_2(:,2:) + Theta2(:,2:) * (lambda / m)
//         gradient_1(:,2:) = gradient_1(:,2:) + Theta1(:,2:) * (lambda / m)

//         Theta2_grad = reshape(gradient_2, (/size(gradient_2),1/))
//         Theta1_grad = reshape(gradient_1, (/size(gradient_1),1/))

//         gradient(1:size(Theta1_grad),1:1) = Theta1_grad
//         gradient((size(Theta1_grad)+1):,1:1) = Theta2_grad

//         costandgradient = J

//         !print '(f8.4, a, f8.4)',average1/counter,' ',average2/counter
//         !timer2=secnds(timer1)
//         !average = average*0.9 + timer2*0.1
//         !print '(a, f8.4)','costandgradient timer: ', timer2
//     end function costandgradient


/**
 * @file recurrent_network_test.cpp
 * @author Marcus Edel
 *
 * Tests the recurrent network.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/layer/linear_layer.hpp>
#include <mlpack/methods/ann/layer/recurrent_layer.hpp>
#include <mlpack/methods/ann/layer/base_layer.hpp>
#include <mlpack/methods/ann/layer/lstm_layer.hpp>
#include <mlpack/methods/ann/layer/binary_classification_layer.hpp>

#include <mlpack/methods/ann/rnn.hpp>
#include <mlpack/methods/ann/performance_functions/mse_function.hpp>
#include <mlpack/core/optimizers/sgd/sgd.hpp>
#include <mlpack/methods/ann/activation_functions/logistic_function.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>
 #include <mlpack/methods/ann/init_rules/nguyen_widrow_init.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::ann;
using namespace mlpack::optimization;

BOOST_AUTO_TEST_SUITE(RecurrentNetworkTest);

/**
 * Construct a 2-class dataset out of noisy sines.
 *
 * @param data Input data used to store the noisy sines.
 * @param labels Labels used to store the target class of the noisy sines.
 * @param points Number of points/features in a single sequence.
 * @param sequences Number of sequences for each class.
 * @param noise The noise factor that influences the sines.
 */
void GenerateNoisySines(arma::mat& data,
                        arma::mat& labels,
                        const size_t points,
                        const size_t sequences,
                        const double noise = 0.3)
{
  arma::colvec x =  arma::linspace<arma::Col<double> >(0,
      points - 1, points) / points * 20.0;
  arma::colvec y1 = arma::sin(x + arma::as_scalar(arma::randu(1)) * 3.0);
  arma::colvec y2 = arma::sin(x / 2.0 + arma::as_scalar(arma::randu(1)) * 3.0);

  data = arma::zeros(points, sequences * 2);
  labels = arma::zeros(2, sequences * 2);

  for (size_t seq = 0; seq < sequences; seq++)
  {
    data.col(seq) = arma::randu(points) * noise + y1 +
        arma::as_scalar(arma::randu(1) - 0.5) * noise;
    labels(0, seq) = 1;

    data.col(sequences + seq) = arma::randu(points) * noise + y2 +
        arma::as_scalar(arma::randu(1) - 0.5) * noise;
    labels(1, sequences + seq) = 1;
  }
}

/**
 * Train the vanilla network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(SequenceClassificationTest)
{
  // It isn't guaranteed that the recurrent network will converge in the
  // specified number of iterations using random weights. If this works 1 of 5
  // times, I'm fine with that. All I want to know is that the network is able
  // to escape from local minima and to solve the task.
  size_t successes = 0;

  for (size_t trial = 0; trial < 5; ++trial)
  {
    // Generate 12 (2 * 6) noisy sines. A single sine contains 10 points/features.
    arma::mat input, labels;
    GenerateNoisySines(input, labels, 10, 6);

    /*
     * Construct a network with 1 input unit, 4 hidden units and 2 output units.
     * The hidden layer is connected to itself. The network structure looks like:
     *
     *  Input         Hidden        Output
     * Layer(1)      Layer(4)      Layer(2)
     * +-----+       +-----+       +-----+
     * |     |       |     |       |     |
     * |     +------>|     +------>|     |
     * |     |    ..>|     |       |     |
     * +-----+    .  +--+--+       +-----+
     *            .     .
     *            .     .
     *            .......
     */
    LinearLayer<> linearLayer0(1, 4);
    RecurrentLayer<> recurrentLayer0(4);
    BaseLayer<LogisticFunction> inputBaseLayer;

    LinearLayer<> hiddenLayer(4, 2);
    BaseLayer<LogisticFunction> hiddenBaseLayer;

    BinaryClassificationLayer classOutputLayer;

    auto modules = std::tie(linearLayer0, recurrentLayer0, inputBaseLayer,
                            hiddenLayer, hiddenBaseLayer);

    RNN<decltype(modules), BinaryClassificationLayer, RandomInitialization,
        MeanSquaredErrorFunction> net(modules, classOutputLayer);

    SGD<decltype(net)> opt(net, 0.5, 500 * input.n_cols, -100);

    net.Train(input, labels, opt);

    arma::mat prediction;
    net.Predict(input, prediction);

    size_t error = 0;
    for (size_t i = 0; i < labels.n_cols; i++)
    {
      if (arma::sum(arma::sum(arma::abs(prediction.col(i) - labels.col(i)))) == 0)
      {
        error++;
      }
    }

    double classificationError = 1 - double(error) / labels.n_cols;
    if (classificationError <= 0.2)
    {
      ++successes;
      break;
    }
  }

  BOOST_REQUIRE_GE(successes, 1);
}

/**
 * Generate a random Reber grammar.
 *
 * For more information, see the following thesis.
 *
 * @code
 * @misc{Gers2001,
 *   author = {Felix Gers},
 *   title = {Long Short-Term Memory in Recurrent Neural Networks},
 *   year = {2001}
 * }
 * @endcode
 *
 * @param transitions Reber grammar transition matrix.
 * @param reber The generated Reber grammar string.
 */
void GenerateReber(const arma::Mat<char>& transitions, std::string& reber)
{
  size_t idx = 0;
  reber = "B";

  do
  {
    const int grammerIdx = rand() % 2;
    reber += arma::as_scalar(transitions.submat(idx, grammerIdx, idx,
        grammerIdx));

    idx = arma::as_scalar(transitions.submat(idx, grammerIdx + 2, idx,
        grammerIdx + 2)) - '0';
  } while (idx != 0);

  reber =  "BPTVVE";
}

/**
 * Generate a random embedded Reber grammar.
 *
 * @param transitions Embedded Reber grammar transition matrix.
 * @param reber The generated embedded Reber grammar string.
 */
void GenerateEmbeddedReber(const arma::Mat<char>& transitions,
                           std::string& reber)
{
  GenerateReber(transitions, reber);
  const char c = (rand() % 2) == 1 ? 'P' : 'T';
  reber = c + reber + c;
  reber = "B" + reber + "E";
}

/**
 * Convert a Reber symbol to a unit vector.
 *
 * @param symbol Reber symbol to be converted.
 * @param translation The converted symbol stored as unit vector.
 */
void ReberTranslation(const char symbol, arma::colvec& translation)
{
  arma::Col<char> symbols;
  symbols << 'B' << 'T' << 'S' << 'X' << 'P' << 'V' << 'E' << arma::endr;
  const int idx = arma::as_scalar(arma::find(symbols == symbol, 1, "first"));

  translation = arma::zeros<arma::colvec>(7);
  translation(idx) = 1;
}

/**
 * Convert a unit vector to a Reber symbol.
 *
 * @param translation The unit vector to be converted.
 * @param symbol The converted unit vector stored as Reber symbol.
 */
void ReberReverseTranslation(const arma::colvec& translation, char& symbol)
{
  arma::Col<char> symbols;
  symbols << 'B' << 'T' << 'S' << 'X' << 'P' << 'V' << 'E' << arma::endr;
  const int idx = arma::as_scalar(arma::find(translation == 1, 1, "first"));

  symbol = symbols(idx);
}

/**
 * Given a Reber string, return a Reber string with all reachable next symbols.
 *
 * @param transitions The Reber transistion matrix.
 * @param reber The Reber string used to generate all reachable next symbols.
 * @param nextReber All reachable next symbols.
 */
void GenerateNextReber(const arma::Mat<char>& transitions,
                       const std::string& reber, std::string& nextReber)
{
  size_t idx = 0;

  for (size_t grammer = 1; grammer < reber.length(); grammer++)
  {
    const int grammerIdx = arma::as_scalar(arma::find(
        transitions.row(idx) == reber[grammer], 1, "first"));

    idx = arma::as_scalar(transitions.submat(idx, grammerIdx + 2, idx,
        grammerIdx + 2)) - '0';
  }

  nextReber = arma::as_scalar(transitions.submat(idx, 0, idx, 0));
  nextReber += arma::as_scalar(transitions.submat(idx, 1, idx, 1));
}

/**
 * Given a embedded Reber string, return a embedded Reber string with all
 * reachable next symbols.
 *
 * @param transitions The Reber transistion matrix.
 * @param reber The Reber string used to generate all reachable next symbols.
 * @param nextReber All reachable next symbols.
 */
void GenerateNextEmbeddedReber(const arma::Mat<char>& transitions,
                               const std::string& reber, std::string& nextReber)
{
  if (reber.length() <= 2)
  {
    nextReber = reber.length() == 1 ? "TP" : "B";
  }
  else
  {
    size_t pos = reber.find('E');
    if (pos != std::string::npos)
    {
      nextReber = pos == reber.length() - 1 ? std::string(1, reber[1]) : "E";
    }
    else
    {
      GenerateNextReber(transitions, reber.substr(2), nextReber);
    }
  }
}

/**
 * Train the specified network and the construct a Reber grammar dataset.
 */
template<typename HiddenLayerType>
void ReberGrammarTestNetwork(HiddenLayerType& hiddenLayer0,
                             bool embedded = false)
{
  // Reber state transition matrix. (The last two columns are the indices to the
  // next path).
  arma::Mat<char> transitions;
  transitions << 'T' << 'P' << '1' << '2' << arma::endr
              << 'X' << 'S' << '3' << '1' << arma::endr
              << 'V' << 'T' << '4' << '2' << arma::endr
              << 'X' << 'S' << '2' << '5' << arma::endr
              << 'P' << 'V' << '3' << '5' << arma::endr
              << 'E' << 'E' << '0' << '0' << arma::endr;

  const size_t trainReberGrammarCount = 1000;
  const size_t testReberGrammarCount = 1000;

  std::string trainReber, testReber;
  arma::field<arma::mat> trainInput(1, trainReberGrammarCount);
  arma::field<arma::mat> trainLabels(1, trainReberGrammarCount);
  arma::field<arma::mat> testInput(1, testReberGrammarCount);
  arma::colvec translation;

  // Generate the training data.
  for (size_t i = 0; i < trainReberGrammarCount; i++)
  {
    if (embedded)
      GenerateEmbeddedReber(transitions, trainReber);
    else
      GenerateReber(transitions, trainReber);

    for (size_t j = 0; j < trainReber.length() - 1; j++)
    {
      ReberTranslation(trainReber[j], translation);
      trainInput(0, i) = arma::join_cols(trainInput(0, i), translation);

      ReberTranslation(trainReber[j + 1], translation);
      trainLabels(0, i) = arma::join_cols(trainLabels(0, i), translation);
    }
  }

  // Generate the test data.
  for (size_t i = 0; i < testReberGrammarCount; i++)
  {
    if (embedded)
      GenerateEmbeddedReber(transitions, testReber);
    else
      GenerateReber(transitions, testReber);

    for (size_t j = 0; j < testReber.length() - 1; j++)
    {
      ReberTranslation(testReber[j], translation);
      testInput(0, i) = arma::join_cols(testInput(0, i), translation);
    }
  }

  /*
   * Construct a network with 7 input units, layerSize hidden units and 7 output
   * units. The hidden layer is connected to itself. The network structure looks
   * like:
   *
   *  Input         Hidden        Output
   * Layer(7)  Layer(layerSize)   Layer(7)
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |    ..>|     |       |     |
   * +-----+    .  +--+--+       +-----+
   *            .     .
   *            .     .
   *            .......
   */
  const size_t lstmSize = 4 * 10;
  LinearLayer<> linearLayer0(7, lstmSize);
  RecurrentLayer<> recurrentLayer0(10, lstmSize);

  LinearLayer<>hiddenLayer(10, 7);
  BaseLayer<LogisticFunction> hiddenBaseLayer;

  BinaryClassificationLayer classOutputLayer;

  auto modules = std::tie(linearLayer0, recurrentLayer0, hiddenLayer0,
                          hiddenLayer, hiddenBaseLayer);

  RNN<decltype(modules), BinaryClassificationLayer, RandomInitialization,
      MeanSquaredErrorFunction> net(modules, classOutputLayer);

  SGD<decltype(net)> opt(net, 0.5, 2, -200);

  arma::mat inputTemp, labelsTemp;
  for (size_t i = 0; i < 15; i++)
  {
    for (size_t j = 0; j < trainReberGrammarCount; j++)
    {
      inputTemp = trainInput.at(0, j);
      labelsTemp = trainLabels.at(0, j);
      net.Train(inputTemp, labelsTemp, opt);
    }
  }

  double error = 0;

  // Ask the network to predict the next Reber grammar in the given sequence.
  for (size_t i = 0; i < testReberGrammarCount; i++)
  {
    arma::mat output;
    arma::mat input = testInput.at(0, i);

    net.Predict(input, output);

    const size_t reberGrammerSize = 7;
    std::string inputReber = "";

    size_t reberError = 0;
    for (size_t j = 0; j < (output.n_elem / reberGrammerSize); j++)
    {
      if (arma::sum(arma::sum(output.submat(j * reberGrammerSize, 0, (j + 1) *
          reberGrammerSize - 1, 0))) != 1) break;

      char predictedSymbol, inputSymbol;
      std::string reberChoices;

      ReberReverseTranslation(output.submat(j * reberGrammerSize, 0, (j + 1) *
          reberGrammerSize - 1, 0), predictedSymbol);
      ReberReverseTranslation(input.submat(j * reberGrammerSize, 0, (j + 1) *
          reberGrammerSize - 1, 0), inputSymbol);
      inputReber += inputSymbol;

      if (embedded)
        GenerateNextEmbeddedReber(transitions, inputReber, reberChoices);
      else
        GenerateNextReber(transitions, inputReber, reberChoices);

      if (reberChoices.find(predictedSymbol) != std::string::npos)
        reberError++;
    }

    if (reberError != (output.n_elem / reberGrammerSize))
      error += 1;
  }

  error /= testReberGrammarCount;
  BOOST_REQUIRE_LE(error, 0.2);
}

/**
 * Train the specified networks on a Reber grammar dataset.
 */
BOOST_AUTO_TEST_CASE(ReberGrammarTest)
{
  LSTMLayer<> hiddenLayerLSTM(10);
  ReberGrammarTestNetwork(hiddenLayerLSTM);
}

/**
 * Train the specified networks on an embedded Reber grammar dataset.
 */
BOOST_AUTO_TEST_CASE(EmbeddedReberGrammarTest)
{
  LSTMLayer<> hiddenLayerLSTM(10);
  ReberGrammarTestNetwork(hiddenLayerLSTM, true);
}

/*
 * This sample is a simplified version of Derek D. Monner's Distracted Sequence
 * Recall task, which involves 10 symbols:
 *
 * Targets: must be recognized and remembered by the network.
 * Distractors: never need to be remembered.
 * Prompts: direct the network to give an answer.
 *
 * A single trial consists of a temporal sequence of 10 input symbols. The first
 * 8 consist of 2 randomly chosen target symbols and 6 randomly chosen
 * distractor symbols in an random order. The remaining two symbols are two
 * prompts, which direct the network to produce the first and second target in
 * the sequence, in order.
 *
 * For more information, see the following paper.
 *
 * @code
 * @misc{Monner2012,
 *   author = {Monner, Derek and Reggia, James A},
 *   title = {A generalized LSTM-like training algorithm for second-order
 *   recurrent neural networks},
 *   year = {2012}
 * }
 * @endcode
 *
 * @param input The generated input sequence.
 * @param input The generated output sequence.
 */
void GenerateDistractedSequence(arma::mat& input, arma::mat& output)
{
  input = arma::zeros<arma::mat>(10, 10);
  output = arma::zeros<arma::mat>(3, 10);

  arma::Col<size_t> index = arma::shuffle(arma::linspace<arma::Col<size_t> >(
      0, 7, 8));

  // Set the target in the input sequence and the corresponding targets in the
  // output sequence by following the correct order.
  for (size_t i = 0; i < 2; i++)
  {
    size_t idx = rand() % 2;
    input(idx, index(i)) = 1;
    output(idx, index(i) > index(i == 0) ? 9 : 8) = 1;
  }

  for (size_t i = 2; i < 8; i++)
    input(2 + rand() % 6, index(i)) = 1;

  // Set the prompts which direct the network to give an answer.
  input(8, 8) = 1;
  input(9, 9) = 1;

  input.reshape(input.n_elem, 1);
  output.reshape(output.n_elem, 1);
}

/**
 * Train the specified network and the construct distracted sequence recall
 * dataset.
 */
template<typename HiddenLayerType>
void DistractedSequenceRecallTestNetwork(HiddenLayerType& hiddenLayer0)
{
  const size_t trainDistractedSequenceCount = 1000;
  const size_t testDistractedSequenceCount = 1000;

  arma::field<arma::mat> trainInput(1, trainDistractedSequenceCount);
  arma::field<arma::mat> trainLabels(1, trainDistractedSequenceCount);
  arma::field<arma::mat> testInput(1, testDistractedSequenceCount);
  arma::field<arma::mat> testLabels(1, testDistractedSequenceCount);

  // Generate the training data.
  for (size_t i = 0; i < trainDistractedSequenceCount; i++)
    GenerateDistractedSequence(trainInput(0, i), trainLabels(0, i));

  // Generate the test data.
  for (size_t i = 0; i < testDistractedSequenceCount; i++)
    GenerateDistractedSequence(testInput(0, i), testLabels(0, i));

  /*
   * Construct a network with 10 input units, layerSize hidden units and 3
   * output units. The hidden layer is connected to itself. The network
   * structure looks like:
   *
   *  Input         Hidden        Output
   * Layer(10)  Layer(layerSize)   Layer(3)
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |    ..>|     |       |     |
   * +-----+    .  +--+--+       +-----+
   *            .     .
   *            .     .
   *            .......
   */
  const size_t lstmSize = 4 * 10;
  LinearLayer<> linearLayer0(10, lstmSize);
  RecurrentLayer<> recurrentLayer0(10, lstmSize);

  LinearLayer<> hiddenLayer(10, 3);
  TanHLayer<> hiddenBaseLayer;

  BinaryClassificationLayer classOutputLayer;

  auto modules = std::tie(linearLayer0, recurrentLayer0, hiddenLayer0,
                          hiddenLayer, hiddenBaseLayer);

  RNN<decltype(modules), BinaryClassificationLayer, NguyenWidrowInitialization,
      MeanSquaredErrorFunction> net(modules, classOutputLayer);

  SGD<decltype(net)> opt(net, 0.04, 2, -200);

  arma::mat inputTemp, labelsTemp;
  for (size_t i = 0; i < 40; i++)
  {
    for (size_t j = 0; j < trainDistractedSequenceCount; j++)
    {
      inputTemp = trainInput.at(0, j);
      labelsTemp = trainLabels.at(0, j);

      net.Train(inputTemp, labelsTemp, opt);
    }
  }

  double error = 0;

  // Ask the network to predict the targets in the given sequence at the
  // prompts.
  for (size_t i = 0; i < testDistractedSequenceCount; i++)
  {
    arma::mat output;
    arma::mat input = testInput.at(0, i);

    net.Predict(input, output);

    if (arma::accu(arma::abs(testLabels.at(0, i) - output)) != 0)
      error += 1;
  }

  error /= testDistractedSequenceCount;

  // Can we reproduce the results from the paper. They provide an 95% accuracy
  // on a test set of 1000 randomly selected sequences.
  // Ensure that this is within tolerance, which is at least as good as the
  // paper's results (plus a little bit for noise).
  BOOST_REQUIRE_LE(error, 0.3);
}

/**
 * Train the specified networks on the Derek D. Monner's distracted sequence
 * recall task.
 */
BOOST_AUTO_TEST_CASE(DistractedSequenceRecallTest)
{
  LSTMLayer<> hiddenLayerLSTMPeephole(10, true);
  DistractedSequenceRecallTestNetwork(hiddenLayerLSTMPeephole);
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef UTILITIES
#define UTILITIES

#include <math.h>
#include <stdlib.h>
#include <string>
#include <vector>
#include <fstream>
#include <iostream>
#include <algorithm>
#include <map>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/legendre.hpp>
#include <complex>
#include "gsl/gsl_sf_gamma.h"
#include "gsl/gsl_sf_result.h"
#include "gsl/gsl_complex_math.h"

namespace utilities {
  using namespace boost::numeric::ublas;

  const double PI = 3.14159265359;
  const double C = 299792458;//m/s
  const double EMASSC2 = 510.9989461;//keV
  const double PMASSC2 = 938272.046;//keV
  const double NMASSC2 = 939565.4133;//keV
  const double ALPHAMASSC2 = 3727379.508;//keV
  const double FINESTRUCTURE = 0.0072973525664;
  const double E = 2.718281828459045;
  const double HBAR = 6.58211889e-16;//ev*s
  const double a_CORR = -1.0;

  const std::string atoms[] = {"H", "He", "Li", "Be", "B", "C", "N", "O", "F", "Ne", "Na", "Mg", "Al", "Si", "P", "S", "Cl", "Ar", "K", "Ca", "Sc", "Ti", "V", "Cr", "Mn", "Fe", "Co", "Ni", "Cu", "Zn", "Ga", "Ge", "As", "Se", "Br", "Kr", "Rb", "Sr", "Y", "Zr", "Nb", "Mo", "Tc", "Ru", "Rh", "Pd", "Ag", "Cd", "In", "Sn", "Sb", "Te", "I", "Xe", "Cs", "Ba", "La", "Ce", "Pr", "Nd", "Pm", "Sm", "Eu", "Gd", "Tb", "Dy", "Ho", "Er", "Tm", "Yb", "Lu", "Hf", "Ta", "W", "Re", "Os", "Ir", "Pt", "Au", "Hg", "Tl", "Pb", "Bi", "Po", "At", "Rn", "Fr", "Ra", "Ac", "Th", "Pa", "U", "Np", "Pu", "Am", "Cm", "Bk", "Cf", "Es", "Fm", "Md", "No", "Lr", "Rf", "Db", "Sg", "Bh", "Hs", "Mt"};

  inline vector<double> RandomDirection () {
    double z, phi;
    vector<double> v(3);

    z = rand() / (double)RAND_MAX * 2. - 1.;
    phi = rand() / (double)RAND_MAX * 2. * PI;

    v(0) = std::sqrt(1.-z*z)*std::cos(phi);
    v(1) = std::sqrt(1.-z*z)*std::sin(phi);
    v(2) = z;

    return v;
  };

  inline double Random(double begin, double end) { return rand() / (double)RAND_MAX * (end - begin) + begin; };

  inline double RandomFromDistribution(std::vector<std::vector<double> >& pd) {
    double begin = pd[0][0];
    double end = pd[pd.size()-1][0];
    double stepSize = pd[1][0] - pd[0][0];
    double max = 0.;

    for (std::vector<std::vector<double> >::size_type i = 0; i != pd.size(); i++) {
      if (pd[i][1] > max) {
        max = pd[i][1];
      }
    }

    double r = 0.;
    double q = 0.;
    do {
      r = rand() / (double)RAND_MAX;
      q = rand() / (double)RAND_MAX * max;
    } while (q > pd[r*pd.size()][1]);
    return r*(end - begin) + begin;
  }

  inline double GetSpeed(double energy, double massc2) {
    double gamma = energy/massc2 + 1.;
    return std::sqrt(1.-1./(gamma*gamma))*C;
  }

  inline double GetNorm(const vector<double>& v) {
    double sum = 0.;
    for (vector<double>::size_type i = 0; i != v.size(); i++) {
      sum+=v[i]*v[i];
    }
    return std::sqrt(sum);
  }

  inline double FourDimDot(vector<double>& a, vector<double>& b) {
    //Mostly minus convention
    return a[0]*b[0]-(a[1]*b[1]+a[2]*b[2]+a[3]*b[3]);
  }

  inline double CalculateXiBetaDecay(double& cs, double& ct, double& cv, double& ca, double& mf, double& mgt) {
    return mf*mf*(2.*cs*cs+2.*cv*cv)+mgt*mgt*(2.*ct*ct+2.*ca*ca);
  }

  inline double CalculateBetaNeutrinoAsymmetry(double cs, double ct, double cv, double ca, double mf, double mgt) {
    return (mf*mf*(-2.*cs*cs+2.*cv*cv)+mgt*mgt/3.*(2.*ct*ct-2.*ca*ca))/CalculateXiBetaDecay(cs, ct, cv, ca, mf, mgt);
  }

  inline vector<double> NormaliseVector(const vector<double>& v) {
    vector<double> newV (v);
    double norm = GetNorm(v);
    if (norm != 0) {
      return newV/norm;
    }
    return v;
  }

  inline double LambdaKinematic(double x, double y, double z) {
    return x*x+y*y+z*z-2*x*y-2*y*z-2*x*z;
  }

  inline double PhaseSpace(double W, double W0) {
    return std::sqrt(W*W-1.)*W*std::pow(W0-W, 2.);
  }

  inline double SimpleFermiFunction(double Z, double v) {
    double nu = Z*FINESTRUCTURE/v*C;
    return 2.*PI*nu/(1.-std::pow(E, -2.*PI*nu));
  }

  inline double FermiFunction(double Z, double W, double R) {
    double gamma = std::sqrt(1.-(FINESTRUCTURE*Z)*(FINESTRUCTURE*Z));
    double p = std::sqrt(W*W-1);
    double first = 2*(gamma+1);
    //the second term will be incorporated in the fifth
    //double second = 1/pow(gsl_sf_gamma(2*gamma+1),2);
    double third = std::pow(2*p*R,2*(gamma-1));
    double fourth = std::exp(M_PI*FINESTRUCTURE*Z*W/p);

    //the fifth is a bit tricky
    //we use the complex gamma function from GSL
    gsl_sf_result magn;
    gsl_sf_result phase;
    gsl_sf_lngamma_complex_e(gamma, FINESTRUCTURE*Z*W/p,&magn,&phase);
    //now we have what we want in magn.val

    //but we incorporate the second term here as well
    double fifth = std::exp(2*(magn.val-gsl_sf_lngamma(2*gamma+1)));

    return first*third*fourth*fifth;
  }

  inline double ApproximateRadius(double A) {
    return (1.15+1.8*std::pow(A, -2./3.)-1.2*std::pow(A, -4./3.))*EMASSC2*1000./HBAR/C*std::pow(A, 1./3.);
  }

  inline double GetSpectrumHeight(double Z, double A, double Q, double E, bool advancedFermi) {
    double W = E/EMASSC2+1.;
    double W0 = Q/EMASSC2+1.;
    double R = ApproximateRadius(A);
    if (advancedFermi) {
      return PhaseSpace(W, W0)*FermiFunction(Z, W, R);
    }
    else {
      return PhaseSpace(W, W0)*SimpleFermiFunction(Z, GetSpeed(E, EMASSC2));
    }
  }

  inline std::vector<std::vector<double> >* GenerateBetaSpectrum(double Z, double A, double Q, bool advancedFermi) {
    std::vector<std::vector<double> >* dist = new std::vector<std::vector<double> >();
    double stepSize = 1.0;

    double currentEnergy = stepSize;
    while(currentEnergy <= Q) {
      double s = GetSpectrumHeight(Z, A, Q, currentEnergy, advancedFermi);
      std::vector<double> pair;
      pair.push_back(currentEnergy);
      pair.push_back(s);
      dist->push_back(pair);
      currentEnergy+=stepSize;
    }
    return dist;
  }

  inline vector<double> CrossProduct(vector<double> first, vector<double> second) {
    vector<double> v (3);

    v(0) = first(1)*second(2) - first(2)*second(1);
    v(1) = first(2)*second(0) - first(0)*second(2);
    v(2) = first(0)*second(1) - first(1)*second(0);

    return v;
  }

  inline vector<double> RotateAroundVector(vector<double>& initial, vector<double>& axis, double angle) {
    vector<double> vect(3);
    double u = axis[0];
    double v = axis[1];
    double w = axis[2];
    double x = initial[0];
    double y = initial[1];
    double z = initial[2];

    double c = std::cos(angle);
    double s = std::sin(angle);

    vect(0) = u*(u*x+v*y+w*z)*(1.-c)+x*c+(-w*y+v*z)*s;
    vect(1) = v*(u*x+v*y+w*z)*(1.-c)+y*c+(w*x-u*z)*s;
    vect(2) = w*(u*x+v*y+w*z)*(1.-c)+z*c+(-v*x+u*y)*s;

    return vect;
  }

  inline vector<double> GetParticleDirection(vector<double>& dir2, std::vector<double>& A) {
    dir2 = NormaliseVector(dir2);

    std::vector<std::vector<double> > dist;
    double stepSize = PI/180;
    double currentAngle = 0.;

    while (currentAngle <= PI) {
      std::vector<double> pair;
      pair.push_back(currentAngle);
      double W = 0.;
      for (std::vector<double>::size_type i = 0; i != A.size(); i++) {
        W+=A[i]*boost::math::legendre_p(i, std::cos(currentAngle));
      }
      pair.push_back(W);
      dist.push_back(pair);
      currentAngle+=stepSize;
    }

    double theta = 1.-RandomFromDistribution(dist)/PI;
    theta = std::acos(2*theta-1.);
    //std::cout << theta;
    vector<double> perp = CrossProduct(dir2, RandomDirection());
    vector<double> dir = RotateAroundVector(dir2, perp, theta);
    return dir;
  }

  inline vector<double> GetParticleDirection(std::vector<vector<double> >& dirs, std::vector<std::vector<double> >& A) {
    vector<double> dir (3);
    //TODO
    return dir;
  }

  inline vector<double> LorentzBoost(vector<double>& velocity, vector<double>& v) {
    double speed = GetNorm(velocity);
    double beta = speed;
    double gamma = 1./std::sqrt(1.-std::pow(speed, 2.));

    vector<double> dir = NormaliseVector(velocity);

    symmetric_matrix<double, upper> boost(4,4);
    boost(0, 0) = gamma;
    boost(0, 1) = -gamma*beta*dir[0];
    boost(0, 2) = -gamma*beta*dir[1];
    boost(0, 3) = -gamma*beta*dir[2];
    boost(1, 1) = 1.+(gamma-1.)*dir[0]*dir[0];
    boost(1, 2) = (gamma-1.)*dir[0]*dir[1];
    boost(1, 3) = (gamma-1.)*dir[0]*dir[2];
    boost(2, 2) = 1.+(gamma-1.)*dir[1]*dir[1];
    boost(2, 3) = (gamma-1.)*dir[1]*dir[2];
    boost(3, 3) = 1.+(gamma-1.)*dir[2]*dir[2];

    return prod(boost, v);
  }

  inline double GetApproximateMass(int Z, int A) {
    double b = 15.5*A-16.8*std::pow(A, 2.0/3.0)- 0.72*Z*(Z-1)*std::pow(A, -1.0/3.0) - 23.*std::pow(A-2*Z, 2.)/A;
    if (A%2 == 0) {
      double asym = 34.*std::pow(A, -3.0/4.0);
      if (Z%2 == 0) {
        b+=asym;
      }
      else {
        b-=asym;
      }
    }
    return Z*PMASSC2+(A-Z)*NMASSC2-b;
  }

  inline std::vector<std::vector<double> > ReadDistribution(const char * filename) {
    std::ifstream distReader(filename, std::ifstream::in);

    if (!distReader.is_open()) {
      std::cout << "Error: Could not find file " << filename << std::endl;
    }
    std::vector<std::vector<double> > dist;

    double x, y;
    while (distReader >> x >> y) {
      std::vector<double> pair;
      pair.push_back(x);
      pair.push_back(y);
      dist.push_back(pair);
    }
   return dist;
  }
}
#endif


/**
 * project  DESCARTES
 *
 * @file     EigenDenseMatrixMatrixProduct.hxx
 *
 * @author Laurent PLAGNE
 *
 * @par Modifications
 * - author date object
 *
 * (c) Copyright EDF R&D 2001-2012
 */
#ifndef __LEGOLAS_EIGENDENSEMATRIXMATRIXPRODUCT_HXX__
#define __LEGOLAS_EIGENDENSEMATRIXMATRIXPRODUCT_HXX__
#include  <vector>

#include "UTILITES.hxx"
#include <Eigen/Core>

namespace Legolas{

  // C=A*B

  class EigenDenseMatrixMatrixProduct{
  public:

    template <class MATRIX>
    static inline void apply(const MATRIX & A ,
			     const MATRIX & B ,
			     MATRIX & C)
    {
      //C mn = Amk * Bkn

      typedef typename MATRIX::RealType RealType;

      int M=A.nrows();
      int K=A.ncols();
      int N=B.ncols();

      assert(A.ncols()==B.nrows());
      assert(A.nrows()==C.nrows());
      assert(B.ncols()==C.ncols());

      typedef Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic > eigen_matrix;

      eigen_matrix Ae,Be,Ce;
      Ae.resize(M,K);
      for (int i=0 ; i<M ; i++)
	for (int j=0 ; j<K ; j++)
	  Ae(i,j)=A.sparseGetElement(i,j);

      Be.resize(K,N);
      for (int i=0 ; i<K ; i++)
	for (int j=0 ; j<N ; j++)
	  Be(i,j)=B.sparseGetElement(i,j);

      Ce.resize(M,N);
      Ce=Ae*Be;

      for (int i=0 ; i<M ; i++)
	for (int j=0 ; j<N ; j++)
	  C.sparseGetElement(i,j)=Ce(i,j);

    }

  };

}

#endif


// TRENTO: Reduced Thickness Event-by-event Nuclear Topology
// Copyright 2015 Jonah E. Bernhard, J. Scott Moreland
// MIT License

#include "event.h"

#include <algorithm>
#include <cmath>
#include <complex>

#include <boost/program_options/variables_map.hpp>

#include "nucleus.h"

namespace trento {

namespace {

typedef std::complex<double> complex_t;

constexpr double TINY = 1e-12;

// Generalized mean for p > 0.
// M_p(a, b) = (1/2*(a^p + b^p))^(1/p)
inline double positive_pmean(double p, double a, double b) {
  return std::pow(.5*(std::pow(a, p) + std::pow(b, p)), 1./p);
}

// Generalized mean for p < 0.
// Same as the positive version, except prevents division by zero.
inline double negative_pmean(double p, double a, double b) {
  if (a < TINY || b < TINY)
    return 0.;
  return positive_pmean(p, a, b);
}

// Generalized mean for p == 0.
inline double geometric_mean(double a, double b) {
  return std::sqrt(a*b);
}

inline int get_max_eccentricity_m(const std::vector<EventQuantity>& quantities) {
  int max_m = -1;
  bool foundmn = false;

  for (auto qty : quantities) {
    if (EventQuantity_GetClass(qty) == EventEpsilon_mn ||
        EventQuantity_GetClass(qty) == EventEpsilonArg_mn)
    {
      int m = EventQuantity_GetSubscript1(qty);
      int n = EventQuantity_GetSubscript2(qty);

      max_m = std::max(max_m, m);
      foundmn = (foundmn || (m != n));
    }
  }

  return (foundmn ? max_m : -1);
}

inline int get_max_eccentricity_n(const std::vector<EventQuantity>& quantities) {
  int max_n = -1;

  for (auto qty : quantities) {
    if (EventQuantity_GetClass(qty) == EventEpsilon_mn ||
        EventQuantity_GetClass(qty) == EventEpsilonArg_mn)
    {
      int n = EventQuantity_GetSubscript2(qty);
      max_n = std::max(max_n, n);
    }
  }

  return max_n;
}

}  // unnamed namespace

Event::Event(const VarMap& var_map)                                    // this constructor assumes that only quantities being displayed need to
  : Event(var_map, var_map["columns"].as<EventQuantityList>().values)  //   be computed; retained for code compatibility but should not be used
{ }

// Determine the grid parameters like so:
//   1. Read and set step size from the configuration.
//   2. Read grid max from the config, then set the number of steps as
//      nsteps = ceil(2*max/step).
//   3. Set the actual grid max as max = nsteps*step/2.  Hence if the step size
//      does not evenly divide the config max, the actual max will be marginally
//      larger (by at most one step size).
Event::Event(
  const VarMap& var_map,
  const std::vector<EventQuantity>& required_quantities)
    : norm_(var_map["normalization"].as<double>()),
      dxy_(var_map["grid-step"].as<double>()),
      nsteps_(std::ceil(2.*var_map["grid-max"].as<double>()/dxy_)),
      xymax_(.5*nsteps_*dxy_),
      TA_(boost::extents[nsteps_][nsteps_]),
      TB_(boost::extents[nsteps_][nsteps_]),
      TR_(boost::extents[nsteps_][nsteps_]),
      max_eccentricity_m_(get_max_eccentricity_m(required_quantities)),
      max_eccentricity_n_(get_max_eccentricity_n(required_quantities)) {
  // Choose which version of the generalized mean to use based on the
  // configuration.  The possibilities are defined above.  See the header for
  // more information.
  auto p = var_map["reduced-thickness"].as<double>();

  if (std::fabs(p) < TINY) {
    compute_reduced_thickness_ = [this]() {
      compute_reduced_thickness(geometric_mean);
    };
  } else if (p > 0.) {
    compute_reduced_thickness_ = [this, p]() {
      compute_reduced_thickness(
        [p](double a, double b) { return positive_pmean(p, a, b); });
    };
  } else {
    compute_reduced_thickness_ = [this, p]() {
      compute_reduced_thickness(
        [p](double a, double b) { return negative_pmean(p, a, b); });
    };
  }
}

void Event::compute(const Nucleus& nucleusA, const Nucleus& nucleusB,
                    const NucleonCommon& nucleon_common) {
  // Reset npart; compute_nuclear_thickness() increments it.
  npart_ = 0;
  compute_nuclear_thickness(nucleusA, nucleon_common, TA_);
  compute_nuclear_thickness(nucleusB, nucleon_common, TB_);
  compute_reduced_thickness_();
  compute_observables();

  failuresA_ = nucleusA.failures();
  failuresB_ = nucleusB.failures();
}

namespace {

// Limit a value to a range.
// Used below to constrain grid indices.
template <typename T>
inline const T& clip(const T& value, const T& min, const T& max) {
  if (value < min)
    return min;
  if (value > max)
    return max;
  return value;
}

}  // unnamed namespace

void Event::compute_nuclear_thickness(
    const Nucleus& nucleus, const NucleonCommon& nucleon_common, Grid& TX) {
  // Construct the thickness grid by looping over participants and adding each
  // to a small subgrid within its radius.  Compared to the other possibility
  // (grid cells as the outer loop and participants as the inner loop), this
  // reduces the number of required distance-squared calculations by a factor of
  // ~20 (depending on the nucleon size).  The Event unit test verifies that the
  // two methods agree.

  // Wipe grid with zeros.
  std::fill(TX.origin(), TX.origin() + TX.num_elements(), 0.);

  // Deposit each participant onto the grid.
  for (const auto& nucleon : nucleus) {
    if (!nucleon.is_participant())
      continue;

    ++npart_;

    // Get nucleon subgrid boundary {xmin, xmax, ymin, ymax}.
    const auto boundary = nucleon_common.boundary(nucleon);

    // Determine min & max indices of nucleon subgrid.
    int ixmin = clip(static_cast<int>((boundary[0]+xymax_)/dxy_), 0, nsteps_-1);
    int ixmax = clip(static_cast<int>((boundary[1]+xymax_)/dxy_), 0, nsteps_-1);
    int iymin = clip(static_cast<int>((boundary[2]+xymax_)/dxy_), 0, nsteps_-1);
    int iymax = clip(static_cast<int>((boundary[3]+xymax_)/dxy_), 0, nsteps_-1);

    // Add profile to grid.
    for (auto iy = iymin; iy <= iymax; ++iy) {
      for (auto ix = ixmin; ix <= ixmax; ++ix) {
        TX[iy][ix] += nucleon_common.thickness(
          nucleon, (ix+.5)*dxy_ - xymax_, (iy+.5)*dxy_ - xymax_
        );
      }
    }
  }
}

template <typename GenMean>
void Event::compute_reduced_thickness(GenMean gen_mean) {
  double sum = 0.;
  double ixcm = 0.;
  double iycm = 0.;

  for (int iy = 0; iy < nsteps_; ++iy) {
    for (int ix = 0; ix < nsteps_; ++ix) {
      auto t = norm_ * gen_mean(TA_[iy][ix], TB_[iy][ix]);
      TR_[iy][ix] = t;
      sum += t;
      // Center of mass grid indices.
      // No need to multiply by dxy since it would be canceled later.
      ixcm += t * static_cast<double>(ix);
      iycm += t * static_cast<double>(iy);
    }
  }

  multiplicity_ = dxy_ * dxy_ * sum;
  ixcm_ = ixcm / sum;
  iycm_ = iycm / sum;
}

void Event::compute_en(int max_n) {
  // Compute eccentricity.
  if (max_n < MinEccentricityHarmonic || max_n > MaxEccentricityHarmonic)
    throw std::invalid_argument{"max_n"};

  // Simple helper class for use in the following loop.
  struct EccentricityAccumulator {
    complex_t z = 0.;
    double wt = 0.;  // weight
    double finish() const  // compute final eccentricity
    { return std::abs(z) / std::fmax(wt, TINY); }
    double arg() const
    { return std::arg(z); }
  } en[NumEccentricityHarmonics];

  for (int iy = 0; iy < nsteps_; ++iy) {
    auto y = static_cast<double>(iy) - iycm_;
    auto y2 = y*y;

    for (int ix = 0; ix < nsteps_; ++ix) {
      const auto& t = TR_[iy][ix];
      if (t < TINY)
        continue;

      // Compute `r` relative to the CM.
      auto x = static_cast<double>(ix) - ixcm_;
      auto r = std::sqrt(x*x + y2);

      // The eccentricity harmonics are weighted averages of r^n*exp(i*n*phi)
      // over the entropy profile (reduced thickness).  Note that:
      //
      //   r^n * exp(i*n*phi)  =  r^n * exp(i*phi)^n
      //                       =  (r*exp(i*phi))^n
      //                       =  (x + i*y)^n
      //
      // The Event unit test verifies that this method agrees with the
      // trigonometric function method.
      auto z    = complex_t{x, y};
      auto enz  = t * z;
      auto enwt = t * r;

      for (int i = MinEccentricityHarmonic; i <= max_n; ++i) {
        en[i - MinEccentricityHarmonic].z  += (enz  *= z);
        en[i - MinEccentricityHarmonic].wt += (enwt *= r);
      }
    }
  }

  for (int n = MinEccentricityHarmonic; n <= max_n; ++n) {
    eccentricity_[n] = eccentricity_mn_[std::make_pair(n,n)] = en[n - MinEccentricityHarmonic].finish();
    eccentricity_mn_arg_[std::make_pair(n,n)] = en[n - MinEccentricityHarmonic].arg();
  }
}

void Event::compute_emn(int max_m, int max_n) {
  // Compute eccentricity.
  if (max_m < MinEccentricityHarmonic || max_m > MaxEccentricityHarmonic)
    throw std::invalid_argument{"max_m"};

  if (max_n < MinEccentricityHarmonic || max_n > MaxEccentricityHarmonic)
    throw std::invalid_argument{"max_n"};

  // Simple helper class for use in the following loop.
  complex_t emn[NumEccentricityHarmonics][NumEccentricityHarmonics] = { };  // important: initializes to zeroes
  double weights[NumEccentricityHarmonics] = { };

  for (int iy = 0; iy < nsteps_; ++iy) {
    auto y = static_cast<double>(iy) - iycm_;
    auto y2 = y*y;

    for (int ix = 0; ix < nsteps_; ++ix) {
      const auto& t = TR_[iy][ix];
      if (t < TINY)
        continue;

      // Compute `r` relative to the CM.
      auto x = static_cast<double>(ix) - ixcm_;
      auto r = std::sqrt(x*x + y2);

      auto wt = t * r;  // initialize to `(t * r)` and multiply `r` again at top of `m` loop, since `m` starts at 2
      for (int m = MinEccentricityHarmonic; m <= max_m; ++m)       // (note: `MinEccentricityHarmonic = 2` assumed)
        weights[m - MinEccentricityHarmonic] += (wt *= r);

      // The eccentricity harmonics are weighted averages of `r^m*exp(i*n*phi)`
      // over the entropy profile (reduced thickness).  Note that:
      //
      //   r^m * exp(i*n*phi)  =  r^m * exp(i*phi)^n
      //                       =  r^(m-n) * (r*exp(i*phi))^n
      //                       =  r^(m-n) * (x + i*y)^n
      //
      auto z = complex_t{x, y};
      auto zfactor = t * z;  // initialize to `(t * z)` and multiply `z` again at top of `n` loop, since `n` starts at 2

      for (int n = MinEccentricityHarmonic; n <= MaxEccentricityHarmonic; n++) {  // (note: `MinEccentricityHarmonic = 2` assumed)
        zfactor *= z;

        double rfactor;

        if (r > TINY) {  // ignore this pixel if `r ~ 0` here; it would be nugatory in an integral but causes problems for our sum
          rfactor = 1.;  // initialize to 1 and divide `r` at top of `m` loop, since `(m - n)` starts at -1 and counts down

          for (int m = n - 1; m >= MinEccentricityHarmonic; --m) {
            rfactor /= r;
            emn[m - MinEccentricityHarmonic][n - MinEccentricityHarmonic] += rfactor * zfactor;
          }

          rfactor = 1.;  // now initialize to 1 and multiply `r` at bottom of `m` loop, since `(m - n)` now starts at 0 and counts up

          for (int m = n; m <= MaxEccentricityHarmonic; ++m) {
            emn[m - MinEccentricityHarmonic][n - MinEccentricityHarmonic] += rfactor * zfactor;
            rfactor *= r;
          }
        }
      }
    }
  }

  for (int m = MinEccentricityHarmonic; m <= max_m; ++m) {
    double wt = std::fmax(weights[m - MinEccentricityHarmonic], TINY);

    for (int n = MinEccentricityHarmonic; n <= max_n; ++n) {
      eccentricity_mn_[std::make_pair(m,n)]     = std::abs(emn[m - MinEccentricityHarmonic][n - MinEccentricityHarmonic]) / wt;
      eccentricity_mn_arg_[std::make_pair(m,n)] = std::arg(emn[m - MinEccentricityHarmonic][n - MinEccentricityHarmonic]);
    }

    if (m <= max_n)  // also update `eccentricity_` for `m == n` if applicable
      eccentricity_[m] = std::abs(emn[m - MinEccentricityHarmonic][m - MinEccentricityHarmonic]) / wt;
  }
}

void Event::compute_observables() {
  if (max_eccentricity_n_ >= 0) {
    if (max_eccentricity_m_ >= 0)
      compute_emn(max_eccentricity_m_, max_eccentricity_n_);
    else compute_en(max_eccentricity_n_);
  }
}

}  // namespace trento


#include <iostream>
#include <Eigen/Dense>
#include "../simple_lib/include/simple_layer.h"

using namespace Eigen;
using namespace MyDL;

int main(){
    using std::cout;
    using std::endl;

    MatrixXd apple = MatrixXd::Zero(1, 1);
    MatrixXd apple_num = MatrixXd::Zero(1, 1);
    MatrixXd orange = MatrixXd::Zero(1, 1);
    MatrixXd orange_num = MatrixXd::Zero(1, 1);
    MatrixXd tax = MatrixXd::Zero(1, 1);
    MatrixXd apple_price, orange_price, all_price, price;

    MatrixXd dprice = MatrixXd::Ones(1, 1);
    MatrixXd dapple_price, dorange_price, dall_price, dtax, dapple_num, dorange_num, dapple, dorange;

    apple(0) = 100;
    apple_num(0) = 2;
    orange(0) = 150;
    orange_num(0) = 3;
    tax(0) = 1.1;

    //layer
    MulLayer mul_apple_layer, mul_orange_layer, mul_tax_layer;
    AddLayer add_apple_orange_layer;

    // forward
    apple_price = mul_apple_layer.forward(apple, apple_num);
    orange_price = mul_orange_layer.forward(orange, orange_num);
    all_price = add_apple_orange_layer.forward(apple_price, orange_price);
    price = mul_tax_layer.forward(all_price, tax);

    // backward
    mul_tax_layer.backward(dprice, dall_price, dtax);
    add_apple_orange_layer.backward(dall_price, dapple_price, dorange_price);
    mul_orange_layer.backward(dorange_price, dorange, dorange_num);
    mul_apple_layer.backward(dapple_price, dapple, dapple_num);

    cout << "price: " << price << endl;
    cout << "dapple_num: " << dapple_num << endl;
    cout << "dapple: " << dapple << endl;
    cout << "dorange_num: " << dorange_num << endl;
    cout << "dorange: " << dorange << endl;
    cout << "dtax: " << dtax << endl;

    return 0;
}


//
// Expansion Hunter
// Copyright 2016-2019 Illumina, Inc.
// All rights reserved.
//
// 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.
//
//

#include "genotyping/AlleleChecker.hh"

#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/log1p.hpp>

#include <numeric>

namespace ehunter
{

namespace
{
double poissonLogPmf(double lambda, double count)
{
    return count * log(lambda) - lambda - boost::math::lgamma(count + 1);
}

double logBeta(int a, int b) { return boost::math::lgamma(a) + boost::math::lgamma(b) - boost::math::lgamma(a + b); }

double logBinomCoef(int n, int k) { return -boost::math::log1p(n) - logBeta(n - k + 1, k + 1); }

double binomLogPmf(int n, double p, int count)
{
    return logBinomCoef(n, count) + count * log(p) + (n - count) * boost::math::log1p(-p);
}

}

AlleleCheckSummary AlleleChecker::check(double haplotypeDepth, int targetAlleleCount, int otherAlleleCount) const
{
    if (haplotypeDepth <= 0)
    {
        throw std::runtime_error("Haplotype depth must be positive");
    }

    if (targetAlleleCount < 0 || otherAlleleCount < 0)
    {
        throw std::runtime_error("Negative read counts are not allowed");
    }

    const int totalReadCount = targetAlleleCount + otherAlleleCount;
    const double ll0 = (totalReadCount > 0) ? binomLogPmf(totalReadCount, errorRate_, targetAlleleCount) : 0;
    const double ll1 = poissonLogPmf(haplotypeDepth, targetAlleleCount);

    AlleleStatus status = AlleleStatus::kUncertain;
    double logLikelihoodRatio = (ll1 - ll0) / log(10);
    if (logLikelihoodRatio < -log10(likelihoodRatioThreshold_))
    {
        status = AlleleStatus::kAbsent;
    }
    else if (logLikelihoodRatio > log10(likelihoodRatioThreshold_))
    {
        status = AlleleStatus::kPresent;
    }

    return AlleleCheckSummary(status, logLikelihoodRatio);
}

std::ostream& operator<<(std::ostream& out, AlleleStatus status)
{
    switch (status)
    {
    case AlleleStatus::kAbsent:
        out << "Absent";
        break;
    case AlleleStatus::kPresent:
        out << "Present";
        break;
    case AlleleStatus::kUncertain:
        out << "Uncertain";
        break;
    }

    return out;
}

}


#pragma once

#include <deal.II/base/tensor.h>
#include <functional>
#include <iomanip>
#include <iostream>
#include <unordered_map>
#include <vector>

#include "aux/filtered_range.hpp"
#include "aux/hash_specializations.hpp"
#include "aux/simple_sparse_matrix.hpp"
#include "matrix/assembly/velocity_angular_integrator.hpp"
#include "matrix/assembly/velocity_radial_integrator.hpp"
#include "matrix/assembly/weight.hpp"
#include "spectral/basis/spectral_basis_dimension_accessor.hpp"

namespace boltzmann {
namespace local_ {
/**
 * @brief Compute and cache overlap integrals in radial direction
 */
struct radial_entry_computer_t
{
  // -----------------------------------------------------------------
  radial_entry_computer_t(int nqpts_ = 90)
      : nqpts(nqpts_)
  {
    /* empty */
  }

  /**
   *  @brief I = \f$\int_\bbR b_1(r) k(r), b_2(r) r \ud r \f$
   *
   *  @tparam E spectral element
   *  @param k additional function argument
   *  @param b1 1st basis function
   *  @param b2 2nd basis function
   *  @return I integral
   */
  template <typename E>
  double compute(const std::function<double(double)>& k,
                 const E& b1,
                 const E& b2,
                 const double a = 0 /* inner product weight exp(-a r**2) */)
  {
    const double FUZZY = 1e7;
    const double nu = a + b1.w() + b2.w();
    const long int key = FUZZY * nu;
    if (quad_cache.find(key) == quad_cache.end()) quad_cache[key] = ptr_t(new QMaxwell(nu, nqpts));
    const auto& quad = *(quad_cache[key]);

    double sum = 0;
    for (unsigned int q = 0; q < quad.size(); ++q) {
      sum += b1.evaluate(quad.pts(q)) * b2.evaluate(quad.pts(q)) * k(quad.pts(q)) * quad.wts(q);
    }

    return sum;
  }

 private:
  const int nqpts;
  typedef std::shared_ptr<QMaxwell> ptr_t;
  std::map<long int, ptr_t> quad_cache;
};
}  // end namespace local_

template <int DIM>
class VelocityVarForm
{ };

/**
 * @brief Storage for velocity domain matrix entries
 *
 */
template <>
class VelocityVarForm<2>
{
 private:
  static const int DIM = 2;
  typedef unsigned int index_t;
  typedef dealii::Tensor<1, DIM, double> t1_t;
  typedef dealii::Tensor<2, DIM, double> t2_t;

  typedef SimpleSparseMatrix<double> s0_entries_t;
  typedef SimpleSparseMatrix<t1_t> t1_entries_t;
  typedef SimpleSparseMatrix<t2_t> t2_entries_t;

 public:
  template <typename BASIS>
  void init(const BASIS& test_basis, const BASIS& trial_basis, const double beta = 2);

  template <typename BASIS>
  void init(const BASIS& trial_basis, const double beta = 2);

 private:
  s0_entries_t s0_;
  t1_entries_t t1_;
  t2_entries_t t2_;

  /// helpers
  VelocityAngularIntegrator<2> vai;

  constexpr const static double TOL = 1e-11; // magic number

 public:
  auto get_s0() const -> decltype(s0_.get_vec()) { return s0_.get_vec(); }
  auto get_s1() const -> decltype(t1_.get_vec()) { return t1_.get_vec(); }
  auto get_t1() const -> decltype(t1_.get_vec()) { return t1_.get_vec(); }
  auto get_t2() const -> decltype(t2_.get_vec()) { return t2_.get_vec(); }

  const SimpleSparseMatrix<double>& get_s0m() const { return s0_; }
  const SimpleSparseMatrix<t1_t>& get_s1m()   const { return t1_; }
  const SimpleSparseMatrix<t1_t>& get_t1m()   const { return t1_; }
  const SimpleSparseMatrix<t2_t>& get_t2m()   const { return t2_; }

  /**
   *  @brief \f$\langle \psi_1, \psi_2\rangle\f$
   *
   *  Detailed description
   *
   *  @return Returns a vector [(entry_t...)] which contains the non-zero
   *    entries of the sparse matrix arising from the overlap integral above.
   *    entry_t has members row, col and val. row and col correspond to the
   *    enumeration of DoFs in the spectral basis.

   */
  auto s0() const -> const decltype(s0_) & { return s0_; }

  /**
   *  @brief \f$\langle v_i \psi_1, \psi_2\rangle\f$
   *
   *  Detailed description
   *
   *  @return Returns a vector [(entry_t...)] which contains the non-zero
   *    entries of the sparse matrix arising from the overlap integral above.
   *    entry_t has members row, col and val. row and col correspond to the
   *    enumeration of DoFs in the spectral basis.
   */
  auto s1() const -> const decltype(t1_) & { return t1_; }

  /**
   *   @brief same as s1
   */
  auto t1() const -> const decltype(t1_) & { return t1_; }

  /**
   *   @brief \f$\langle v_i v_j \psi_1, \psi_2 \rangle\f$
   *
   *  Detailed description
   *
   *  @return Returns a vector [(entry_t...)] which contains the non-zero
   *    entries of the sparse matrix arising from the overlap integral above.
   *    entry_t has members row, col and val. row and col correspond to the
   *    enumeration of DoFs in the spectral basis.
   */
  auto t2() const -> const decltype(t2_) & { return t2_; }

  const VelocityAngularIntegrator<2>& get_vai() const { return vai; }

  void print_info() const;
};

// --------------------------------------------------------------------------------
template <typename BASIS>
void
VelocityVarForm<2>::init(const BASIS& test_basis, const BASIS& trial_basis, const double beta)
{
  typedef typename BASIS::elem_t elem_t;

  // angular basis
  typedef typename std::tuple_element<0, typename BASIS::elem_t::container_t>::type angular_elem_t;
  // radial basis
  typedef typename std::tuple_element<1, typename BASIS::elem_t::container_t>::type radial_elem_t;
  /// vector of basis functions in dim1
  typedef typename BASIS::DimAcc::template get_vec<angular_elem_t> a1_t;
  typedef typename BASIS::DimAcc::template get_vec<radial_elem_t> a2_t;

  typedef typename radial_elem_t::id_t rid_t;
  typedef typename radial_elem_t::numeric_t numeric_t;

  // element accessors for radial and angular parts
  typename elem_t::Acc::template get<radial_elem_t> acc_rad;
  typename elem_t::Acc::template get<angular_elem_t> acc_ang;

  /// they correspond to rows and columns in R1
  const auto& test_angular_basis = a1_t()(test_basis);
  //  const auto& test_radial_basis  = a2_t()(test_basis);

  const auto& trial_angular_basis = a1_t()(trial_basis);
  // const auto& trial_radial_basis  = a2_t()(trial_basis);

  // matrix assembly
  vai.init(trial_angular_basis);
  // regular L2-inner product
  L2Weight weight(beta);

  // ------------------------------
  // Compute Radial Entries
  local_::radial_entry_computer_t radial_entry_computer;
  // create a cache for the radial entries
  std::unordered_map<std::tuple<rid_t, rid_t>, numeric_t> cache;

  typedef std::pair<unsigned int, unsigned int> index_pair_t;

  // ----------------------------------------------------------------------
  // S0
  s0_.reinit(trial_basis.n_dofs());
  cache.clear();
  auto k0 = [](double r __attribute__((unused))) { return 1; };
  // iterate over nonzero entries in s0
  const auto& s0A = vai.get_s0();
  for (auto itA = s0A.begin(); itA != s0A.end(); ++itA) {
    // get ids from anuglar basis
    unsigned int iA1 = itA->first.first;
    unsigned int iA2 = itA->first.second;
    const auto& idA1 = test_angular_basis[iA1].get_id();
    const auto& idA2 = trial_angular_basis[iA2].get_id();

    std::function<bool(const elem_t&)> pred1 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA1;
    };

    std::function<bool(const elem_t&)> pred2 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA2;
    };

    auto test_range_rad = filtered_range(test_basis.begin(), test_basis.end(), pred1);
    auto trial_range_rad = filtered_range(trial_basis.begin(), trial_basis.end(), pred2);

    // iterate over test functions
    for (auto itR1 = std::get<0>(test_range_rad); itR1 != std::get<1>(test_range_rad); ++itR1) {
      const auto& br1 = acc_rad(*itR1);  // extract radial part
      const auto& id1 = br1.get_id();

      // iterate over trial functions
      for (auto itR2 = std::get<0>(trial_range_rad); itR2 != std::get<1>(trial_range_rad); ++itR2) {
        const auto& br2 = acc_rad(*itR2);  // extract radial part
        const auto& id2 = br2.get_id();
        // create key
        auto key = std::make_tuple(id1, id2);
        // lookup
        auto cit = cache.find(key);
        // found?
        double vr;
        if (cit != cache.end()) {
          vr = cit->second;
        } else {
          vr = radial_entry_computer.compute(k0, br1, br2, 1 - 2.0 / beta);
          cache[key] = vr;
        }

        // insert value into entries structure
        if (std::abs(vr) > TOL) {
          unsigned int gidx1 = test_basis.get_dof_index(itR1->get_id());
          unsigned int gidx2 = trial_basis.get_dof_index(itR2->get_id());
          s0_.insert(gidx1, gidx2, (itA->second) * vr);
        }
      }
    }
  }
  s0_.compress();

  // ----------------------------------------------------------------------
  // T1
  t1_.reinit(trial_basis.n_dofs());
  cache.clear();
  auto k1 = [](double r) { return r; };
  // iterate over nonzero entries in t1
  const auto& t1A = vai.get_t1();
  for (auto itA = t1A.begin(); itA != t1A.end(); ++itA) {
    // get ids from anuglar basis
    unsigned int iA1 = itA->first.first;
    unsigned int iA2 = itA->first.second;
    const auto& idA1 = test_angular_basis[iA1].get_id();
    const auto& idA2 = trial_angular_basis[iA2].get_id();

    std::function<bool(const elem_t&)> pred1 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA1;
    };
    std::function<bool(const elem_t&)> pred2 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA2;
    };

    auto test_range_rad = filtered_range(test_basis.begin(), test_basis.end(), pred1);
    auto trial_range_rad = filtered_range(trial_basis.begin(), trial_basis.end(), pred2);

    // iterate over test functions
    for (auto itR1 = std::get<0>(test_range_rad); itR1 != std::get<1>(test_range_rad); ++itR1) {
      const auto& br1 = acc_rad(*itR1);  // extract radial part
      const auto& id1 = br1.get_id();

      // iterate over trial functions
      for (auto itR2 = std::get<0>(trial_range_rad); itR2 != std::get<1>(trial_range_rad); ++itR2) {
        const auto& br2 = acc_rad(*itR2);  // extract radial part
        const auto& id2 = br2.get_id();
        // create key
        auto key = std::make_tuple(id1, id2);
        // lookup
        auto cit = cache.find(key);
        // found?
        double vr;
        if (cit != cache.end()) {
          vr = cit->second;
        } else {
          vr = radial_entry_computer.compute(k1, br1, br2, 1 - 2.0 / beta);
          cache[key] = vr;
        }

        // insert value into entries structure
        if (std::abs(vr) > TOL) {
          unsigned int gidx1 = test_basis.get_dof_index(itR1->get_id());
          unsigned int gidx2 = trial_basis.get_dof_index(itR2->get_id());
          t1_.insert(gidx1, gidx2, (itA->second) * vr);
        }
      }
    }
  }
  t1_.compress();

  // ----------------------------------------------------------------------
  // T2
  t2_.reinit(trial_basis.n_dofs());
  cache.clear();
  auto k2 = [](double r) { return r * r; };
  const auto& t2A = vai.get_t2();
  for (auto itA = t2A.begin(); itA != t2A.end(); ++itA) {
    // get ids from anuglar basis
    unsigned int iA1 = itA->first.first;
    unsigned int iA2 = itA->first.second;
    const auto& idA1 = test_angular_basis[iA1].get_id();
    const auto& idA2 = trial_angular_basis[iA2].get_id();

    std::function<bool(const elem_t&)> pred1 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA1;
    };
    std::function<bool(const elem_t&)> pred2 = [&](const elem_t& e) {
      return acc_ang(e).get_id() == idA2;
    };

    auto test_range_rad = filtered_range(test_basis.begin(), test_basis.end(), pred1);
    auto trial_range_rad = filtered_range(trial_basis.begin(), trial_basis.end(), pred2);

    // iterate over test functions
    for (auto itR1 = std::get<0>(test_range_rad); itR1 != std::get<1>(test_range_rad); ++itR1) {
      const auto& br1 = acc_rad(*itR1);  // extract radial part
      const auto& id1 = br1.get_id();

      // iterate over trial functions
      for (auto itR2 = std::get<0>(trial_range_rad); itR2 != std::get<1>(trial_range_rad); ++itR2) {
        const auto& br2 = acc_rad(*itR2);  // extract radial part
        const auto& id2 = br2.get_id();
        // create key
        auto key = std::make_tuple(id1, id2);
        // lookup
        auto cit = cache.find(key);
        // found?
        double vr;
        if (cit != cache.end()) {
          vr = cit->second;
        } else {
          vr = radial_entry_computer.compute(k2, br1, br2, 1 - 2.0 / beta);
          cache[key] = vr;
        }

        // insert value into entries structure
        if (std::abs(vr) > TOL) {
          unsigned int gidx1 = test_basis.get_dof_index(itR1->get_id());
          unsigned int gidx2 = trial_basis.get_dof_index(itR2->get_id());
          t2_.insert(gidx1, gidx2, (itA->second) * vr);
        }
      }
    }
  }
  t2_.compress();
}

// ------------------------------------------------------------
template <typename BASIS>
void
VelocityVarForm<2>::init(const BASIS& trial_basis, const double beta)
{
  this->init(trial_basis, trial_basis, beta);
}

// -------------------------------------------------------------
inline void
VelocityVarForm<2>::print_info() const
{
  std::cout << "s0_.size()"
            << "\t" << s0_.size() << std::endl
            << "t1_.size()"
            << "\t" << t1_.size() << std::endl
            << "t2_.size()"
            << "\t" << t2_.size() << std::endl;
}

}  // end namespace boltzmann


//  Boost common_factor.hpp header file  -------------------------------------//

//  (C) Copyright Daryle Walker, Stephen Cleary, Paul Moore 2001.  Permission
//  to copy, use, modify, sell and distribute this software is granted provided
//  this copyright notice appears in all copies.  This software is provided "as
//  is" without express or implied warranty, and with no claim as to its
//  suitability for any purpose.

//  See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_MATH_COMMON_FACTOR_HPP
#define BOOST_MATH_COMMON_FACTOR_HPP

#include <boost/math_fwd.hpp>  // self include

#include <boost/config.hpp>  // for BOOST_STATIC_CONSTANT, etc.
#include <boost/limits.hpp>  // for std::numeric_limits

namespace boost
{
namespace math
{

//  Forward declarations for function templates  -----------------------------//

template < typename IntegerType >
    IntegerType  gcd( IntegerType const &a, IntegerType const &b );

template < typename IntegerType >
    IntegerType  lcm( IntegerType const &a, IntegerType const &b );

//  Greatest common divisor evaluator class declaration  ---------------------//

template < typename IntegerType >
class gcd_evaluator
{
public:
    // Types
    typedef IntegerType  result_type, first_argument_type, second_argument_type;

    // Function object interface
    result_type  operator ()( first_argument_type const &a,
     second_argument_type const &b ) const;

};  // boost::math::gcd_evaluator

//  Least common multiple evaluator class declaration  -----------------------//

template < typename IntegerType >
class lcm_evaluator
{
public:
    // Types
    typedef IntegerType  result_type, first_argument_type, second_argument_type;

    // Function object interface
    result_type  operator ()( first_argument_type const &a,
     second_argument_type const &b ) const;

};  // boost::math::lcm_evaluator

//  Implementation details  --------------------------------------------------//

namespace detail
{
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    // Build GCD with Euclid's recursive algorithm
    template < unsigned long Value1, unsigned long Value2 >
    struct static_gcd_helper_t
    {
    private:
        BOOST_STATIC_CONSTANT( unsigned long, new_value1 = Value2 );
        BOOST_STATIC_CONSTANT( unsigned long, new_value2 = Value1 % Value2 );

        #ifndef __BORLANDC__
        #define BOOST_DETAIL_GCD_HELPER_VAL(Value)  Value
        #else
        typedef static_gcd_helper_t  self_type;
        #define BOOST_DETAIL_GCD_HELPER_VAL(Value)  (self_type:: Value )
        #endif

        typedef static_gcd_helper_t< BOOST_DETAIL_GCD_HELPER_VAL(new_value1),
         BOOST_DETAIL_GCD_HELPER_VAL(new_value2) >  next_step_type;

        #undef BOOST_DETAIL_GCD_HELPER_VAL

    public:
        BOOST_STATIC_CONSTANT( unsigned long, value = next_step_type::value );
    };

    // Non-recursive case
    template < unsigned long Value1 >
    struct static_gcd_helper_t< Value1, 0UL >
    {
        BOOST_STATIC_CONSTANT( unsigned long, value = Value1 );
    };
#else
    // Use inner class template workaround from Peter Dimov
    template < unsigned long Value1 >
    struct static_gcd_helper2_t
    {
        template < unsigned long Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( unsigned long, value
             = static_gcd_helper2_t<Value2>::helper<Value1 % Value2>::value );
        };

        template <  >
        struct helper< 0UL >
        {
            BOOST_STATIC_CONSTANT( unsigned long, value = Value1 );
        };
    };

    // Special case
    template <  >
    struct static_gcd_helper2_t< 0UL >
    {
        template < unsigned long Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( unsigned long, value = Value2 );
        };
    };

    // Build the GCD from the above template(s)
    template < unsigned long Value1, unsigned long Value2 >
    struct static_gcd_helper_t
    {
        BOOST_STATIC_CONSTANT( unsigned long, value
         = static_gcd_helper2_t<Value1>::BOOST_NESTED_TEMPLATE
         helper<Value2>::value );
    };
#endif

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    // Build the LCM from the GCD
    template < unsigned long Value1, unsigned long Value2 >
    struct static_lcm_helper_t
    {
        typedef static_gcd_helper_t<Value1, Value2>  gcd_type;

        BOOST_STATIC_CONSTANT( unsigned long, value = Value1 / gcd_type::value
         * Value2 );
    };

    // Special case for zero-GCD values
    template < >
    struct static_lcm_helper_t< 0UL, 0UL >
    {
        BOOST_STATIC_CONSTANT( unsigned long, value = 0UL );
    };
#else
    // Adapt GCD's inner class template workaround for LCM
    template < unsigned long Value1 >
    struct static_lcm_helper2_t
    {
        template < unsigned long Value2 >
        struct helper
        {
            typedef static_gcd_helper_t<Value1, Value2>  gcd_type;

            BOOST_STATIC_CONSTANT( unsigned long, value = Value1
             / gcd_type::value * Value2 );
        };

        template <  >
        struct helper< 0UL >
        {
            BOOST_STATIC_CONSTANT( unsigned long, value = 0UL );
        };
    };

    // Special case
    template <  >
    struct static_lcm_helper2_t< 0UL >
    {
        template < unsigned long Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( unsigned long, value = 0UL );
        };
    };

    // Build the LCM from the above template(s)
    template < unsigned long Value1, unsigned long Value2 >
    struct static_lcm_helper_t
    {
        BOOST_STATIC_CONSTANT( unsigned long, value
         = static_lcm_helper2_t<Value1>::BOOST_NESTED_TEMPLATE
         helper<Value2>::value );
    };
#endif

    // Greatest common divisor for rings (including unsigned integers)
    template < typename RingType >
    RingType
    gcd_euclidean
    (
        RingType  a,
        RingType  b
    )
    {
        // Avoid repeated construction
        #ifndef __BORLANDC__
        RingType const  zero = static_cast<RingType>( 0 );
        #else
        RingType  zero = static_cast<RingType>( 0 );
        #endif

        // Reduce by GCD-remainder property [GCD(a,b) == GCD(b,a MOD b)]
        while ( true )
        {
            if ( a == zero )
                return b;
            b %= a;

            if ( b == zero )
                return a;
            a %= b;
        }
    }

    // Greatest common divisor for (signed) integers
    template < typename IntegerType >
    inline
    IntegerType
    gcd_integer
    (
        IntegerType const &  a,
        IntegerType const &  b
    )
    {
        // Avoid repeated construction
        IntegerType const  zero = static_cast<IntegerType>( 0 );
        IntegerType const  result = gcd_euclidean( a, b );

        return ( result < zero ) ? -result : result;
    }

    // Least common multiple for rings (including unsigned integers)
    template < typename RingType >
    inline
    RingType
    lcm_euclidean
    (
        RingType const &  a,
        RingType const &  b
    )
    {
        RingType const  zero = static_cast<RingType>( 0 );
        RingType const  temp = gcd_euclidean( a, b );

        return ( temp != zero ) ? ( a / temp * b ) : zero;
    }

    // Least common multiple for (signed) integers
    template < typename IntegerType >
    inline
    IntegerType
    lcm_integer
    (
        IntegerType const &  a,
        IntegerType const &  b
    )
    {
        // Avoid repeated construction
        IntegerType const  zero = static_cast<IntegerType>( 0 );
        IntegerType const  result = lcm_euclidean( a, b );

        return ( result < zero ) ? -result : result;
    }

    // Function objects to find the best way of computing GCD or LCM
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    template < typename T, bool IsSpecialized, bool IsSigned >
    struct gcd_optimal_evaluator_helper_t
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_euclidean( a, b );
        }
    };

    template < typename T >
    struct gcd_optimal_evaluator_helper_t< T, true, true >
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_integer( a, b );
        }
    };
#else
    template < bool IsSpecialized, bool IsSigned >
    struct gcd_optimal_evaluator_helper2_t
    {
        template < typename T >
        struct helper
        {
            T  operator ()( T const &a, T const &b )
            {
                return gcd_euclidean( a, b );
            }
        };
    };

    template < >
    struct gcd_optimal_evaluator_helper2_t< true, true >
    {
        template < typename T >
        struct helper
        {
            T  operator ()( T const &a, T const &b )
            {
                return gcd_integer( a, b );
            }
        };
    };

    template < typename T, bool IsSpecialized, bool IsSigned >
    struct gcd_optimal_evaluator_helper_t
        : gcd_optimal_evaluator_helper2_t<IsSpecialized, IsSigned>
           ::BOOST_NESTED_TEMPLATE helper<T>
    {
    };
#endif

    template < typename T >
    struct gcd_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            typedef ::std::numeric_limits<T>  limits_type;

            typedef gcd_optimal_evaluator_helper_t<T,
             limits_type::is_specialized, limits_type::is_signed>  helper_type;

            helper_type  solver;

            return solver( a, b );
        }
    };
#else // BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T >
    struct gcd_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            return gcd_integer( a, b );
        }
    };
#endif

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    template < typename T, bool IsSpecialized, bool IsSigned >
    struct lcm_optimal_evaluator_helper_t
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_euclidean( a, b );
        }
    };

    template < typename T >
    struct lcm_optimal_evaluator_helper_t< T, true, true >
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_integer( a, b );
        }
    };
#else
    template < bool IsSpecialized, bool IsSigned >
    struct lcm_optimal_evaluator_helper2_t
    {
        template < typename T >
        struct helper
        {
            T  operator ()( T const &a, T const &b )
            {
                return lcm_euclidean( a, b );
            }
        };
    };

    template < >
    struct lcm_optimal_evaluator_helper2_t< true, true >
    {
        template < typename T >
        struct helper
        {
            T  operator ()( T const &a, T const &b )
            {
                return lcm_integer( a, b );
            }
        };
    };

    template < typename T, bool IsSpecialized, bool IsSigned >
    struct lcm_optimal_evaluator_helper_t
        : lcm_optimal_evaluator_helper2_t<IsSpecialized, IsSigned>
           ::BOOST_NESTED_TEMPLATE helper<T>
    {
    };
#endif

    template < typename T >
    struct lcm_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            typedef ::std::numeric_limits<T>  limits_type;

            typedef lcm_optimal_evaluator_helper_t<T,
             limits_type::is_specialized, limits_type::is_signed>  helper_type;

            helper_type  solver;

            return solver( a, b );
        }
    };
#else // BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    template < typename T >
    struct lcm_optimal_evaluator
    {
        T  operator ()( T const &a, T const &b )
        {
            return lcm_integer( a, b );
        }
    };
#endif

    // Functions to find the GCD or LCM in the best way
    template < typename T >
    inline
    T
    gcd_optimal
    (
        T const &  a,
        T const &  b
    )
    {
        gcd_optimal_evaluator<T>  solver;

        return solver( a, b );
    }

    template < typename T >
    inline
    T
    lcm_optimal
    (
        T const &  a,
        T const &  b
    )
    {
        lcm_optimal_evaluator<T>  solver;

        return solver( a, b );
    }

}  // namespace detail

//  Compile-time greatest common divisor evaluator class declaration  --------//

template < unsigned long Value1, unsigned long Value2 >
struct static_gcd
{
    BOOST_STATIC_CONSTANT( unsigned long, value
     = (detail::static_gcd_helper_t<Value1, Value2>::value) );

};  // boost::math::static_gcd

//  Compile-time least common multiple evaluator class declaration  ----------//

template < unsigned long Value1, unsigned long Value2 >
struct static_lcm
{
    BOOST_STATIC_CONSTANT( unsigned long, value
     = (detail::static_lcm_helper_t<Value1, Value2>::value) );

};  // boost::math::static_lcm

//  Greatest common divisor evaluator member function definition  ------------//

template < typename IntegerType >
inline
typename gcd_evaluator<IntegerType>::result_type
gcd_evaluator<IntegerType>::operator ()
(
    first_argument_type const &   a,
    second_argument_type const &  b
) const
{
    return detail::gcd_optimal( a, b );
}

//  Least common multiple evaluator member function definition  --------------//

template < typename IntegerType >
inline
typename lcm_evaluator<IntegerType>::result_type
lcm_evaluator<IntegerType>::operator ()
(
    first_argument_type const &   a,
    second_argument_type const &  b
) const
{
    return detail::lcm_optimal( a, b );
}

//  Greatest common divisor and least common multiple function definitions  --//

template < typename IntegerType >
inline
IntegerType
gcd
(
    IntegerType const &  a,
    IntegerType const &  b
)
{
    gcd_evaluator<IntegerType>  solver;

    return solver( a, b );
}

template < typename IntegerType >
inline
IntegerType
lcm
(
    IntegerType const &  a,
    IntegerType const &  b
)
{
    lcm_evaluator<IntegerType>  solver;

    return solver( a, b );
}

}  // namespace math
}  // namespace boost

#endif  // BOOST_MATH_COMMON_FACTOR_HPP


#ifndef INCLUDED_scheme_numeric_euler_angles_HH
#define INCLUDED_scheme_numeric_euler_angles_HH

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

namespace scheme { namespace numeric {

	template<class M> typename M::Scalar const & xx( M const & m ) { return m(0,0); }
	template<class M> typename M::Scalar const & xy( M const & m ) { return m(0,1); }
	template<class M> typename M::Scalar const & xz( M const & m ) { return m(0,2); }
	template<class M> typename M::Scalar const & yx( M const & m ) { return m(1,0); }
	template<class M> typename M::Scalar const & yy( M const & m ) { return m(1,1); }
	template<class M> typename M::Scalar const & yz( M const & m ) { return m(1,2); }
	template<class M> typename M::Scalar const & zx( M const & m ) { return m(2,0); }
	template<class M> typename M::Scalar const & zy( M const & m ) { return m(2,1); }
	template<class M> typename M::Scalar const & zz( M const & m ) { return m(2,2); }
	template<class M> typename M::Scalar       & xx( M       & m ) { return m(0,0); }
	template<class M> typename M::Scalar       & xy( M       & m ) { return m(0,1); }
	template<class M> typename M::Scalar       & xz( M       & m ) { return m(0,2); }
	template<class M> typename M::Scalar       & yx( M       & m ) { return m(1,0); }
	template<class M> typename M::Scalar       & yy( M       & m ) { return m(1,1); }
	template<class M> typename M::Scalar       & yz( M       & m ) { return m(1,2); }
	template<class M> typename M::Scalar       & zx( M       & m ) { return m(2,0); }
	template<class M> typename M::Scalar       & zy( M       & m ) { return m(2,1); }
	template<class M> typename M::Scalar       & zz( M       & m ) { return m(2,2); }

	template<class M> struct get_scalar { typedef typename M::Scalar type; };

	template<class T> T sin_cos_range(T const & t) { return t; }

	/// @brief COPIED FROM ROSETTA
	/// Return the three euler angles (in radians) that describe this HomogeneousTransform as the series
	/// of a Z axis rotation by the angle phi (returned in position 1 of the output vector), followed by
	/// an X axis rotation by the angle theta (returned in position 3 of the output vector), followed by another
	/// Z axis rotation by the angle psi (returned in position 2 of the output vector).
	/// This code is a modified version of Alex Z's code from r++.
	///
	/// @details
	/// The range of phi is [ -pi, pi ];
	/// The range of psi is [ -pi, pi ];
	/// The range of theta is [ 0, pi ];
	///
	/// The function pretends that this HomogeneousTransform is the result of these three transformations;
	/// if it were, then the rotation matrix would be
	///
	/// FIGURE 1:
	/// R = [
	///       cos(psi)cos(phi)-cos(theta)sin(phi)sin(psi)        cos(psi)sin(phi)+cos(theta)cos(phi)sin(psi)      sin(psi)sin(theta)
	///      -sin(psi)cos(phi)-cos(theta)sin(phi)cos(psi)       -sin(psi)sin(phi)+cos(theta)cos(phi)cos(psi)      cos(psi)sin(theta)
	///                   sin(theta)sin(phi)                                 -sin(theta)cos(phi)                        cos(theta)
	/// ]
	///
	/// where each axis above is represented as a ROW VECTOR (to be distinguished from the
	/// HomogeneousTransform's representation of axes as COLUMN VECTORS).
	///
	/// The zz_ coordinate gives away theta.
	/// Theta may be computed as acos( zz_ ), or, as Alex does it, asin( sqrt( 1 - zz^2))
	/// Since there is redundancy in theta, this function chooses a theta with a positive
	/// sin(theta): i.e. quadrants I and II.  Assuming we have a positive sin theta
	/// pushes phi and psi into conforming angles.
	///
	/// NOTE on theta: asin returns a value in the range [ -pi/2, pi/2 ], and we have artificially
	/// created a positive sin(theta), so we will get a asin( pos_sin_theta ), we have a value
	/// in the range [ 0, pi/2 ].  To convert this into the actual angle theta, we examine the zz sign.
	/// If zz is negative, we chose the quadrant II theta.
	/// That is, asin( pos_sin_theta) returned an angle, call it theta'.  Now, if cos( theta ) is negative,
	/// then we want to choose the positive x-axis rotation that's equivalent to -theta'.  To do so,
	/// we reflect q through the y axis (See figure 2 below) to get p and then measure theta as pi - theta'.
	///
	/// FIGURE 2:
	///
	///  II        |         I
	///            |
	///    p.      |      .q (cos(-theta'), abs(sin(theta')))
	///       .    |    .
	/// theta'( .  |  .  )  theta' = asin( abs(sin(theta))
	/// -----------------------
	///            |
	///            |
	///            |
	///  III       |        IV
	///            |
	///  The angle between the positive x axis and p is pi - theta'.
	///
	///
	///
	/// Since zx and zy contain only phi terms and a constant sin( theta ) term,
	/// phi is given by atan2( sin_phi, cos_phi ) = atan2( c*sin_phi, c*cos_phi ) = atan2( zx, -zy )
	/// for c positive and non-zero.  If sin_theta is zero, or very close to zero, we're at gimbal lock.
	///
	/// Moreover, since xz and yz contain only psi terms, psi may also be deduced using atan2.
	///
	/// There are 2 degenerate cases (gimbal lock)
	/// 1. theta close to 0  (North Pole singularity), or
	/// 2. theta close to pi (South Pole singularity)
	/// For these, we take: phi=acos(xx), theta = 0 (resp. Pi/2), psi = 0
	template<class M,class E> void
	euler_angles(M const & m, E & euler){

		typedef typename get_scalar<M>::type T;
		static T const pi = boost::math::constants::pi<T>();
		static T const pi_2 = boost::math::constants::pi<T>() * (T)2;
		static T const epsilon = (T)10*std::numeric_limits<T>::epsilon();
		static T const FLOAT_PRECISION = std::sqrt(epsilon);
		if ( zz(m) >= (T)1 - FLOAT_PRECISION ){
			euler[0] = std::atan2( sin_cos_range( yx(m) ), sin_cos_range( xx(m) ) );
			euler[1] = 0.0;
			euler[2] = 0.0;
		} else if ( zz(m) <= (T)-1 + FLOAT_PRECISION ){
			euler[0] = std::atan2( sin_cos_range( yx(m) ), sin_cos_range( xx(m) ) );
			euler[1] = 0.0;
			euler[2] = M_PI;
		} else {
			T pos_sin_theta = std::sqrt( (T)1 - zz(m)*zz(m) ); // sin2theta = 1 - cos2theta.
			euler[2] = std::asin( pos_sin_theta );
			if ( zz(m) < 0 ) {
				euler[2] = pi - euler[2];
			}
			euler[0] = std::atan2( xz(m), -yz(m) );
			euler[1] = std::atan2( zx(m),  zy(m) );
		}
		euler[0] += euler[0]<0.0 ? pi_2 : 0.0;
		euler[1] += euler[1]<0.0 ? pi_2 : 0.0;

		euler[0] = std::min(  std::max( 0.0, euler[0] ),  pi_2-epsilon  );
		euler[1] = std::min(  std::max( 0.0, euler[1] ),  pi_2-epsilon  );
		euler[2] = std::min(  std::max( 0.0, euler[2] ),  pi  -epsilon  );

		assert( 0 <= euler[0] ); assert( euler[0] <  pi_2 );
		assert( 0 <= euler[1] ); assert( euler[1] <  pi_2 );
		assert( 0 <= euler[2] ); assert( euler[2] <= pi   );
	}

	///@brief euler to matrix
	template<class M, class E> void
	from_euler_angles( E const & euler, M & m ){
		typedef typename get_scalar<M>::type T;
		T const ce1( std::cos( euler[0] ) ),  se1( std::sin( euler[0] ) );
		T const ce2( std::cos( euler[1] ) ),  se2( std::sin( euler[1] ) );
		T const ce3( std::cos( euler[2] ) ),  se3( std::sin( euler[2] ) );
		xx(m) =  ce2 * ce1 - ce3 * se1 * se2;  yx(m) =  ce2 * se1 + ce3 * ce1 * se2;  zx(m) =  se2 * se3;
		xy(m) = -se2 * ce1 - ce3 * se1 * ce2;  yy(m) = -se2 * se1 + ce3 * ce1 * ce2;  zy(m) =  ce2 * se3;
		xz(m) =                    se3 * se1;  yz(m) =                   -se3 * ce1;  zz(m) =        ce3;
	}

	template<class M, class E> void
	euler_angles_deg(M const & m, E & euler) {
		typedef typename get_scalar<M>::type T;
		static T const rad_to_deg = 180.0 / boost::math::constants::pi<T>();
		euler_angles(m,euler);
		euler[0] = euler[0] * rad_to_deg;
		euler[1] = euler[1] * rad_to_deg;
		euler[2] = euler[2] * rad_to_deg;
	}
	template<class M, class E> void
	from_euler_angles_deg(E const & erad, M & m) {
		typedef typename get_scalar<M>::type T;
		static T const deg_to_rad = boost::math::constants::pi<T>() / 180.0;
		E euler; euler[0];
		euler[0] = erad[0] * deg_to_rad;
		euler[1] = erad[1] * deg_to_rad;
		euler[2] = erad[2] * deg_to_rad;
		euler_angles(m,euler);
	}

}}

#endif


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

using namespace std;
using namespace Eigen;

// 文件路径
string left_file = "./left.png";
string right_file = "./right.png";

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

int main(int argc, char **argv) {
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    double b=0.573;

    cv::Mat left=cv::imread(left_file);
    cv::Mat right=cv::imread(right_file);
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left,right,disparity_sgbm);
    disparity_sgbm.convertTo(disparity,CV_32F,1.0/16.0f);//signed short 2byte -> float 4byte

    vector<Vector4d,Eigen::aligned_allocator<Vector4d>> pointcloud;

    cout<<left.rows<<" "<<left.cols<<" "<<left.step[0]<<" "<<left.step[1]<<" "<<left.channels()<<endl;
    cout<<disparity_sgbm.rows<<" "<<disparity_sgbm.cols<<" "<<disparity_sgbm.step[0]<<" "<<disparity_sgbm.step[1]<<" "<<disparity_sgbm.channels()<<endl;
    cout<<disparity.rows<<" "<<disparity.cols<<" "<<disparity.step[0]<<" "<<disparity.step[1]<<" "<<disparity.channels()<<endl;

    cout<<disparity_sgbm.type()<<" "<<disparity.type()<<" "<<disparity_sgbm.at<short>(10,0)<<" "<< disparity.at<float>(10,0)<<endl;
/*
+--------+----+----+----+----+------+------+------+------+
|        | C1 | C2 | C3 | C4 | C(5) | C(6) | C(7) | C(8) |
+--------+----+----+----+----+------+------+------+------+
| CV_8U  |  0 |  8 | 16 | 24 |   32 |   40 |   48 |   56 |
| CV_8S  |  1 |  9 | 17 | 25 |   33 |   41 |   49 |   57 |
| CV_16U |  2 | 10 | 18 | 26 |   34 |   42 |   50 |   58 |
| CV_16S |  3 | 11 | 19 | 27 |   35 |   43 |   51 |   59 |
| CV_32S |  4 | 12 | 20 | 28 |   36 |   44 |   52 |   60 |
| CV_32F |  5 | 13 | 21 | 29 |   37 |   45 |   53 |   61 |
| CV_64F |  6 | 14 | 22 | 30 |   38 |   46 |   54 |   62 |
+--------+----+----+----+----+------+------+------+------+
*/
    for(int v=0;v<left.rows;v++)
    {
        for(int u=0;u<left.cols;u++)
        {
            if(disparity.at<float>(v,u)<=0||disparity.at<float>(v,u)>=96.0) continue;

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

            double depth=b*fx/(disparity.at<float>(v,u));//disparity in pixel
            double x=depth*(u-cx)/fx;
            double y=depth*(v-cy)/fy;

            point[0]=x;
            point[1]=y;
            point[2]=depth;

            pointcloud.push_back(point);
            //cout<<(int)left.at<Eigen::Matrix<uchar,3,1>>(v,u)[0]<<" "<<(int)left.at<Eigen::Matrix<uchar,3,1>>(v,u)[1]<<" "<<(int)left.at<Eigen::Matrix<uchar,3,1>>(v,u)[2]<<" "<<endl;
            //cout<<point<<endl;

        }

    }
    cv::imshow("disparity",disparity/255.0);
    cv::waitKey(0);
    showPointCloud(pointcloud);
    return 0;

}

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

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

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

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

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

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

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

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


//PerpAmerOption.hpp

//Perpetual American Option class

#ifndef PerpAmerOption_hpp
#define PerpAmerOption_cpp

#include "Option.hpp"
#include "GlobalFunctions.hpp"
#include <vector>
#include <boost/math/distributions/normal.hpp>

//Perpetual american option class will be derived from Option base class and hold no data members
class PerpAmerOption : public Option
{
public:
	PerpAmerOption() : Option() {}
	PerpAmerOption(double S, double K, double r, double sig, double b) : Option(S, K, r, sig, b) {}
	PerpAmerOption(PerpAmerOption& source) : Option(source) {}

	~PerpAmerOption() {}

	PerpAmerOption& operator = (const PerpAmerOption& source);

	//All the setters and Getters are inherited

	double CallPrice()const { return PerpetualCall(S, K, r, sig, b); }
	double PutPrice()const { return PerpetualPut(S, K, r, sig, b); }
	double CallPrice_S(double S1) const { return PerpetualCall(S1, K, r, sig, b); }
	double PutPrice_S(double S1) const { return PerpetualPut(S1, K, r, sig, b); }

	//Pricing function that returns vector. Used in combination with function pointer for better versatility
	typedef double(PerpAmerOption::*FunctionPointer)(double)const;
	std::vector<double> Range_of_Prices(double LowerLimit, double UpperLimit, int Num, FunctionPointer Ptr);

};

#endif