//
// Created by robinjodon on 29.04.20.
//
#include <cassert>
#include <iostream>
#include "tbb/tbb.h"
#include "InterlockingSolver_Ipopt.h"
#include <Eigen/SparseQR>
#include "Utility/SparseOperations.h"

#define HAVE_CSTDDEF
#include <IpIpoptApplication.hpp>
#undef HAVE_CSTDDEF

using namespace Ipopt;

const double COIN_DBL_MAX = std::numeric_limits<double>::max();

/* ----------------------------------------------------------------------------------------------------------------- */
/* ----IPOPT INTERLOCKING SOLVER------------------------------------------------------------------------------------ */
/* ----------------------------------------------------------------------------------------------------------------- */

template<typename Scalar>
bool InterlockingSolver_Ipopt<Scalar>::isTranslationalInterlocking(InterlockingSolver_Ipopt::pInterlockingData &data) {
    vector<EigenTriple> tris;
    Eigen::Vector2i size;

    InterlockingSolver<Scalar>::computeTranslationalInterlockingMatrix(tris, size);

    if (!checkSpecialCase(data, tris, false, size)) {
        return false;
    }

    int num_var = size[1];
    InterlockingSolver<Scalar>::appendAuxiliaryVariables(tris, size);
    InterlockingSolver<Scalar>::appendMergeConstraints(tris, size, false);

    return solve(data, tris, false, size[0], size[1], num_var);
}

template<typename Scalar>
bool InterlockingSolver_Ipopt<Scalar>::isRotationalInterlocking(InterlockingSolver_Ipopt::pInterlockingData &data) {
    vector<EigenTriple> tris;
    Eigen::Vector2i size;

    InterlockingSolver<Scalar>::computeRotationalInterlockingMatrix(tris, size);

//    std::cout << "special case" << std::endl;
    if (!checkSpecialCase(data, tris, true, size)) {
        return false;
    }

    int num_var = size[1];
    InterlockingSolver<Scalar>::appendAuxiliaryVariables(tris, size);
    InterlockingSolver<Scalar>::appendMergeConstraints(tris, size, true);

//    std::cout << "solve" << std::endl;
    return solve(data, tris, true, size[0], size[1], num_var);
}

template<typename Scalar>
bool InterlockingSolver_Ipopt<Scalar>::checkSpecialCase(pInterlockingData &data,
                                                        vector<EigenTriple> copy_tris,
                                                        bool rotationalInterlockingCheck,
                                                        Eigen::Vector2i copy_size) {
    return true;
}

template<typename Scalar>
bool InterlockingSolver_Ipopt<Scalar>::solve(InterlockingSolver_Ipopt::pInterlockingData &data, vector<EigenTriple> &tris,
                                             bool rotationalInterlockingCheck,
                                             int num_row,
                                             int num_col,
                                             int num_var) {

    // [0] - Instance for Ipopt App and NLP
    SmartPtr<IpoptProblem> interlock_pb = new IpoptProblem();       // problem to solve
    SmartPtr<IpoptApplication> app = IpoptApplicationFactory();     // solver

    // [1] - Define the matrix B
    EigenSpMat b(num_row, num_col);
    b.setFromTriplets(tris.begin(), tris.end());

    interlock_pb->initialize(b);

    // [2] - Set some options for the solver
    app->Options()->SetIntegerValue("print_level", 5);

    // C.2 Termination
    app->Options()->SetNumericValue("tol", 1e-5);
    app->Options()->SetNumericValue("acceptable_tol", 1e-5);

    // C.4 NLP
    app->Options()->SetStringValue("jac_c_constant", "yes");
    app->Options()->SetStringValue("jac_d_constant", "yes");
    app->Options()->SetStringValue("hessian_constant", "yes");

    // C.7 Multiplier update
    app->Options()->SetStringValue("alpha_for_y", "primal-and-full");   // step size use the primal step size and full step if delta x ¡= alpha for y tol
    app->Options()->SetNumericValue("alpha_for_y_tol", 100);            // Tolerance for switching to full equality multiplier steps

    // C.8 Line search
    app->Options()->SetIntegerValue("max_soc", 0);                      // Disable 2ndOrder correction for trial steps at each iter.

    // C.10 Restoration phase
    app->Options()->SetStringValue("expect_infeasible_problem", "yes"); // enable heuristics to detect infeasibility quicker

    // todo: Exiting if t > 0 and lambda = 0

    // C.11 Linear Solver
    app->Options()->SetStringValue("linear_solver", "mumps");           // only available yet with installed IPOPT lib
    app->Options()->SetIntegerValue("min_refinement_steps", 0);         // iterative refinement steps/linear solve. Default=1 (changes Sum of the final values of constraints)
    app->Options()->SetIntegerValue("max_refinement_steps", 5);         //
    // C.20 MUMPS settings
    //  pivot_order is the most significant parameter
    //  * 1, 3 not accessible (can't install SCOTCH, 1 is manual mode)
    //  * 2 (AMF) is very slow (approximate minimum fill)
    //  * 4 (PORD) is slow.
    //  * 0 (AMD) quite fast (approximate minimum degree ordering)
    //  * 5 (METIS) is slow
    //  * 6 (QAMD the fastest (aproximate minimum degree ordering + quasi-dense row detection)
    //  * 77 (auto) seems to use AMD
    app->Options()->SetIntegerValue("mumps_pivot_order", 6);            // 0, 2, 6 are showing best perfs           (see MUMPS ICNTL(7))
    app->Options()->SetIntegerValue("mumps_scaling",   4);             // Huge differences (7 and 8 are slower),    (see MUMPS ICNTL(8))

    // C.6 Barrier param
    bool mehrotra = false;
    if (mehrotra) {
        // Runs Mehrotra predictor-corrector algo. Works well with LPs. A bit more aggressive but slightly slower than adaptive coupled with LOQO.
        app->Options()->SetStringValue("mehrotra_algorithm", "yes");
        app->Options()->SetNumericValue("tol", 1e+1);
        app->Options()->SetNumericValue("acceptable_tol", 1e+1);
        app->Options()->SetIntegerValue("min_refinement_steps", 0);         // iterative refinement steps/linear solve. Default=1 (changes Sum of the final values of constraints)
        app->Options()->SetIntegerValue("max_refinement_steps", 5);         //
    } else {
        app->Options()->SetStringValue("mu_strategy", "adaptive");
        /* Oracle for the new barrier param - Deetermines how the new barrier is computed in each "free mode" iteration
         * probing/loqo/quality-function (default) */
        app->Options()->SetStringValue("mu_oracle", "loqo");
    }

    // For debugging purposes
    // app->Options()->SetStringValue("derivative_test", "first-order");

    // [3] - Intialize the IpoptApplication and process the options
    ApplicationReturnStatus status;
    status = app->Initialize();
    if (status != Solve_Succeeded) {
        printf("\n\n*** Error during initialization!\n");
    }

    // [5] - Optimzation
    status = app->OptimizeTNLP(interlock_pb);

    if (status == Solve_Succeeded) {
        printf("\n\n*** The problem solved!\n");
    } else {
        printf("\n\n*** The problem FAILED!\n");
    }

    unpackSolution(data, rotationalInterlockingCheck, interlock_pb->x_solution.data(), num_var);
    if(interlock_pb->max_abs_t < 1E-4){
        return true;
    }
    else{
        return false;
    }
}

template<typename Scalar>
void InterlockingSolver_Ipopt<Scalar>::unpackSolution(InterlockingSolver_Ipopt::pInterlockingData &data,
                                                      bool rotationalInterlockingCheck,
                                                      const double *solution,
                                                      int num_var) {
    data = make_shared<typename InterlockingSolver<Scalar>::InterlockingData>();
    for (pContactGraphNode node: graph->nodes) {
        Vector3 trans(0, 0, 0);
        Vector3 rotate(0, 0, 0);
        Vector3 center = (node->centroid).template cast<double>();
        if (node->dynamicID != -1) {
            if (rotationalInterlockingCheck) {
                trans = Vector3(solution[node->dynamicID * 6],
                                solution[node->dynamicID * 6 + 1],
                                solution[node->dynamicID * 6 + 2]);
                rotate = -Vector3(solution[node->dynamicID * 6 + 3],
                                  solution[node->dynamicID * 6 + 4],
                                  solution[node->dynamicID * 6 + 5]);
            } else {
                trans = Vector3(solution[node->dynamicID * 3],
                                solution[node->dynamicID * 3 + 1],
                                solution[node->dynamicID * 3 + 2]);
            }
        }

        data->traslation.push_back(trans);
        data->rotation.push_back(rotate);
        data->center.push_back(center);

//        std::cout << node->staticID << ":" << trans.transpose() << ", " << rotate.transpose() << std::endl;
    }
}

/* ------------------------------------------------------------------------------------------------------------------ */
/* ----IPOPT INTERLOCKING PROBLEM------------------------------------------------------------------------------------ */
/* ------------------------------------------------------------------------------------------------------------------ */

IpoptProblem::IpoptProblem() {
    index_style = TNLP::C_STYLE;
    big_m = 5E7;                    // a smaller bigM works 5e6, solving is faster but final lambda is not exactly 0
}

// destructor
IpoptProblem::~IpoptProblem() = default;

// returns the problem dimensions
bool IpoptProblem::get_nlp_info(int &n, int &m, int &nnz_jac_g, int &nnz_h_lag, IndexStyleEnum &_index_style) {
    n = n_var;                                    // N+M variables [x, t, lamdba]
    m = n_constraints;                            // M inequalities
    nnz_jac_g = non_zero_jacobian_elements;       // non zero elements in Jacobian
    nnz_h_lag = non_zero_hessian_elements;        // non-zero elements in Lagrangian Hessian
    _index_style = index_style;                    // use the C style indexing (0-based)

    return true;
}

bool IpoptProblem::initialize(EigenSpMat &mat) {
    n_var_real = mat.cols() - mat.rows();         // variables without big M multiplier lambda and aux vars
    n_var = 1 + mat.cols();                       // variables including big M multiplier and auxiliary vars
    n_constraints = mat.rows();                   // Constraints inequalities

    mat.prune(0.0, 1E-9);
    b_coeff = mat;

    set_vectors_dimensions();

    set_bounds_info();

    append_bigm_variables(mat);

    non_zero_jacobian_elements = b_coeff.nonZeros();     // non zero elements in Jacobian (initial B matrix + id(m,m))
    non_zero_hessian_elements = 0;                             // non-zero elements in Lagrangian Hessian

    return true;
}

void IpoptProblem::set_vectors_dimensions() {
    // allocate size for x vector
    x.resize(n_var);
    x_solution.resize(n_var);

    x_l.resize(n_var);
    x_u.resize(n_var);

    g_l.resize(n_constraints);
    g_u.resize(n_constraints);
}

void IpoptProblem::append_bigm_variables(EigenSpMat &mat) {
    b_coeff.conservativeResize(mat.rows(), n_var);
    b_coeff.reserve(mat.nonZeros() + mat.rows());
    for (int c = 0; c < mat.rows(); ++c) {
        b_coeff.insert(c, n_var-1) = 1.0;
    }
    b_coeff.finalize();
    b_coeff.prune(0.0, 1E-9);
}

// returns the variable bounds
bool IpoptProblem::get_bounds_info(int n, Number *x_l, Number *x_u, int m, Number *g_l, Number *g_u) {

    for (int i = 0; i < n_var; i++) {
        x_l[i] = this->x_l[i];
        x_u[i] = this->x_u[i];
    }

    for (int i = 0; i < this->n_constraints; i++) {
        g_l[i] = this->g_l[i];
        g_u[i] = this->g_u[i];
    }

    return true;
}

bool IpoptProblem::set_bounds_info() {
    // Dynamic allocation

    for (int i = 0; i < n_constraints; i++) {
        g_l[i] = 0;
        g_u[i] = COIN_DBL_MAX;
    }

    for (int i = 0; i < n_var; i++) {
        if (i < n_var_real) {
            // x_i is defined in R
            x_l[i] = COIN_DBL_MAX * (-1.0);
            x_u[i] = COIN_DBL_MAX;
        } else {
            // lambda and aux variables >= 0
            x_l[i] = 0.0;
            x_u[i] = COIN_DBL_MAX;  //don't need to be inifinite, 1 is enough. However unbounding reduce execution time
        }
    }
    return true;
}

// returns the initial point for the problem
bool IpoptProblem::get_starting_point(int n, bool init_x, Number *x, bool init_z, Number *z_L, Number *z_U,
                                      int m, bool init_lambda, Number *lambda) {

    for (int i = 0; i < n_var; i++) {
        if (i < n_var_real) {
            this->x[i] = x[i] = 0.0;
        } else {
            this->x[i] = x[i] = 0.0;  // note: setting 0 instead of 1 makes the code faster
        }
    }
    return true;
}

// return the objective function
bool IpoptProblem::eval_f(int n, const Number *x, bool new_x, Number &obj_value) {
    this->obj_value = 0.0;
    for (int i = 0; i < n_var-1; i++) {
        if (i >= n_var_real)
            obj_value -= x[i];
    }
    this->obj_value += x[this->n_var-1] * this->big_m;

    obj_value = this->obj_value;
    return true;
}

// return the gradient of the objective function grad_{x} f(x)
bool IpoptProblem::eval_grad_f(int n, const Number *x, bool new_x, Number *grad_f) {
    // minus everywhere --> searching for maximum
    // loop through n_var -1
    for (int i = 0; i < n_var-1; i++)
        if (i < n_var_real) {
            grad_f[i] = 0.0;
        } else {
            grad_f[i] = -1.0;
        }
    // Last constribition: the big_m
    grad_f[n_var-1] = big_m;
    return true;
}

// return the value of the constraints: g(x)
// Computes g = B * X
bool IpoptProblem::eval_g(int n, const Number *x, bool new_x, int m, Number *g) {
    Eigen::Map<const Eigen::VectorXd> vecx(x, n);
    VectorXd vecg = b_coeff * vecx;
    std::copy(vecg.data(), vecg.data() + m, g);
    return true;
}

// return the triplet structure or values of the Jacobian
bool IpoptProblem::eval_jac_g(int n, const Number *x, bool new_x,
                              int m, int nele_jac, int *iRow, int *jCol, Number *values) {
    if (values == nullptr) {
        // return the 2 first triplet index row, col for the structure of the Jacobian
        int idx = 0;
        for (int k = 0; k < b_coeff.outerSize(); ++k) {
            for (SparseMatrix<double>::InnerIterator it(b_coeff, k); it; ++it) {
                iRow[idx] = it.row();
                jCol[idx] = it.col();
                idx++;
            }
        }
    } else {
        // loop through B sparse elements and get values.
        int idx = 0;
        for (int k = 0; k < b_coeff.outerSize(); ++k) {
            for (SparseMatrix<double>::InnerIterator it(b_coeff, k); it; ++it) {
                values[idx] = it.value();
                idx++;
            }
        }

    }
    return true;
}

//return the structure or values of the Hessian
bool IpoptProblem::eval_h(int n, const Number *x, bool new_x, Number obj_factor, int m, const Number *lambda,
                          bool new_lambda, int nele_hess, int *iRow, int *jCol, Number *values) {

    if (values == nullptr) {
        // return the structure. This is a symmetric matrix, fill the lower left triangle only.
        int idx = 0;
        for (int row = 0; row < non_zero_hessian_elements; row++) {
            for (int col = 0; col <= row; col++) {
                iRow[idx] = row;
                jCol[idx] = col;
                idx++;
            }
        }
    } else {
        // Hessian is zero
        // fixme: faster and neater declaration here
        int idx = 0;
        for (int k = 0; k < b_coeff.outerSize(); ++k) {
            for (SparseMatrix<double>::InnerIterator it(b_coeff, k); it; ++it) {
                values[idx] = 0;
            }
        }
    }

    return true;
}

void IpoptProblem::finalize_solution(SolverReturn status,
                                     int n,
                                     const Number *x,
                                     const Number *z_L,
                                     const Number *z_U,
                                     int m,
                                     const Number *g,
                                     const Number *lambda,
                                     Number obj_value,
                                     const IpoptData *ip_data,
                                     IpoptCalculatedQuantities *ip_cq) {

    // For this example, we write the solution to the console
    max_abs_t = 0;
//    printf("Solution of the primal variables, x\n");
    for (int i = 0; i < n; i++) {
        this->x[i] = x_solution[i] = x[i];
        if(i >= n_var_real && i < n_var - 1){
            max_abs_t = std::max(x[i], max_abs_t);
        }
    }

    printf("Maximum |t|:\t %E\n", max_abs_t);
    printf("Lambda     :\t %E\n", x[n_var-1]);

    Number sum_g = 0;
    for (int i = 0; i < m; i++)
        sum_g+= std::abs(g[i]);
    printf("Sum of the final values of the constraints:\t %.3f\n", sum_g);
}

void TemporaryFunction_InterlockingSolver_Ipopt ()
{
    InterlockingSolver_Ipopt<double> solver(nullptr, nullptr);
}

template class InterlockingSolver_Ipopt<float>;
template class InterlockingSolver_Ipopt<double>;


#include <boost/math/tools/roots.hpp>
#include <boost/math/distributions/binomial.hpp>

#include <sys/types.h>
#include <sys/stat.h>
#include <time.h>
#include <sstream>
#include <unordered_map>
#include <iostream>

using namespace std;
extern "C" double tau_c(double edit_error, int kmer_size, double MAX_ERROR,double MAX_EDIT_ERROR)
{
	// cout << MAX_EDIT_ERROR << "\t" << (MAX_ERROR - MAX_EDIT_ERROR) << endl;
	const double ERROR_RATIO = (MAX_ERROR - MAX_EDIT_ERROR) / MAX_EDIT_ERROR;

	double gap_error = std::min(1.0, ERROR_RATIO * edit_error);
	double a = (1 - gap_error) / (1 + gap_error);
	double b = 1 / (2 * std::exp(kmer_size * edit_error) - 1);
	return a * b;
}

extern "C" double solve_inverse_jaccard_c(int j, int kmer_size, double MAX_ERROR,double MAX_EDIT_ERROR)
{
	if (j == 0)
	{
		return 1;
	}
	if (j == 1)
	{
		return 0;
	}
	return boost::math::tools::newton_raphson_iterate([j, kmer_size,MAX_ERROR,MAX_EDIT_ERROR](double d){
		const double ERROR_RATIO = (MAX_ERROR - MAX_EDIT_ERROR) / MAX_EDIT_ERROR;
		double E = exp(d * kmer_size);
		return make_tuple(
			((1 - d * ERROR_RATIO) / (1 + d * ERROR_RATIO)) * (1.0 / (2 * E - 1)) - j,
			2 * (- kmer_size * E + ERROR_RATIO - 2 * ERROR_RATIO * E + E * kmer_size * pow(d * ERROR_RATIO, 2)) /
				pow((2 * E - 1) * (1 + d * ERROR_RATIO), 2)
		);
	}, 0.10, 0.0, 1.0, numeric_limits<double>::digits);
}

extern "C" double relaxed_jaccard_estimate_c(int s, int kmer_size, double  MAX_EDIT_ERROR, double MAX_ERROR, double result)
{

	using namespace boost::math;
	const double CI = 0.75;
	const double Q2 = (1.0 - CI) / 2; // one side interval probability
	// cout << "tau" << endl;
	result = ceil(s * tau_c(MAX_EDIT_ERROR, kmer_size,MAX_ERROR,MAX_EDIT_ERROR));
	// cout << result << endl;
	for (; result >= 0; result--) {

		double d = solve_inverse_jaccard_c(result / s, kmer_size,MAX_ERROR,MAX_EDIT_ERROR); // returns edit error

		double x = quantile(complement(binomial(s, tau_c(d, kmer_size,MAX_ERROR,MAX_EDIT_ERROR)), Q2)); // inverse binomial
		double low_d = solve_inverse_jaccard_c(x / s, kmer_size,MAX_ERROR,MAX_EDIT_ERROR);
		if (100 * (1 - low_d) < MAX_EDIT_ERROR) {
			result++;
			break;
		}
	}

	result = max(result, 0.0);
	return result;
}

extern "C" int fun1(int a){
	return a*4;
}


#include <mass.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(mass);

BOOST_AUTO_TEST_CASE(capsule)
{
  float const rho = 1.0f;
  float const R   = 1.0f;
  float const H   = 2.0f;

  mass::Properties<float> P = mass::compute_capsule(rho,R,H);

  BOOST_CHECK( P.m_m   > 0.0f );
  BOOST_CHECK( P.m_Ixx > 0.0f );
  BOOST_CHECK( P.m_Iyy > 0.0f );
  BOOST_CHECK( P.m_Izz > 0.0f );
  BOOST_CHECK_CLOSE( P.m_Ixx, P.m_Izz, 0.01f );
  BOOST_CHECK( P.m_Izz > P.m_Iyy );

  BOOST_CHECK(  P.is_body_space() );
  BOOST_CHECK( !P.is_model_space() );
}

BOOST_AUTO_TEST_SUITE_END();


/*
gammaGenerator.cpp - This file is part of the Bayesembler (v1.1.1)

The MIT License (MIT)

Copyright (c) 2014 Lasse Maretty and Jonas Andreas Sibbesen

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <math.h>
#include <gammaGenerator.h>
#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/lognormal.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/lognormal_distribution.hpp>

/*
 Gamma-sampler class
 */

SymmetricGammaGenerator::SymmetricGammaGenerator(HyperPrior * hyper_prior_in, int slice_iterations_in, double slice_window_size_in, int slice_max_windows_in, boost::random::mt19937* mt_rng_pt_in) {

    hyper_prior = hyper_prior_in;
    slice_iterations = slice_iterations_in;
    slice_window_size = slice_window_size_in;
    slice_max_windows = slice_max_windows_in;
	mt_rng_pt = mt_rng_pt_in;
}

// Init gamma with sample from the hyperprior
double SymmetricGammaGenerator::initGamma() {

	return hyper_prior->init();
}

double SymmetricGammaGenerator::generateGamma(ExpressionValueContainer expression) {

	vector<double> expression_s_plus;

	for (int i = 0; i < expression.getPlusSize(); i++) {

		int idx = expression.getPlus(i);
		double value = expression.getValue(idx);
		expression_s_plus.push_back(value);
    }

    // Init gamma and uniform sampler
    uniform_01_sampler_t sample_uniform_01(mt_rng_pt);

    double gamma_current = sample_uniform_01();

	if (gamma_current < double_underflow) {

		gamma_current = double_underflow;
	}

	double gamma = gamma_current;

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

        double y = calculateLogDensity(expression_s_plus, gamma_current) + hyper_prior->calculateLogDensity(gamma_current) + log(1-sample_uniform_01());

        // Find slice by "step-out"
        double left = gamma_current - sample_uniform_01() * slice_window_size;
        double right = left + slice_window_size;

        int j = floor(slice_max_windows*sample_uniform_01());
        int k = slice_max_windows-1-j;

		// Truncate at zero
        if (left < double_underflow) {

            left = double_underflow;
            j = 0;
        }

        while (j > 0 && y < (calculateLogDensity(expression_s_plus, left) + hyper_prior->calculateLogDensity(left))) {

			left = left - slice_window_size;
            j--;

            if (left < double_underflow) {

                left = double_underflow;
                break;
            }
		}

        // Expand window to the right
        while (k > 0 && y < (calculateLogDensity(expression_s_plus, right) + hyper_prior->calculateLogDensity(right))) {

			right = right + slice_window_size;
            k--;
        }

		// Sample from the window until in slice
		gamma = sample_uniform_01()*(right-left) + left;

		while ( y >= (calculateLogDensity(expression_s_plus, gamma) + hyper_prior->calculateLogDensity(gamma))) {

			if (gamma < gamma_current) {

				left = gamma;
				gamma = sample_uniform_01()*(right-left) + left;

			} else {

				right = gamma;
				gamma = sample_uniform_01()*(right-left) + left;
			}
		}

		gamma_current = gamma;
	}

    return gamma;

}

double SymmetricGammaGenerator::calculateLogDensity(vector<double>& expression, double gamma) {

    int size = expression.size();

    // Init prob with normalisation constant
    double prob = boost::math::lgamma(size*gamma) - size*boost::math::lgamma(gamma);

    for (int i=0; i < size; i++) {
        prob += (gamma-1)*log(expression[i]);
    }

    return prob;
}

FixedGammaGenerator::FixedGammaGenerator(double gamma_in) {
    gamma = gamma_in;
}

double FixedGammaGenerator::initGamma() {

	return gamma;
}

double FixedGammaGenerator::generateGamma(ExpressionValueContainer expression) {

    return gamma;
}

GammaHyperPrior::GammaHyperPrior(double shape_in, double scale_in, mt_rng_pt_t mt_rng_pt_in) {

    shape = shape_in;
    scale = scale_in;
	mt_rng_pt = mt_rng_pt_in;
};

double GammaHyperPrior::init() {

	boost::random::gamma_distribution<> gamma_dist(shape, scale);
	boost::random::variate_generator<boost::random::mt19937*, boost::random::gamma_distribution<> > sample_gamma(mt_rng_pt, gamma_dist);

	double gamma = sample_gamma();

	return gamma;
}

double GammaHyperPrior::calculateLogDensity(double gamma) {

	double prob = (shape-1)*log(gamma) - (gamma/scale);

    return prob;

};

LogNormalHyperPrior::LogNormalHyperPrior(double location_in, double scale_in, mt_rng_pt_t mt_rng_pt_in) {

    location = location_in;
    scale = scale_in;
	mt_rng_pt = mt_rng_pt_in;
}

double LogNormalHyperPrior::init() {

	boost::random::lognormal_distribution<> lognormal_dist(location, scale);
	boost::random::variate_generator<boost::random::mt19937*, boost::random::lognormal_distribution<> > sample_gamma(mt_rng_pt, lognormal_dist);

	double gamma = sample_gamma();

	return gamma;
}

double LogNormalHyperPrior::calculateLogDensity(double gamma) {

	double prob = - log(gamma) - pow(log(gamma)-location,2)/(2*pow(scale,2));

    return prob;
}


/* Copyright 2020 CNRS-AIST JRL */

#include <Eigen/Core>

#include <benchmark/benchmark.h>

#include "common.h"

using namespace Eigen;

// A = B
static void BM_Copy_MatrixXd(benchmark::State & state)
{
  MatrixXd source = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target = source;
}

// A = B^T
static void BM_Copy_MatrixXd_Source_Transpose(benchmark::State & state)
{
  MatrixXd source = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target = source.transpose();
}

// A^T = B
static void BM_Copy_MatrixXd_Target_Transpose(benchmark::State & state)
{
  MatrixXd source = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target.transpose() = source;
}

// A += B
static void BM_Add_MatrixXd(benchmark::State & state)
{
  MatrixXd source = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target += source;
}

// A += B^T
static void BM_Add_MatrixXd_Source_Transpose(benchmark::State & state)
{
  MatrixXd source = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target += source.transpose();
}

// A += B^T
static void BM_Add_MatrixXd_Target_Transpose(benchmark::State & state)
{
  MatrixXd source = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target.transpose() += source;
}

// y = A*x
static void BM_Mult_VectorXd(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  VectorXd x = VectorXd::Random(state.range(0));
  VectorXd y(state.range(0));

  for(auto _ : state) y.noalias() = A * x;
}

// row = (A*x)^T
static void BM_Mult_VectorXd_Source_Transpose(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  VectorXd x = VectorXd::Random(state.range(0));
  MatrixXd y(2, state.range(0));

  for(auto _ : state) y.row(1).noalias() = (A * x).transpose();
}

// row^T = A*x
static void BM_Mult_VectorXd_Target_Transpose(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  VectorXd x = VectorXd::Random(state.range(0));
  MatrixXd y(2, state.range(0));

  for(auto _ : state) y.row(1).transpose().noalias() = A * x;
}

// y = L*x
static void BM_Mult_VectorXd_Triangular(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  VectorXd x = VectorXd::Random(state.range(0));
  VectorXd y(state.range(0));

  for(auto _ : state) y.noalias() = A.template triangularView<Lower>() * x;
}

// y = L*x
static void BM_Mult_VectorXd_TriangularOptim4(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  A.template triangularView<StrictlyUpper>().setZero();
  VectorXd x = VectorXd::Random(state.range(0));
  VectorXd y(state.range(0));

  const int bsize = 4;
  for(auto _ : state)
  {
    int nBlock = A.cols() / bsize;
    y.setZero();
    int s = 0;
    int r = A.cols();
    for(int i = 0; i < nBlock; ++i)
    {
      y.tail(r).noalias() += A.block(s, s, r, bsize) * x.segment(s, bsize);
      s += bsize;
      r -= bsize;
    }
    y.tail(r).noalias() += A.bottomRightCorner(r, r) * x.tail(r);
  }
}

// y = L*x
static void BM_Mult_VectorXd_TriangularOptim8(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  A.template triangularView<StrictlyUpper>().setZero();
  VectorXd x = VectorXd::Random(state.range(0));
  VectorXd y(state.range(0));

  const int bsize = 8;
  for(auto _ : state)
  {
    int nBlock = A.cols() / bsize;
    y.setZero();
    int s = 0;
    int r = A.cols();
    for(int i = 0; i < nBlock; ++i)
    {
      y.tail(r).noalias() += A.block(s, s, r, bsize) * x.segment(s, bsize);
      s += bsize;
      r -= bsize;
    }
    y.tail(r).noalias() += A.bottomRightCorner(r, r) * x.tail(r);
  }
}

// y = L*x
static void BM_Mult_VectorXd_TriangularOptim16(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  A.template triangularView<StrictlyUpper>().setZero();
  VectorXd x = VectorXd::Random(state.range(0));
  VectorXd y(state.range(0));

  const int bsize = 16;
  for(auto _ : state)
  {
    int nBlock = A.cols() / bsize;
    y.setZero();
    int s = 0;
    int r = A.cols();
    for(int i = 0; i < nBlock; ++i)
    {
      y.tail(r).noalias() += A.block(s, s, r, bsize) * x.segment(s, bsize);
      s += bsize;
      r -= bsize;
    }
    y.tail(r).noalias() += A.bottomRightCorner(r, r) * x.tail(r);
  }
}

// C = A*B
static void BM_Mult_MatrixXd(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd B = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target.noalias() = A * B;
}

// C = A^T*B
static void BM_Mult_MatrixXd_AT(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd B = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target.noalias() = A.transpose() * B;
}

// C = A*B^T
static void BM_Mult_MatrixXd_BT(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd B = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target.noalias() = A * B.transpose();
}

// C = A^T*B^T
static void BM_Mult_MatrixXd_AT_BT(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd B = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target.noalias() = A.transpose() * B.transpose();
}

// C^T = A*B
static void BM_Mult_MatrixXd_Target_Transpose(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd B = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd target(state.range(0), state.range(0));

  for(auto _ : state) target.transpose().noalias() = A * B;
}

// Y = L*X
static void BM_Mult_MatrixXd_Triangular(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd B = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd C = MatrixXd::Random(state.range(0), state.range(0));

  for(auto _ : state) C.noalias() = A.template triangularView<Lower>() * B;
}

// y = L*x
static void BM_Mult_MatrixXd_TriangularOptim4(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  A.template triangularView<StrictlyUpper>().setZero();
  MatrixXd B = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd C = MatrixXd::Random(state.range(0), state.range(0));

  const int bsize = 4;
  for(auto _ : state)
  {
    int nBlock = A.cols() / bsize;
    C.setZero();
    int s = 0;
    int r = A.cols();
    for(int i = 0; i < nBlock; ++i)
    {
      C.bottomRows(r).noalias() += A.block(s, s, r, bsize) * B.middleRows(s, bsize);
      s += bsize;
      r -= bsize;
    }
    C.bottomRows(r).noalias() += A.bottomRightCorner(r, r) * B.bottomRows(r);
  }
}

// y = L*x
static void BM_Mult_MatrixXd_TriangularOptim8(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  A.template triangularView<StrictlyUpper>().setZero();
  MatrixXd B = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd C = MatrixXd::Random(state.range(0), state.range(0));

  const int bsize = 8;
  for(auto _ : state)
  {
    int nBlock = A.cols() / bsize;
    C.setZero();
    int s = 0;
    int r = A.cols();
    for(int i = 0; i < nBlock; ++i)
    {
      C.bottomRows(r).noalias() += A.block(s, s, r, bsize) * B.middleRows(s, bsize);
      s += bsize;
      r -= bsize;
    }
    C.bottomRows(r).noalias() += A.bottomRightCorner(r, r) * B.bottomRows(r);
  }
}

// y = L*x
static void BM_Mult_MatrixXd_TriangularOptim16(benchmark::State & state)
{
  MatrixXd A = MatrixXd::Random(state.range(0), state.range(0));
  A.template triangularView<StrictlyUpper>().setZero();
  MatrixXd B = MatrixXd::Random(state.range(0), state.range(0));
  MatrixXd C = MatrixXd::Random(state.range(0), state.range(0));

  const int bsize = 16;
  for(auto _ : state)
  {
    int nBlock = A.cols() / bsize;
    C.setZero();
    int s = 0;
    int r = A.cols();
    for(int i = 0; i < nBlock; ++i)
    {
      C.bottomRows(r).noalias() += A.block(s, s, r, bsize) * B.middleRows(s, bsize);
      s += bsize;
      r -= bsize;
    }
    C.bottomRows(r).noalias() += A.bottomRightCorner(r, r) * B.bottomRows(r);
  }
}

// y = D*x
static void BM_Mult_Diagonal_VectorXd(benchmark::State & state)
{
  VectorXd d = VectorXd::Random(state.range(0));
  VectorXd x = VectorXd::Random(state.range(0));
  VectorXd y(state.range(0));

  for(auto _ : state) y.noalias() = d.asDiagonal() * x;
}

// y = d*x
static void BM_Mult_scalar_VectorXd(benchmark::State & state)
{
  double d = VectorXd::Random(1)[0];
  VectorXd x = VectorXd::Random(state.range(0));
  VectorXd y(state.range(0));

  for(auto _ : state) y.noalias() = d * x;
}

// MAT_BENCHMARK(BM_Copy_MatrixXd);
// MAT_BENCHMARK(BM_Copy_MatrixXd_Source_Transpose);
// MAT_BENCHMARK(BM_Copy_MatrixXd_Target_Transpose);
// MAT_BENCHMARK(BM_Add_MatrixXd);
// MAT_BENCHMARK(BM_Add_MatrixXd_Source_Transpose);
// MAT_BENCHMARK(BM_Add_MatrixXd_Target_Transpose);
// MAT_BENCHMARK(BM_Mult_VectorXd);
// MAT_BENCHMARK(BM_Mult_VectorXd_Source_Transpose);
// MAT_BENCHMARK(BM_Mult_VectorXd_Target_Transpose);
// MAT_BENCHMARK(BM_Mult_VectorXd_Triangular);
// MAT_BENCHMARK(BM_Mult_VectorXd_TriangularOptim4);
// MAT_BENCHMARK(BM_Mult_VectorXd_TriangularOptim8);
// MAT_BENCHMARK(BM_Mult_VectorXd_TriangularOptim16);
MAT_BENCHMARK(BM_Mult_MatrixXd);
// MAT_BENCHMARK(BM_Mult_MatrixXd_AT);
// MAT_BENCHMARK(BM_Mult_MatrixXd_BT);
// MAT_BENCHMARK(BM_Mult_MatrixXd_AT_BT);
// MAT_BENCHMARK(BM_Mult_MatrixXd_Target_Transpose);
// MAT_BENCHMARK(BM_Mult_MatrixXd_Triangular);
// MAT_BENCHMARK(BM_Mult_MatrixXd_TriangularOptim4);
// MAT_BENCHMARK(BM_Mult_MatrixXd_TriangularOptim8);
// MAT_BENCHMARK(BM_Mult_MatrixXd_TriangularOptim16);
MAT_BENCHMARK(BM_Mult_Diagonal_VectorXd);
MAT_BENCHMARK(BM_Mult_scalar_VectorXd);

BENCHMARK_MAIN();


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

/*
 Copyright (C) 2015 Jose Aparicio

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

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

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

#ifndef quantlib_cva_swap_engine_hpp
#define quantlib_cva_swap_engine_hpp

#include <ql/handle.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/termstructures/defaulttermstructure.hpp>

namespace QuantLib {

    class YieldTermStructure;
    class Quote;

  /*! Bilateral (CVA and DVA) default adjusted vanilla swap pricing
    engine. Collateral is not considered. No wrong way risk is
    considered (rates and counterparty default are uncorrelated).
    Based on:
    Sorensen,  E.H.  and  Bollier,  T.F.,  Pricing  swap  default
    risk. Financial Analysts Journal, 1994, 50, 23–33
    Also see sect. II-5 in: Risk Neutral Pricing of Counterparty Risk
    D. Brigo, M. Masetti, 2004
    or in sections 3 and 4 of "A Formula for Interest Rate Swaps
      Valuation under Counterparty Risk in presence of Netting Agreements"
    D. Brigo and M. Masetti; May 4, 2005

    to do: Compute fair rate through iteration instead of the
    current approximation .
    to do: write Issuer based constructors (event type)
    to do: Check consistency between option engine discount and the one given
   */
  class CounterpartyAdjSwapEngine : public VanillaSwap::engine {
    public:
      //! \name Constructors
      //@{
      //!
      /*! Creates the engine from an arbitrary swaption engine.
        If the investor default model is not given a default
        free one is assumed.
        @param discountCurve Used in pricing.
        @param swaptionEngine Determines the volatility and thus the
        exposure model.
        @param ctptyDTS Counterparty default curve.
        @param ctptyRecoveryRate Counterparty recovey rate.
        @param invstDTS Investor (swap holder) default curve.
        @param invstRecoveryRate Investor recovery rate.
       */
      CounterpartyAdjSwapEngine(
          const Handle<YieldTermStructure>& discountCurve,
          const Handle<PricingEngine>& swaptionEngine,
          const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
          Real ctptyRecoveryRate,
          const Handle<DefaultProbabilityTermStructure>& invstDTS =
              Handle<DefaultProbabilityTermStructure>(),
          Real invstRecoveryRate = 0.999);
      /*! Creates an engine with a black volatility model for the
        exposure.
        If the investor default model is not given a default
        free one is assumed.
        @param discountCurve Used in pricing.
        @param blackVol Black volatility used in the exposure model.
        @param ctptyDTS Counterparty default curve.
        @param ctptyRecoveryRate Counterparty recovey rate.
        @param invstDTS Investor (swap holder) default curve.
        @param invstRecoveryRate Investor recovery rate.
       */
      CounterpartyAdjSwapEngine(
          const Handle<YieldTermStructure>& discountCurve,
          const Volatility blackVol,
          const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
          Real ctptyRecoveryRate,
          const Handle<DefaultProbabilityTermStructure>& invstDTS =
              Handle<DefaultProbabilityTermStructure>(),
          Real invstRecoveryRate = 0.999);
      /*! Creates an engine with a black volatility model for the
        exposure. The volatility is given as a quote.
        If the investor default model is not given a default
        free one is assumed.
        @param discountCurve Used in pricing.
        @param blackVol Black volatility used in the exposure model.
        @param ctptyDTS Counterparty default curve.
        @param ctptyRecoveryRate Counterparty recovey rate.
        @param invstDTS Investor (swap holder) default curve.
        @param invstRecoveryRate Investor recovery rate.
      */
      CounterpartyAdjSwapEngine(
          const Handle<YieldTermStructure>& discountCurve,
          const Handle<Quote>& blackVol,
          const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
          Real ctptyRecoveryRate,
          const Handle<DefaultProbabilityTermStructure>& invstDTS =
              Handle<DefaultProbabilityTermStructure>(),
          Real invstRecoveryRate = 0.999);
      //@}
      void calculate() const;
    private:
      Handle<PricingEngine> baseSwapEngine_;
      Handle<PricingEngine> swaptionletEngine_;
      Handle<YieldTermStructure> discountCurve_;
      Handle<DefaultProbabilityTermStructure> defaultTS_;
      Real ctptyRecoveryRate_;
      Handle<DefaultProbabilityTermStructure> invstDTS_;
      Real invstRecoveryRate_;
  };

}

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

/*
 Copyright (C) 2015 Jose Aparicio

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

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

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

#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/floatingratecoupon.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/instruments/makevanillaswap.hpp>
#include <ql/exercise.hpp>
#include <ql/pricingengines/swap/discountingswapengine.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/pricingengines/swaption/blackswaptionengine.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

  inline CounterpartyAdjSwapEngine::CounterpartyAdjSwapEngine(
      const Handle<YieldTermStructure>& discountCurve,
      const Handle<PricingEngine>& swaptionEngine,
      const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
      Real ctptyRecoveryRate,
      const Handle<DefaultProbabilityTermStructure>& invstDTS,
      Real invstRecoveryRate)
  : baseSwapEngine_(Handle<PricingEngine>(
      boost::make_shared<DiscountingSwapEngine>(discountCurve))),
    swaptionletEngine_(swaptionEngine),
    discountCurve_(discountCurve),
    defaultTS_(ctptyDTS),
    ctptyRecoveryRate_(ctptyRecoveryRate),
    invstDTS_(invstDTS.empty() ? Handle<DefaultProbabilityTermStructure>(
        boost::make_shared<FlatHazardRate>(0, ctptyDTS->calendar(), 1.e-12,
        ctptyDTS->dayCounter()) ) : invstDTS ),
    invstRecoveryRate_(invstRecoveryRate)
  {
      registerWith(discountCurve);
      registerWith(ctptyDTS);
      registerWith(invstDTS_);
      registerWith(swaptionEngine);
  }

    inline CounterpartyAdjSwapEngine::CounterpartyAdjSwapEngine(
        const Handle<YieldTermStructure>& discountCurve,
        const Volatility blackVol,
        const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
        Real ctptyRecoveryRate,
        const Handle<DefaultProbabilityTermStructure>& invstDTS,
        Real invstRecoveryRate)
  : baseSwapEngine_(Handle<PricingEngine>(
      boost::make_shared<DiscountingSwapEngine>(discountCurve))),
    swaptionletEngine_(Handle<PricingEngine>(
      boost::make_shared<BlackSwaptionEngine>(discountCurve,
        blackVol))),
    discountCurve_(discountCurve),
    defaultTS_(ctptyDTS),
    ctptyRecoveryRate_(ctptyRecoveryRate),
    invstDTS_(invstDTS.empty() ? Handle<DefaultProbabilityTermStructure>(
        boost::make_shared<FlatHazardRate>(0, ctptyDTS->calendar(), 1.e-12,
        ctptyDTS->dayCounter()) ) : invstDTS ),
    invstRecoveryRate_(invstRecoveryRate)
  {
      registerWith(discountCurve);
      registerWith(ctptyDTS);
      registerWith(invstDTS_);
  }

  inline CounterpartyAdjSwapEngine::CounterpartyAdjSwapEngine(
        const Handle<YieldTermStructure>& discountCurve,
        const Handle<Quote>& blackVol,
        const Handle<DefaultProbabilityTermStructure>& ctptyDTS,
        Real ctptyRecoveryRate,
        const Handle<DefaultProbabilityTermStructure>& invstDTS,
        Real invstRecoveryRate)
  : baseSwapEngine_(Handle<PricingEngine>(
      boost::make_shared<DiscountingSwapEngine>(discountCurve))),
    swaptionletEngine_(Handle<PricingEngine>(
      boost::make_shared<BlackSwaptionEngine>(discountCurve,
        blackVol))),
    discountCurve_(discountCurve),
    defaultTS_(ctptyDTS),
    ctptyRecoveryRate_(ctptyRecoveryRate),
    invstDTS_(invstDTS.empty() ? Handle<DefaultProbabilityTermStructure>(
        boost::make_shared<FlatHazardRate>(0, ctptyDTS->calendar(), 1.e-12,
        ctptyDTS->dayCounter()) ) : invstDTS ),
    invstRecoveryRate_(invstRecoveryRate)
  {
      registerWith(discountCurve);
      registerWith(ctptyDTS);
      registerWith(invstDTS_);
      registerWith(blackVol);
  }

  inline void CounterpartyAdjSwapEngine::calculate() const {
      /* both DTS, YTS ref dates and pricing date consistency
         checks? settlement... */
    QL_REQUIRE(!discountCurve_.empty(),
                 "no discount term structure set");
    QL_REQUIRE(!defaultTS_.empty(),
                 "no ctpty default term structure set");
    QL_REQUIRE(!swaptionletEngine_.empty(),
                 "no swap option engine set");

    Date priceDate = defaultTS_->referenceDate();

    Real cumOptVal = 0.,
        cumPutVal = 0.;
    // Vanilla swap so 0 leg is floater

    std::vector<Date>::const_iterator nextFD =
      arguments_.fixedPayDates.begin();
    Date swapletStart = priceDate;
    while (*nextFD < priceDate) ++nextFD;

    // Compute fair spread for strike value:
    // copy args into the non risky engine
    Swap::arguments * noCVAArgs = dynamic_cast<Swap::arguments*>(
      baseSwapEngine_->getArguments());
    QL_REQUIRE(noCVAArgs != 0, "wrong argument type");

    noCVAArgs->legs = this->arguments_.legs;
    noCVAArgs->payer = this->arguments_.payer;

    baseSwapEngine_->calculate();

    boost::shared_ptr<FixedRateCoupon> coupon = boost::dynamic_pointer_cast<FixedRateCoupon>(arguments_.legs[0][0]);
    QL_REQUIRE(coupon,"dynamic cast of fixed leg coupon failed.");
    Rate baseSwapRate = coupon->rate();

    const Swap::results * vSResults =
        dynamic_cast<const Swap::results *>(baseSwapEngine_->getResults());
    QL_REQUIRE(vSResults != 0, "wrong result type");

    Rate baseSwapFairRate = -baseSwapRate * vSResults->legNPV[1] /
        vSResults->legNPV[0];
    Real baseSwapNPV = vSResults->value;

    VanillaSwap::Type reversedType = arguments_.type == VanillaSwap::Payer ?
        VanillaSwap::Receiver : VanillaSwap::Payer;

    // Swaplet options summatory:
    while(nextFD != arguments_.fixedPayDates.end()) {
      // iFD coupon not fixed, create swaptionlet:
      boost::shared_ptr<FloatingRateCoupon> floatCoupon = boost::dynamic_pointer_cast<FloatingRateCoupon>(arguments_.legs[1][0]);
      QL_REQUIRE(floatCoupon,"dynamic cast of floating leg coupon failed.");
      boost::shared_ptr<IborIndex> swapIndex = boost::dynamic_pointer_cast<IborIndex>(floatCoupon->index());
      QL_REQUIRE(swapIndex,"dynamic cast of floating leg index failed.");

      // Alternatively one could cap this period to, say, 1M
      // Period swapPeriod = boost::dynamic_pointer_cast<FloatingRateCoupon>(
      //   arguments_.legs[1][0])->index()->tenor();

      Period baseSwapsTenor(arguments_.fixedPayDates.back().serialNumber()
	    - swapletStart.serialNumber(), Days);
      boost::shared_ptr<VanillaSwap> swaplet = MakeVanillaSwap(
        baseSwapsTenor,
        swapIndex,
        baseSwapFairRate // strike
        )
	    .withType(arguments_.type)
	    .withNominal(arguments_.nominal)
          ////////	    .withSettlementDays(2)
        .withEffectiveDate(swapletStart)
        .withTerminationDate(arguments_.fixedPayDates.back());
      boost::shared_ptr<VanillaSwap> revSwaplet = MakeVanillaSwap(
        baseSwapsTenor,
        swapIndex,
        baseSwapFairRate // strike
        )
	    .withType(reversedType)
	    .withNominal(arguments_.nominal)
          /////////	    .withSettlementDays(2)
        .withEffectiveDate(swapletStart)
        .withTerminationDate(arguments_.fixedPayDates.back());

      Swaption swaptionlet(swaplet,
        boost::make_shared<EuropeanExercise>(swapletStart));
      Swaption putSwaplet(revSwaplet,
        boost::make_shared<EuropeanExercise>(swapletStart));
      swaptionlet.setPricingEngine(swaptionletEngine_.currentLink());
      putSwaplet.setPricingEngine(swaptionletEngine_.currentLink());

      // atm underlying swap means that the value of put = value
      // call so this double pricing is not needed
      cumOptVal += swaptionlet.NPV() * defaultTS_->defaultProbability(
          swapletStart, *nextFD);
      cumPutVal += putSwaplet.NPV()  * invstDTS_->defaultProbability(
	      swapletStart, *nextFD);

      swapletStart = *nextFD;
      ++nextFD;
    }

    results_.value = baseSwapNPV - (1.-ctptyRecoveryRate_) * cumOptVal
        + (1.-invstRecoveryRate_) * cumPutVal;

    results_.fairRate =  -baseSwapRate * (vSResults->legNPV[1]
        - (1.-ctptyRecoveryRate_) * cumOptVal +
          (1.-invstRecoveryRate_) * cumPutVal )
      / vSResults->legNPV[0];

  }

}

#endif


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifdef Cygwin
// availability of std::to_string
#define _GLIBCXX_USE_C99 1
#endif

#include <iostream>

// #include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"
#include <dune/istl/operators.hh>

#include "utilities/enums.hh"
#include "fem/assemble.hh"
#include "fem/spaces.hh"
#include "io/vtk.hh"
#include "io/matlab.hh"
#include "linalg/direct.hh"
#include "linalg/apcg.hh"
#include "linalg/threadedMatrix.hh"
#include "linalg/triplet.hh"
#include "mg/multigrid.hh"
#include "mg/additiveMultigrid.hh"
#include "utilities/gridGeneration.hh"
#include "utilities/kaskopt.hh"
#include "utilities/memory.hh"

using namespace Kaskade;
#include "elastomechanics.hh"

int main(int argc, char *argv[])
{
  using namespace boost::fusion;

  std::cout << "Start elastomechanics tutorial program" << std::endl;

  int coarseGridSize, maxit, order, refinements, solver, verbose;
  bool additive, direct, vtk;
  double atol;
  std::string material;

  if (getKaskadeOptions(argc,argv,Options
    ("refinements",      refinements,     3,          "number of uniform grid refinements")
    ("coarse",           coarseGridSize,  1,          "number of coarse grid elements along each cube edge")
    ("order",            order,           1,          "finite element ansatz order")
    ("material",         material,        "steel",    "type of material")
    ("direct",           direct,          true,       "if true, use a direct solver")
    ("solver",           solver,          2,          "0=UMFPACK, 1=PARDISO 2=MUMPS 3=SUPERLU 4=UMFPACK32/64 5=UMFPACK64")
    ("additive",         additive,        false,      "use additive multigrid")
    ("verbosity",        verbose,         0,          "amount of reported details")
    ("vtk",              vtk,             false,      "write solution to VTK file")
    ("atol",             atol,            1e-8,       "absolute energy error tolerance for iterative solver")
    ("maxit",            maxit,           100,        "maximum number of iterations")))
    return 1;

  boost::timer::cpu_timer totalTimer;

  std::cout << "refinements of original mesh : " << refinements << std::endl;
  std::cout << "discretization order         : " << order << std::endl;

  constexpr int DIM = 3;
  using Grid = Dune::UGGrid<DIM>;
  using Spaces = boost::fusion::vector<H1Space<Grid> const*>;
  using VariableDescriptions = boost::fusion::vector<VariableDescription<0,DIM,0> >;
  using VarSetDesc = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = ElasticityFunctional<VarSetDesc>;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  using CoefficientVectors = VarSetDesc::CoefficientVectorRepresentation<0,1>::type;

  Dune::FieldVector<double,DIM> c0(0.0), dc(1.0);
  GridManager<Grid> gridManager( createCuboid<Grid>(c0,dc,1.0/coarseGridSize,true) );
  gridManager.globalRefine(refinements);
  gridManager.enforceConcurrentReads(true);
  std::cout << "grid creation & refinement time: " << totalTimer.format();

  // construction of finite element space for the scalar solution T.
  H1Space<Grid> h1Space(gridManager,gridManager.grid().leafGridView(),order);

  Spaces spaces(&h1Space);

  std::string varNames[1] = { "u" };

  VarSetDesc varSetDesc(spaces,varNames);

  // Create the variational functional.
  Functional F(ElasticModulus::material(material));

  // construct Galerkin representation
  Assembler assembler(gridManager,spaces);
  VarSetDesc::VariableSet x(varSetDesc);
  VarSetDesc::VariableSet dx(varSetDesc);

  boost::timer::cpu_timer assembTimer;
  assembler.assemble(linearization(F,x));
  std::cout << "computing time for assemble: " << boost::timer::format(assembTimer.elapsed()) << "\n";

  CoefficientVectors rhs(assembler.rhs());
  CoefficientVectors solution(VarSetDesc::CoefficientVectorRepresentation<0,1>::init(spaces));
  boost::timer::cpu_timer solveTimer;
  if (direct)
  {
    DirectType directType = static_cast<DirectType>(solver);
    AssembledGalerkinOperator<Assembler> A(assembler, directType == DirectType::MUMPS || directType == DirectType::PARDISO);
    directInverseOperator(A,directType,MatrixProperties::POSITIVEDEFINITE).applyscaleadd(-1.0,rhs,solution);
    x.data = solution.data;
  }
  else
  {
    using X = Dune::BlockVector<Dune::FieldVector<double,DIM>>;
    DefaultDualPairing<X,X> dp;
    using Matrix = NumaBCRSMatrix<Dune::FieldMatrix<double,DIM,DIM>>;
    using LinOp = Dune::MatrixAdapter<Matrix,X,X>;
    Matrix Amat(assembler.get<0,0>(),true);
    LinOp A(Amat);
    SymmetricLinearOperatorWrapper<X,X> sa(A,dp);
    PCGEnergyErrorTerminationCriterion<double> term(atol,maxit);

    Dune::InverseOperatorResult res;
    X xi(component<0>(rhs).N());

    std::unique_ptr<SymmetricPreconditioner<X,X>> mg;
    if (additive)
    {
      if (order==1)
        mg = moveUnique(makeBPX(Amat,gridManager));
      else
      {
        H1Space<Grid> p1Space(gridManager,gridManager.grid().leafGridView(),1);
        mg = moveUnique(makePBPX(Amat,h1Space,p1Space));
      }
    }
    else
      mg = makeMultigrid(Amat,h1Space);

    Pcg<X,X> pcg(sa,*mg,term,verbose);
    pcg.apply(xi,component<0>(rhs),res);
    xi *= -1;
    component<0>(x) = xi;
  }
  std::cout << "computing time for solve: " << solveTimer.format();

  // output of solution in VTK format for visualization,
  // the data are written as ascii stream into file elasto.vtu,
  // possible is also binary
  if (vtk)
    writeVTKFile(x,"elasto",IoOptions().setOrder(order).setPrecision(7));

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End elastomechanics tutorial program" << std::endl;

  return 0;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the graph_labeler and
    structural_graph_labeling_trainer objects.

    Suppose you have a bunch of objects and you need to label each of them as true or
    false.  Suppose further that knowing the labels of some of these objects tells you
    something about the likely label of the others.  This is common in a number of domains.
    For example, in image segmentation problems you need to label each pixel, and knowing
    the labels of neighboring pixels gives you information about the likely label since
    neighboring pixels will often have the same label.

    We can generalize this problem by saying that we have a graph and our task is to label
    each node in the graph as true or false.  Additionally, the edges in the graph connect
    nodes which are likely to share the same label.  In this example program, each node
    will have a feature vector which contains information which helps tell if the node
    should be labeled as true or false.  The edges also contain feature vectors which give
    information indicating how strong the edge's labeling consistency constraint should be.
    This is useful since some nodes will have uninformative feature vectors and the only
    way to tell how they should be labeled is by looking at their neighbor's labels.

    Therefore, this program will show you how to learn two things using machine learning.
    The first is a linear classifier which operates on each node and predicts if it should
    be labeled as true or false.  The second thing is a linear function of the edge
    vectors.  This function outputs a penalty for giving two nodes connected by an edge
    differing labels.  The graph_labeler object puts these two things together and uses
    them to compute a labeling which takes both into account.  In what follows, we will use
    a structural SVM method to find the parameters of these linear functions which minimize
    the number of mistakes made by a graph_labeler.

    Finally, you might also consider reading the book Structured Prediction and Learning in
    Computer Vision by Sebastian Nowozin and Christoph H. Lampert since it contains a good
    introduction to machine learning methods such as the algorithm implemented by the
    structural_graph_labeling_trainer.
*/

#include <dlib/svm_threaded.h>
#include <iostream>

using namespace std;
using namespace dlib;

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

// The first thing we do is define the kind of graph object we will be using.
// Here we are saying there will be 2-D vectors at each node and 1-D vectors at
// each edge.  (You should read the matrix_ex.cpp example program for an introduction
// to the matrix object.)
typedef matrix<double,2,1> node_vector_type;
typedef matrix<double,1,1> edge_vector_type;
typedef graph<node_vector_type, edge_vector_type>::kernel_1a_c graph_type;

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

template <
    typename graph_type,
    typename labels_type
    >
void make_training_examples(
    dlib::array<graph_type>& samples,
    labels_type& labels
)
{
    /*
        This function makes 3 graphs we will use for training.   All of them
        will contain 4 nodes and have the structure shown below:

          (0)-----(1)
           |       |
           |       |
           |       |
          (3)-----(2)

        In this example, each node has a 2-D vector.  The first element of this vector
        is 1 when the node should have a label of false while the second element has
        a value of 1 when the node should have a label of true.  Additionally, the
        edge vectors will contain a value of 1 when the nodes connected by the edge
        should share the same label and a value of 0 otherwise.

        We want to see that the machine learning method is able to figure out how
        these features relate to the labels.  If it is successful it will create a
        graph_labeler which can predict the correct labels for these and other
        similarly constructed graphs.

        Finally, note that these tools require all values in the edge vectors to be >= 0.
        However, the node vectors may contain both positive and negative values.
    */

    samples.clear();
    labels.clear();

    std::vector<bool> label;
    graph_type g;

    // ---------------------------
    g.set_number_of_nodes(4);
    label.resize(g.number_of_nodes());
    // store the vector [0,1] into node 0.  Also label it as true.
    g.node(0).data = 0, 1; label[0] = true;
    // store the vector [0,0] into node 1.
    g.node(1).data = 0, 0; label[1] = true;  // Note that this node's vector doesn't tell us how to label it.
                                             // We need to take the edges into account to get it right.
    // store the vector [1,0] into node 2.
    g.node(2).data = 1, 0; label[2] = false;
    // store the vector [0,0] into node 3.
    g.node(3).data = 0, 0; label[3] = false;

    // Add the 4 edges as shown in the ASCII art above.
    g.add_edge(0,1);
    g.add_edge(1,2);
    g.add_edge(2,3);
    g.add_edge(3,0);

    // set the 1-D vector for the edge between node 0 and 1 to the value of 1.
    edge(g,0,1) = 1;
    // set the 1-D vector for the edge between node 1 and 2 to the value of 0.
    edge(g,1,2) = 0;
    edge(g,2,3) = 1;
    edge(g,3,0) = 0;
    // output the graph and its label.
    samples.push_back(g);
    labels.push_back(label);

    // ---------------------------
    g.set_number_of_nodes(4);
    label.resize(g.number_of_nodes());
    g.node(0).data = 0, 1; label[0] = true;
    g.node(1).data = 0, 1; label[1] = true;
    g.node(2).data = 1, 0; label[2] = false;
    g.node(3).data = 1, 0; label[3] = false;

    g.add_edge(0,1);
    g.add_edge(1,2);
    g.add_edge(2,3);
    g.add_edge(3,0);

    // This time, we have strong edges between all the nodes.  The machine learning
    // tools will have to learn that when the node information conflicts with the
    // edge constraints that the node information should dominate.
    edge(g,0,1) = 1;
    edge(g,1,2) = 1;
    edge(g,2,3) = 1;
    edge(g,3,0) = 1;
    samples.push_back(g);
    labels.push_back(label);
    // ---------------------------

    g.set_number_of_nodes(4);
    label.resize(g.number_of_nodes());
    g.node(0).data = 1, 0; label[0] = false;
    g.node(1).data = 1, 0; label[1] = false;
    g.node(2).data = 1, 0; label[2] = false;
    g.node(3).data = 0, 0; label[3] = false;

    g.add_edge(0,1);
    g.add_edge(1,2);
    g.add_edge(2,3);
    g.add_edge(3,0);

    edge(g,0,1) = 0;
    edge(g,1,2) = 0;
    edge(g,2,3) = 1;
    edge(g,3,0) = 0;
    samples.push_back(g);
    labels.push_back(label);
    // ---------------------------

}

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

int main()
{
    try
    {
        // Get the training samples we defined above.
        dlib::array<graph_type> samples;
        std::vector<std::vector<bool> > labels;
        make_training_examples(samples, labels);

        // Create a structural SVM trainer for graph labeling problems.  The vector_type
        // needs to be set to a type capable of holding node or edge vectors.
        typedef matrix<double,0,1> vector_type;
        structural_graph_labeling_trainer<vector_type> trainer;
        // This is the usual SVM C parameter.  Larger values make the trainer try
        // harder to fit the training data but might result in overfitting.  You
        // should set this value to whatever gives the best cross-validation results.
        trainer.set_c(10);

        // Do 3-fold cross-validation and print the results.  In this case it will
        // indicate that all nodes were correctly classified.
        cout << "3-fold cross-validation: " << cross_validate_graph_labeling_trainer(trainer, samples, labels, 3) << endl;

        // Since the trainer is working well.  Let's have it make a graph_labeler
        // based on the training data.
        graph_labeler<vector_type> labeler = trainer.train(samples, labels);

        /*
            Let's try the graph_labeler on a new test graph.  In particular, let's
            use one with 5 nodes as shown below:

            (0 F)-----(1 T)
              |         |
              |         |
              |         |
            (3 T)-----(2 T)------(4 T)

            I have annotated each node with either T or F to indicate the correct
            output (true or false).
        */
        graph_type g;
        g.set_number_of_nodes(5);
        g.node(0).data = 1, 0;  // Node data indicates a false node.
        g.node(1).data = 0, 1;  // Node data indicates a true node.
        g.node(2).data = 0, 0;  // Node data is ambiguous.
        g.node(3).data = 0, 0;  // Node data is ambiguous.
        g.node(4).data = 0.1, 0; // Node data slightly indicates a false node.

        g.add_edge(0,1);
        g.add_edge(1,2);
        g.add_edge(2,3);
        g.add_edge(3,0);
        g.add_edge(2,4);

        // Set the edges up so nodes 1, 2, 3, and 4 are all strongly connected.
        edge(g,0,1) = 0;
        edge(g,1,2) = 1;
        edge(g,2,3) = 1;
        edge(g,3,0) = 0;
        edge(g,2,4) = 1;

        // The output of this shows all the nodes are correctly labeled.
        cout << "Predicted labels: " << endl;
        std::vector<bool> temp = labeler(g);
        for (unsigned long i = 0; i < temp.size(); ++i)
            cout << " " << i << ": " << temp[i] << endl;

        // Breaking the strong labeling consistency link between node 1 and 2 causes
        // nodes 2, 3, and 4 to flip to false.  This is because of their connection
        // to node 4 which has a small preference for false.
        edge(g,1,2) = 0;
        cout << "Predicted labels: " << endl;
        temp = labeler(g);
        for (unsigned long i = 0; i < temp.size(); ++i)
            cout << " " << i << ": " << temp[i] << endl;
    }
    catch (std::exception& e)
    {
        cout << "Error, an exception was thrown!" << endl;
        cout << e.what() << endl;
    }
}


#include "UCCSD.hpp"
#include "GateFunction.hpp"
#include "FermionToSpinTransformation.hpp"
#include "CommutingSetGenerator.hpp"
#include <boost/math/constants/constants.hpp>

using namespace xacc::quantum;

namespace xacc {
namespace vqe {

std::shared_ptr<Function> UCCSD::generate(
		std::shared_ptr<AcceleratorBuffer> buffer,
		std::vector<InstructionParameter> parameters) {

	auto runtimeOptions = RuntimeOptions::instance();

	if (!runtimeOptions->exists("n-electrons")) {
		xacc::error("To use this UCCSD State Prep IRGenerator, you "
				"must specify the number of electrons.");
	}

	if (!runtimeOptions->exists("n-qubits")) {
		xacc::error("To use this UCCSD State Prep IRGenerator, you "
				"must specify the number of qubits.");
	}

	auto nQubits = std::stoi((*runtimeOptions)["n-qubits"]);
	auto nElectrons = std::stoi((*runtimeOptions)["n-electrons"]);

	// Compute the number of parameters
	auto _nOccupied = (int) std::ceil(nElectrons / 2.0);
	auto _nVirtual = nQubits / 2 - _nOccupied;
	auto nSingle = _nOccupied * _nVirtual;
	auto nDouble = std::pow(nSingle, 2);
	auto _nParameters = nSingle + nDouble;

	auto singletIndex = [=](int i, int j) -> int {
		return i * _nOccupied + j;
	};

	auto doubletIndex = [=](int i, int j, int k, int l) -> int {
		return
		(i * _nOccupied * _nVirtual * _nOccupied +
				j * _nVirtual * _nOccupied +
				k * _nOccupied +
				l);
	};

	std::vector<xacc::InstructionParameter> variables;
	std::vector<std::string> params;
	for (int i = 0; i < _nParameters; i++) {
		params.push_back("theta" + std::to_string(i));
		variables.push_back(InstructionParameter("theta" + std::to_string(i)));
	}

	auto kernel = std::make_shared<FermionKernel>("fermiUCCSD");
	xacc::info("Constructing UCCSD Fermion Operator.");
	for (int i = 0; i < _nVirtual; i++) {
		for (int j = 0; j < _nOccupied; j++) {
			for (int l = 0; l < 2; l++) {
				std::vector<std::pair<int, int>> operators { { 2
						* (i + _nOccupied) + l, 1 }, { 2 * j + l, 0 } };
				auto fermiInstruction1 = std::make_shared<FermionInstruction>(
						operators, params[singletIndex(i, j)]);
				kernel->addInstruction(fermiInstruction1);

				std::vector<std::pair<int, int>> operators2 { { 2 * j + l, 1 },
						{ 2 * (i + _nOccupied) + l, 0 } };
				auto fermiInstruction2 = std::make_shared<FermionInstruction>(
						operators2, params[singletIndex(i, j)]);

				auto nP = fermiInstruction2->nParameters();
				InstructionParameter p(-1.0*std::complex<double>(1,0));
				fermiInstruction2->setParameter(nP-2,p);

				kernel->addInstruction(fermiInstruction2);

			}
		}
	}

	for (int i = 0; i < _nVirtual; i++) {
		for (int j = 0; j < _nOccupied; j++) {
			for (int l = 0; l < 2; l++) {
				for (int i2 = 0; i2 < _nVirtual; i2++) {
					for (int j2 = 0; j2 < _nOccupied; j2++) {
						for (int l2 = 0; l2 < 2; l2++) {
							std::vector<std::pair<int, int>> operators1 { { 2
									* (i + _nOccupied) + l, 1 },
									{ 2 * j + l, 0 }, { 2 * (i2 + _nOccupied)
											+ l2, 1 }, { 2 * j2 + l2, 0 } };

							std::vector<std::pair<int, int>> operators2 { { 2
									* j2 + l2, 1 }, { 2 * (i2 + _nOccupied)
									+ l2, 0 }, { 2 * j + l, 1 }, { 2
									* (i + _nOccupied) + l, 0 } };

							auto doubletIdx1 = nSingle
									+ doubletIndex(i, j, i2, j2);
							auto doubletIdx2 = nSingle
									+ doubletIndex(i, j, i2, j2);

							auto fermiInstruction1 = std::make_shared<
									FermionInstruction>(operators1,
									params[doubletIdx1]);

							kernel->addInstruction(fermiInstruction1);

							auto fermiInstruction2 = std::make_shared<
									FermionInstruction>(operators2,
									params[doubletIdx2]);

							auto nP = fermiInstruction2->nParameters();
							InstructionParameter p(-1.0*std::complex<double>(1,0));
							fermiInstruction2->setParameter(nP-2,p);
							kernel->addInstruction(fermiInstruction2);

						}
					}
				}
			}
		}
	}

//	std::cout << "KERNEL: \n" << kernel->toString("") << "\n";
	// Create the FermionIR to pass to our transformation.
	auto fermionir = std::make_shared<FermionIR>();
	fermionir->addKernel(kernel);

	xacc::info("Done constructing UCCSD Fermion Operator.");
	xacc::info("Mapping UCCSD Fermion Operator to Spin. ");

	std::shared_ptr<FermionToSpinTransformation> transform;
	if (xacc::optionExists("fermion-transformation")) {
		auto transformStr = xacc::getOption("fermion-transformation");
		transform = xacc::getService<FermionToSpinTransformation>(
				transformStr);
	} else {
		transform = xacc::getService<FermionToSpinTransformation>(
				"jw");
	}

	auto compositeResult = transform->transform(*kernel.get());
//	auto resultsStr = compositeResult.toString();
//	boost::replace_all(resultsStr, "+", "+\n");

	// Create the Spin Hamiltonian
	auto transformedIR = compositeResult.toXACCIR();
	xacc::info("Done mapping UCCSD Fermion Operator to Spin.");

	std::unordered_map<std::string, Term> terms = compositeResult.getTerms();

	CommutingSetGenerator gen;
	auto commutingSets = gen.getCommutingSet(compositeResult, nQubits);
	auto pi = boost::math::constants::pi<double>();
	auto gateRegistry = xacc::getService<IRProvider>("gate");

	auto uccsdGateFunction = gateRegistry->createFunction("uccsdPrep",{},
			variables);

	// Perform Trotterization...
	for (auto s : commutingSets) {

		for (auto inst : s) {
			Term spinInst = inst;

			// Get the individual pauli terms
			auto termsMap = std::get<2>(spinInst);

			std::vector<std::pair<int,std::string>> terms;
			for (auto& kv : termsMap) {
				if (kv.second != "I" && !kv.second.empty()) {
					terms.push_back({kv.first, kv.second});
				}
			}
			// The largest qubit index is on the last term
			int largestQbitIdx = terms[terms.size() - 1].first;
			auto tempFunction = gateRegistry->createFunction("temp", {}, {});

			for (int i = 0; i < terms.size(); i++) {

				auto qbitIdx = terms[i].first;
				auto gateName = terms[i].second;

				if (i < terms.size() - 1) {
					auto cnot =
							gateRegistry->createInstruction(
									"CNOT",
									std::vector<int> { qbitIdx,
											terms[i + 1].first });
					tempFunction->addInstruction(cnot);
				}

				if (gateName == "X") {
					auto hadamard =
							gateRegistry->createInstruction(
									"H", std::vector<int> { qbitIdx });
					tempFunction->insertInstruction(0, hadamard);
				} else if (gateName == "Y") {
					auto rx =
							gateRegistry->createInstruction(
									"Rx", std::vector<int> { qbitIdx });
					InstructionParameter p(pi / 2.0);
					rx->setParameter(0, p);
					tempFunction->insertInstruction(0, rx);
				}

				// Add the Rotation for the last term
				if (i == terms.size() - 1) {
					// FIXME DONT FORGET DIVIDE BY 2
					std::stringstream ss;
					ss << 2*std::imag(std::get<0>(spinInst)) << " * "
							<< std::get<1>(spinInst);
					auto rz =
							gateRegistry->createInstruction(
									"Rz", std::vector<int> { qbitIdx });

					InstructionParameter p(ss.str());
					rz->setParameter(0, p);
					tempFunction->addInstruction(rz);

				}
			}

			int counter = tempFunction->nInstructions();
			// Add the instruction on the backend of the circuit
			for (int i = terms.size() - 1; i >= 0; i--) {

				auto qbitIdx = terms[i].first;
				auto gateName = terms[i].second;

				if (i < terms.size() - 1) {
					auto cnot =
							gateRegistry->createInstruction(
									"CNOT",
									std::vector<int> { qbitIdx,
											terms[i + 1].first });
					tempFunction->insertInstruction(counter, cnot);
					counter++;
				}

				if (gateName == "X") {
					auto hadamard =
							gateRegistry->createInstruction(
									"H", std::vector<int> { qbitIdx });
					tempFunction->addInstruction(hadamard);
				} else if (gateName == "Y") {
					auto rx =
							gateRegistry->createInstruction(
									"Rx", std::vector<int> { qbitIdx });
					InstructionParameter p(4 * pi - (pi /2.0));
					rx->setParameter(0, p);
					tempFunction->addInstruction(rx);
				}

			}
			// Add to the total UCCSD State Prep function
			for (auto inst : tempFunction->getInstructions()) {
				uccsdGateFunction->addInstruction(inst);
			}
		}
	}

	for (int i = nElectrons-1; i >= 0; i--) {
		auto xGate = gateRegistry->createInstruction(
				"X", std::vector<int>{i});
		uccsdGateFunction->insertInstruction(0,xGate);
	}

	return uccsdGateFunction;
}

}
}


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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
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 or http://www.mitk.org for details.

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

#include <mitkImageCast.h>
#include <itkExceptionObject.h>
#include <itkImageFileWriter.h>
#include <mitkBaseDataIOFactory.h>
#include <mitkQBallImage.h>
#include <itkTensorDerivedMeasurementsFilter.h>
#include <itkDiffusionQballGeneralizedFaImageFilter.h>
#include <mitkTensorImage.h>
#include "mitkCommandLineParser.h"
#include <boost/algorithm/string.hpp>
#include <itksys/SystemTools.hxx>
#include <itkMultiThreader.h>

/**
 * Calculate indices derived from Qball or tensor images
 */
int main(int argc, char* argv[])
{
    mitkCommandLineParser parser;

    parser.setTitle("Diffusion Indices");
    parser.setCategory("Diffusion Related Measures");
    parser.setDescription("");
    parser.setContributor("MBI");

    parser.setArgumentPrefix("--", "-");
    parser.addArgument("input", "i", mitkCommandLineParser::InputFile, "Input:", "input image (tensor, Q-ball or FSL/MRTrix SH-coefficient image)", us::Any(), false);
    parser.addArgument("index", "idx", mitkCommandLineParser::String, "Index:", "index (fa, gfa, ra, ad, rd, ca, l2, l3, md)", us::Any(), false);
    parser.addArgument("outFile", "o", mitkCommandLineParser::OutputFile, "Output:", "output file", us::Any(), false);

    map<string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
    if (parsedArgs.size()==0)
        return EXIT_FAILURE;

    string inFileName = us::any_cast<string>(parsedArgs["input"]);
    string index = us::any_cast<string>(parsedArgs["index"]);
    string outFileName = us::any_cast<string>(parsedArgs["outFile"]);

    string ext = itksys::SystemTools::GetFilenameLastExtension(outFileName);
    if (ext.empty())
        outFileName += ".nrrd";

    try
    {
        // load input image
        const std::string s1="", s2="";
        std::vector<mitk::BaseData::Pointer> infile = mitk::BaseDataIO::LoadBaseDataFromFile( inFileName, s1, s2, false );

        if( boost::algorithm::ends_with(inFileName, ".qbi") && index=="gfa" )
        {
            typedef itk::Vector<float, QBALL_ODFSIZE>   OdfVectorType;
            typedef itk::Image<OdfVectorType,3>         ItkQballImageType;
            mitk::QBallImage::Pointer mitkQballImage = dynamic_cast<mitk::QBallImage*>(infile.at(0).GetPointer());
            ItkQballImageType::Pointer itk_qbi = ItkQballImageType::New();
            mitk::CastToItkImage(mitkQballImage, itk_qbi);

            typedef itk::DiffusionQballGeneralizedFaImageFilter<float,float,QBALL_ODFSIZE> GfaFilterType;
            GfaFilterType::Pointer gfaFilter = GfaFilterType::New();
            gfaFilter->SetInput(itk_qbi);
            gfaFilter->SetComputationMethod(GfaFilterType::GFA_STANDARD);
            gfaFilter->Update();

            itk::ImageFileWriter< itk::Image<float,3> >::Pointer fileWriter = itk::ImageFileWriter< itk::Image<float,3> >::New();
            fileWriter->SetInput(gfaFilter->GetOutput());
            fileWriter->SetFileName(outFileName);
            fileWriter->Update();
        }
        else if( boost::algorithm::ends_with(inFileName, ".dti") )
        {
            typedef itk::Image< itk::DiffusionTensor3D<float>, 3 >    ItkTensorImage;
            mitk::TensorImage::Pointer mitkTensorImage = dynamic_cast<mitk::TensorImage*>(infile.at(0).GetPointer());
            ItkTensorImage::Pointer itk_dti = ItkTensorImage::New();
            mitk::CastToItkImage(mitkTensorImage, itk_dti);

            typedef itk::TensorDerivedMeasurementsFilter<float> MeasurementsType;
            MeasurementsType::Pointer measurementsCalculator = MeasurementsType::New();
            measurementsCalculator->SetInput(itk_dti.GetPointer() );

            if(index=="fa")
                measurementsCalculator->SetMeasure(MeasurementsType::FA);
            else if(index=="ra")
                measurementsCalculator->SetMeasure(MeasurementsType::RA);
            else if(index=="ad")
                measurementsCalculator->SetMeasure(MeasurementsType::AD);
            else if(index=="rd")
                measurementsCalculator->SetMeasure(MeasurementsType::RD);
            else if(index=="ca")
                measurementsCalculator->SetMeasure(MeasurementsType::CA);
            else if(index=="l2")
                measurementsCalculator->SetMeasure(MeasurementsType::L2);
            else if(index=="l3")
                measurementsCalculator->SetMeasure(MeasurementsType::L3);
            else if(index=="md")
                measurementsCalculator->SetMeasure(MeasurementsType::MD);
            else
            {
                MITK_WARN << "No valid diffusion index for input image (tensor image) defined";
                return EXIT_FAILURE;
            }

            measurementsCalculator->Update();

            itk::ImageFileWriter< itk::Image<float,3> >::Pointer fileWriter = itk::ImageFileWriter< itk::Image<float,3> >::New();
            fileWriter->SetInput(measurementsCalculator->GetOutput());
            fileWriter->SetFileName(outFileName);
            fileWriter->Update();
        }
        else
            std::cout << "Diffusion index " << index << " not supported for supplied file type.";
    }
    catch (itk::ExceptionObject e)
    {
        std::cout << e;
        return EXIT_FAILURE;
    }
    catch (std::exception e)
    {
        std::cout << e.what();
        return EXIT_FAILURE;
    }
    catch (...)
    {
        std::cout << "ERROR!?!";
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}


/*
    This file is part of Nori, a simple educational ray tracer

    Copyright (c) 2015 by Wenzel Jakob
*/

#include <nori/mesh.h>
#include <nori/bsdf.h>
#include <nori/emitter.h>
#include <nori/warp.h>
#include <Eigen/Geometry>

NORI_NAMESPACE_BEGIN

Mesh::Mesh() { }

Mesh::~Mesh() {
    delete m_bsdf;
    delete m_emitter;
}

void Mesh::activate() {
    if (!m_bsdf) {
        /* If no material was assigned, instantiate a diffuse BRDF */
        m_bsdf = static_cast<BSDF *>(
            NoriObjectFactory::createInstance("diffuse", PropertyList()));
    }

    //generate cdf if emitted
    if(isEmitter()){
        dpdf.reserve(getTriangleCount());
        for(size_t i = 0; i < getTriangleCount(); ++i){
            dpdf.append(surfaceArea(i));
        }
        dpdf.normalize();
    }
}

float Mesh::surfaceArea(uint32_t index) const {
    uint32_t i0 = m_F(0, index), i1 = m_F(1, index), i2 = m_F(2, index);

    const Point3f p0 = m_V.col(i0), p1 = m_V.col(i1), p2 = m_V.col(i2);

    return 0.5f * Vector3f((p1 - p0).cross(p2 - p0)).norm();
}

bool Mesh::rayIntersect(uint32_t index, const Ray3f &ray, float &u, float &v, float &t) const {
    uint32_t i0 = m_F(0, index), i1 = m_F(1, index), i2 = m_F(2, index);
    const Point3f p0 = m_V.col(i0), p1 = m_V.col(i1), p2 = m_V.col(i2);

    /* Find vectors for two edges sharing v[0] */
    Vector3f edge1 = p1 - p0, edge2 = p2 - p0;

    /* Begin calculating determinant - also used to calculate U parameter */
    Vector3f pvec = ray.d.cross(edge2);

    /* If determinant is near zero, ray lies in plane of triangle */
    float det = edge1.dot(pvec);

    if (det > -1e-8f && det < 1e-8f)
        return false;
    float inv_det = 1.0f / det;

    /* Calculate distance from v[0] to ray origin */
    Vector3f tvec = ray.o - p0;

    /* Calculate U parameter and test bounds */
    u = tvec.dot(pvec) * inv_det;
    if (u < 0.0 || u > 1.0)
        return false;

    /* Prepare to test V parameter */
    Vector3f qvec = tvec.cross(edge1);

    /* Calculate V parameter and test bounds */
    v = ray.d.dot(qvec) * inv_det;
    if (v < 0.0 || u + v > 1.0)
        return false;

    /* Ray intersects triangle -> compute t */
    t = edge2.dot(qvec) * inv_det;

    return t >= ray.mint && t <= ray.maxt;
}

BoundingBox3f Mesh::getBoundingBox(uint32_t index) const {
    BoundingBox3f result(m_V.col(m_F(0, index)));
    result.expandBy(m_V.col(m_F(1, index)));
    result.expandBy(m_V.col(m_F(2, index)));
    return result;
}

Point3f Mesh::getCentroid(uint32_t index) const {
    return (1.0f / 3.0f) *
        (m_V.col(m_F(0, index)) +
         m_V.col(m_F(1, index)) +
         m_V.col(m_F(2, index)));
}

void Mesh::addChild(NoriObject *obj) {
    switch (obj->getClassType()) {
        case EBSDF:
            if (m_bsdf)
                throw NoriException(
                    "Mesh: tried to register multiple BSDF instances!");
            m_bsdf = static_cast<BSDF *>(obj);
            break;

        case EEmitter: {
                Emitter *emitter = static_cast<Emitter *>(obj);
                if (m_emitter)
                    throw NoriException(
                        "Mesh: tried to register multiple Emitter instances!");
                m_emitter = emitter;
            }
            break;

        default:
            throw NoriException("Mesh::addChild(<%s>) is not supported!",
                                classTypeName(obj->getClassType()));
    }
}

std::string Mesh::toString() const {
    return tfm::format(
        "Mesh[\n"
        "  name = \"%s\",\n"
        "  vertexCount = %i,\n"
        "  triangleCount = %i,\n"
        "  bsdf = %s,\n"
        "  emitter = %s\n"
        "]",
        m_name,
        m_V.cols(),
        m_F.cols(),
        m_bsdf ? indent(m_bsdf->toString()) : std::string("null"),
        m_emitter ? indent(m_emitter->toString()) : std::string("null")
    );
}

std::string Intersection::toString() const {
    if (!mesh)
        return "Intersection[invalid]";

    return tfm::format(
        "Intersection[\n"
        "  p = %s,\n"
        "  t = %f,\n"
        "  uv = %s,\n"
        "  shFrame = %s,\n"
        "  geoFrame = %s,\n"
        "  mesh = %s\n"
        "]",
        p.toString(),
        t,
        uv.toString(),
        indent(shFrame.toString()),
        indent(geoFrame.toString()),
        mesh ? mesh->toString() : std::string("null")
    );
}

Point3f Mesh::squareToUniformMesh(Sampler *sampler, Normal3f &n, float &pdf) const {
    Point2f sample2D = sampler->next2D();
    float ep1 = sample2D.x(), ep2 = sample2D.y();

    //important: use another random number!
    size_t tri_idx = dpdf.sample(sampler->next1D());

    uint32_t idx0 = m_F(0, tri_idx), idx1 = m_F(1, tri_idx), idx2 = m_F(2, tri_idx);
    Point3f p0 = m_V.col(idx0), p1 = m_V.col(idx1), p2 = m_V.col(idx2);

    float alpha = 1.0f - sqrt(1.0f - ep1), beta = ep2 * sqrt(1.0f - ep1), gamma = 1.0f - alpha - beta;
    if(m_N.size() > 0){
        n = alpha * m_N.col(idx0) + beta * m_N.col(idx1) + gamma * m_N.col(idx2);
    }else{
        n = Normal3f((p1 - p0).cross(p2 - p0));
    }
    n.normalize();
    pdf = dpdf.getNormalization();
    return alpha * p0 + beta * p1 + gamma * p2;
}

float Mesh::squareToUniformMeshPDF() const {
    return dpdf.getNormalization();
}
NORI_NAMESPACE_END


#include <string>

#define BOOST_TEST_MODULE SparseCompareTests

#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif
#include "DenseMIA.h"
#include "SparseMIA.h"

template<typename data_type>
void do_work(size_t dim1, size_t dim2, size_t dim3){

    typedef LibMIA::SparseMIA<data_type,3> MIAType;
    MIAType test1(dim1,dim2,dim3);
    test1.resize(test1.dimensionality()/2);
    //testing resize functionality
    BOOST_CHECK_MESSAGE(test1.data_end()-test1.data_begin()==test1.dimensionality()/2,std::string("Resize test of data for ")+typeid(data_type).name());
    BOOST_CHECK_MESSAGE(test1.index_end()-test1.index_begin()==test1.dimensionality()/2,std::string("Resize test of indices for ")+typeid(data_type).name());

    //test sorting
    test1.rand_indices();
    test1.sort();
    bool passed=true;
    for(auto i=test1.index_begin()+1;i<test1.index_end();++i)
        passed=(passed&& *(i-1)<=*i);
    BOOST_CHECK_MESSAGE(passed,std::string("Sorting test for ")+typeid(data_type).name());

    //testing removal of duplicates
    test1.randu(0,2);
    *(test1.data_begin())=2;
    *(test1.data_begin()+1)=1;
    *(test1.index_begin())=2;
    *(test1.index_begin()+1)=1;
    test1.setSorted(false);
    auto i=test1.index_begin()+2;
    for(auto data_it=test1.data_begin()+2;i<test1.index_end();++i,++data_it)
        if (*data_it>=1){
            *data_it=2;
            *i=2;
        }
        else{
            *data_it=1;
            *i=1;
        }

    test1.collect_duplicates();
    BOOST_CHECK_MESSAGE(test1.data_end()-test1.data_begin()==2,std::string("Collect duplicates test 1 of data size for ")+typeid(data_type).name());
    BOOST_CHECK_MESSAGE(test1.index_end()-test1.index_begin()==2,std::string("Collect duplicates test 1 of indices size for ")+typeid(data_type).name());
    passed=(*(test1.data_begin())==1 && *(test1.data_begin()+1)==2);
    BOOST_CHECK_MESSAGE(passed,std::string("Collect duplicates test 1 of data content for ")+typeid(data_type).name());

    //testing collection of duplicates where we add duplicated entries
    test1.resize(test1.dimensionality()/2);
    test1.randu(0,2);
    *(test1.data_begin())=1;
    *(test1.data_begin()+1)=1;
    *(test1.index_begin())=2;
    *(test1.index_begin()+1)=1;
    test1.setSorted(false);
    size_t counter1=1,counter2=1;
    i=test1.index_begin()+2;
    for(auto data_it=test1.data_begin()+2;i<test1.index_end();++i,++data_it)
        if (*data_it>=1){
            *data_it=1;
            *i=1;
            ++counter1;
        }
        else{
            *data_it=1;
            *i=2;
            ++counter2;
        }

    test1.collect_duplicates(std::plus<data_type>());
    BOOST_CHECK_MESSAGE(test1.data_end()-test1.data_begin()==2,std::string("Collect duplicates test 2 of data size for ")+typeid(data_type).name());
    BOOST_CHECK_MESSAGE(test1.index_end()-test1.index_begin()==2,std::string("Collect duplicates test 2 of indices size for ")+typeid(data_type).name());
    passed=(*(test1.data_begin())==(data_type)counter1 && *(test1.data_begin()+1)==(data_type)counter2);
    BOOST_CHECK_MESSAGE(passed,std::string("Collect duplicates test 2 of data content for ")+typeid(data_type).name());

    MIAType test2; //zero dimensionality

    test2.rand_indices(); //make sure no error thrown when dimensionality is zero

    LibMIA::SparseMIA<data_type,4> test_reorder(dim1,dim2,dim3,dim2);
    LibMIA::SparseMIA<data_type,4> test_reorder2;
    auto linIdxSequence=test_reorder.linIdxSequence();

    linIdxSequence={{2,0,3,1}};

    test_reorder.resize(test_reorder.dimensionality()/2);
    test_reorder.randu(-5,5);
    test_reorder.rand_indices();
    test_reorder.collect_duplicates();
    test_reorder2=test_reorder;
    test_reorder.sort(linIdxSequence);
    test_reorder2.change_linIdx_sequence(linIdxSequence);
    test_reorder2.sort();
    passed=true;
    auto data_it=test_reorder.data_begin();
    auto data_it2=test_reorder2.data_begin();
    auto it=test_reorder.index_begin();
    auto it2=test_reorder2.index_begin();
    for(;data_it<test_reorder.data_end();++data_it,++it,++data_it2,++it2){ //do a manual check, b/c if we use == operator, a sort will occur, nullifying the test
        if(*data_it!=*data_it2){
            passed=false;
            break;
        }
        if(*it!=*it2){
            passed=false;
            break;
        }
    }

    BOOST_CHECK_MESSAGE(passed,std::string("Sparse reording sort 1 for ")+typeid(data_type).name());

    //try another linIdxSequence
    linIdxSequence={{1,2,3,0}};
    test_reorder2=test_reorder;
    test_reorder.sort(linIdxSequence);
    test_reorder2.change_linIdx_sequence(linIdxSequence);
    test_reorder2.sort();
    passed=true;
    data_it=test_reorder.data_begin();
    data_it2=test_reorder2.data_begin();
    it=test_reorder.index_begin();
    it2=test_reorder2.index_begin();
    for(;data_it<test_reorder.data_end();++data_it,++it,++data_it2,++it2){ //do a manual check, b/c if we use == operator, a sort will occur, nullifying the test
        if(*data_it!=*data_it2){
            passed=false;
            break;
        }
        if(*it!=*it2){
            passed=false;
            break;
        }
    }

    BOOST_CHECK_MESSAGE(passed,std::string("Sparse reording sort 2 for ")+typeid(data_type).name());

    //try another linIdxSequence
    linIdxSequence={{0,1,2,3}};
    test_reorder2=test_reorder;
    test_reorder.sort(linIdxSequence);
    test_reorder2.change_linIdx_sequence(linIdxSequence);
    test_reorder2.sort();
    passed=true;
    data_it=test_reorder.data_begin();
    data_it2=test_reorder2.data_begin();
    it=test_reorder.index_begin();
    it2=test_reorder2.index_begin();
    for(;data_it<test_reorder.data_end();++data_it,++it,++data_it2,++it2){ //do a manual check, b/c if we use == operator, a sort will occur, nullifying the test
        if(*data_it!=*data_it2){
            passed=false;
            break;
        }
        if(*it!=*it2){
            passed=false;
            break;
        }
    }

    BOOST_CHECK_MESSAGE(passed,std::string("Sparse reording sort 3 for ")+typeid(data_type).name());

    //try another linIdxSequence
    linIdxSequence={{2,3,0,1}};
    test_reorder2=test_reorder;
    test_reorder.sort(linIdxSequence);
    test_reorder2.change_linIdx_sequence(linIdxSequence);
    test_reorder2.sort();
    passed=true;
    data_it=test_reorder.data_begin();
    data_it2=test_reorder2.data_begin();
    it=test_reorder.index_begin();
    it2=test_reorder2.index_begin();
    for(;data_it<test_reorder.data_end();++data_it,++it,++data_it2,++it2){ //do a manual check, b/c if we use == operator, a sort will occur, nullifying the test
        if(*data_it!=*data_it2){
            passed=false;
            break;
        }
        if(*it!=*it2){
            passed=false;
            break;
        }
    }

    BOOST_CHECK_MESSAGE(passed,std::string("Sparse reording sort 3 for ")+typeid(data_type).name());

}

BOOST_AUTO_TEST_CASE( SparseCompareTests )
{

    //size_t dim1=3,dim2=4,dim3=5;
    size_t dim1=3,dim2=3,dim3=3;

    do_work<float>(dim1,dim2,dim3);
    do_work<double>(dim1,dim2,dim3);
    do_work<int32_t>(dim1,dim2,dim3);
    do_work<int64_t>(dim1,dim2,dim3);
}


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ConstExprReduceProdUnittest

#include <boost/optional.hpp>
#include <boost/test/unit_test.hpp>

#include <popart/ces/reduceprodce.hpp>
#include <popart/graph.hpp>
#include <popart/ir.hpp>
#include <popart/op/reduceprod.hpp>

#include <string>

using namespace popart;

const Shape SHAPE          = {2, 3, 5};
const int64_t RANK         = SHAPE.size();
const int64_t ELEMENT_SIZE = sizeof(int64_t);

// Constant folds a ReduceProd with the provided keepdims and axes atributes and
// a 2 * 3 * 5 constant tensor of 2s as input.
void check_combination(nonstd::optional_lite::optional<Shape> axes,
                       int64_t keepdims,
                       int64_t num_elements) {

  // Print out axes and keepdims
  BOOST_TEST_MESSAGE("keepdims: " + std::to_string(keepdims));
  std::string axes_string;
  if (axes.has_value()) {
    for (auto axis : axes.value()) {
      axes_string += std::to_string(axis) + " ";
    }
    BOOST_TEST_MESSAGE("axes: " + axes_string);
  } else
    BOOST_TEST_MESSAGE("no axes provided");
  BOOST_TEST_MESSAGE("");

  // Create a small graph with one node and one constant tensor
  Ir ir;
  Graph &g = ir.getMainGraph();

  Op::Settings settings(g, "test_reduceprod");

  // set up input and output tensors
  const TensorInfo input_info{DataType::INT64, SHAPE};
  const std::vector<int64_t> input_data(input_info.nelms(), 2);
  g.addConstInit("input", input_info, input_data.data(), "input_init");
  g.addActGrad("output");

  auto op = g.createConnectedOp<ReduceProdOp>(
      {{ReduceProdOp::getInIndex(), "input"}},
      {{ReduceProdOp::getOutIndex(), "output"}},
      Onnx::Operators::ReduceProd_11,
      axes,
      keepdims,
      settings);

  ConstExprReduceProd ce_reduce_prod(op);
  ce_reduce_prod;

  int64_t expected_element = 1
                             << (SHAPE[0] * SHAPE[1] * SHAPE[2] / num_elements);

  // The result returned is just a memcpy of the raw array in the resulting
  // tensor. 8 bytes per element.
  auto result = ce_reduce_prod.compute();

  // Check that the result has the size we expect
  BOOST_REQUIRE_MESSAGE(num_elements * ELEMENT_SIZE == result.size(),
                        "Number of returned bytes was " +
                            std::to_string(result.size()) + "; expected " +
                            std::to_string(num_elements * ELEMENT_SIZE));

  // Compare the bytes to what we expect to see
  for (int i = 0; i < num_elements; i++) {
    int64_t returned_element;
    std::memcpy(
        &returned_element, &result.data()[i * ELEMENT_SIZE], ELEMENT_SIZE);

    BOOST_CHECK_MESSAGE(expected_element == returned_element,
                        "expected element " + std::to_string(expected_element) +
                            ", got " + std::to_string(returned_element));
  }
}

BOOST_AUTO_TEST_CASE(TestConstExprReduceProd) {
  // positive axes
  for (int combination = 0; combination < (1 << RANK); ++combination) {
    Shape axes;
    int64_t num_elements = 1;
    for (int i = 0; i < RANK; ++i) {
      if ((combination >> i) & 1)
        axes.push_back(i);
      else
        num_elements *= SHAPE[i];
    }
    check_combination(axes, 0, num_elements);
    check_combination(axes, 1, num_elements);
  }

  // negative axes
  for (int combination = 1; combination < (1 << RANK); ++combination) {
    Shape axes;
    int64_t num_elements = 1;
    for (int i = 0; i < RANK; i++) {
      if ((combination >> i) & 1)
        axes.push_back(-i - 1);
      else
        num_elements *= SHAPE[2 - i];
    }
    check_combination(axes, 0, num_elements);
    check_combination(axes, 1, num_elements);
  }

  // no axes
  check_combination(nonstd::nullopt, 0, 1);
  check_combination(nonstd::nullopt, 1, 1);

  // duplicated axes
  check_combination(Shape{1, 1}, 0, 10);
  check_combination(Shape{1, -2}, 1, 10);

  // mixed negative and positive axes
  check_combination(Shape{1, -1}, 0, 2);
  check_combination(Shape{1, -3}, 1, 5);
}