/**
    @file bayes_classifier.cpp

    @author Terence Henriod

    Project 1: Bayesian Minimum Error Classification

    @brief Class implementations for the BayesClassifier defined in
           bayes_classifier.h.

    @version Original Code 1.00 (3/8/2014) - T. Henriod
*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   HEADER FILES / NAMESPACES
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

// Class Declaration
#include "bayes_classifier.h"

// Other Dependencies
#include <cassert>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
================================================================================
                   CLASS FUNCTION IMPLEMENTATIONS
================================================================================
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   CONSTRUCTOR(S) / DESTRUCTOR
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/**
BayesClassifier

Description

@pre
-# The GameState object is given an appropriate identifier.

@post
-# A new, empty GameState will be initialized.

@code
@endcode
*/
BayesClassifier::BayesClassifier()
{
  // variables
  int ndx = 0;
  Eigen::Vector2d temp_mean;
    temp_mean << 1, 1;
  Eigen::Matrix2d temp_matrix;
    temp_matrix << 1, 0,
                   0, 1;

  // initialize all members
  mean_vector_ << 1, 1;
  covariance_matrix_ << 1, 0,
                       0, 1;
  inverse_covariance_matrix_ = covariance_matrix_.inverse();
  covariance_determinant_ = covariance_matrix_.determinant();
  prior_probability_ = 0.5;
  class_name_ = "Give me a name!";

  // no return - constructor
}

BayesClassifier::BayesClassifier( const BayesClassifier& other )
{
  // no return - copy constructor
}

BayesClassifier& BayesClassifier::operator=( const BayesClassifier& other )
{
  // return *this
  return *this;
}

BayesClassifier::~BayesClassifier()
{
  // currently nothing to destruct

  // no return - destructor
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   MUTATORS
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void BayesClassifier::clear()
{
  // no return - void
}

void BayesClassifier::setMean( const Eigen::Vector2d& new_mean_vector )
{
  // set the appropriate mean vector
  mean_vector_ = new_mean_vector;

  // no return - void
}

void BayesClassifier::setMean( const vector<DataItem>& data )
{
  // variables
  Eigen::Vector2d new_mean;
    new_mean << 0, 0;
  int i = 0;
  int num_data = 0;

  // sum the values of the features over all of the data
  for( i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sum
      new_mean( 0 ) += data[i].feature_vector( 0 );
      new_mean( 1 ) += data[i].feature_vector( 1 );
      num_data++;
    }
  }

  // scale the data
  new_mean( 0 ) /= num_data;
  new_mean( 1 ) /= num_data;

  // update the new mean member
  mean_vector_ = new_mean;

  // no return - void
}

void BayesClassifier::setCovariance(
    const Eigen::Matrix2d& new_covariance_matrix )
{
  // set the appropriate covariance matrix
  covariance_matrix_ = new_covariance_matrix;

  // update the other covariance related members
  inverse_covariance_matrix_ = covariance_matrix_.inverse();
  covariance_determinant_ = covariance_matrix_.determinant();

  // no return - void
}

void BayesClassifier::setCovariance( const vector<DataItem>& data,
                                     const Eigen::Vector2d& mean )
{
  // variables
  Eigen::Matrix2d new_covariance;
     new_covariance << 0, 0,
                       0, 0;
  double temp = 0;
  int i = 0;
  int num_data = 0;

  // sum the values of the features over all of the data
  for( num_data = 0, temp = 0, i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sum
      temp += ( data[i].feature_vector( 0 ) - mean( 0 ) ) *
              ( data[i].feature_vector( 0 ) - mean( 0 ) );
      num_data++;
    }
  }

  // scale the result
  new_covariance( 0, 0 ) /= (num_data - 1);

  // sum the values of the features over all of the data
  for( temp = 0, i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sum
      temp += ( data[i].feature_vector( 0 ) - mean( 0 ) ) *
              ( data[i].feature_vector( 1 ) - mean( 1 ) );
    }
  }

  // scale the result
  new_covariance( 0, 1 ) /= (num_data - 1);
  new_covariance( 1, 0 ) = new_covariance( 0, 1 );

  // sum the values of the features over all of the data
  for( temp = 0, i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sum
      temp += ( data[i].feature_vector( 1 ) - mean( 1 ) ) *
              ( data[i].feature_vector( 1 ) - mean( 1 ) );
    }
  }

  // scale the result
  new_covariance( 1, 1 ) /= (num_data - 1);

  // update the new mean member
  covariance_matrix_ = new_covariance;

  // update the other covariance related members
  inverse_covariance_matrix_ = covariance_matrix_.inverse();
  covariance_determinant_ = covariance_matrix_.determinant();

  // no return - void
}

void BayesClassifier::setPriorProbability( const double new_probability )
{
  // assert pre-conditions
  assert( ( new_probability >= 0.0 ) && ( new_probability <= 1.0 ) );

  // set the new prior probability
  prior_probability_ = new_probability;

  // no return - void
}

void BayesClassifier::set_class_name( const string& new_name )
{
  // set the class name member
  class_name_ = new_name;
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   ACCESSORS
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
double BayesClassifier::prior_probability() const
{
  // return the prior probability of the class
  return prior_probability_;
}

Eigen::Vector2d BayesClassifier::mean_vector() const
{
  // return the prior mean feature vector of the class
  return mean_vector_;
}

Eigen::Matrix2d BayesClassifier::covariance_matrix() const
{
  // return the covariance matrix of the class
  return covariance_matrix_;
}

Eigen::Matrix2d BayesClassifier::inverse_covariance_matrix() const
{
  // return the inverse of the covariance matrix of the class
  return inverse_covariance_matrix_;
}

double BayesClassifier::covariance_determinant() const
{
  // return the determinant of the covariance matrix of the class
  return covariance_determinant_;
}

string BayesClassifier::class_name() const
{
  // return the class name
  return class_name_;
}

void BayesClassifier::performAnalysis(
    const string input_file,
    const string output_file,
    const vector<BayesClassifier>& classifiers )
{

/*
  // variables
  fstream file;
  TestData temp;
  char delimiter;
  int num_data = 0;
  int ndx = 0;
  int num_misclassified = 0;
  double test_error_rate = 0.5;
  double beta_start = 0.5;
  vector<TestData> data;
  Chernoff chernoff_bound;

  // read in all of the data
  file.clear();
  file.open( input_file.c_str(), fstream::in );
  while( file.good() )
  {
    // read in a line of data
    file >> temp.feature_vector(0) >> delimiter
         >> temp.feature_vector(1) >> delimiter
         >> temp.actual_class;

    // store the data
    data.push_back( temp );

    // count the data
    num_data++;
  }
  file.close();

  // classify all of the data
  for( ndx = 0; ndx < num_data; ndx++ )
  {
    // classify an object
    data[ndx].classified_as = assignToClass( data[ndx].feature_vector,
                                             classifiers );

    // case: it was classified correctly
    if( data[ndx].classified_as == data[ndx].actual_class )
    {
      // mark and count this as a correct classification
      data[ndx].correctly_classified = CORRECT;
    }
    // case: it was not classified correctly
    else
    {
      // mark this as an incorrect classification
      data[ndx].correctly_classified = INCORRECT;
      num_misclassified++;
    }
  }

  // compute the error rate of the test
  test_error_rate = double( num_misclassified ) / double( num_data );

  // find chernoff bound
  chernoff_bound = findChernoffBound( beta_start, 0 );

  // output the results to file
  file.clear();
  file.open( output_file.c_str(), fstream::out );
  file << "Number of data: " << ( num_data - 1 ) << endl
       << "Number of incorrect classifications: "
           << num_misclassified << endl
       << "Test Sample Error Rate: " << test_error_rate << endl
       << "Battacharyya bound: " << findBattacharyyaBound() << endl
       << "Chernoff bound: " << chernoff_bound.bound << endl
       << "         beta*: " << chernoff_bound.beta << endl;
  for( ndx = 0; ndx < num_data; ndx++ )
  {
    // write the delimited data to the file
    file << data[ndx].feature_vector(0) << ", "
         << data[ndx].feature_vector(1) << ", "
         << data[ndx].actual_class << ", "
         << data[ndx].classified_as << ", "
         << data[ndx].correctly_classified << endl;
  }
  file.close();
*/
  // no return - void
}

string BayesClassifier::assignToClass(
    Eigen::Vector2d& input_vector,
    vector<BayesClassifier>& classifiers )
{
  // variables
  string classification_result = "NO_RESULT";
  int i = 0;
  double largest_discriminant = 0;
  double largest_discriminant_ndx = 0;

  // calculate each of the discriminants, keep track of the most likely class
  for( i = 0; i < classifiers.size(); ++i )
  {
    // case: this discriminant is larger than the previously largest one
    if( largest_discriminant <
        classifiers[i].calculateDiscriminant( input_vector ) )
    {
      // store the index of the class
      largest_discriminant_ndx = i;
    }
  }

  // whichever class produced the largest discriminant is the most likely
  classification_result = classifiers[largest_discriminant_ndx].class_name();

  // return the resulting assignment
  return classification_result;
}

double BayesClassifier::calculateDiscriminant(
    const Eigen::Vector2d& input_vector )
{
  // variables
  double discriminant_result = 0;
  double first_sum_term = 0;
  double second_sum_term = 0;
  double third_sum_term = 0;
  double fourth_sum_term = 0;
  double fifth_sum_term = 0;
  Eigen::Vector2d intermediate_row;
  Eigen::Vector2d intermediate_col;
  Eigen::Matrix2d intermediate_matrix;

  // compute the first summative term of the discriminant function
  intermediate_matrix = -0.5 * inverse_covariance_matrix_;
  intermediate_row = ( input_vector.transpose() * intermediate_matrix );
  first_sum_term = intermediate_row.dot( input_vector );

  // compute the second summative term of the discriminant function
  second_sum_term =
  ( inverse_covariance_matrix_ * mean_vector_ ).transpose().dot( input_vector );

  // compute the third summative term of the discriminant function
  intermediate_row = -0.5 * mean_vector_;
  intermediate_row = intermediate_row.transpose() * inverse_covariance_matrix_;
  third_sum_term = intermediate_row.dot( mean_vector_ );

  // compute the fourth summative term of the discriminant function
  fourth_sum_term = -0.5 * log( covariance_determinant_ );

/*  // case: we are assuming case 1 assumptions
  else
  {
    // compute the first term ( 1/s^2 * mean * x )
    intermediate_row = ( 1.0 / variance ) * mean;
    first_sum_term = intermediate_row.transpose().dot( input_vector );

    // compute the second term
    intermediate_row = ( -1.0 / ( 2 * variance ) ) * mean;
    second_sum_term = intermediate_row.transpose().dot( mean );
  }
 */

  // compute the last summative term of the discriminant function
  fifth_sum_term = log( prior_probability_ );

  // sum the terms to get the discriminant result
  discriminant_result = first_sum_term + second_sum_term + third_sum_term +
                        fourth_sum_term + fifth_sum_term;

  // return the discriminant result
  return discriminant_result;
}

Chernoff BayesClassifier::findChernoffBound(
    const vector<BayesClassifier>& classifiers )
{
  // variables
  Chernoff chernoff_bound;
    chernoff_bound.bound = 1.0;
    chernoff_bound.beta_star = 1.0;
  double beta = 0;
  double prior_product = 0.0;
  double kappa_of_beta = 0;
  double new_attempt;

  // test many possibilities to find the ideal beta*
  for( beta = 0.0; beta < 1.0; beta += EPSILON )
  {
    // compute the prior product
    prior_product = pow( classifiers[0].prior_probability(), beta ) *
                    pow( classifiers[1].prior_probability(), ( 1.0 - beta ) );

    // kappa( beta* )
    kappa_of_beta = kappaF( beta, classifiers );

    // attempt to find a lower bound
    new_attempt = prior_product * exp( -1.0 * kappa_of_beta );

    // case: the new bound is a tighter one
    if( new_attempt < chernoff_bound.bound )
    {
      // set the new bound and beta_star
      chernoff_bound.bound = new_attempt;
      chernoff_bound.beta_star = beta;
    }
  }

  // return the Chernoff bound
  return chernoff_bound;
}

double BayesClassifier::findBattacharyyaBound(
    const vector<BayesClassifier>& classifiers )
{
  // variables
  double battacharyya_bound = 1;
  double kappa_of_beta = 0;
  double root_prior_product = 0;
  double root_covariance_det_product = 0;
  Eigen::Vector2d mean_difference;
  Eigen::Matrix2d covariance_sum;

  // compute the square root term
  root_prior_product = sqrt( classifiers[0].prior_probability() *
                             classifiers[1].prior_probability() );

  // compute kappa( 0.5 )
  kappa_of_beta = kappaF( 0.5, classifiers );

  // compute sqrt( P( w1 ) * P( w2 ) ) * e^( -kappa( 0.5 ) )
  battacharyya_bound = root_prior_product * exp( -1.0 * kappa_of_beta );

  // return the Battacharrya bound
  return battacharyya_bound;
}

double BayesClassifier::kappaF( const double beta,
                                const vector<BayesClassifier>& classifiers )
{
  // variables
  double kappa_of_beta = 0;
  double beta_complement = 1.0 - beta;
  double beta_product_over_two = 0;
  double root_prior_product = 0;
  double root_covariance_det_product = 0;
  double log_denominator = 0;
  Eigen::Vector2d mean_difference;
  Eigen::Vector2d intermediate_row;
  Eigen::Matrix2d scaled_covariance_sum;

  // compute (beta * beta^c) / 2
  beta_product_over_two = ( beta * beta_complement ) / 2;

  // compute the mean difference u2 - u1 (to be used later)
  mean_difference = classifiers[0].mean_vector() - classifiers[1].mean_vector();

  // compute the scaled covariance sum beta^c * E1 + beta * E2
  // (to be used later)
  scaled_covariance_sum =
      ( beta_complement * classifiers[0].covariance_matrix() ) +
      ( beta * classifiers[1].covariance_matrix() );

  // compute the logarithm denominator (to be used later)
  log_denominator = pow( classifiers[0].covariance_determinant(),
                         beta_complement );
  log_denominator *= pow( classifiers[1].covariance_determinant(),
                          beta );

  // compute the first term in the sum
  intermediate_row = beta_product_over_two * mean_difference;
  intermediate_row = intermediate_row.transpose() *
                     scaled_covariance_sum.inverse();
  kappa_of_beta = intermediate_row.transpose() * mean_difference;

  // compute the second term in the sum
  kappa_of_beta += 0.5 * log( scaled_covariance_sum.determinant() /
                              log_denominator );

  // return the result
  return kappa_of_beta;
}


//
// Created by Cephas Svosve on 17/6/2021.
//

#include "dividend.h"
#include <iostream>
#include <vector>
#include <math.h>
#include <iostream>
#include <boost/random.hpp>

//get dividend growth rate i.e average of the percentage change in dividend paid-out, dD(t)/D(t-1).

double Div::getGrowthRate(int tickerID)
{
 return growthRate[tickerID];
}

//get volatility i.e stdev of the percentage changes in dividend paid-out, dD(t)/D(t-1).

double Div::getVolatility(int tickerID)
{
    return volatility[tickerID];
}

//we retrieve the autocorrelation of a particular asset by this function

double Div::getAutoCorr(int tickerID)
{
    return autoCorr[tickerID];
}

//this is a setter for the average growth rate of an asset's growth rate

void Div::setGrowthRate(double x)
{
 growthRate.push_back(x);
}

//this is a setter for saving the volatility of an asset's dividend process

void Div::setVolatility(double x)
{
 volatility.push_back(x);
}

//this is a setter for saving the auto-correlation of an asset's dividend process

void Div::setAutoCorr(double x)
{
 autoCorr.push_back(x);
}

//work-in-progress-*the function is meant to retrieve the cross-sectional correlation from the market object

void Div::setCrossCorr()
{
 // std::cout <<  "Div " << crossCorrMat << std::endl;
}

//because our system depends on accuracy of the randomness of the random matrix,
//we create a function that carefully generates a purely random matrix,
//i.e. auto-correlation is 0 precise to the order of e-03 as well as the cross-sectional correlation

MatrixXd Div::generateWhiteNoise(int numOfAssets, int numOfTicks)
{

    MatrixXd randoms(numOfAssets,numOfTicks);
    VectorXd a;

    //algorithm for generating random numbers that are shuffled continuously through seeding them on time

    time_t now = time(0);
    boost::random::mt19937 gen{static_cast<uint32_t>(now)};
    boost::normal_distribution<> nd(0.0, clock.getDt());
    boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<> > var_nor(gen, nd);

    //we generate the matrix of random numbers

    for(int rw=0; rw < numOfAssets; rw++)
    {

        //we make sure the naturally occurring auto-correlation is sufficiently small by using a do-while loop

     do
        {
            //here we load each row with appropriate vector of random numbers
            a = VectorXd(numOfTicks);

            for(int i = 0; i < numOfTicks; ++i)
            {
                a(i) = var_nor();
            }

        }
        while (abs(lateralcorrcoef(a)) > 0.001);

        randoms.row(rw) = a;

    }

    //We then remove any cross-sectional correlation

    return verticallyWhiten(randoms);
}

//this function removes cross-sectional correlation by multiplying a matrix
// by the inverse of the lower cholesky decomposition of its correlation matrix

MatrixXd Div::verticallyWhiten(MatrixXd mx)
{
    Matrix crossCorrs = crossCorr(mx);
    LLT<MatrixXd> llt(crossCorrs);
    MatrixXd L = llt.matrixL();
    MatrixXd vWhite = L.inverse() * mx;

    return vWhite;
}

//this function calculates the coefficient of determination -correlation- between two vectors
//it requires the two vectors x and y, then computes the variables required for the formula
//i.e. variables xx = x^2  ,xy = x*y  and yy = y^2

double Div::corrcoef(VectorXd x, VectorXd y)
{
    if(x.size() == y.size()) {

        int n = x.size();

        //variable xx represents x^2

        double xx[n];
        double xy[n];
        double yy[n];

        //sumx is the sum of random variables x

        double sumx = x.sum();
        double sumy = y.sum();

        //sumxx is the sum of random variables x^2

        double sumxx;
        double sumxy;
        double sumyy;

        for (int i=0; i < x.size(); i++)
        {
            xx[i] = pow(x[i],2);
            xy[i] = x[i] * y[i];
            yy[i] = pow(y[i],2);

            sumxx = sumxx + xx[i];
            sumxy = sumxy + xy[i];
            sumyy = sumyy + yy[i];
        }

        //this is the formula used to calculate coefficient of determination, *see
        //https://www.jstor.org/stable/2965177?seq=6#metadata_info_tab_contents
        //Reed, William Gardner. “The Coefficient of Correlation.” Publications of the American Statistical Association,
        //vol. 15, no. 118, 1917, pp. 675–684. JSTOR, www.jstor.org/stable/2965177. Accessed 20 June 2021.

        double r = (n*sumxy - sumx*sumy)/sqrt((n*sumxx-pow(sumx,2))*(n*sumyy-pow(sumy,2)));

        return r;
    }
    else
    {
     cout << "found vectors of different size on computing correlation";
    }

}

//this function calculates the auto-correlation coefficient of a vector of random numbers
//we use it to verify that the naturally occuring auto-correlation in the random numbers-
//-we are using is sufficiently small i.e. of order e-03

double Div::lateralcorrcoef(VectorXd a){

    int n = a.size()-1;

    double x[n];
    double y[n];
    double xx[n];
    double xy[n];
    double yy[n];

    double sumx;
    double sumy;
    double sumxx;
    double sumxy;
    double sumyy;

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

        x[i] = a[i+1];
        y[i] = a[i];
        xx[i] = pow(x[i],2);
        xy[i] = x[i] * y[i];
        yy[i] = pow(y[i],2);

        sumx = sumx + x[i];
        sumy = sumy + y[i];

        sumxx = sumxx + xx[i];
        sumxy = sumxy + xy[i];
        sumyy = sumyy + yy[i];

    }

    double r = (n*sumxy - sumx*sumy)/sqrt((n*sumxx-pow(sumx,2))*(n*sumyy-pow(sumy,2)));
    return r;

}

//this function calculates the correlation matrix from the random matrix

MatrixXd Div::crossCorr(MatrixXd x){
    int m = x.col(0).size();
    int n = x.row(0).size();

    MatrixXd corrMat(m,m);

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

            corrMat(i,j) = corrcoef( x.row(i),x.row(j));
            corrMat(j,i) = corrMat(i,j);
        }
    }

    return corrMat;
}

//work-in-progress

MatrixXd Div::verticallyColor(MatrixXd x){}

VectorXd Div::laterallyColor(VectorXd x){}

VectorXd Div::standardize(VectorXd x){}

MatrixXd Div::generateColoredNoise(int numOfAssets, int numOfTicks)
{
    MatrixXd X = generateWhiteNoise(numOfAssets, numOfTicks);
    MatrixXd Y = verticallyWhiten(X);

    //vector<double> lC = laterallyColor(Y(0:));
}


#include <iostream>
#include <Eigen/Dense>
#include "../include/utils.h"

int main() {
  Eigen::Array<double, 1, -1> a(10);
  double b[] = {0., 1., 2., 3., 4., 5., 6., 7., 8., 9.};
  a << 0., 1., 2., 3., 4., 5., 6., 7., 8., 9.;
  std::cout << "Array []logsumexp(range(10)) = " << logsumexp(b, 10) << "\n";
  std::cout << "Eigen - logsumexp(range(10)) = " << logsumexp(a, 10) << "\n";
  Eigen::ArrayXd a_copy = a;
  normalize(a_copy);
  std::cout << "normalize(a) = \n" << a_copy << "\n";

  Eigen::ArrayXXd A(3, 3);
  A << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
  Eigen::ArrayXXd A_log = A;
  Eigen::ArrayXXd A_ = A;
  normalize(A_);
  std::cout << "normalize(A(3x3)) = \n";
  std::cout << A_ << "\n";
  log_normalize(A_log);
  std::cout << "log_normalize(A(3x3)) = \n";
  std::cout << A_log << "\n";

  double a_scaler = 2.0, b_scaler = 4.0;
  std::cout << "logaddexp(" << a_scaler <<
    " + " << b_scaler << ") = "
    << logaddexp(a_scaler, b_scaler) << "\n";

  Eigen::ArrayXd means(2);
  means << 0, 5;
  Eigen::ArrayXd covar = Eigen::ArrayXd::Ones(2);
  std::vector<double> X(b, b + 10);
  Eigen::ArrayXXd logprob(X.size(), 2);
  log_univariate_normal_density(X, means, covar, logprob);
  std::cout << "log_univariate_normal_density(X) = \n"
    << logprob << "\n";

  std::cout << "==================================Forward - backward Test\n";
  size_t n_observations = 10;
  size_t n_components = 2;
  Eigen::ArrayXd log_start(2);
  Eigen::ArrayXXd log_trans(2, 2);
  log_start << std::log(0.5), std::log(0.5);
  log_trans << std::log(0.5), std::log(0.5),
               std::log(0.5), std::log(0.5);
  Eigen::ArrayXXd alpha(10, 2);
  forward(n_observations, n_components, log_start,
          log_trans, logprob, alpha);
  std::cout << "forward -> alpha: \n"
            << alpha << "\n";
  Eigen::ArrayXXd beta(10, 2);
  backward(n_observations, n_components,
           log_trans, logprob, beta);
  std::cout << "backward -> beta: \n"
            << beta << "\n";

  std::cout << "================================Compute log xi\n";
  Eigen::ArrayXXd log_xi_sum(2, 2);
  log_xi_sum = -INFINITY * Eigen::ArrayXXd::Ones(2, 2);
  compute_log_xi_sum(n_observations, n_components, alpha,
                     log_trans, beta, logprob, log_xi_sum);
  std::cout << "Compute_log_xi_sum: \n"
    << log_xi_sum << "\n";

  return 0;
}


/*=============================================================================
  PHAS0100ASSIGNMENT1: PHAS0100 Assignment 1 Game of Life Simulation.
  Copyright (c) University College London (UCL). All rights reserved.
  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.
  See LICENSE.txt in the top level directory for details.
=============================================================================*/

#include <nbsimParticle.h>
#include <nbsimMassiveParticle.h>
#include <nbsimSolarSystemData.ipp>
#include <iostream>
#include <Eigen/Dense>
#include <chrono>

// Example, header-only library, included in project for simplicity's sake.

/**
 * \brief Demo file to check that includes and library linkage is correct.
 */

static void show_usage(std::string name)
{
    std::cerr << "Usage: " << name
    << "Options:\n"
    << "\tOption: (Solar System Simulator):\tUsers should input .1) step-size(unit: year) .2) length of time(unit: year)\n"
    << "Options:\n"
    << "\t-h,--help\t\t\tShow this help message.\n"
    << std::endl;
}

int main(int argc, char** argv)
{
    // -h and --help:
    if (argc == 2){
        if((argv[1] == "-h") or (argv[1] == "--help")){
            show_usage(argv[0]);
            return 0;
        }
        else{
            show_usage(argv[0]);
            return 0;
        }
    }

    // Option: Solar System:
    if (argc == 3){

        double step_size = std::stod(argv[1]);
        double totalTime = std::stod(argv[2]);

        int NPLANETS = 9;

        std::string name[NPLANETS];
        Eigen::Vector3d position;
        Eigen::Vector3d velocity;
        double mu;

        std::shared_ptr<nbsim::MassiveParticle> planet[NPLANETS];

        // Split the planet:
        for (int i=0;i<NPLANETS;i++){
            name[i] =  nbsim::solarSystemData.at(i).name;
            position = nbsim::solarSystemData.at(i).position;
            velocity = nbsim::solarSystemData.at(i).velocity;
            mu =  nbsim::solarSystemData.at(i).mu;

            std::shared_ptr<nbsim::MassiveParticle> particle(new nbsim::MassiveParticle(position,velocity,mu));
            planet[i] = particle;
        }

        // Add the attractor:
        for(int i=0;i<NPLANETS;i++){
            for(int j=0;j<NPLANETS;j++){
                if(i != j){
                    planet[i]->addAttractor(planet[j]);
                }
            }
        }

        // At the begining: calculate the Energy
        planet[0]->addAttractor(planet[0]); // add itself

        planet[0]->calculateEtotal();
        std::cout<<"Beginning:"<<"\n";
        std::cout<<"The kinetic energy of system is: "<<planet[0]->getEkinetic()<<"\n";
        std::cout<<"The potential Energy of system is: "<<planet[0]->getEpotential()<<"\n";
        std::cout<<"The Total Energy of system is: "<<planet[0]->getEtotal()<<"\n";
        planet[0]->removeAttractor(planet[0]); // remove itself

        // // Benchmark the time of the solar system: Begin
        std::clock_t c_start = std::clock();
        auto t_start = std::chrono::high_resolution_clock::now();

        // Parallel:
        omp_set_num_threads (8);

        #pragma omp parallel
        // Outer time:
        for (double t = 0;t<totalTime;t+=step_size){

            // Loop 1: Acceleration:
            #pragma omp for
            for (int i=0;i<NPLANETS;i++){
                planet[i]->calculateAcceleration();
            }
            // Loop 2: intergateTimestep:
            #pragma omp for
            for (int i=0;i<NPLANETS;i++){
                planet[i]->integrateTimestep(step_size);

            }
        }

        // Benchmark the time of the solar system: End
        std::clock_t c_end = std::clock();
        auto t_end = std::chrono::high_resolution_clock::now();

        // Summarising the position:
        for(int i=0;i<NPLANETS;i++){
            std::cout<<name[i] <<":\n Original Position: \n"<<nbsim::solarSystemData.at(i).position<<"\n";
            std::cout<<" Current Position: \n"<<planet[i]->getPosition()<<"\n\n";
        }

        // Calculate the System Energy:
        planet[0]->addAttractor(planet[0]); // add itself

        planet[0]->calculateEtotal();
        std::cout<<"End:"<<"\n";
        std::cout<<"The Kinetic Energy of system is: "<<planet[0]->getEkinetic()<<"\n";
        std::cout<<"The Potential Energy of system is: "<<planet[0]->getEpotential()<<"\n";
        std::cout<<"The Total Energy of system is: "<<planet[0]->getEtotal()<<"\n";

        planet[0]->removeAttractor(planet[0]); // remove itself

        // Benchmark and output the time:
        std::cout<<"\nThe run time is: "
                 <<1000.0*(c_end - c_start)/CLOCKS_PER_SEC<<" ms\n"
                 <<"Wall clock time passed: "
                 <<std::chrono::duration<double, std::milli>(t_end-t_start).count()<<" ms\n";

        return 1;
    }

    // Default Option:
    else{
        show_usage(argv[0]);
        return 0;
    }
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>
#include <time.h>
#include <memory>

//#include <boost/random/uniform_int_distribution.hpp>
//#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>

#include <dpMM/distribution.hpp>
#include <dpMM/sampler.hpp>

using namespace Eigen;
using std::cout;
using std::endl;

template<typename T>
class Cat : public Distribution<T>
{
public:
  uint32_t K_;
  Matrix<T,Dynamic,1> pdf_;
  Matrix<T,Dynamic,1> cdf_;

  /* constructor from pdf */
  Cat(const Matrix<T,Dynamic,1>& pdf, boost::mt19937 *pRndGen);
  /* constructor from indicators - estimates from counts */
  Cat(const VectorXu& z, boost::mt19937 *pRndGen);
  /* copy constructor */
  Cat(const Cat& other);
  virtual ~Cat();

  uint32_t sample();
  void sample(VectorXu& z);

  T logPdf(const Matrix<T,Dynamic,1>& x) const
  {
    //assuming x is all zeros except one element
    assert(x.rows()==K_); //data dimmension should agree with # categories
    for(uint32_t d=0; d<K_; ++d) {
      if(x(d)==1) {
        return(log(pdf_(d)));
      }
    }
    assert(false); //invalid data (at least one element must be one) [this should never happen]
    return(-1);
  };

  T logPdf(uint32_t x) const
  {
    assert(x < K_);
    return(log(pdf_(x)));
  };

  T logPdfOfSS(const Matrix<T,Dynamic,1>& x) const
  {

    assert(x.rows()==K_); //data dimmension should agree with # categories
    T logPdf = 0;
    for(uint32_t d=0; d<K_; ++d) {
      logPdf += x(d) * log(pdf_(d));
    }
    return logPdf;
  };

  const Matrix<T,Dynamic,1>& pdf() const {return pdf_;};
  void pdf(const Matrix<T,Dynamic,1>& pdf){
    pdf_ = pdf;
    updateCdf();
  };
  const Matrix<T,Dynamic,1>& cdf() const {return cdf_;};

  void print() const;

private:
  boost::uniform_01<T> unif_;
  void updateCdf();
};

typedef Cat<float> Catf;
typedef Cat<double> Catd;

template<typename T>
inline T logSumExpRow(const Matrix<T,Dynamic,Dynamic>& pdf, uint32_t i)
{
  T max = pdf.row(i).maxCoeff();
  return log((pdf.row(i).array()-max).exp().matrix().sum()) + max;
}

template<typename T>
inline T logSumExpCol(const Matrix<T,Dynamic,Dynamic>& pdf, uint32_t i)
{
  T max = pdf.col(i).maxCoeff();
  return log((pdf.col(i).array()-max).exp().matrix().sum()) + max;
}

template<typename T>
inline T logSumExp(const Matrix<T,Dynamic,1>& pdf)
{
  T max = pdf.maxCoeff();
  return log((pdf.array()-max).exp().matrix().sum()) + max;
}

//template<typename T>
//inline T logSumExp(const Matrix<T,1,Dynamic>& pdf)
//{
//  T max = pdf.maxCoeff();
//  return log((pdf.array()-max).exp().matrix().sum()) + max;
//}


/* ----------------------------------------------------------------------------
 * Copyright 2022, Jeferson Lima
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   examples/1/dc_motor_ex.cpp
 *  @author Jeferson Lima
 *  @brief  DC Motor Simulation Example
 *  @date   Mar 15, 2022
 **/

#include <iostream>
#include <boost/numeric/odeint.hpp>
#include <boost/array.hpp>
#include <robocore.hpp>
#ifdef USE_MATPLOTLIB
 #include <matplotlibcpp.h>
 namespace plt = matplotlibcpp;
#endif

 using namespace boost::numeric::odeint;

 const double j = 0.01;         // (J)     moment of inertia of the rotor     0.01 kg.m^2
 const double b = 0.1;          // (b)     motor viscous friction constant    0.1 N.m.s
 const double Ke = 0.01;        // (Ke)    electromotive force constant       0.01 V/rad/sec
 const double Kt = 0.01;        // (Kt)    motor torque constant              0.01 N.m/Amp
 const double R = 1;            // (R)     electric resistance                1 Ohm
 const double L = 0.5;          // (L)     electric inductance                0.5 H
 const double V = 12;           // (V)     motor votage                       12 V

 typedef boost::array< double , 2> state;
 std::vector<double> ts, dTheta, di;

void dc_motor_model(const state& x, state& dxdt, double t)
{
  dxdt[0] = -b*x[0]/j + Kt*x[1]/j;
  dxdt[1] = -R*x[1]/L + V/L - Ke*x[0];
}

void log_model(const state& x, const double t)
{
  ts.push_back(t);
  dTheta.push_back(x[0]);
  di.push_back(x[1]);
  std::cout << t << ';' << x[0] << ';' << x[1]  << std::endl;
}

int main(int argc, char** argv)
{
  state x = { 0.0 , 0.0 }; // initial conditions
  runge_kutta4< state > stepper;
  integrate_const( stepper , dc_motor_model, x , 0.0 , 20.0 , 0.1, log_model );

#ifdef USE_MATPLOTLIB
  plt::figure();
  plt::named_plot("Velocity (rad/s)", ts, dTheta);
  plt::named_plot("Current (A)", ts, di);
  plt::legend();
  plt::show();
#endif

#ifdef SAVE_OUTPUT_CSV
  columns vals = {{"Time", ts}, {"Velocity", dTheta}, {"Current", di}};
  RoboCore::write_csv("/tmp/dc_motor_output.csv", vals);
  std::cout << "Output file was saved in /tmp" << std::endl;
#endif

  return 0;
}


#include <mpi.h>
#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>

using namespace std;
using namespace Eigen;

typedef Matrix<double, Dynamic, Dynamic, RowMajor> RowMatrixXd;
typedef Matrix<complex<double>, Dynamic, Dynamic, RowMajor> RowMatrixXcd;

int main(int argc, char *argv[]) {
    int npes, myrank;

    MPI::Init(argc, argv);

    myrank = MPI::COMM_WORLD.Get_rank();
    npes = MPI::COMM_WORLD.Get_size();

    long xn = atol(argv[2]);
    long tn = atol(argv[3]);
    long long position = 0;

    int xnProc = ceil((xn + 0.0) / (npes + 0.0));
    int outputPrecision = 9;
    int modeCount = tn;

    RowMatrixXd snapshotsProc(xnProc, tn + 1);
    RowMatrixXd snapshotsInProdProc(tn, tn);
    RowMatrixXd r(tn, tn);
    RowMatrixXd sigmaPseudoInverse(tn, tn);
    RowMatrixXd v(tn, tn), uProc(xnProc, tn);
    RowMatrixXd aTildeProc(tn, tn), aTilde(tn, tn);
    RowMatrixXd x0TildeProc(tn, 1), x0Tilde(tn, 1);

    RowMatrixXcd eigenvec(tn, tn);
    RowMatrixXcd dmdModesAllProc(xnProc, tn);

    VectorXd dmdModeProc(xnProc);
    VectorXd dmdMode(xnProc * npes);

    double *snapshotsRecvBuf = new double[xnProc * (tn + 1)];
    double *tntnBuf = new double[tn * tn];
    double *tn1Buf = new double[tn * 1];

    double *tntn_pt = new double[tn * tn];
    double *tn1_pt = new double[tn * 1];
    double *tntn2_pt = new double[tn * tn];
    double *xnProc_pt = new double[xnProc];
    double *xnProcnpes_pt = new double[xnProc * npes];

    complex<double> *tntnc_pt = new complex<double>[tn * tn];

    MPI::COMM_WORLD.Barrier();
    double timeBegin = MPI::Wtime();

    /* input matrix from a file */
    double *snapshots = NULL;

    if (myrank == 0) {
        if (modeCount > tn) {
            cout << "Invalid output modes number" << endl;
            exit(0);
        }

        cout << "Initializing" << endl;

        snapshots = new double[xn * (tn + 1)];
        ifstream inputFile(argv[1]);
        if (inputFile.is_open()) {
            while (!inputFile.eof() && position < (xn * (tn + 1))) {
                inputFile >> snapshots[position];
                position++;
            }
        } else {
            cout << "input file not found." << endl;
            exit(0);
        }
        inputFile.close();

        cout << "Running" << endl;
    }
    /****************************/

    MPI::COMM_WORLD.Scatter(snapshots, xnProc * (tn + 1), MPI::DOUBLE,
                            snapshotsRecvBuf, xnProc * (tn + 1), MPI::DOUBLE,
                            0);

    // delete[] snapshots;

    snapshotsProc = Map<RowMatrixXd>(snapshotsRecvBuf, xnProc, tn + 1);

    // delete[] snapshotsRecvBuf;

    snapshotsInProdProc = snapshotsProc.block(0, 0, xnProc, tn).transpose() *
                          snapshotsProc.block(0, 0, xnProc, tn);

    tntn_pt = snapshotsInProdProc.data();

    MPI::COMM_WORLD.Reduce(tntn_pt, tntnBuf, tn * tn, MPI::DOUBLE, MPI::SUM, 0);

    if (myrank == 0) {
        VectorXd sigma(tn);

        RowMatrixXd snapshotsInProd = Map<RowMatrixXd>(tntnBuf, tn, tn);

        LLT<RowMatrixXd, Upper> cholesky(snapshotsInProd);
        r = cholesky.matrixU();

        JacobiSVD<RowMatrixXd> svd(r, ComputeThinV);
        sigma = svd.singularValues();
        v = svd.matrixV();

        for (int i = 0; i < tn; ++i) {
            for (int j = 0; j < tn; ++j) {
                if (i == j && sigma(i) != 0) {
                    sigmaPseudoInverse(i, j) = 1.0 / sigma(i);
                } else
                    sigmaPseudoInverse(i, j) = 0;
            }
        }

        tntn_pt = sigmaPseudoInverse.data();
        tntn2_pt = v.data();
    }

    MPI::COMM_WORLD.Bcast(tntn_pt, tn * tn, MPI::DOUBLE, 0);
    MPI::COMM_WORLD.Bcast(tntn2_pt, tn * tn, MPI::DOUBLE, 0);

    sigmaPseudoInverse = Map<RowMatrixXd>(tntn_pt, tn, tn);
    v = Map<RowMatrixXd>(tntn2_pt, tn, tn);

    uProc = snapshotsProc.block(0, 0, xnProc, tn) * v * sigmaPseudoInverse;

    if (myrank != npes - 1) {
        aTildeProc = uProc.adjoint() * snapshotsProc.block(0, 1, xnProc, tn) *
                     v * sigmaPseudoInverse;
        x0TildeProc = uProc.adjoint() * snapshotsProc.block(0, 0, xnProc, 1);

    } else {
        aTildeProc = uProc.block(0, 0, xn - xnProc * (npes - 1), tn).adjoint() *
                     snapshotsProc.block(0, 1, xn - xnProc * (npes - 1), tn) *
                     v * sigmaPseudoInverse;
        x0TildeProc =
          uProc.block(0, 0, xn - xnProc * (npes - 1), tn).adjoint() *
          snapshotsProc.block(0, 0, xn - xnProc * (npes - 1), 1);
    }

    tntn_pt = aTildeProc.data();
    tn1_pt = x0TildeProc.data();

    MPI::COMM_WORLD.Reduce(tntn_pt, tntnBuf, tn * tn, MPI::DOUBLE, MPI::SUM, 0);
    MPI::COMM_WORLD.Reduce(tn1_pt, tn1Buf, tn * 1, MPI::Double, MPI::SUM, 0);

    if (myrank == 0) {
        aTilde = Map<RowMatrixXd>(tntnBuf, tn, tn);
        x0Tilde = Map<RowMatrixXd>(tn1Buf, tn, 1);

        ComplexEigenSolver<RowMatrixXd> ces(aTilde);

        eigenvec = ces.eigenvectors();
        tntnc_pt = eigenvec.data();

        ofstream ofeigenvalReal, ofeigenvalImag, ofamplitudeReal,
          ofamplitudeImag;

        ofeigenvalReal.open("eigenvalReal.dat",
                            ofstream::trunc | ofstream::out);
        ofeigenvalImag.open("eigenvalImag.dat",
                            ofstream::trunc | ofstream::out);
        ofamplitudeReal.open("amplitudeReal.dat",
                             ofstream::trunc | ofstream::out);
        ofamplitudeImag.open("amplitudeImag.dat",
                             ofstream::trunc | ofstream::out);

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

          ofeigenvalReal << setprecision(outputPrecision)
                         << ces.eigenvalues().row(i).real() << endl;

          ofeigenvalImag << setprecision(outputPrecision)
                         << ces.eigenvalues().row(i).imag() << endl;

          ofamplitudeReal << setprecision(outputPrecision)
                          << (eigenvec.inverse() * x0Tilde).row(i).real()
                          << endl;

          ofamplitudeImag << setprecision(outputPrecision)
                          << (eigenvec.inverse() * x0Tilde).row(i).imag()
                          << endl;
        }
    }

    MPI::COMM_WORLD.Bcast(tntnc_pt, tn * tn, MPI::DOUBLE_COMPLEX, 0);
    eigenvec = Map<RowMatrixXcd>(tntnc_pt, tn, tn);

    dmdModesAllProc = uProc * eigenvec;

    for (int i = 0; i < modeCount; ++i) {
        dmdModeProc = dmdModesAllProc.col(i).real();

        xnProc_pt = dmdModeProc.data();

        MPI::COMM_WORLD.Barrier();

        MPI::COMM_WORLD.Gather(xnProc_pt, xnProc, MPI::DOUBLE, xnProcnpes_pt,
                               xnProc, MPI::DOUBLE, 0);

        if (myrank == 0) {
            dmdMode = Map<VectorXd>(xnProcnpes_pt, xnProc * npes);

            ofstream ofdmdMode;
            ofdmdMode.open("dmdMode" + to_string(i) + ".dat",
                           ofstream::trunc | ofstream::out);
            ofdmdMode << setprecision(outputPrecision) << dmdMode.head(xn)
                      << endl;
        }
    }

    MPI::COMM_WORLD.Barrier();

    double timeFinish = MPI::Wtime();
    double timing = timeFinish - timeBegin;
    double timingMax;

    MPI::COMM_WORLD.Reduce(&timing, &timingMax, 1, MPI::DOUBLE, MPI::MAX, 0);

    if (myrank == 0) {
        cout << "Calculation finished successfully." << endl
             << "Spatial: " << xn << " points." << endl
             << "Temporal: " << tn + 1 << " state vectors." << endl
             << "Parallelism: " << npes << " cores." << endl
             << "Time consumption: " << timingMax << " s." << endl
             << "Output DMD modes: " << modeCount << endl;
    }

    MPI::Finalize();
    return 0;
}


/* Bubble Dynamics with Chebyshev Spectral Collocation */

#include <iostream>
#include <fstream>
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <string>
#include <chrono>
#include <boost/numeric/odeint.hpp>
#include <Eigen/Core>

using namespace std;
using namespace boost::numeric::odeint;

const double rho_L = 9.970639504998557e+02;
const double p_inf = 1.0e+5;
const double sigma = 0.071977583160056;
const double gamma = 1.33;
const double c_L = 1.497251785455527e+03; // water 25 Celsius
const double mu_L = 8.902125058209557e-04; //25 Celsius
const double lambda = 0.6084; //water 25 Celsius
const double T_inf = 298.15; // 25 Celsius
const int N = 16;

string file_name = "../bubble_sim_rk4_p05_f100_Re10_t5.txt";

typedef double value_type;
typedef vector<value_type> state_type;
typedef Eigen::Matrix<value_type, N/2, N/2> matrix_type;

using namespace boost::numeric;

//ode function of bubble dynamic
class bubble {
public:
	std::vector<value_type> C; //constants of the right hand side
	Eigen::Matrix<value_type, N/2, 1> y; //collocation points (half)
	Eigen::Matrix<value_type, N/2, 1> y_sq; //same, every entry squared
	matrix_type D_E; //Derivative matrix for even functions
	matrix_type D_O; //Derivative matrix for odd functions
	Eigen::Matrix<value_type, 1, N/2> D_E0; //first row of even D matrix

	//p_A pressure amplitude, f pressure frequence, N number of collocation points
	bubble(value_type p_A, value_type f, value_type R_E){
		const double omega = 2*M_PI*f;
		value_type pi2wRE = 2*M_PI/(omega*R_E);

		C = std::vector<value_type>(13);
		C[0] = omega*R_E/(2*M_PI*c_L);
		C[1] = 4*mu_L/(c_L*rho_L*R_E);
		C[2] = 4*mu_L/(rho_L*R_E)*pi2wRE;
		C[3] = 2*sigma*pi2wRE*pi2wRE/(rho_L*R_E);
		C[4] = p_inf/rho_L*pi2wRE*pi2wRE;
		C[5] = p_A/rho_L*pi2wRE*pi2wRE;
		C[6] = pi2wRE*p_inf/(c_L*rho_L);
		C[7] = pi2wRE*p_A/(c_L*rho_L);
		C[8] = 2*M_PI*pi2wRE*p_A/(c_L*rho_L);
		C[9] = lambda*(gamma-1)/gamma*pi2wRE/R_E*T_inf/p_inf;
		C[10] = lambda*(gamma-1)*pi2wRE/R_E*T_inf/p_inf;
		C[11] = (gamma-1)/gamma;
		C[12] = 1.0/(3.0*gamma);

		Eigen::Matrix<value_type, N, 1> y_full(N);
		value_type rec_cpn =  1.0/(N-1);
		for(int i = 0; i < N; i++){
			y_full[i] = cos(M_PI*i*rec_cpn);
		}
		Eigen::Matrix<value_type, N, N> D(N, N);
		for(int i = 0; i < N; i++){
			for(int j = 0; j < N; j++){
				if(i == j){
					if(i == N-1){
						D(N-1,N-1) = -(1+2*(N-1)*(N-1))/6.0;
					}else if(i == 0){
						D(0,0) = (1+2*(N-1)*(N-1))/6.0;
					}else{
						D(i,i) = -y_full[i]/(2.0*(1.0-y_full[i]*y_full[i]));
					}
				}else{
					D(i,j) = std::pow(-1,i+j)*(i==0 || i==N-1?2.0:1.0)
							/((j==0 || j==N-1?2.0:1.0) * (y_full[i]-y_full[j]) );
				}
			}
		}
		D_E = matrix_type(N/2,N/2);
		D_O = matrix_type(N/2,N/2);
		for(int i = 0; i < N/2;i++){
		    for(int j = 0; j < N/2; j++){
		    	D_E(i,j) = D(i,j) + D(i,N-1-j);
		    	if(i==0) D_E0(j) = D_E(i,j);
			}
		}
		for(int i = 0; i < N/2;i++){
			for(int j = 0; j < N/2; j++){
				D_O(i,j) = D(i,j) - D(i,N-1-j);
		    }
		}
		y = y_full.head<N/2>();
		y_sq = y.cwiseProduct(y);
    }

	void operator()(const state_type &x, state_type &dxdt, const value_type t){
	    value_type rec_xR = 1.0 / x[0];
	    value_type rec_xp = 1.0 / x[2];

	    Eigen::Map<const Eigen::Matrix<value_type, N/2,1>> z(x.data()+3);
	    Eigen::Map<Eigen::Matrix<value_type, N/2,1>> dzdt(dxdt.data()+3);

	    Eigen::Matrix<value_type, N/2, 1> De_x = D_E*z; //derivative of z (dimless temperature)

	    //bubble pressure evolution
	    dxdt[2] = 3*rec_xR*(C[10]*rec_xR* De_x[0] - gamma*x[1]*x[2]);

	    //discretized PDE of bubble temperature
	    dzdt = De_x.cwiseProduct( x[1]*rec_xR*y - C[9]*rec_xR*rec_xR*rec_xp* De_x //this might show error, but it will not fail at compile time, valid syntax
	    		+ C[12]*rec_xp*dxdt[2]*y )
	    		+ C[11]*rec_xp*dxdt[2]*z + C[9]*rec_xp*rec_xR*rec_xR* z
				.cwiseProduct(y_sq.cwiseInverse()).cwiseProduct(D_O * (y_sq.cwiseProduct(De_x)));
	    dxdt[3] = 0.0; //Boundary condition

	    //Keller-Miksis equation
	    dxdt[0] = x[1];
	    value_type sin2pit = sin(2*M_PI*t);
	    value_type den = x[0] - C[0]*x[0]*x[1] + C[1];
	    value_type nom = 0.5*C[0] * x[1]*x[1]*x[1] - 1.5* x[1]*x[1] - C[2]*x[1]*rec_xR - C[3]*rec_xR
	    		+ C[4] * x[2] - C[4] - C[5] * sin2pit + C[6] * x[1]*x[2] - C[6] * x[1] - C[7] * x[1]*sin2pit
	            - C[8] * x[0]*cos(2*M_PI*t) + C[6] * x[0]*dxdt[2];
	    dxdt[1] = nom/den;
	}
};

class all_save_observer
{
	ostream &os;
	double file_size = 0;
public:
	int line_number = 0;

	all_save_observer(ostream &output):os(output){}

	void operator()(const state_type &x, const value_type t){
		os << t << ", " << x[0];
		for(int i =1; i < 3+N/2;i++){
			os << ", " << x[i];
		}
		os << endl;
		line_number++;
	}
};

int main() {
	double tolerance = 1e-10;
	cout << "Bubble dynamics started\n" << setprecision(17) << endl;

	typedef runge_kutta_cash_karp54< state_type , value_type , state_type , value_type > stepper_type;

	value_type f = 100e3;
	value_type p_A = 0.5e5;
	value_type R_E = 10e-6;
	state_type x(3+N/2);

	auto t1 = chrono::high_resolution_clock::now();

	bubble bubi(p_A, f, R_E);

	state_type dxdt(3+N/2);

	//auto stepper = euler< state_type , value_type>();
	auto stepper = make_controlled( tolerance , tolerance, stepper_type() );

	ofstream ofs(file_name);
	if(!ofs.is_open())exit(-1);
	ofs.precision(17);
	ofs.flags(ios::scientific);

	all_save_observer observer(ofs);

	//initial conditions
	x[0] = 1.0;
	x[1] = 0.0;
	x[2] = 1.0 + 2.0*sigma/(R_E*p_inf);
	for(int i=0; i < N/2;i++) x[3+i] = 1.0;

	double t_start = 0.0;
	integrate_adaptive(boost::ref(stepper), boost::ref(bubi), x, t_start, 5.0, 1e-5, boost::ref(observer));
	auto t2 = chrono::high_resolution_clock::now();

	cout << "Done" << endl;
	cout << "Time (ms):" << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << endl;

	ofs.flush();
	ofs.close();

	cout << "Ready"<< endl;

	return 0;
}


/**
 * @file linquadfelshaped.cc
 * @brief Creates convergence plots for experiment 3.2.3.10
 * @author Tobias Rohner
 * @date April 2020
 * @copyright MIT License
 */

#define _USE_MATH_DEFINES

#include <lf/assemble/assemble.h>
#include <lf/fe/fe.h>
#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <boost/program_options.hpp>
#include <cmath>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <memory>

namespace po = boost::program_options;

/**
 * @brief Solves the Poisson problem on a given mesh
 * @param mesh The mesh on which to solve the PDE
 * @param fe_space The Finite Element Space to use
 * @returns The dof vector corresponding to the solution of the PDE
 *
 * The problem that is solved has load zero and Dirichlet boundary conditions
 given by
 * \f[
        g(r, \phi) = r^{\frac{2}{3}}\sin(\frac{2}{3}\phi)
   \f]
 */
Eigen::VectorXd solvePoisson(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh,
    const std::shared_ptr<const lf::uscalfe::UniformScalarFESpace<double>>
        &fe_space) {
  // Define the boundary values
  const auto u_bd = [](const Eigen::Vector2d &x) -> double {
    const double r = x.norm();
    double phi = std::atan2(x[1], x[0]);
    if (phi < 0) {
      phi += 2 * M_PI;
    }
    return std::pow(r, 2. / 3) * std::sin(2. / 3 * phi);
  };

  // Initialize the matrix provider
  const lf::mesh::utils::MeshFunctionConstant<double> mf_alpha(1);
  const lf::mesh::utils::MeshFunctionConstant<double> mf_gamma(0);
  lf::uscalfe::ReactionDiffusionElementMatrixProvider element_matrix_provider(
      fe_space, mf_alpha, mf_gamma);

  // Assemble the system matrix (RHS is zero because we have no load)
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
  lf::assemble::COOMatrix<double> A_COO(dofh.NumDofs(), dofh.NumDofs());
  Eigen::VectorXd rhs = Eigen::VectorXd::Zero(dofh.NumDofs());
  std::cout << "\t\t> Assembling System Matrix" << std::endl;
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, element_matrix_provider,
                                      A_COO);

  // Enforce the dirichlet boundary conditions
  std::cout << "\t\t> Enforcing Boundary Conditions" << std::endl;
  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh);
  const auto selector = [&](unsigned int idx) -> std::pair<bool, double> {
    const lf::mesh::Entity &entity = dofh.Entity(idx);
    if (!boundary(entity)) {
      return {false, 0};
    }
    const lf::geometry::Geometry *geom = entity.Geometry();
    // Find out where the evaluation node corresponding to the DOF is
    const auto *const shape_function_layout =
        fe_space->ShapeFunctionLayout(entity.RefEl());
    const auto num_dofs = dofh.NumLocalDofs(entity);
    const auto glob_dof_idxs = dofh.GlobalDofIndices(entity);
    int dof_idx;
    for (dof_idx = 0; dof_idx < num_dofs; ++dof_idx) {
      if (glob_dof_idxs[dof_idx] == idx) {
        break;
      }
    }
    const Eigen::VectorXd eval_node =
        shape_function_layout->EvaluationNodes().col(dof_idx);
    const Eigen::Vector2d pos = geom->Global(eval_node);
    // Return the value on the boundary at the position of the evaluation node
    // corresponding to the dof
    return {true, u_bd(pos)};
  };
  lf::assemble::FixFlaggedSolutionComponents(selector, A_COO, rhs);

  // Solve the LSE using Cholesky decomposition
  std::cout << "\t\t> Solving LSE" << std::endl;
  Eigen::SparseMatrix<double> A = A_COO.makeSparse();
  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver(A);
  Eigen::VectorXd solution = solver.solve(rhs);

  // Return the resulting solution vector
  return solution;
}

int main(int argc, char *argv[]) {
  const int num_meshes = 7;

  po::options_description desc("allowed options");
  desc.add_options()("output,o", po::value<std::string>(),
                     "Name of the output file");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  if (vm.count("output") == 0) {
    std::cout << desc << std::endl;
    exit(1);
  }
  const std::string output_file = vm["output"].as<std::string>();

  // The analytic solution
  const auto u = [](const Eigen::Vector2d &x) -> double {
    const double r = x.norm();
    double phi = std::atan2(x[1], x[0]);
    if (phi < 0) {
      phi += 2 * M_PI;
    }
    return std::pow(r, 2. / 3) * std::sin(2. / 3 * phi);
  };
  lf::mesh::utils::MeshFunctionGlobal mf_u(u);
  // The gradient of the analytic solution
  const auto u_grad = [](const Eigen::Vector2d &x) -> Eigen::Vector2d {
    const double r = x.norm();
    double phi = std::atan2(x[1], x[0]);
    if (phi < 0) {
      phi += 2 * M_PI;
    }
    Eigen::Vector2d grad;
    grad[0] = 2. / 3 * std::pow(r, -4. / 3) *
              (x[0] * std::sin(2. / 3 * phi) - x[1] * std::cos(2. / 3 * phi));
    grad[1] = 2. / 3 * std::pow(r, -4. / 3) *
              (x[1] * std::sin(2. / 3 * phi) + x[0] * std::cos(2. / 3 * phi));
    return grad;
  };
  lf::mesh::utils::MeshFunctionGlobal mf_u_grad(u_grad);

  const std::filesystem::path here = __FILE__;
  const std::filesystem::path mesh_folder = here.parent_path() / "meshes";
  Eigen::MatrixXd results(num_meshes, 5);
  for (int mesh_idx = 0; mesh_idx < num_meshes; ++mesh_idx) {
    std::cout << "> Mesh Nr. " << mesh_idx << std::endl;

    // Load the mesh
    std::cout << "\t> Loading Mesh" << std::endl;
    const std::string mesh_name = "L" + std::to_string(mesh_idx) + ".msh";
    const std::filesystem::path mesh_file = mesh_folder / mesh_name;
    auto factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    const lf::io::GmshReader reader(std::move(factory), mesh_file.string());
    const auto mesh = reader.mesh();

    // Solve the problem with linear finite elements
    std::cout << "\t> Linear Lagrangian FE";
    const auto fe_space_o1 =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);
    std::cout << " (" << fe_space_o1->LocGlobMap().NumDofs() << " DOFs)"
              << std::endl;
    const Eigen::VectorXd solution_o1 = solvePoisson(mesh, fe_space_o1);
    const lf::fe::MeshFunctionGradFE<double, double> mf_grad_o1(fe_space_o1,
                                                                solution_o1);

    // Solve the problem with quadratic finite elements
    std::cout << "\t> Quadratic Lagrangian FE";
    const auto fe_space_o2 =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO2<double>>(mesh);
    std::cout << " (" << fe_space_o2->LocGlobMap().NumDofs() << " DOFs)"
              << std::endl;
    const Eigen::VectorXd solution_o2 = solvePoisson(mesh, fe_space_o2);
    const lf::fe::MeshFunctionGradFE<double, double> mf_grad_o2(fe_space_o2,
                                                                solution_o2);

    // Compute the errors
    std::cout << "\t> Computing Error Norms" << std::endl;
    const auto quadrule_provider = [](const lf::mesh::Entity &entity) {
      return lf::quad::make_QuadRule(entity.RefEl(), 6);
    };
    const double H1_err_o1 = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh, lf::mesh::utils::squaredNorm(mf_grad_o1 - mf_u_grad),
        quadrule_provider));
    const double H1_err_o2 = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh, lf::mesh::utils::squaredNorm(mf_grad_o2 - mf_u_grad),
        quadrule_provider));

    // Store the mesh width, the number of DOFs and the errors
    results(mesh_idx, 0) = std::sqrt(1. / mesh->NumEntities(0));
    results(mesh_idx, 1) = fe_space_o1->LocGlobMap().NumDofs();
    results(mesh_idx, 2) = fe_space_o2->LocGlobMap().NumDofs();
    results(mesh_idx, 3) = H1_err_o1;
    results(mesh_idx, 4) = H1_err_o2;
  }

  // Output the resulting errors to a file
  const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::ofstream file;
  file.open(output_file);
  file << results.format(CSVFormat);
  file.close();

  return 0;
}


#ifndef _COCONUT_PULP_MATH_VECTOR_HPP_
#define _COCONUT_PULP_MATH_VECTOR_HPP_

#include <cmath>
#include <cassert>
#include <array>
#include <algorithm>
#include <functional>
#include <memory>
#include <numeric>
#include <iosfwd>
#include <initializer_list>
#include <type_traits>

#include <boost/operators.hpp>

#include "coconut-tools/utils/InfixOstreamIterator.hpp"
#include "ScalarEqual.hpp"

namespace coconut {
namespace pulp {
namespace math {

template <class ScalarType, size_t DIMENSIONS_PARAM, class ScalarEqualityFunc = ScalarEqual<ScalarType>>
class Vector :
	boost::equality_comparable<Vector<ScalarType, DIMENSIONS_PARAM, ScalarEqualityFunc>,
	boost::additive<Vector<ScalarType, DIMENSIONS_PARAM, ScalarEqualityFunc>,
	boost::multiplicative<Vector<ScalarType, DIMENSIONS_PARAM, ScalarEqualityFunc>, ScalarType
	>>>
{
public:

	using Scalar = ScalarType;

	static const auto DIMENSIONS = DIMENSIONS_PARAM;

	using Elements = std::array<Scalar, DIMENSIONS>;

	// --- CONSTRUCTORS AND OPERATORS

	template <
		class... CompatibleTypes,
		class = std::enable_if_t<sizeof...(CompatibleTypes) == DIMENSIONS || sizeof...(CompatibleTypes) == 0>
		>
	explicit constexpr Vector(CompatibleTypes&&... values) noexcept :
		elements_{ std::forward<CompatibleTypes>(values)... }
	{
		static_assert(sizeof...(values) == DIMENSIONS || sizeof...(values) == 0, "Bad number of arguments");
	}

	template <
		class CompatibleScalarType,
		size_t OTHER_DIMENSIONS,
		class OtherScalarEqualityFunc,
		class... TailTypes,
		class = std::enable_if_t<sizeof...(TailTypes) + OTHER_DIMENSIONS == DIMENSIONS>
		>
	explicit constexpr Vector(
		const Vector<CompatibleScalarType, OTHER_DIMENSIONS, OtherScalarEqualityFunc>& other,
		TailTypes&&... tail
		)
	{
		std::copy(other.elements().begin(), other.elements().end(), elements_.begin());
		setTail_(elements_, OTHER_DIMENSIONS, std::forward<TailTypes>(tail)...);
	}

	// TODO: consider removing std::initializer_list constructor, as the number of values cannot be
	// checked at compile-time
	constexpr Vector(std::initializer_list<Scalar> values) noexcept {
		assert(values.size() == DIMENSIONS || values.size() == 0);
		std::copy(values.begin(), values.end(), elements_.begin());
		std::uninitialized_fill(
			elements_.begin() + values.size(),
			elements_.end(),
			Scalar(0)
			);
	}

	friend std::ostream& operator<<(std::ostream& os, const Vector& vector) {
		os << '<';
		std::copy(vector.elements_.begin(), vector.elements_.end(),
			coconut_tools::InfixOstreamIterator<ScalarType>(os, ", "));
		os << '>';
		return os;
	}

	friend bool operator==(const Vector& lhs, const Vector& rhs) noexcept {
		return std::equal(lhs.elements_.begin(), lhs.elements_.end(), rhs.elements_.begin(), ScalarEqualityFunc());
	}

	Vector& operator+=(const Vector& other) noexcept {
		std::transform(elements_.begin(), elements_.end(), other.elements_.begin(), elements_.begin(), std::plus<>());
		return *this;
	}

	Vector& operator-=(const Vector& other) noexcept {
		std::transform(elements_.begin(), elements_.end(), other.elements_.begin(), elements_.begin(), std::minus<>());
		return *this;
	}

	Vector operator-() const noexcept {
		auto result = Vector();
		std::transform(elements_.begin(), elements_.end(), result.elements_.begin(), std::negate<>());
		return result;
	}

	Vector& operator*=(Scalar scalar) noexcept {
		std::transform(elements_.begin(), elements_.end(), elements_.begin(), [scalar](auto element) {
				return element * scalar;
			});
		return *this;
	}

	Vector& operator/=(Scalar scalar) noexcept {
		std::transform(elements_.begin(), elements_.end(), elements_.begin(), [scalar](auto element) {
			return element / scalar;
		});
		return *this;
	}

	// --- CONVERTERS

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	std::enable_if_t<(DIMENSIONS_PARAM_> 2), Vector<Scalar, 3, ScalarEqualityFunc>>
		xyz() const noexcept
	{
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return Vector<Scalar, 3, ScalarEqualityFunc>(x(), y(), z());
	}

	// --- VECTOR-SPECIFIC OPERATIONS

	Scalar dot(const Vector& other) const noexcept {
		return std::inner_product(elements_.begin(), elements_.end(), other.elements_.begin(), Scalar(0));
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	constexpr std::enable_if_t<(DIMENSIONS_PARAM_ == 3), Vector> cross(const Vector& other) const noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return {
			(y() * other.z()) - (z() * other.y()),
			(z() * other.x()) - (x() * other.z()),
			(x() * other.y()) - (y() * other.x())
			};
	}

	Scalar length() const noexcept {
		using std::sqrt;
		return sqrt(lengthSq());
	}

	Scalar lengthSq() const noexcept {
		return dot(*this);
	}

	Vector& normalise() noexcept {
		const auto l = length();
		if (l > Scalar(0)) {
			*this /= l;
		}
		return *this;
	}

	Vector normalised() const noexcept {
		auto result = *this;
		return result.normalise();
	}

	// --- ACCESSORS

	constexpr const Scalar& operator[](size_t index) const noexcept {
		assert(index < DIMENSIONS);
		return elements_[index];
	}

	Scalar& operator[](size_t index) noexcept {
		assert(index < DIMENSIONS);
		return elements_[index];
	}

	template <size_t INDEX>
	constexpr std::enable_if_t<(DIMENSIONS > INDEX), const Scalar&> get() const noexcept {
		return elements_[INDEX];
	}

	template <size_t INDEX>
	std::enable_if_t<(DIMENSIONS > INDEX), Scalar&> get() noexcept {
		return elements_[INDEX];
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	constexpr std::enable_if_t<(DIMENSIONS_PARAM_> 0), const Scalar&> x() const noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<0>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	std::enable_if_t<(DIMENSIONS_PARAM_> 0), Scalar&> x() noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<0>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	constexpr std::enable_if_t<(DIMENSIONS_PARAM_> 1), const Scalar&> y() const noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<1>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	std::enable_if_t<(DIMENSIONS_PARAM_> 1), Scalar&> y() noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<1>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	constexpr std::enable_if_t<(DIMENSIONS_PARAM_ > 2), const Scalar&> z() const noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<2>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	std::enable_if_t<(DIMENSIONS_PARAM_> 2), Scalar&> z() noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<2>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	constexpr std::enable_if_t<(DIMENSIONS_PARAM_> 3), const Scalar&> w() const noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<3>();
	}

	template <size_t DIMENSIONS_PARAM_ = DIMENSIONS_PARAM>
	std::enable_if_t<(DIMENSIONS_PARAM_> 3), Scalar&> w() noexcept {
		static_assert(DIMENSIONS_PARAM_ == DIMENSIONS_PARAM, "Dimensions changed");
		return get<3>();
	}

	constexpr const Elements& elements() const noexcept {
		return elements_;
	}

private:

	Elements elements_;

	template <class HeadType, class... TailTypes>
	static constexpr void setTail_(
		Elements& elems,
		size_t index,
		HeadType&& head,
		TailTypes&&... tailTypes
		) noexcept
	{
		elems[index] = std::forward<HeadType>(head);
		setTail_(elems, index + 1, std::forward<TailTypes>(tailTypes)...);
	}

	static constexpr void setTail_(Elements&, size_t) noexcept {
	}

};

template <class S, size_t D, class SEF>
S dot(const Vector<S, D, SEF>& lhs, const Vector<S, D, SEF>& rhs) noexcept {
	return lhs.dot(rhs);
}

template <class S, size_t D, class SEF>
constexpr std::enable_if_t<D == 3, Vector<S, D, SEF>>
	cross(const Vector<S, D, SEF>& lhs, const Vector<S, D, SEF>& rhs) noexcept
{
	return lhs.cross(rhs);
}

using Vec2 = Vector<float, 2>;
static_assert(sizeof(Vec2) == sizeof(float) * 2, "Empty base optimisation didn't work");
static_assert(std::is_trivially_copyable<Vec2>::value, "Vector is not trivially copiable");

using Vec3 = Vector<float, 3>;
static_assert(sizeof(Vec3) == sizeof(float) * 3, "Empty base optimisation didn't work");
static_assert(std::is_trivially_copyable<Vec3>::value, "Vector is not trivially copiable");

using Vec4 = Vector<float, 4>;
static_assert(sizeof(Vec4) == sizeof(float) * 4, "Empty base optimisation didn't work");
static_assert(std::is_trivially_copyable<Vec4>::value, "Vector is not trivially copiable");

} // namespace math

// TODO: re-enable when primitive::Vector is removed
// using math::Vector;
using math::Vec2;
using math::Vec3;
using math::Vec4;

} // namespace pulp
} // namespace coconut

#endif /* _COCONUT_PULP_MATH_VECTOR_HPP_ */


#include <iostream>
#include <cassert>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/max_cardinality_matching.hpp>
#include <string>

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> Graph;

void testcase()
{
  int w, h;
  std::cin >> w >> h;
  assert(w >= 1 && w <= 50 && h >= 1 && h <= 50);

  std::vector<std::vector<bool>> should_tile_at_cell(h, std::vector<bool>(w, false));
  int num_nodes = w * h;
  int num_nodes_to_cover = 0;
  for (int y = 0; y < h; y++)
  {
    std::string row;
    std::cin >> row;
    assert(int(row.size()) == w);
    for (int x = 0; x < w; x++)
    {
      if (row.at(x) == '.')
      {
        num_nodes_to_cover++;
        should_tile_at_cell.at(y).at(x) = true;
      }
    }
  }

  if (num_nodes_to_cover % 2 != 0)
  {
    DEBUG(2, "fast path");
    std::cout << "no\n";
    return;
  }

  Graph G(num_nodes);
  auto node_at_y_x = [w, h, num_nodes](int y, int x) -> Graph::vertex_descriptor {
    assert(x >= 0 && x < w && y >= 0 && y < h);
    int node = y * w + x;
    assert(node >= 0 && node < num_nodes);
    return node;
  };

  for (int y = 0; y < h; y++)
  {
    for (int x = 0; x < w; x++)
    {
      if (!should_tile_at_cell.at(y).at(x))
      {
        continue;
      }
      if (y > 0 && should_tile_at_cell.at(y - 1).at(x))
      {
        DEBUG(3, "adding vertical edge " << y << " " << x);
        boost::add_edge(node_at_y_x(y - 1, x), node_at_y_x(y, x), G);
      }
      if (x > 0 && should_tile_at_cell.at(y).at(x - 1))
      {
        DEBUG(3, "adding horizontal edge " << y << " " << x);
        boost::add_edge(node_at_y_x(y, x - 1), node_at_y_x(y, x), G);
      }
    }
  }

  std::vector<Graph::vertex_descriptor> mate(num_nodes);
  assert(boost::checked_edmonds_maximum_cardinality_matching(G, mate.data()));
  int num_nodes_covered = 2 * boost::matching_size(G, mate.data());
  DEBUG(2, "num_nodes_covered " << num_nodes_covered);
  assert(num_nodes_covered >= 0 && num_nodes_covered <= num_nodes_to_cover);
  std::cout << (num_nodes_covered == num_nodes_to_cover ? "yes\n" : "no\n");
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  assert(t >= 0 && t <= 100);
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main() {
  ArrayXXf a(3, 3);
  ArrayXXf b(3, 3);
  a << 1, 2, 3,
      4, 5, 6,
      7, 8, 9;
  b << 1, 2, 3,
      1, 2, 3,
      1, 2, 3;

  // Adding two arrays
  cout << "a + b = " << endl << a + b << endl << endl;

  // Subtracting a scalar from an array
  cout << "a - 2 = " << endl << a - 2 << endl;
}


#pragma once
#include "Optimizer.hpp"

#include <Eigen/Dense>
#include <array>
#include <nlohmann/json.hpp>

namespace yavque
{
class SGDMomentum : public Optimizer
{
public:
	static constexpr std::array<double, 4> DEFAULT_PARAMS = {0.01, 0.0, 0.9, 1e-4};

private:
	double alpha_;
	double p_;
	double gamma_;
	double min_alpha_;

	Eigen::VectorXd m_;
	int t_ = 0;

public:
	explicit SGDMomentum(double alpha = DEFAULT_PARAMS[0], double p = DEFAULT_PARAMS[1],
	                     double gamma = DEFAULT_PARAMS[2],
	                     double min_alpha = DEFAULT_PARAMS[3])
		: alpha_{alpha}, p_{p}, gamma_{gamma}, min_alpha_{min_alpha}
	{
	}

	explicit SGDMomentum(const nlohmann::json& params)
		: alpha_{params.value("alpha", DEFAULT_PARAMS[0])},
		  p_{params.value("p", DEFAULT_PARAMS[1])}, gamma_{params.value(
														"gamma", DEFAULT_PARAMS[2])},
		  min_alpha_{params.value("min_alpha", DEFAULT_PARAMS[3])}
	{
	}

	[[nodiscard]] nlohmann::json desc() const override
	{
		return nlohmann::json{{"name", "SGD"},
		                      {"alhpa", alpha_},
		                      {"gamma", gamma_},
		                      {"p", p_},
		                      {"min_alpha", min_alpha_}};
	}

	Eigen::VectorXd getUpdate(const Eigen::VectorXd& v) override
	{
		using std::pow;
		if(t_ == 0)
		{
			m_ = Eigen::VectorXd::Zero(v.size());
		}

		++t_;
		m_ *= gamma_;
		m_ += (1 - gamma_) * v;
		double eta
			= std::max((alpha_ / pow(t_, p_)), min_alpha_) / (1.0 - pow(gamma_, t_));
		return -eta * m_;
	}
};
} // namespace yavque


//////////////////////////////////////////////////////////////////////////////
// random::poisson_ext::poisson_devroye::detail::parameters.hpp         	//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_PARAMETERS_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_PARAMETERS_HPP_ER_2010
#include <boost/random/poisson_ext/devroye/detail/math.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace detail{

    // These are quantities that remain fixed throughout sampling and are only
    // modified if the poisson mean is changed.
	template<typename Int,typename T, typename P>
    struct parameters{

		typedef devroye::detail::math<Int,T,P> ma_;

    	public:

        typedef Int result_type;
        typedef T input_type;

        parameters(){}
    	explicit parameters(const Int& mean)
        {
			this->m1_ 		= 	ma_::to_float(mean);
			this->m2_ 		= 	ma_::to_float(2*mean);
			input_type m8 	= 	ma_::to_float(8*mean);
            input_type m32 	= 	ma_::to_float(32*mean);

            this->c1_ = ma_::to_float(1)/m8; // Equation (6)

            // Equation (10)
            this->delta_ 	= 	ma_::log1p( m32/ma_::pi(), P() );
            this->delta_ 	= 	ma_::sqrt(this->delta() * this->m1());

			// Below equation (7)
			this->sd1_ = ma_::sqrt(
            	this->m1() + this->delta() / ma_::to_float(2)
            );
			this->shape2_ = this->delta() / (this->m2()+this->delta());

            input_type a1, a2, a3, a;

			a1 = ma_::sqrt( ma_::pi() * (this->m2() + this->delta()) );
            a1 *= ma_::exp(this->c1());
            a2 = ma_::exp(-(this->delta()+ma_::to_float(1)) * this->shape2());
            a2 /= this->shape2();
            a3 = ma_::to_float(1);
            a = a1 + a2 + a3;
            this->p1_ = a1 / a;
            this->p2_ = a2 / a;
            this->p3_ = a3 / a;

			// Reconciliation with Fortran code
            // RL					m1
            // TWO					m2
			// CON 					c1
            // D 					delta
            // D2					delta + m2
			// D3 					1 / shape2
            // STDDEV 				sd1
            // SUM 					a1 + a2 + a3
            // PBODY 				p3 = a3/(a+a2+a3)
			// PTAIL 				p2 + p3

        }

		std::ostream& parameters_description(std::ostream& os)const{
        	return os
                << '('
                << "m1 = " 		<< this->m1()
                << ','
                << " m2 = "		<< this->m2()
                << ','
                << " delta = " 	<< this->delta()
                << ','
                << " loc1 = "	<< loc1()
                << ','
                << " sd1 = " 	<< this->sd1()
                << ','
                << " shape2 = "	<< this->shape2()
                << ','
                << " c1 = "		<< this->c1()
                << ','
                << " p1 = "		<< this->p1()
                << ','
                << " p2 = "		<< this->p2()
                << ','
                << " p3 = "		<< this->p3()
                << ')';
        }

		const input_type& m1()const{ return this->m1_; }
		const input_type& m2()const{ return this->m2_; }
		const input_type& delta()const{ return this->delta_; }
		static input_type loc1(){ return -ma_::to_float(1)/ma_::to_float(2); }
		const input_type& sd1()const{ return this->sd1_; }
		const input_type& shape2()const{ return this->shape2_; }
        const input_type& c1()const{ return this->c1_; }
		const input_type& p1()const{ return this->p1_; }
		const input_type& p2()const{ return this->p2_; }
		const input_type& p3()const{ return this->p3_; }

		private:
        input_type inv_m1_;
		input_type m1_;
		input_type m2_;
        input_type delta_;
		input_type sd1_;
		input_type shape2_;

        input_type c1_;
        input_type p1_;
        input_type p2_;
		input_type p3_;

    };

}// detail
}// devroye
}// poisson
}// random
}// boost

#endif


/*!
 * @file
 * @brief
 * @copyright alphya 2018-2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef NYARUGA_UTIL_VALUE_INCLUDING_ERROR_HPP
#define NYARUGA_UTIL_VALUE_INCLUDING_ERROR_HPP

#pragma once

#include <nyaruga_util/partial_diff.hpp>
#include <nyaruga_util/diff.hpp>
#include <boost/hana/functional/arg.hpp>

namespace nyaruga {

namespace util {

// 誤差を含む値
struct value_including_error {
   num_t value; // 値
   num_t error; // 誤差
   operator num_t() { return value; }
   operator num_t() const { return value; }
};

namespace nyaruga_util_impl {

// 偏微分^2 * 誤差^2 を、一変数分だけ計算して、再帰
template <std::size_t count, std::size_t max, typename F, typename... Args>
decltype(auto) generate_error_impl(F && f, Args &&... args)
{
   if constexpr (count > max)
      return num_t{ 0 };
   else {
      auto && current_val_obj = boost::hana::arg<count>(args...);
      auto && current_val_error = current_val_obj.error;
      auto && partial_diff = nyaruga::util::partial_diff<count>(f)(static_cast<num_t>(args)...);
      auto && result = boost::multiprecision::pow(partial_diff, 2) *
                       boost::multiprecision::pow(current_val_error, 2);
      return static_cast<num_t>(result + generate_error_impl<count + 1, max>(std::forward<F>(f), std::forward<Args>(args)...));
   }
}

} // namespace nyaruga_util_impl

// 誤差の伝播式より、計算。誤差を返す
// Args には、value_including_errorを使用してください
template <typename F, typename... Args>
decltype(auto) generate_error(F && f, Args &&... args)
{
   return static_cast<num_t>(boost::multiprecision::pow(
      nyaruga_util_impl::generate_error_impl<1, sizeof...(args)>(std::forward<F>(f), std::forward<decltype(args)>(args)...), 0.5l));
}

} // namespace nyaruga_util

/*
#include "value_including_error.hpp"
#include <iostream>
auto main() -> int
{
   // 誤差を含む値の定義 { 値, 誤差 }
   auto&& x = nyaruga::util::value_including_error{ 20, 0.5 };
   auto&& y = nyaruga::util::value_including_error{ 50, 0.8 };
   // 誤差を含む値の関数定義　これは単純な掛け算
   decltype(auto) f = [](nyaruga::util::num_t x, nyaruga::util::num_t y) {
      return static_cast<nyaruga::util::num_t>(x * y);
   };
   // 普通に関数を実行した値
   auto&& v = f(x, y);
   // 誤差を計算
   auto&& e = nyaruga::util::generate_error(f, x, y);
   // だいたいあってる程度の値です
   // 単純な掛け算で、有効数字17桁くらいでした
   std::cout << std::setprecision(18)
      << "値は：" << v << "\n誤差は：" << e << "\n";
}
*/

#endif // #ifndef NYARUGA_UTIL_VALUE_INCLUDING_ERROR_HPP


#include <bits/stdc++.h>
#include <boost/tokenizer.hpp>

int64_t departure_time;
std::vector<int64_t> buses;
std::vector<std::pair<int64_t, int64_t>> buses_ids;

struct Euclid {
  int64_t mi, mj;
};

int64_t next_time(const int64_t bus, int64_t departure_time) {
  return bus * (departure_time / bus + 1) % departure_time;
}

void part1() {
  auto comp = [&] (const int64_t b1, const int64_t b2) {
    return next_time(b1, departure_time) < next_time(b2, departure_time);
  };
  int64_t best_bus = *(std::min_element(buses.begin(), buses.end(), comp));
  int64_t time = next_time(best_bus, departure_time);
  int64_t res = best_bus * time;

  std::cout << "Part 1 : " << res << std::endl;
}

int64_t get_modulo(int64_t ni, int64_t bi) {
  int64_t xi = (ni - bi);
  while (xi < 0)
    xi += ni;
  return xi % ni;
}

void part2() {
  int64_t res = 0;

  bool first = true;
  int64_t xi, ni;

  // Init
  auto cur = buses_ids.begin();
  ni = cur->second;
  xi = get_modulo(ni, cur->first);
  auto next = std::next(cur);

  // Chinese remainder theorem !
  while (next != buses_ids.end()) {
    int64_t nip1 = next->second;
    int64_t ai = get_modulo(nip1, next->first);
    std::cout << xi << " " << nip1 << " " << ai << std::endl;
    while (xi % nip1 != ai)
      xi += ni;

    ni *= nip1;

    cur = next;
    std::advance(next, 1);
  }

  std::cout << "Part 2 : " << xi << std::endl;
}

int main(int argc, char **argv) {
  std::ifstream f_in;
  f_in.open("13.in");
  std::string line;
  std::getline(f_in, line);
  departure_time = std::stoi(line);
  std::getline(f_in, line);
  f_in.close();

  boost::char_separator<char> sep{",", ""};
  boost::tokenizer tokenizer{line, sep};

  int tok = -1;
  for (const auto &t: tokenizer) {
    tok++;
    if (t == "x")
      continue;
    int64_t bus_id = std::stoi(t);
    buses.push_back(bus_id);
    buses_ids.push_back(std::make_pair(tok, bus_id));
    std::cout << tok << " " << bus_id << std::endl;
  }

  part1();
  part2();

  return 0;
}


// solving A * X = B
// A symmetric/hermitian positive definite in packed format
// driver function posv()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/ppsv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_symmetric.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<real_t, ublas::column_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;

typedef
  ublas::symmetric_matrix<real_t, ublas::lower, ublas::column_major> symml_t;
typedef
  ublas::hermitian_matrix<cmplx_t, ublas::lower, ublas::column_major> herml_t;

typedef
  ublas::symmetric_matrix<real_t, ublas::upper, ublas::column_major> symmu_t;
typedef
  ublas::hermitian_matrix<cmplx_t, ublas::upper, ublas::column_major> hermu_t;

int main() {

  cout << endl;

  // symmetric
  cout << "real symmetric\n" << endl;

  size_t n = 5;
  size_t nrhs = 2;
  symml_t sal (n, n);   // symmetric matrix
  symmu_t sau (n, n);   // symmetric matrix
  m_t x (n, nrhs);
  m_t bl (n, nrhs), bu (n, nrhs);  // RHS matrices

  init_symm (sal, 'l');
  //        [5 0 0 0 0]
  //        [4 5 0 0 0]
  //   al = [3 4 5 0 0]
  //        [2 3 4 5 0]
  //        [1 2 3 4 5]

  init_symm (sau, 'u');
  //        [5 4 3 2 1]
  //        [0 5 4 3 2]
  //   au = [0 0 5 4 3]
  //        [0 0 0 5 4]
  //        [0 0 0 0 5]

  print_m (sal, "sal");
  cout << endl;
  print_m_data (sal, "sal");
  cout << endl;

  print_m (sau, "sau");
  cout << endl;
  print_m_data (sau, "sau");
  cout << endl;

  for (int i = 0; i < x.size1(); ++i) {
    x (i, 0) = 1.;
    x (i, 1) = 2.;
  }
  bl = prod (sal, x);
  bu = prod (sau, x);

  print_m (bl, "bl");
  cout << endl;
  print_m (bu, "bu");
  cout << endl;

  lapack::ppsv (sal, bl);
  print_m (bl, "xl");
  cout << endl;

  lapack::ppsv (sau, bu);
  print_m (bu, "xu");
  cout << endl;

  //////////////////////////////////////////////////////////
  // hermitian
  cout << "\n==========================================\n" << endl;
  cout << "complex hermitian (well, not really ;o)\n" << endl;

  herml_t hal (3, 3);   // hermitian matrix
  hermu_t hau (3, 3);   // hermitian matrix
  cm_t cx (3, 1);
  cm_t cbl (3, 1), cbu (3, 1);  // RHS

  init_symm (hal, 'l');
  init_symm (hau, 'u');

  print_m (hal, "hal");
  cout << endl;
  print_m (hau, "hau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, -1);
  print_m (cx, "cx");
  cout << endl;
  cbl = prod (hal, cx);
  cbu = prod (hau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

  int ierr = lapack::ppsv (hal, cbl);
  if (ierr == 0)
    print_m (cbl, "cxl");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

  ierr = lapack::ppsv (hau, cbu);
  if (ierr == 0)
    print_m (cbu, "cxu");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

  cout << "\n===========================\n" << endl;
  cout << "complex hermitian\n" << endl;

  // regular (see ublas_gesv.cc), but not positive definite:

  hal (0, 0) = cmplx_t (3, 0);
  hal (1, 0) = cmplx_t (4, -2);
  hal (1, 1) = cmplx_t (5, 0);
  hal (2, 0) = cmplx_t (-7, -5);
  hal (2, 1) = cmplx_t (0, 3);
  hal (2, 2) = cmplx_t (2, 0);

  hau (0, 0) = cmplx_t (3, 0);
  hau (0, 1) = cmplx_t (4, 2);
  hau (0, 2) = cmplx_t (-7, 5);
  hau (1, 1) = cmplx_t (5, 0);
  hau (1, 2) = cmplx_t (0, -3);
  hau (2, 2) = cmplx_t (2, 0);

  print_m (hal, "hal");
  cout << endl;
  print_m (hau, "hau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, 1);
  print_m (cx, "cx");
  cout << endl;
  cbl = prod (hal, cx);
  cbu = prod (hau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

  ierr = lapack::ppsv (hal, cbl);
  if (ierr == 0)
    print_m (cbl, "cxl");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

  ierr = lapack::ppsv (hau, cbu);
  if (ierr == 0)
    print_m (cbu, "cxu");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

  cout << "\n===========================\n" << endl;
  cout << "complex hermitian\n" << endl;

  // positive definite:

  hal (0, 0) = cmplx_t (25, 0);
  hal (1, 0) = cmplx_t (-5, 5);
  hal (1, 1) = cmplx_t (51, 0);
  hal (2, 0) = cmplx_t (10, -5);
  hal (2, 1) = cmplx_t (4, 6);
  hal (2, 2) = cmplx_t (71, 0);

  hau (0, 0) = cmplx_t (25, 0);
  hau (0, 1) = cmplx_t (-5, -5);
  hau (0, 2) = cmplx_t (10, 5);
  hau (1, 1) = cmplx_t (51, 0);
  hau (1, 2) = cmplx_t (4, -6);
  hau (2, 2) = cmplx_t (71, 0);

  print_m (hal, "hal");
  cout << endl;
  print_m (hau, "hau");
  cout << endl;

  cm_t cbl2 (3, 2);
  cbl2 (0, 0) = cmplx_t (60, -55);
  cbl2 (1, 0) = cmplx_t (34, 58);
  cbl2 (2, 0) = cmplx_t (13, -152);
  cbl2 (0, 1) = cmplx_t (70, 10);
  cbl2 (1, 1) = cmplx_t (-51, 110);
  cbl2 (2, 1) = cmplx_t (75, 63);
  cm_t cbu2 (cbl2);
  print_m (cbl2, "cbl");
  cout << endl;

  ierr = lapack::ppsv (hal, cbl2);
  if (ierr == 0)
    print_m (cbl2, "cxl");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl << endl;
  cout << endl;

  ierr = lapack::ppsv (hau, cbu2);
  if (ierr == 0)
    print_m (cbu2, "cxu");
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl << endl;
  cout << endl;

}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <boost/math/special_functions/factorials.hpp>

#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h"
#include "Tudat/Astrodynamics/Gravitation/triAxialEllipsoidGravity.h"

namespace tudat
{

namespace gravitation
{

//! Function to calculate (non-normalized) cosine spherical harmonic coefficient for a homogeneous
//! triaxial ellipsoid
double calculateCosineTermForTriaxialEllipsoidSphericalHarmonicGravity(
        const double aSquaredMinusCSquared, const double bSquaredMinusCSquared,
        const double referenceRadius, const int degree, const int order )
{
    // Initialize coefficient to zero
    double powerSeries = 0.0;

    // Only non-zero terms are for even degree and order
    if( !( ( degree % 2 != 0 || order % 2 != 0 ) ) )
    {
        using boost::math::factorial;

        // Calculate indices for algorithm
        int l = degree / 2;
        int m = order / 2;
        int maximumIndex = std::floor( ( l - m ) / 2 );

        // Evaluate single term in power series of final equation of Boyce (1997)
        for( int i = 0; i <= maximumIndex; i++ )
        {
            powerSeries +=
                    ( std::pow( ( aSquaredMinusCSquared - bSquaredMinusCSquared ) /
                                2.0, m + 2 * i ) *
                      std::pow( -( aSquaredMinusCSquared + bSquaredMinusCSquared ) /
                                2.0, l - m - 2 * i ) ) /
                    ( std::pow( 2.0, m + 2 * i ) * factorial< double >( l - m - 2 * i ) *
                      factorial< double >( m + i ) * factorial< double >( i ) );
        }

        // Calculate multiplier of power series in final equation of Boyce (1997)
        double multiplier = 3.0 / std::pow( referenceRadius, 2 * l ) *
                ( factorial< double >( l ) * factorial< double >( 2 * l - 2 * m ) ) /
                ( ( 2.0 * static_cast< double >( l ) + 3.0 ) * factorial< double >( 2 * l + 1 ) );
        if( order != 0 )
        {
            multiplier *= 2.0;
        }

        // Complete calculation of coefficient
        powerSeries *= multiplier;
    }
    return powerSeries;
}

//! Function to calculate triaxial ellipsoid reference radius
double calculateTriAxialEllipsoidReferenceRadius(
        const double axisA, const double axisB, const double axisC )
{
    return std::sqrt( 3.0 / ( 1.0 / ( axisA * axisA ) + 1.0 / ( axisB * axisB ) +
                              1.0 / ( axisC * axisC ) ) );
}

//! Function to calculate triaxial ellipsoid volume
double calculateTriAxialEllipsoidVolume(
        const double axisA, const double axisB, const double axisC )
{
    return 4.0 / 3.0 * mathematical_constants::PI * axisA * axisB * axisC;
}

//! Function to calculate (non-normalized) cosine spherical harmonic coefficients for a
//! homogeneous triaxial ellipsoid
Eigen::MatrixXd createTriAxialEllipsoidSphericalHarmonicCosineCoefficients(
        const double axisA, const double axisB, const double axisC,
        const int maximumDegree, const int maximumOrder )
{
    // Initialize vector to zeros
    Eigen::MatrixXd cosineCoefficients = Eigen::MatrixXd::Zero(
                maximumDegree + 1, maximumOrder + 1 );

    // Pre-calculate for single coefficient calculations.
    double aSquaredMinusCSquared = axisA * axisA - axisC * axisC;
    double bSquaredMinusCSquared = axisB * axisB - axisC * axisC;
    double referenceRadius =  calculateTriAxialEllipsoidReferenceRadius( axisA, axisB, axisC );

    // Iterate over all requested degrees.
    for( int i = 0; i <= maximumDegree; i++ )
    {
        // Iterate over all requested orders.
        for( int j = 0; ( ( j <= maximumOrder ) && ( j <= i ) ); j++ )
        {
            // Only even degree and order terms are non-zero
            if( ( i % 2 == 0 ) && ( j % 2 == 0 ) )
            {
                // Calculate coefficient at single degree and order.
                cosineCoefficients( i, j ) =
                        calculateCosineTermForTriaxialEllipsoidSphericalHarmonicGravity(
                            aSquaredMinusCSquared, bSquaredMinusCSquared, referenceRadius, i, j );
            }
        }
    }

    return cosineCoefficients;
}

//! Function to calculate (non-normalized) cosine and sine spherical harmonic coefficients for a
//! homogeneous triaxial ellipsoid
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > createTriAxialEllipsoidSphericalHarmonicCoefficients(
        const double axisA, const double axisB, const double axisC,
        const int maximumDegree, const int maximumOrder )
{
    // Calculate cosine coefficients and add sine matrix (all zeroes)
    return std::make_pair( createTriAxialEllipsoidSphericalHarmonicCosineCoefficients(
                               axisA, axisB, axisC, maximumDegree, maximumOrder ),
                           Eigen::MatrixXd::Zero( maximumDegree + 1, maximumOrder + 1 ) );
}

//! Function to calculate (normalized) cosine and sine spherical harmonic coefficients for a
//! homogeneous triaxial ellipsoid
std::pair< Eigen::MatrixXd, Eigen::MatrixXd >
createTriAxialEllipsoidNormalizedSphericalHarmonicCoefficients(
        const double axisA, const double axisB, const double axisC,
        const int maximumDegree, const int maximumOrder )
{
    // Calculate non-normalized coefficients
    std::pair< Eigen::MatrixXd, Eigen::MatrixXd > unNormalizedCoefficients =
            createTriAxialEllipsoidSphericalHarmonicCoefficients(
                axisA, axisB, axisC, maximumDegree, maximumOrder );

    // Retrieve cosine coefficients
    Eigen::MatrixXd normalizedCosineCoefficients = unNormalizedCoefficients.first;

    // Iterate over all degrees and orders and normalized coefficients
    for( int i = 2; i < normalizedCosineCoefficients.rows( ); i++ )
    {
        for( int j = 0; ( ( j < normalizedCosineCoefficients.cols( ) ) &&
                                   ( j <= i ) ); j++ )
        {
            normalizedCosineCoefficients( i, j ) = normalizedCosineCoefficients( i, j ) /
                    basic_mathematics::calculateLegendreGeodesyNormalizationFactor( i, j );
        }
    }

    // Return geodesy-normalized coefficients.
    return std::make_pair( normalizedCosineCoefficients, unNormalizedCoefficients.second );
}

}

}


#include "geoutil.h"
#include "position.h"
#include <Eigen/Dense>
#include <cmath>

using namespace himan;
using namespace Eigen;

double geoutil::Distance(const point& a, const point& b, double r)
{
	const double alpha = std::pow(std::sin((b.Y() - a.Y()) / 360 * M_PI), 2) +
	               std::cos(a.Y() / 180 * M_PI) * std::cos(b.Y() / 180 * M_PI) *
	                   std::pow(std::sin((b.X() - a.X()) / 360 * M_PI), 2);
	return r * 2 * std::atan2(std::sqrt(alpha), std::sqrt(1 - alpha));
}

double geoutil::Area(const point& P1, const point& P2, const point& P3, double r)
{
	const position<double> p1(P1.Y() / 180 * M_PI, P1.Y() / 180 * M_PI, 0, earth_shape<double>(r));
	const position<double> p2(P2.Y() / 180 * M_PI, P2.X() / 180 * M_PI, 0, earth_shape<double>(r));
	const position<double> p3(P3.Y() / 180 * M_PI, P3.X() / 180 * M_PI, 0, earth_shape<double>(r));

	const Matrix<double, 3, 1> A(p1.Data());
	const Matrix<double, 3, 1> B(p2.Data());
	const Matrix<double, 3, 1> C(p3.Data());
	const double a = std::atan(A.cross(B).norm() / A.dot(B));
	const double b = std::atan(B.cross(C).norm() / B.dot(C));
	const double c = std::atan(C.cross(A).norm() / C.dot(A));
	const double s = (a + b + c) / 2;
	return 4 *
	       std::atan(
	           std::sqrt(std::tan(s / 2) * std::tan((s - a) / 2) * std::tan((s - b) / 2) * std::tan((s - c) / 2))) *
	       r * r;
}

double geoutil::Bearing(const point& a, const point& b)
{
	return std::atan2(std::sin((b.X() - a.X())/180*M_PI) * std::cos(b.Y()/180*M_PI) , std::cos(a.Y()/180*M_PI) * std::sin(b.Y()/180*M_PI) - std::sin(a.Y()/180*M_PI) * std::cos(b.Y()/180*M_PI) * std::cos((b.X() - a.X())/180*M_PI));
}

bool geoutil::InsideTriangle(const point& a, const point& b, const point& c, const point& p)
{
	const double epsilon = 1e-12;

	position<double> A(a.Y() / 180 * M_PI, a.X() / 180 * M_PI, 0, earth_shape<double>(1));
	position<double> B(b.Y() / 180 * M_PI, b.X() / 180 * M_PI, 0, earth_shape<double>(1));
	position<double> C(c.Y() / 180 * M_PI, c.X() / 180 * M_PI, 0, earth_shape<double>(1));

	position<double> P(p.Y() / 180 * M_PI, p.X() / 180 * M_PI, 0, earth_shape<double>(1));

	Matrix<double, 3, 1> v(P.Data());
	Matrix<double, 3, 3> M;
	M << A.X(), B.X(), C.X(), A.Y(), B.Y(), C.Y(), A.Z(), B.Z(), C.Z();

	ColPivHouseholderQR<Matrix<double, 3, 3>> dec(M);

	Matrix<double, 3, 1> beta = dec.solve(v);

	return beta(0) >= -epsilon && beta(1) >= -epsilon && beta(2) >= -epsilon;
}


// solving A * X = B
// A symmetric/hermitian positive definite
// factor (potrf()) and solve (potrs())

// #define BOOST_UBLAS_STRICT_HERMITIAN
// .. doesn't work (yet?)

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_symmetric.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::size_t;
using std::cout;
using std::endl;

typedef std::complex<double> cmplx_t;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<double, ublas::column_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;
#else
typedef ublas::matrix<double, ublas::row_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> cm_t;
#endif

#ifndef F_UPPER
typedef ublas::symmetric_adaptor<m_t, ublas::lower> symm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::lower> herm_t;
#else
typedef ublas::symmetric_adaptor<m_t, ublas::upper> symm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::upper> herm_t;
#endif

int main() {

  // for more descriptive comments see ublas_posv.cc
  cout << endl;

  // symmetric
  cout << "real symmetric\n" << endl;

  size_t n = 5;
  m_t a (n, n);
  symm_t sa (a);
#ifdef F_UPPER
  init_symm (sa, 'u');
#else
  init_symm (sa, 'l');
#endif
  print_m (sa, "sa");
  cout << endl;

  size_t nrhs = 2;
  m_t x (n, nrhs);
#ifndef F_ROW_MAJOR
  m_t b (n, nrhs);
#else
  m_t b (nrhs, n);
#endif
  ublas::matrix_column<m_t> xc0 (x, 0), xc1 (x, 1);
  atlas::set (1., xc0);
  atlas::set (2., xc1);
#ifndef F_ROW_MAJOR
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
  atlas::symm (sa, x, b);
#else
  atlas::symm (CblasLeft, 1.0, sa, x, 0.0, b);
#endif
#else
  ublas::matrix_row<m_t> br0 (b, 0), br1 (b, 1);
  atlas::symv (sa, xc0, br0);
  atlas::symv (sa, xc1, br1);
#endif
  print_m (b, "b");
  cout << endl;

  int ierr = atlas::cholesky_factor (sa);  // potrf()
  if (!ierr) {
    atlas::cholesky_substitute (sa, b);  // potrs()
    print_m (b, "x");
  }
  cout << endl;

  /////////////////////////////////////////////////////////
  // hermitian
  cout << "\n===========================\n" << endl;
  cout << "complex hermitian\n" << endl;

  cm_t ca (3, 3);
  herm_t ha (ca);
  cm_t cx (3, 1);
#ifndef F_ROW_MAJOR
  cm_t cb (3, 1);
#else
  cm_t cb (1, 3);
#endif

#ifndef F_UPPER
  ha (0, 0) = cmplx_t (3, 0);
  ha (1, 0) = cmplx_t (4, -2);
  ha (1, 1) = cmplx_t (5, 0);
  ha (2, 0) = cmplx_t (-7, -5);
  ha (2, 1) = cmplx_t (0, 3);
  ha (2, 2) = cmplx_t (2, 0);
#else
  ha (0, 0) = cmplx_t (3, 0);
  ha (0, 1) = cmplx_t (4, 2);
  ha (0, 2) = cmplx_t (-7, 5);
  ha (1, 1) = cmplx_t (5, 0);
  ha (1, 2) = cmplx_t (0, -3);
  ha (2, 2) = cmplx_t (2, 0);
#endif
  print_m (ha, "ha");
  cout << endl;

  ublas::matrix_column<cm_t> cx0 (cx, 0);
  atlas::set (cmplx_t (1, -1), cx0);
  print_m (cx, "cx");
  cout << endl;
#ifndef F_ROW_MAJOR
  ublas::matrix_column<cm_t> cb0 (cb, 0);
#else
  ublas::matrix_row<cm_t> cb0 (cb, 0);
#endif
  atlas::hemv (ha, cx0, cb0);
  print_m (cb, "cb");
  cout << endl;

  ierr = atlas::potrf (ha);
  if (ierr == 0) {
    atlas::potrs (ha, cb);
    print_m (cb, "cx");
  }
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

  cout << "\n===========================\n" << endl;
  cout << "complex hermitian\n" << endl;

#ifndef F_UPPER
  ha (0, 0) = cmplx_t (25, 0);
  ha (1, 0) = cmplx_t (-5, 5);
  ha (1, 1) = cmplx_t (51, 0);
  ha (2, 0) = cmplx_t (10, -5);
  ha (2, 1) = cmplx_t (4, 6);
  ha (2, 2) = cmplx_t (71, 0);
#else
  ha (0, 0) = cmplx_t (25, 0);
  ha (0, 1) = cmplx_t (-5, -5);
  ha (0, 2) = cmplx_t (10, 5);
  ha (1, 1) = cmplx_t (51, 0);
  ha (1, 2) = cmplx_t (4, -6);
  ha (2, 2) = cmplx_t (71, 0);
#endif
  print_m (ha, "ha");
  cout << endl;

#ifndef F_ROW_MAJOR
  cm_t cb32 (3, 2);
  cb32 (0, 0) = cmplx_t (60, -55);
  cb32 (1, 0) = cmplx_t (34, 58);
  cb32 (2, 0) = cmplx_t (13, -152);
  cb32 (0, 1) = cmplx_t (70, 10);
  cb32 (1, 1) = cmplx_t (-51, 110);
  cb32 (2, 1) = cmplx_t (75, 63);
#else
  cm_t cb32 (2, 3);
  cb32 (0, 0) = cmplx_t (60, -55);
  cb32 (0, 1) = cmplx_t (34, 58);
  cb32 (0, 2) = cmplx_t (13, -152);
  cb32 (1, 0) = cmplx_t (70, 10);
  cb32 (1, 1) = cmplx_t (-51, 110);
  cb32 (1, 2) = cmplx_t (75, 63);
#endif
  print_m (cb32, "cb");
  cout << endl;

  ierr = atlas::potrf (ha);
  if (ierr == 0) {
    atlas::potrs (ha, cb32);
    print_m (cb32, "cx");
  }
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;
  cout << endl;

}