/*!
  \file gpp_knowledge_gradient_optimization.cpp
  \rst
\endrst*/

#include "gpp_knowledge_gradient_optimization.hpp"

#include <cmath>

#include <algorithm>
#include <memory>
#include <vector>

#include <boost/math/distributions/normal.hpp>  // NOLINT(build/include_order)

#include "gpp_common.hpp"
#include "gpp_domain.hpp"
#include "gpp_logging.hpp"
#include "gpp_math.hpp"
#include "gpp_optimizer_parameters.hpp"

namespace optimal_learning {

template <typename DomainType>
KnowledgeGradientEvaluator<DomainType>::KnowledgeGradientEvaluator(const GaussianProcess& gaussian_process_in, const int num_fidelity,
                                                                   double const * discrete_pts,
                                                                   int num_pts,
                                                                   int num_mc_iterations,
                                                                   const DomainType& domain,
                                                                   const GradientDescentParameters& optimizer_parameters,
                                                                   double best_so_far)
  : dim_(gaussian_process_in.dim()),
    num_fidelity_(num_fidelity),
    num_mc_iterations_(num_mc_iterations),
    best_so_far_(best_so_far),
    optimizer_parameters_(optimizer_parameters.num_multistarts, optimizer_parameters.max_num_steps,
                          optimizer_parameters.max_num_restarts, optimizer_parameters.num_steps_averaged,
                          optimizer_parameters.gamma, optimizer_parameters.pre_mult,
                          optimizer_parameters.max_relative_change, optimizer_parameters.tolerance),
    domain_(domain),
    gaussian_process_(&gaussian_process_in),
    discrete_pts_(discrete_points(discrete_pts, num_pts)),
    num_pts_(num_pts){
}

template <typename DomainType>
KnowledgeGradientEvaluator<DomainType>::KnowledgeGradientEvaluator(KnowledgeGradientEvaluator&& other)
  : dim_(other.dim()),
    num_fidelity_(other.num_fidelity()),
    num_mc_iterations_(other.num_mc_iterations()),
    best_so_far_(other.best_so_far()),
    optimizer_parameters_(other.gradient_descent_params().num_multistarts, other.gradient_descent_params().max_num_steps,
                          other.gradient_descent_params().max_num_restarts, other.gradient_descent_params().num_steps_averaged,
                          other.gradient_descent_params().gamma, other.gradient_descent_params().pre_mult,
                          other.gradient_descent_params().max_relative_change, other.gradient_descent_params().tolerance),
    domain_(other.domain()),
    gaussian_process_(other.gaussian_process()),
    discrete_pts_(other.discrete_pts_copy()),
    num_pts_(other.number_discrete_pts()){
}

/*!\rst
  Compute Knowledge Gradient
  This version requires the discretization of A (the feasibe domain).
  The discretization usually is: some set + points previous sampled + points being sampled + points to sample
\endrst*/
template <typename DomainType>
double KnowledgeGradientEvaluator<DomainType>::ComputeKnowledgeGradient(StateType * kg_state) const {
  int num_union = kg_state->num_union;
  int num_gradients_to_sample = kg_state->num_gradients_to_sample;

  double best_posterior = best_so_far_;
  for (int j = 0; j < num_union; ++j) {
    if (kg_state->to_sample_mean_[j*(1+kg_state->num_gradients_to_sample)] < best_posterior){
      best_posterior = kg_state->to_sample_mean_[j*(1+kg_state->num_gradients_to_sample)];
    }
  }

  double aggregate = 0.0;
  kg_state->normal_rng->ResetToMostRecentSeed();

  GaussianProcess gaussian_process_after(*gaussian_process_);
  std::vector<double> make_up_function_value(num_union*(1+num_gradients_to_sample));
  gaussian_process_after.AddSampledPointsToGP(kg_state->union_of_points.data(), make_up_function_value.data(), num_union);

  for (int i = 0; i < num_mc_iterations_; ++i) {
    if (i % 2 == 1){
      for (int j = 0; j < num_union*(1+num_gradients_to_sample); ++j) {
        kg_state->normals[j + i*num_union*(1+num_gradients_to_sample)] = -kg_state->normals[j + (i-1)*num_union*(1+num_gradients_to_sample)];
      }
    }
    else {
      for (int j = 0; j < num_union*(1+num_gradients_to_sample); ++j) {
        kg_state->normals[j + i*num_union*(1+num_gradients_to_sample)] = (*(kg_state->normal_rng))();
      }
    }

    double best_function_value = 0.0;
    bool found_flag;

    std::copy(kg_state->to_sample_mean_.begin(), kg_state->to_sample_mean_.end(), make_up_function_value.begin());
    GeneralMatrixVectorMultiply(kg_state->cholesky_to_sample_var.data(), 'N', kg_state->normals.data() + i*num_union*(1+num_gradients_to_sample),
                                1.0, 1.0, num_union*(1+num_gradients_to_sample), num_union*(1+num_gradients_to_sample), num_union*(1+num_gradients_to_sample),
                                make_up_function_value.data());

    gaussian_process_after.NewSampledValue(make_up_function_value.data(), num_union, gaussian_process_->num_sampled(), false);

    ComputeOptimalPosteriorMean(gaussian_process_after, num_fidelity_, optimizer_parameters_,
                                domain_, kg_state->discretized_set.data(), num_union + num_pts_,
                                &found_flag, kg_state->best_point.data() + i*dim_, &best_function_value);
    aggregate += best_posterior + best_function_value;
  }
  return aggregate/static_cast<double>(num_mc_iterations_);
}

/*!\rst
  Computes gradient of KG (see KnowledgeGradientEvaluator::ComputeGradKnowledgeGradient) wrt points_to_sample (stored in
  ``union_of_points[0:num_to_sample]``).
  Mechanism is similar to the computation of KG, where points' contributions to the gradient are thrown out of their
  corresponding ``improvement <= 0.0``.
  Thus ``\nabla(\mu)`` only contributes when the ``winner`` (point w/best improvement this iteration) is the current point.
  That is, the gradient of ``\mu`` at ``x_i`` wrt ``x_j`` is 0 unless ``i == j`` (and only this result is stored in
  ``kg_state->grad_mu``).  The interaction with ``kg_state->grad_chol_decomp`` is harder to know a priori (like with
  ``grad_mu``) and has a more complex structure (rank 3 tensor), so the derivative wrt ``x_j`` is computed fully, and
  the relevant submatrix (indexed by the current ``winner``) is accessed each iteration.
  .. Note:: comments here are copied to _compute_grad_knowledge_gradient_monte_carlo() in python_version/knowledge_gradient.py
\endrst*/
template <typename DomainType>
double KnowledgeGradientEvaluator<DomainType>::ComputeGradKnowledgeGradient(StateType * kg_state, double * restrict grad_KG) const {
  const int num_union = kg_state->num_union;
  int num_gradients_to_sample = kg_state->num_gradients_to_sample;

  std::vector<double> grad_mu_temp(dim_*(kg_state->num_to_sample)*(1+num_gradients_to_sample), 0.0);
  gaussian_process_->ComputeGradMeanOfPoints(kg_state->points_to_sample_state, grad_mu_temp.data());
  for (int i = 0; i < kg_state->num_to_sample; ++i){
    for (int d = 0; d < dim_; ++d){
      kg_state->grad_mu[d + i*dim_] = grad_mu_temp[d + i*(1+num_gradients_to_sample)*dim_];
    }
  }

  // compute the grad of chol among points to sample.
  gaussian_process_->ComputeGradCholeskyVarianceOfPoints(&(kg_state->points_to_sample_state),
                                                         kg_state->cholesky_to_sample_var.data(),
                                                         kg_state->grad_chol_decomp.data());
  int winner_so_far = -1;
  double best_posterior = best_so_far_;
  for (int j = 0; j < num_union; ++j){
    if (kg_state->to_sample_mean_[j*(1+kg_state->num_gradients_to_sample)] < best_posterior){
      winner_so_far = j;
      best_posterior = kg_state->to_sample_mean_[j*(1+kg_state->num_gradients_to_sample)];
    }
  }

  std::fill(kg_state->aggregate.begin(), kg_state->aggregate.end(), 0.0);

  if (winner_so_far >= 0 && winner_so_far < kg_state->num_to_sample){
    for (int k = 0; k < dim_; ++k) {
      kg_state->aggregate[winner_so_far*dim_ + k] += num_mc_iterations_ * kg_state->grad_mu[winner_so_far*dim_ + k];
    }
  }
  std::fill(kg_state->best_point.begin(), kg_state->best_point.end(), 1.0);
  double aggregate = 0.0;
  kg_state->normal_rng->ResetToMostRecentSeed();

  GaussianProcess gaussian_process_after(*gaussian_process_);
  std::vector<double> make_up_function_value(num_union*(1+num_gradients_to_sample));
  gaussian_process_after.AddSampledPointsToGP(kg_state->union_of_points.data(), make_up_function_value.data(), num_union);

  for (int i = 0; i < num_mc_iterations_; ++i) {
    if (i % 2 == 1){
      for (int j = 0; j < num_union*(1+num_gradients_to_sample); ++j) {
        kg_state->normals[j + i*num_union*(1+num_gradients_to_sample)] = -kg_state->normals[j + (i-1)*num_union*(1+num_gradients_to_sample)];
      }
    }
    else {
      for (int j = 0; j < num_union*(1+num_gradients_to_sample); ++j) {
        kg_state->normals[j + i*num_union*(1+num_gradients_to_sample)] = (*(kg_state->normal_rng))();// - 1.0;
      }
    }

    double best_function_value = 0.0;
    bool found_flag;

    std::copy(kg_state->to_sample_mean_.begin(), kg_state->to_sample_mean_.end(), make_up_function_value.begin());
    GeneralMatrixVectorMultiply(kg_state->cholesky_to_sample_var.data(), 'N', kg_state->normals.data() + i*num_union*(1+num_gradients_to_sample),
                                1.0, 1.0, num_union*(1+num_gradients_to_sample), num_union*(1+num_gradients_to_sample), num_union*(1+num_gradients_to_sample),
                                make_up_function_value.data());

    gaussian_process_after.NewSampledValue(make_up_function_value.data(), num_union, gaussian_process_->num_sampled(), false);

    ComputeOptimalPosteriorMean(gaussian_process_after, num_fidelity_, optimizer_parameters_,
                                domain_, kg_state->discretized_set.data(), num_union + num_pts_,
                                &found_flag, kg_state->best_point.data() + i*dim_, &best_function_value);
    aggregate += best_posterior + best_function_value;
  }  // end for i: num_mc_iterations_
  double KG =aggregate/static_cast<double>(num_mc_iterations_);

  gaussian_process_->ComputeCovarianceOfPoints(&(kg_state->points_to_sample_state), kg_state->best_point.data(), num_mc_iterations_,
                                               nullptr, 0, false, nullptr, kg_state->chol_inverse_cov.data());
  TriangularMatrixMatrixSolve(kg_state->cholesky_to_sample_var.data(), 'N', num_union*(1+num_gradients_to_sample), num_mc_iterations_,
                              num_union*(1+num_gradients_to_sample), kg_state->chol_inverse_cov.data());

  gaussian_process_->ComputeGradInverseCholeskyCovarianceOfPoints(&(kg_state->points_to_sample_state),
                                                                  kg_state->cholesky_to_sample_var.data(),
                                                                  kg_state->grad_chol_decomp.data(),
                                                                  kg_state->chol_inverse_cov.data(),
                                                                  kg_state->best_point.data(), num_mc_iterations_, false, nullptr,
                                                                  kg_state->grad_chol_inverse_cov.data());

  // let L_{d,i,j,k} = grad_chol_decomp, d over dim_, i, j over num_union, k over num_to_sample
  // we want to compute: agg_dx_{d,k} = L_{d,i,j=winner,k} * normals_i
  // TODO(GH-92): Form this as one GeneralMatrixVectorMultiply() call by storing data as L_{d,i,k,j} if it's faster.
  double const * restrict grad_chol_decomp_winner_block = kg_state->grad_chol_inverse_cov.data();
  for (int k = 0; k < kg_state->num_to_sample; ++k) {
    for (int i = 0; i < num_mc_iterations_; ++i){
      GeneralMatrixVectorMultiply(grad_chol_decomp_winner_block, 'N', kg_state->normals.data() + i*num_union*(1+num_gradients_to_sample), -1.0, 1.0,
                                  dim_, num_union*(1+num_gradients_to_sample), dim_, kg_state->aggregate.data() + k*dim_);
      grad_chol_decomp_winner_block += dim_*num_union*(1+num_gradients_to_sample);
    }
  }

  for (int k = 0; k < kg_state->num_to_sample*dim_; ++k) {
    grad_KG[k] = kg_state->aggregate[k]/static_cast<double>(num_mc_iterations_);
  }
  return KG;
}

template class KnowledgeGradientEvaluator<TensorProductDomain>;
template class KnowledgeGradientEvaluator<SimplexIntersectTensorProductDomain>;

template <typename DomainType>
void KnowledgeGradientState<DomainType>::SetCurrentPoint(const EvaluatorType& kg_evaluator,
                                                         double const * restrict points_to_sample) {
  // update points_to_sample in union_of_points
  std::copy(points_to_sample, points_to_sample + num_to_sample*dim, union_of_points.data());

  // evaluate derived quantities for the GP
  points_to_sample_state.SetupState(*kg_evaluator.gaussian_process(), union_of_points.data(),
                                    num_union, num_gradients_to_sample, num_derivatives, true, (num_derivatives>0));

  PreCompute(kg_evaluator, points_to_sample);
}

template <typename DomainType>
KnowledgeGradientState<DomainType>::KnowledgeGradientState(const EvaluatorType& kg_evaluator, double const * restrict points_to_sample,
                                                           double const * restrict points_being_sampled, int num_to_sample_in, int num_being_sampled_in, int num_pts_in,
                                                           int const * restrict gradients_in, int num_gradients_in, bool configure_for_gradients, NormalRNGInterface * normal_rng_in)
  : dim(kg_evaluator.dim()),
    num_to_sample(num_to_sample_in),
    num_being_sampled(num_being_sampled_in),
    num_derivatives(configure_for_gradients ? num_to_sample : 0),
    num_union(num_to_sample + num_being_sampled),
    num_iterations(kg_evaluator.num_mc_iterations()),
    gradients(gradients_in, gradients_in+num_gradients_in),
    num_gradients_to_sample(num_gradients_in),
    union_of_points(BuildUnionOfPoints(points_to_sample, points_being_sampled,
                                       num_to_sample, num_being_sampled, dim)),
    subset_union_of_points(SubsetData(union_of_points.data(), num_union, kg_evaluator.num_fidelity())),
    discretized_set(BuildUnionOfPoints(subset_union_of_points.data(), kg_evaluator.discrete_pts_copy().data(),
                                       num_union, kg_evaluator.number_discrete_pts(), dim - kg_evaluator.num_fidelity())),
    points_to_sample_state(*kg_evaluator.gaussian_process(), union_of_points.data(), num_union,
                           gradients_in, num_gradients_in, num_derivatives, true, configure_for_gradients),
    normal_rng(normal_rng_in),
    cholesky_to_sample_var(Square(num_union*(1+num_gradients_to_sample))),
    grad_chol_decomp(dim*Square(num_union*(1+num_gradients_to_sample))*num_derivatives),
    to_sample_mean_(num_union*(1+num_gradients_to_sample)),
    grad_mu(dim*num_derivatives),
    aggregate(dim*num_derivatives),
    normals(num_union*(1+num_gradients_to_sample)*num_iterations),
    best_point(dim*num_iterations),
    chol_inverse_cov(num_iterations*num_union*(1+num_gradients_to_sample)),
    grad_chol_inverse_cov(dim*num_iterations*num_union*(1+num_gradients_to_sample)*num_derivatives) {
  PreCompute(kg_evaluator, points_to_sample);
}

template <typename DomainType>
KnowledgeGradientState<DomainType>::KnowledgeGradientState(KnowledgeGradientState&& OL_UNUSED(other)) = default;

template <typename DomainType>
void KnowledgeGradientState<DomainType>::SetupState(const EvaluatorType& kg_evaluator,
                                                    double const * restrict points_to_sample) {
  if (unlikely(dim != kg_evaluator.dim())) {
    OL_THROW_EXCEPTION(InvalidValueException<int>, "Evaluator's and State's dim do not match!", dim, kg_evaluator.dim());
  }

  // update quantities derived from points_to_sample
  SetCurrentPoint(kg_evaluator, points_to_sample);
}

template <typename DomainType>
void KnowledgeGradientState<DomainType>::PreCompute(const EvaluatorType& kg_evaluator,
                                                    double const * restrict points_to_sample) {
  if (unlikely(dim != kg_evaluator.dim())) {
    OL_THROW_EXCEPTION(InvalidValueException<int>, "Evaluator's and State's dim do not match!", dim, kg_evaluator.dim());
  }

  kg_evaluator.gaussian_process()->ComputeMeanOfAdditionalPoints(union_of_points.data(), num_union, gradients.data(), num_gradients_to_sample,
                                                                 to_sample_mean_.data());

  kg_evaluator.gaussian_process()->ComputeVarianceOfPoints(&(points_to_sample_state), gradients.data(),
                                                          num_gradients_to_sample, cholesky_to_sample_var.data());
  //Adding the variance of measurement noise to the covariance matrix
  for (int i = 0;i < num_union; i++){
    for (int j = 0; j < 1+num_gradients_to_sample; ++j){
      int row = i*(1+num_gradients_to_sample)+j;
      cholesky_to_sample_var[row+row*num_union*(1+num_gradients_to_sample)] += kg_evaluator.gaussian_process()->noise_variance()[j];// + 1.e-6;
    }
  }
  int leading_minor_index = ComputeCholeskyFactorL(num_union*(1+num_gradients_to_sample), cholesky_to_sample_var.data());
  if (unlikely(leading_minor_index != 0)) {
    OL_THROW_EXCEPTION(SingularMatrixException,
    "GP-Variance matrix singular. Check for duplicate points_to_sample/being_sampled or points_to_sample/being_sampled duplicating points_sampled with 0 noise.",
    cholesky_to_sample_var.data(), num_union*(1+num_gradients_to_sample), leading_minor_index);
  }
  ZeroUpperTriangle(num_union*(1+num_gradients_to_sample), cholesky_to_sample_var.data());
}

template struct KnowledgeGradientState<TensorProductDomain>;
template struct KnowledgeGradientState<SimplexIntersectTensorProductDomain>;

PosteriorMeanEvaluator::PosteriorMeanEvaluator(
  const GaussianProcess& gaussian_process_in)
  : dim_(gaussian_process_in.dim()),
    gaussian_process_(&gaussian_process_in) {
}

/*!\rst
  Uses analytic formulas to compute EI when ``num_to_sample = 1`` and ``num_being_sampled = 0`` (occurs only in 1,0-EI).
  In this case, the single-parameter (posterior) GP is just a Gaussian.  So the integral in EI (previously eval'd with MC)
  can be computed 'exactly' using high-accuracy routines for the pdf & cdf of a Gaussian random variable.
  See Ginsbourger, Le Riche, and Carraro.
\endrst*/
double PosteriorMeanEvaluator::ComputePosteriorMean(StateType * ps_state) const {
  double to_sample_mean;
  gaussian_process_->ComputeMeanOfAdditionalPoints(ps_state->point_to_sample.data(),
                                                   1, nullptr, 0,
                                                   &to_sample_mean);
  return -to_sample_mean;
}

/*!\rst
  Differentiates OnePotentialSampleExpectedImprovementEvaluator::ComputeExpectedImprovement wrt
  ``points_to_sample`` (which is just ONE point; i.e., 1,0-EI).
  Again, this uses analytic formulas in terms of the pdf & cdf of a Gaussian since the integral in EI (and grad EI)
  can be evaluated exactly for this low dimensional case.
  See Ginsbourger, Le Riche, and Carraro.
\endrst*/
void PosteriorMeanEvaluator::ComputeGradPosteriorMean(
    StateType * ps_state,
    double * restrict grad_PS) const {
  double * restrict grad_mu = ps_state->grad_mu.data();
  gaussian_process_->ComputeGradMeanOfAdditionalPoints(ps_state->point_to_sample.data(),
                                                       1, nullptr, 0,
                                                       grad_mu);
  for (int i = 0; i < dim_-ps_state->num_fidelity; ++i) {
    grad_PS[i] = -grad_mu[i];
  }
}

void PosteriorMeanState::SetCurrentPoint(const EvaluatorType& ps_evaluator,
                                         double const * restrict point_to_sample_in) {
  // update current point in union_of_points
  std::copy(point_to_sample_in, point_to_sample_in + dim - num_fidelity, point_to_sample.data());
  std::fill(point_to_sample.data() + dim - num_fidelity, point_to_sample.data() + dim, 1.0);
}

PosteriorMeanState::PosteriorMeanState(
  const EvaluatorType& ps_evaluator,
  const int num_fidelity_in,
  double const * restrict point_to_sample_in,
  bool configure_for_gradients)
  : dim(ps_evaluator.dim()),
    num_fidelity(num_fidelity_in),
    num_derivatives(configure_for_gradients ? num_to_sample : 0),
    point_to_sample(BuildUnionOfPoints(point_to_sample_in)),
    grad_mu(dim*num_derivatives) {
}
PosteriorMeanState::PosteriorMeanState(PosteriorMeanState&& OL_UNUSED(other)) = default;

//void PosteriorMeanState::SetupState(const EvaluatorType& ps_evaluator,
//                                    double const * restrict point_to_sample_in) {
//  if (unlikely(dim != ps_evaluator.dim())) {
//    OL_THROW_EXCEPTION(InvalidValueException<int>, "Evaluator's and State's dim do not match!", dim, ps_evaluator.dim());
//  }
//
//  SetCurrentPoint(ps_evaluator, point_to_sample_in);
//}

/*!\rst
  Perform multistart gradient descent (MGD) to solve the q,p-EI problem (see ComputeOptimalPointsToSample and/or
  header docs), starting from ``num_multistarts`` points selected randomly from the within th domain.
  This function is a simple wrapper around ComputeOptimalPointsToSampleViaMultistartGradientDescent(). It additionally
  generates a set of random starting points and is just here for convenience when better initial guesses are not
  available.
  See ComputeOptimalPointsToSampleViaMultistartGradientDescent() for more details.
  \param
    :gaussian_process: GaussianProcess object (holds ``points_sampled``, ``values``, ``noise_variance``, derived quantities)
      that describes the underlying GP
    :optimizer_parameters: GradientDescentParameters object that describes the parameters controlling EI optimization
      (e.g., number of iterations, tolerances, learning rate)
    :domain: object specifying the domain to optimize over (see ``gpp_domain.hpp``)
    :thread_schedule: struct instructing OpenMP on how to schedule threads; i.e., (suggestions in parens)
      max_num_threads (num cpu cores), schedule type (omp_sched_dynamic), chunk_size (0).
    :points_being_sampled[dim][num_being_sampled]: points that are being sampled in concurrent experiments
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-EI)
    :num_being_sampled: number of points being sampled concurrently (i.e., the "p" in q,p-EI)
    :best_so_far: value of the best sample so far (must be ``min(points_sampled_value)``)
    :max_int_steps: maximum number of MC iterations
    :uniform_generator[1]: a UniformRandomGenerator object providing the random engine for uniform random numbers
    :normal_rng[thread_schedule.max_num_threads]: a vector of NormalRNG objects that provide
      the (pesudo)random source for MC integration
  \output
    :found_flag[1]: true if best_next_point corresponds to a nonzero EI
    :uniform_generator[1]: UniformRandomGenerator object will have its state changed due to random draws
    :normal_rng[thread_schedule.max_num_threads]: NormalRNG objects will have their state changed due to random draws
    :best_next_point[dim][num_to_sample]: points yielding the best EI according to MGD
\endrst*/
template <typename DomainType>
void ComputeOptimalPosteriorMean(const GaussianProcess& gaussian_process, const int num_fidelity,
                                 const GradientDescentParameters& optimizer_parameters,
                                 const DomainType& domain, double const * restrict initial_guess, const int num_starts,
                                 bool * restrict found_flag, double * restrict best_next_point, double * best_function_value) {
  if (unlikely(optimizer_parameters.max_num_restarts <= 0)) {
    return;
  }
  bool configure_for_gradients = true;
  OL_VERBOSE_PRINTF("Posterior Mean Optimization via %s:\n", OL_CURRENT_FUNCTION_NAME);

  // special analytic case when we are not using (or not accounting for) multiple, simultaneous experiments
  PosteriorMeanEvaluator ps_evaluator(gaussian_process);
  typename PosteriorMeanEvaluator::StateType ps_state(ps_evaluator, num_fidelity, initial_guess, configure_for_gradients);

  std::priority_queue<std::pair<double, int>> q;
  double val;
  int k = std::min(1, num_starts); // number of indices we need
  for (int i = 0; i < num_starts; ++i) {
    ps_state.SetCurrentPoint(ps_evaluator, initial_guess + i*(gaussian_process.dim()-num_fidelity));
    val = ps_evaluator.ComputePosteriorMean(&ps_state);
    if (i < k){
      q.push(std::pair<double, int>(-val, i));
    }
    else{
      if (q.top().first > -val){
        q.pop();
        q.push(std::pair<double, int>(-val, i));
      }
    }
  }

  std::vector<double> top_k_starting(k*(gaussian_process.dim()-num_fidelity));
  for (int i = 0; i < k; ++i) {
    int ki = q.top().second;
    for (int d = 0; d<gaussian_process.dim()-num_fidelity; ++d){
      top_k_starting[i*(gaussian_process.dim()-num_fidelity) + d] = initial_guess[ki*(gaussian_process.dim()-num_fidelity) + d];
    }
    q.pop();
  }

  GradientDescentOptimizerLineSearch<PosteriorMeanEvaluator, DomainType> gd_opt;
  double function_value_temp = -INFINITY;
  *best_function_value = -INFINITY;
  for (int i = 0; i < k; ++i){
    ps_state.SetCurrentPoint(ps_evaluator, top_k_starting.data() + i*(gaussian_process.dim()-num_fidelity));
    gd_opt.Optimize(ps_evaluator, optimizer_parameters, domain, &ps_state);
    function_value_temp = ps_evaluator.ComputePosteriorMean(&ps_state);
    if (function_value_temp > *best_function_value){
      *best_function_value = function_value_temp;
      ps_state.GetCurrentPoint(best_next_point);
    }
  }
}

// template explicit instantiation definitions, see gpp_common.hpp header comments, item 6
template void ComputeOptimalPosteriorMean(const GaussianProcess& gaussian_process, const int num_fidelity,
                                          const GradientDescentParameters& optimizer_parameters,
                                          const TensorProductDomain& domain, double const * restrict initial_guess, const int num_starts,
                                          bool * restrict found_flag, double * restrict best_next_point, double * best_function_value);
template void ComputeOptimalPosteriorMean(const GaussianProcess& gaussian_process, const int num_fidelity,
                                          const GradientDescentParameters& optimizer_parameters,
                                          const SimplexIntersectTensorProductDomain& domain, double const * restrict initial_guess, const int num_starts,
                                          bool * restrict found_flag, double * restrict best_next_point, double * best_function_value);

/*!\rst
  This is a simple wrapper around ComputeKGOptimalPointsToSampleWithRandomStarts() and
  ComputeKGOptimalPointsToSampleViaLatinHypercubeSearch(). That is, this method attempts multistart gradient descent
  and falls back to latin hypercube search if gradient descent fails (or is not desired).
\endrst*/
template <typename DomainType>
void ComputeKGOptimalPointsToSample(const GaussianProcess& gaussian_process, const int num_fidelity,
                                    const GradientDescentParameters& optimizer_parameters,
                                    const GradientDescentParameters& optimizer_parameters_inner,
                                    const DomainType& domain, const DomainType& inner_domain, const ThreadSchedule& thread_schedule,
                                    double const * restrict points_being_sampled,
                                    double const * discrete_pts,
                                    int num_to_sample, int num_being_sampled,
                                    int num_pts, double best_so_far,
                                    int max_int_steps, bool lhc_search_only,
                                    int num_lhc_samples, bool * restrict found_flag,
                                    UniformRandomGenerator * uniform_generator,
                                    NormalRNG * normal_rng, double * restrict best_points_to_sample) {
  if (unlikely(num_to_sample <= 0)) {
    return;
  }

  std::vector<double> next_points_to_sample(gaussian_process.dim()*num_to_sample);
  bool found_flag_local = false;
  if (lhc_search_only == false) {
    ComputeKGOptimalPointsToSampleWithRandomStarts(gaussian_process, num_fidelity, optimizer_parameters, optimizer_parameters_inner,
                                                   domain, inner_domain, thread_schedule, points_being_sampled, discrete_pts,
                                                   num_to_sample, num_being_sampled, num_pts,
                                                   best_so_far, max_int_steps,
                                                   &found_flag_local, uniform_generator, normal_rng,
                                                   next_points_to_sample.data());
  }

  // if gradient descent EI optimization failed OR we're only doing latin hypercube searches
  if (found_flag_local == false || lhc_search_only == true) {
    if (unlikely(lhc_search_only == false)) {
      OL_WARNING_PRINTF("WARNING: %d,%d-KG opt DID NOT CONVERGE\n", num_to_sample, num_being_sampled);
      OL_WARNING_PRINTF("Attempting latin hypercube search\n");
    }

    if (num_lhc_samples > 0) {
      // Note: using a schedule different than "static" may lead to flakiness in monte-carlo KG optimization tests.
      // Besides, this is the fastest setting.
      ThreadSchedule thread_schedule_naive_search(thread_schedule);
      thread_schedule_naive_search.schedule = omp_sched_static;
      ComputeKGOptimalPointsToSampleViaLatinHypercubeSearch(gaussian_process, num_fidelity, optimizer_parameters_inner, domain, inner_domain,
                                                            thread_schedule_naive_search,
                                                            points_being_sampled, discrete_pts,
                                                            num_lhc_samples, num_to_sample,
                                                            num_being_sampled, num_pts, best_so_far,
                                                            max_int_steps,
                                                            &found_flag_local, uniform_generator,
                                                            normal_rng, next_points_to_sample.data());

      // if latin hypercube 'dumb' search failed
      if (unlikely(found_flag_local == false)) {
        OL_ERROR_PRINTF("ERROR: %d,%d-KG latin hypercube search FAILED on\n", num_to_sample, num_being_sampled);
      }
    } else {
      OL_WARNING_PRINTF("num_lhc_samples <= 0. Skipping latin hypercube search\n");
    }
  }

  // set outputs
  *found_flag = found_flag_local;
  std::copy(next_points_to_sample.begin(), next_points_to_sample.end(), best_points_to_sample);
}

// template explicit instantiation definitions, see gpp_common.hpp header comments, item 6
template void ComputeKGOptimalPointsToSample(
    const GaussianProcess& gaussian_process, const int num_fidelity, const GradientDescentParameters& optimizer_parameters,
    const GradientDescentParameters& optimizer_parameters_inner,
    const TensorProductDomain& domain, const TensorProductDomain& inner_domain, const ThreadSchedule& thread_schedule,
    double const * restrict points_being_sampled, double const * discrete_pts,
    int num_to_sample, int num_being_sampled,
    int num_pts, double best_so_far, int max_int_steps, bool lhc_search_only,
    int num_lhc_samples, bool * restrict found_flag, UniformRandomGenerator * uniform_generator,
    NormalRNG * normal_rng, double * restrict best_points_to_sample);
template void ComputeKGOptimalPointsToSample(
    const GaussianProcess& gaussian_process, const int num_fidelity, const GradientDescentParameters& optimizer_parameters,
    const GradientDescentParameters& optimizer_parameters_inner,
    const SimplexIntersectTensorProductDomain& domain, const SimplexIntersectTensorProductDomain& inner_domain, const ThreadSchedule& thread_schedule,
    double const * restrict points_being_sampled,double const * discrete_pts,
    int num_to_sample, int num_being_sampled, int num_pts, double best_so_far, int max_int_steps, bool lhc_search_only, int num_lhc_samples, bool * restrict found_flag,
    UniformRandomGenerator * uniform_generator, NormalRNG * normal_rng, double * restrict best_points_to_sample);
}  // end namespace optimal_learning


/****************************************************************

  jjjt_operator.cpp

  Mark A. Caprio, University of Notre Dame.

****************************************************************/

#include <cassert>
#include <cmath>
#include <cstddef>
#include <cstdlib>
#include <iomanip>
#include <ostream>
#include <tuple>
#include <vector>

#include "jjjt_operator.h"

#include "am/halfint.h"
#include <Eigen/Core>
#include "basis.h"
#include "many_body.h"
#include "jjjt_scheme.h"
#include "jt_operator.h"
#include "operator.h"

namespace basis {

  ////////////////////////////////////////////////////////////////
  // two-body JJJT operator -- gather N blocks
  ////////////////////////////////////////////////////////////////

  inline
  void RecastLabelsTwoBodyJJJTToTwoBodyJJJTN(
      const TwoBodySubspaceJJJTLabels& two_body_jjjt_subspace_labels,
      const TwoBodyStateJJJTLabels& two_body_jjjt_state_labels,
      TwoBodySubspaceJJJTNLabels& two_body_jjjtn_subspace_labels,
      TwoBodyStateJJJTNLabels& two_body_jjjtn_state_labels
    )
  // Recast labels for (subspace,state) from TwoBodyJJJT scheme to
  // TwoBodyJJJTN scheme.
  //
  // This direction of conversion (from target labels to source
  // labels) is as needed for looking up the source matrix element.
  //
  // This switches N from being the most-significant state label (that
  // is, least-rapidly-varying in the lexicographic ordering scheme),
  // to being the least-significant subspace label (that is,
  // most-rapidly-varying):
  //
  //   (L,S,J,T,g) : ([N],N1,l1,N2,l2) -> (L,S,J,T,g,N) : (N1,l1,N2,l2)
  //
  // As a state label, N is actually an implicit label, not stored,
  // but effectively the first label for ordering purposes.  Its value
  // may be recovered as N1+N2.
  //
  // Arguments:
  //   two_body_jjjt_subspace_labels (...) : source subspace labels
  //   two_body_jjjt_state_labels (...) : source state labels
  //   two_body_jjjtn_subspace_labels (...,output) : target subspace labels
  //   two_body_jjjtn_state_labels (...,output) : target state labels
  {
    // extract labels
    int J, T, g;
    std::tie(J,T,g) = two_body_jjjt_subspace_labels;
    int N1, l1, N2, l2;
    HalfInt j1, j2;
    std::tie(N1,j1,N2,j2) = two_body_jjjt_state_labels;

    // repackage labels
    int N = N1+N2;
    two_body_jjjtn_subspace_labels = TwoBodySubspaceJJJTNLabels(J,T,g,N);
    two_body_jjjtn_state_labels = two_body_jjjt_state_labels;
  }

  void GatherOperatorTwoBodyJJJTNToTwoBodyJJJT(
      const basis::OperatorLabelsJT& operator_labels,
      const basis::TwoBodySpaceJJJTN& two_body_jjjtn_space,
      const std::array<basis::TwoBodySectorsJJJTN,3>& two_body_jjjtn_component_sectors,
      const std::array<basis::OperatorBlocks<double>,3>& two_body_jjjtn_component_matrices,
      const basis::TwoBodySpaceJJJT& two_body_jjjt_space,
      std::array<basis::TwoBodySectorsJJJT,3>& two_body_jjjt_component_sectors,
      std::array<basis::OperatorBlocks<double>,3>& two_body_jjjt_component_matrices
    )
  {
  for (int T0=operator_labels.T0_min; T0<=operator_labels.T0_max; ++T0)
    // for each isospin component
    {

      // enumerate sectors
      two_body_jjjt_component_sectors[T0]
        = basis::TwoBodySectorsJJJT(two_body_jjjt_space,operator_labels.J0,T0,operator_labels.g0);

      // populate matrices
      two_body_jjjt_component_matrices[T0].resize(two_body_jjjt_component_sectors[T0].size());
      for (std::size_t sector_index=0; sector_index<two_body_jjjt_component_sectors[T0].size(); ++sector_index)
        {
          // retrieve target sector
          const basis::TwoBodySectorsJJJT::SectorType& two_body_jjjt_sector
            = two_body_jjjt_component_sectors[T0].GetSector(sector_index);

          // initialize matrix
          Eigen::MatrixXd& two_body_jjjt_matrix = two_body_jjjt_component_matrices[T0][sector_index];
          two_body_jjjt_matrix = Eigen::MatrixXd::Zero(
              two_body_jjjt_sector.bra_subspace().size(),
              two_body_jjjt_sector.ket_subspace().size()
            );

          // populate matrix elements
          for (std::size_t bra_index = 0; bra_index < two_body_jjjt_sector.bra_subspace().size(); ++bra_index)
            for (std::size_t ket_index = 0; ket_index < two_body_jjjt_sector.ket_subspace().size(); ++ket_index)
              // for each target matrix element
              {

                // ensure canonical matrix element if diagonal sector
                if (two_body_jjjt_sector.IsDiagonal())
                  if (!(bra_index<=ket_index))
                    continue;

                // retrieve target states
                basis::TwoBodyStateJJJT two_body_jjjt_bra(two_body_jjjt_sector.bra_subspace(),bra_index);
                basis::TwoBodyStateJJJT two_body_jjjt_ket(two_body_jjjt_sector.ket_subspace(),ket_index);

                // extract source bra labels
                TwoBodySubspaceJJJTLabels two_body_jjjt_subspace_labels_bra
                  = two_body_jjjt_bra.subspace().labels();
                TwoBodyStateJJJTLabels two_body_jjjt_state_labels_bra
                  = two_body_jjjt_bra.labels();
                TwoBodySubspaceJJJTNLabels two_body_jjjtn_subspace_labels_bra;
                TwoBodyStateJJJTNLabels two_body_jjjtn_state_labels_bra;
                RecastLabelsTwoBodyJJJTToTwoBodyJJJTN(
                    two_body_jjjt_subspace_labels_bra,
                    two_body_jjjt_state_labels_bra,
                    two_body_jjjtn_subspace_labels_bra,
                    two_body_jjjtn_state_labels_bra
                  );

                // extract source bra indices
                std::size_t two_body_jjjtn_subspace_index_bra
                  = two_body_jjjtn_space.LookUpSubspaceIndex(
                      two_body_jjjtn_subspace_labels_bra
                    );
                std::size_t two_body_jjjtn_state_index_bra
                  = two_body_jjjtn_space.GetSubspace(two_body_jjjtn_subspace_index_bra).LookUpStateIndex(
                      two_body_jjjtn_state_labels_bra
                    );

                // extract source ket labels
                TwoBodySubspaceJJJTLabels two_body_jjjt_subspace_labels_ket
                  = two_body_jjjt_ket.subspace().labels();
                TwoBodyStateJJJTLabels two_body_jjjt_state_labels_ket
                  = two_body_jjjt_ket.labels();
                TwoBodySubspaceJJJTNLabels two_body_jjjtn_subspace_labels_ket;
                TwoBodyStateJJJTNLabels two_body_jjjtn_state_labels_ket;
                RecastLabelsTwoBodyJJJTToTwoBodyJJJTN(
                    two_body_jjjt_subspace_labels_ket,
                    two_body_jjjt_state_labels_ket,
                    two_body_jjjtn_subspace_labels_ket,
                    two_body_jjjtn_state_labels_ket
                  );

                // extract source ket indices
                std::size_t two_body_jjjtn_subspace_index_ket
                  = two_body_jjjtn_space.LookUpSubspaceIndex(
                      two_body_jjjtn_subspace_labels_ket
                    );
                std::size_t two_body_jjjtn_state_index_ket
                  = two_body_jjjtn_space.GetSubspace(two_body_jjjtn_subspace_index_ket).LookUpStateIndex(
                      two_body_jjjtn_state_labels_ket
                    );

                // look up matrix element
                std::size_t two_body_jjjtn_sector_index
                  = two_body_jjjtn_component_sectors[T0].LookUpSectorIndex(
                      two_body_jjjtn_subspace_index_bra,
                      two_body_jjjtn_subspace_index_ket
                    );

                const Eigen::MatrixXd& two_body_jjjtn_matrix
                  = two_body_jjjtn_component_matrices[T0][two_body_jjjtn_sector_index];
                double two_body_jjjtn_matrix_element = two_body_jjjtn_matrix(
                    two_body_jjjtn_state_index_bra,two_body_jjjtn_state_index_ket
                  );

                two_body_jjjt_matrix(bra_index,ket_index) = two_body_jjjtn_matrix_element;

              }
        }
    }

  }

  ////////////////////////////////////////////////////////////////
  // two-body JJJT operator output
  ////////////////////////////////////////////////////////////////

  void WriteTwoBodyOperatorComponentJJJT(
      std::ostream& os,
      int T0,
      const TwoBodySectorsJJJT& sectors,
      const OperatorBlocks<double>& matrices,
      basis::NormalizationConversion conversion_mode
    )
  {

    // iterate over sectors
    for (std::size_t sector_index = 0; sector_index < sectors.size(); ++sector_index)
      {

        // extract sector
        const typename TwoBodySectorsJJJT::SectorType& sector = sectors.GetSector(sector_index);
        const typename TwoBodySectorsJJJT::SubspaceType& bra_subspace = sector.bra_subspace();
        const typename TwoBodySectorsJJJT::SubspaceType& ket_subspace = sector.ket_subspace();

        // verify that sector is canonical
        //
        // This is a check that the caller's sector construction
        // followed the specification that only "upper triangle"
        // sectors are stored.
        assert(sector.bra_subspace_index()<=sector.ket_subspace_index());

        // iterate over matrix elements
        for (std::size_t bra_index=0; bra_index<bra_subspace.size(); ++bra_index)
          for (std::size_t ket_index=0; ket_index<ket_subspace.size(); ++ket_index)
            {

              // diagonal sector: restrict to upper triangle
              if (sector.IsDiagonal())
                if (!(bra_index<=ket_index))
                  continue;

              // define states
              const basis::TwoBodyStateJJJT bra(bra_subspace,bra_index);
              const basis::TwoBodyStateJJJT ket(ket_subspace,ket_index);

              // determine matrix element normalization factor
              double conversion_factor = 1.;
              if (conversion_mode == basis::NormalizationConversion::kASToNAS)
                {
                  if ((bra.N1()==bra.N2())&&(bra.l1()==bra.l2())&&(bra.j1()==bra.j2()))
                    conversion_factor *= (1/sqrt(2.));
                  if ((ket.N1()==ket.N2())&&(ket.l1()==ket.l2())&&(ket.j1()==ket.j2()))
                    conversion_factor *= (1/sqrt(2.));
                }
              else if (conversion_mode == basis::NormalizationConversion::kNASToAS)
                {
                  if ((bra.N1()==bra.N2())&&(bra.l1()==bra.l2())&&(bra.j1()==bra.j2()))
                    conversion_factor *= sqrt(2.);
                  if ((ket.N1()==ket.N2())&&(ket.l1()==ket.l2())&&(ket.j1()==ket.j2()))
                    conversion_factor *= sqrt(2.);
                }

              // extract matrix element
              const double matrix_element = conversion_factor*matrices[sector_index](bra_index,ket_index);

              // generate output line
              const int width = 3;
              const int precision = 8;  // for approximately single precision output
              os << std::setprecision(precision);
              os
                << " " << std::setw(width) << T0
                << " " << "  "
                << " " << std::setw(width) << bra.N1()
                << " " << std::setw(width) << bra.l1()
                << " " << std::showpoint << std::fixed << std::setprecision(1) << std::setw(4) << float(bra.j1())
                << " " << std::setw(width) << bra.N2()
                << " " << std::setw(width) << bra.l2()
                << " " << std::showpoint << std::fixed << std::setprecision(1) << std::setw(4) << float(bra.j2())
                << " " << std::setw(width) << bra.J()
                << " " << std::setw(width) << bra.T()
                << " " << std::setw(width) << bra.g()
                << " " << "    "
                << " " << std::setw(width) << ket.N1()
                << " " << std::setw(width) << ket.l1()
                << " " << std::showpoint << std::fixed << std::setprecision(1) << std::setw(4) << float(ket.j1())
                << " " << std::setw(width) << ket.N2()
                << " " << std::setw(width) << ket.l2()
                << " " << std::showpoint << std::fixed << std::setprecision(1) << std::setw(4) << float(ket.j2())
                << " " << std::setw(width) << ket.J()
                << " " << std::setw(width) << ket.T()
                << " " << std::setw(width) << ket.g()
                << " " << "    "
                << " " << std::showpoint << std::scientific << std::setprecision(precision) << matrix_element
                << std::endl;

            }

      }
  }

  ////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////
} // namespace


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file gaussian.hpp
 * \date October 2014
 * \author Jan Issac (jan.issac@gmail.com)
 * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <array>
#include <string>
#include <cstddef>
#include <type_traits>

#include <fl/util/traits.hpp>
#include <fl/exception/exception.hpp>
#include <fl/distribution/interface/moments.hpp>
#include <fl/distribution/interface/evaluation.hpp>
#include <fl/distribution/interface/standard_gaussian_mapping.hpp>

namespace fl
{

/**
 * \ingroup exceptions
 *
 * Exception used in case of accessing dynamic-size distribution attributes
 * without initializing the Gaussian using a dimension greater 0.
 */
class GaussianUninitializedException
    : public Exception
{
public:
    /**
     * Creates a GaussianUninitializedException
     */
    GaussianUninitializedException()
        : Exception("Accessing uninitialized dynamic-size distribution. "
                    "Gaussian dimension is 0. "
                    "Use ::dimension(dimension) to initialize the "
                    "distribution!") { }

    /**
     * \return Exception name
     */
    virtual std::string name() const noexcept
    {
        return "fl::GaussianUninitializedException";
    }
};

/**
 * \ingroup exceptions
 *
 * Exception representing a unsupported representation ID
 */
class InvalidGaussianRepresentationException
    : public Exception
{
public:
    /**
     * Creates an InvalidGaussianRepresentationException
     */
    InvalidGaussianRepresentationException()
        : Exception("Invalid Gaussian covariance representation") { }

    /**
     * \return Exception name
     */
    virtual std::string name() const noexcept
    {
        return "fl::InvalidGaussianRepresentationException";
    }
};

/**
 * \class Gaussian
 *
 * \brief General Gaussian Distribution
 * \ingroup distributions
 * \{
 *
 * The Gaussian is a general purpose distribution representing a multi-variate
 * \f${\cal N}(x; \mu, \Sigma)\f$. It can be used in various
 * ways while maintaining efficienty at the same time. This is due to it's
 * multi-representation structure. The distribution can be represented either by
 *
 *  - the covariance matrix \f$\Sigma\f$,
 *  - the precision matrix \f$\Sigma^{-1} = \Lambda\f$,
 *  - the covariance square root matrix (Cholesky decomposition or LDLT)
 *    \f$\sqrt{\Sigma} = L\sqrt{D}\f$,
 *  - or the diagonal form of the previous three options
 *    \f$diag(\sigma_1, \ldots, \sigma_n)\f$.
 *
 * A change in one representation results in change of all other
 * representations.
 *
 * Two key features of the distribution are its aibility to evaluation the
 * probability of a given sample and to map a noise sample into the distribution
 * sample space.
 *
 * \cond internal
 * The Gaussian internal structure uses lazy assignments or write on read
 * technique. Due to the multi-representation of the Gaussian, modifying one
 * representation affects all remaining ones. If one of the representation is
 * modified, the other representations are only then updated when needed. This
 * minimizes redundant computation and increases efficienty.
 * \endcond
 */
template <typename Variate>
class Gaussian
    : public Moments<Variate>,
      public Evaluation<Variate>,
      public StandardGaussianMapping<Variate, SizeOf<Variate>::Value>
{
private:
    typedef StandardGaussianMapping<
                Variate,
                SizeOf<Variate>::Value
            > StdGaussianMappingBase;

public:
    /**
     * \brief Second moment matrix type, i.e covariance matrix, precision
     *        matrix, and their diagonal and square root representations
     */
    typedef typename Moments<Variate>::SecondMoment SecondMoment;

    /**
     * \brief Represents the StandardGaussianMapping standard variate type which
     *        is of the same dimension as the Gaussian Variate. The
     *        StandardVariate type is used to sample from a standard normal
     *        Gaussian and map it to this Gaussian
     */
    typedef typename StdGaussianMappingBase::StandardVariate StandardVariate;

protected:
    /** \cond internal */
    /**
     * \enum Attribute
     * Implementation attributes. The enumeration lists the different
     * representations along with other properties such as the rank of the
     * second moment and the log normalizer.
     */
    enum Attribute
    {
        CovarianceMatrix = 0,     /**< Covariance mat. */
        PrecisionMatrix,          /**< Inverse of the cov. mat. */
        SquareRootMatrix,         /**< Cholesky decomp. of the cov. mat. */
        DiagonalCovarianceMatrix, /**< Diagonal form of the of cov. mat. */
        DiagonalPrecisionMatrix,  /**< Diagonal form of the inv cov. mat. */
        DiagonalSquareRootMatrix, /**< Diagonal form of the Cholesky decomp. */
        Rank,                     /**< Covariance Rank */
        Normalizer,               /**< Log probability normalizer */
        Determinant,              /**< Determinant of covariance */

        Attributes                /**< Total number of attribute */
    };

    /**
     * \brief Flags array type which contains the content status if different
     *        distribution representation
     */
    typedef std::array<bool, Attributes> FlagArray;
    /** \endcond */

public:
    /**
     * Creates a dynamic or fixed size Gaussian.
     *
     * \param dimension Dimension of the Gaussian. The default is defined by the
     *                  dimension of the variable type \em Vector. If the size
     *                  of the Variate at compile time is fixed, this will be
     *                  adapted. For dynamic-sized Variable the dimension is
     *                  initialized to 0.
     */
    explicit Gaussian(int dim = DimensionOf<Variate>()):
        StdGaussianMappingBase(dim)
    {
        static_assert(Variate::SizeAtCompileTime != 0,
                      "Illegal static dimension");

        std::fill(dirty_.begin(), dirty_.end(), true);
        set_standard();
    }

    /**
     * \brief Overridable default destructor
     */
    virtual ~Gaussian() noexcept { }

    /**
     * \return Gaussian dimension
     */
    virtual int dimension() const
    {
        return StdGaussianMappingBase::standard_variate_dimension();
    }

    /**
     * \return Gaussian first moment
     */
    virtual const Variate& mean() const
    {
        return mean_;
    }

    /**
     * \return Gaussian second centered moment
     *
     * Computes the covariance from other representation of not available
     *
     * \throws GaussianUninitializedException if the Gaussian is of dynamic-size
     *         and has not been initialized using SetStandard(dimension).
     * \throws InvalidGaussianRepresentationException if non-of theember function)
     *         representation can be used as a source
     */
    virtual const SecondMoment& covariance() const
    {
        if (dimension() == 0)
        {
            fl_throw(GaussianUninitializedException());
        }

        if (is_dirty(CovarianceMatrix) && is_dirty(DiagonalCovarianceMatrix))
        {
            switch (select_first_representation<4>({{DiagonalSquareRootMatrix,
                                                    DiagonalPrecisionMatrix,
                                                    SquareRootMatrix,
                                                    PrecisionMatrix}}))
            {
            case SquareRootMatrix:
                covariance_ = square_root_ * square_root_.transpose();
                break;

            case PrecisionMatrix:
                covariance_ = precision_.inverse();
                break;

            case DiagonalSquareRootMatrix:
                covariance_.setZero(dimension(), dimension());
                for (int i = 0; i < square_root_.diagonalSize(); ++i)
                {
                    covariance_(i, i) = square_root_(i, i) * square_root_(i, i);
                }
                break;

            case DiagonalPrecisionMatrix:
                covariance_.setZero(dimension(), dimension());
                for (int i = 0; i < precision_.diagonalSize(); ++i)
                {
                    covariance_(i, i) = 1./precision_(i, i);
                }
                break;

            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(CovarianceMatrix);
        }

        return covariance_;
    }

    /**
     * \return Gaussian second centered moment in the precision form (inverse
     * of the covariance)
     *
     * Computes the precision from other representation of not available
     *
     * \throws GaussianUninitializedException if the Gaussian is of dynamic-size
     *         and has not been initialized using SetStandard(dimension).
     * \throws InvalidGaussianRepresentationException if non-of the
     *         representation can be used as a source
     */
    virtual const SecondMoment& precision() const
    {
        if (dimension() == 0)
        {
            fl_throw(GaussianUninitializedException());
        }

        if (is_dirty(PrecisionMatrix) && is_dirty(DiagonalPrecisionMatrix))
        {
            const SecondMoment& cov = covariance();

            switch (select_first_representation<4>({{DiagonalCovarianceMatrix,
                                                    DiagonalSquareRootMatrix,
                                                    CovarianceMatrix,
                                                    SquareRootMatrix}}))
            {
            case CovarianceMatrix:
            case SquareRootMatrix:
                precision_ = covariance().inverse();
                break;

            case DiagonalCovarianceMatrix:
            case DiagonalSquareRootMatrix:
                precision_.setZero(dimension(), dimension());
                for (int i = 0; i < cov.rows(); ++i)
                {
                    precision_(i, i) = 1./cov(i, i);
                }
                break;

            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(PrecisionMatrix);
        }

        return precision_;
    }

    /**
     * \return Gaussian second centered moment in the square root form (
     * Cholesky decomposition)
     *
     * Computes the square root from other representation of not available
     *
     * \throws GaussianUninitializedException if the Gaussian is of dynamic-size
     *         and has not been initialized using SetStandard(dimension).
     * \throws InvalidGaussianRepresentationException if non-of the
     *         representation can be used as a source
     */
    virtual const SecondMoment& square_root() const
    {
        if (dimension() == 0)
        {
            fl_throw(GaussianUninitializedException());
        }

        if (is_dirty(SquareRootMatrix) && is_dirty(DiagonalSquareRootMatrix))
        {
            const SecondMoment& cov = covariance();

            switch (select_first_representation<4>({{DiagonalCovarianceMatrix,
                                                    DiagonalPrecisionMatrix,
                                                    CovarianceMatrix,
                                                    PrecisionMatrix}}))
            {
            case CovarianceMatrix:
            case PrecisionMatrix:
                fl::square_root(cov, square_root_);
                break;

            case DiagonalCovarianceMatrix:
            case DiagonalPrecisionMatrix:
                square_root_.setZero(dimension(), dimension());
                for (int i = 0; i < square_root_.rows(); ++i)
                {
                    square_root_(i, i) = std::sqrt(cov(i, i));
                }
                break;

            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(SquareRootMatrix);
        }

        return square_root_;
    }

    /**
     * \return True if the covariance matrix has a full rank
     *
     * \throws see covariance()
     */
    virtual bool has_full_rank() const
    {
        if (is_dirty(Rank))
        {
            full_rank_ =
               covariance().colPivHouseholderQr().rank() == covariance().rows();

            updated_internally(Rank);
        }

        return full_rank_;
    }

    /**
     * \return Log normalizing constant
     *
     * \throws see has_full_rank()
     */
    virtual Real log_normalizer() const
    {
        if (is_dirty(Normalizer))
        {
            if (has_full_rank())
            {
                log_norm_ =
                    -0.5
                    * (std::log(covariance_determinant())
                       + Real(dimension()) * std::log(2.0 * M_PI));
            }
            else
            {
                log_norm_ = 0.0; // FIXME
            }

            updated_internally(Normalizer);
        }

        return log_norm_;
    }

    /**
     * \return Covariance determinant
     *
     * \throws see covariance
     */
    virtual Real covariance_determinant() const
    {
        if (is_dirty(Determinant))
        {
            determinant_ = covariance().determinant();

            updated_internally(Determinant);
        }

        return determinant_;
    }

    /**
     * \return Log of the probability of the given sample \c vector
     *
     * \param vector sample which should be evaluated
     *
     * \throws see has_full_rank()
     */
    Real log_probability(const Variate& vector) const override
    {
        // assert(has_full_rank());

        if(has_full_rank())
        {
            return log_normalizer() - 0.5
                    * (vector - mean()).transpose()
                    * precision()
                    * (vector - mean());
        }

        return -std::numeric_limits<Real>::infinity();
    }

    /**
     * \return a Gaussian sample of the type \c Variate determined by mapping a
     * noise sample into the Gaussian sample space
     *
     * \param sample    Noise Sample
     *
     * \throws see square_root()
     */
    Variate map_standard_normal(const StandardVariate& sample) const override
    {
        return mean() + square_root() * sample;
    }

    /**
     * Sets the Gaussian to a standard distribution with zero mean and identity
     * covariance.
     */
    virtual void set_standard()
    {
        mean_.resize(dimension());
        covariance_.resize(dimension(), dimension());
        precision_.resize(dimension(), dimension());
        square_root_.resize(dimension(), dimension());

        mean(Variate::Zero(dimension()));
        covariance(SecondMoment::Identity(dimension(), dimension()));

        full_rank_ = true;
        updated_internally(Rank);
    }

    /**
     * Changes the dimension of the dynamic-size Gaussian and sets it to a
     * standard distribution with zero mean and identity covariance.
     *
     * \param new_dimension New dimension of the Gaussian
     *
     * \throws ResizingFixedSizeEntityException
     *         see GaussianMap::standard_variate_dimension(int)
     */
    virtual void dimension(int new_dimension)
    {
        StdGaussianMappingBase::standard_variate_dimension(new_dimension);
        set_standard();
    }

    /**
     * Sets the mean
     *
     * \param mean New Gaussian mean
     *
     * \throws WrongSizeException
     */
    virtual void mean(const Variate& mean) noexcept
    {
        if (mean_.size() != mean.size())
        {
            fl_throw(fl::WrongSizeException(mean.size(), mean_.size()));
        }

        mean_ = mean;
    }

    /**
     * Sets the covariance matrix
     * \param covariance New covariance matrix
     *
     * \throws WrongSizeException
     */

    virtual void covariance(const SecondMoment& covariance)
    {
        if (covariance_.size() != covariance.size())
        {
            fl_throw(fl::WrongSizeException(
                         covariance.size(), covariance_.size()));
        }

        covariance_ = covariance;
        updated_externally(CovarianceMatrix);
    }

    /**
     * Sets the covariance matrix in the form of its square root (Cholesky f
     * actor)
     *
     * \param square_root New covariance square root
     *
     * \throws WrongSizeException
     */
    virtual void square_root(const SecondMoment& square_root)
    {
        if (square_root_.size() != square_root.size())
        {
            fl_throw(fl::WrongSizeException(
                         square_root.size(), square_root_.size()));
        }

        square_root_ = square_root;
        updated_externally(SquareRootMatrix);
    }

    /**
     * Sets the covariance matrix in the precision form (inverse of covariance)
     *
     * \param precision New precision matrix
     *
     * \throws WrongSizeException
     */
    virtual void precision(const SecondMoment& precision) noexcept
    {
        if (precision_.size() != precision.size())
        {
            fl_throw(fl::WrongSizeException(
                         precision.size(), precision_.size()));
        }

        precision_ = precision;
        updated_externally(PrecisionMatrix);
    }

    /**
     * Sets the covariance matrix as a diagonal matrix
     *
     * \param diag_covariance New diagonal covariance matrix
     *
     * \throws WrongSizeException
     */
    virtual void diagonal_covariance(const SecondMoment& diag_covariance)
    {
        if (diag_covariance.size() != covariance_.size())
        {
            fl_throw(
                fl::WrongSizeException(
                    diag_covariance.size(), covariance_.size()));
        }

        covariance_ = diag_covariance.diagonal().asDiagonal();
        updated_externally(DiagonalCovarianceMatrix);
    }

    /**
     * Sets the covariance matrix in its diagonal square root form
     *
     * \param diag_square_root New diagonal square root of the covariance
     *
     * \throws WrongSizeException
     */
    virtual void diagonal_square_root(const SecondMoment& diag_square_root)
    {
        if (diag_square_root.size() != square_root_.size())
        {
            fl_throw(
                fl::WrongSizeException(
                    diag_square_root.size(), square_root_.size()));
        }

        square_root_ = diag_square_root.diagonal().asDiagonal();
        updated_externally(DiagonalSquareRootMatrix);
    }

    /**
     * Sets the covariance matrix in its diagonal precision form
     *
     * \param diag_precision New diagonal precision matrix
     *
     * \throws WrongSizeException
     */
    virtual void diagonal_precision(const SecondMoment& diag_precision)
    {
        if (diag_precision.size() != precision_.size())
        {
            fl_throw(
                fl::WrongSizeException(
                    diag_precision.size(), precision_.size()));
        }

        precision_ = diag_precision.diagonal().asDiagonal();
        updated_externally(DiagonalPrecisionMatrix);
    }

    /**
     * \brief Returns true if the specified Gaussian is close to this one w.r.t.
     *        \a an epsilon.
     *
     * \throws See covariance() and mean()
     */
    template <typename OtherVariate>
    bool is_approx(const Gaussian<OtherVariate>& other)
    {

        return fl::are_similar(mean(), other.mean()) &&
               fl::are_similar(covariance(), other.covariance());
    }

    /**
     * \brief Returns true if the specified Gaussian is close to this one w.r.t.
     *        \a an epsilon.
     *
     * \throws See covariance() and mean()
     */
    template <typename OtherVariate>
    bool is_approx(const Gaussian<OtherVariate>& other,
                   Real eps = 1.e-9,
                   bool scale_with_size = false)
    {
        Real eps_mean = eps;
        Real eps_cov = eps;

        if (scale_with_size)
        {
            eps_mean *= mean().size();
            eps_cov *= covariance().size();
        }

        return fl::are_similar(mean(), other.mean(), eps_mean) &&
               fl::are_similar(covariance(), other.covariance(), eps_cov);
    }

protected:
    /** \cond internal */
    /**
     * Flags the specified attribute as valid and the rest of attributes as
     * dirty.
     *
     * \param attribute Modified attribute
     */
    void updated_externally(Attribute attribute) const noexcept
    {
        std::fill(dirty_.begin(), dirty_.end(), true);
        updated_internally(attribute);
    }

    /**
     * Flags the specified attribute as valid.
     *
     * \param attribute Modified attribute
     */
    void updated_internally(Attribute attribute) const noexcept
    {
        dirty_[attribute] = false;
    }

    /**
     * \return True if any of the other representation was modified.
     * \param attribute     Attribute in question
     */
    bool is_dirty(Attribute attribute) const noexcept
    {
        return dirty_[int(attribute)];
    }

    /**
     * \return First representation ID that is available
     *
     * \param representations   Representation list
     *
     * Example:
     * If the last invoked functions were
     *
     * \code
     * diagonal_covariance(my_diagonal);
     * my_covariance = covariance();
     * \endcode
     *
     * Now, the representation is set to \c DiagonalCovarianceMatrix and
     * \c CovarianceMatrix since \c diagonal_covariance() was used to set the
     * covariance matrix followed by requesting \c covariance().
     * The following subsequent call
     *
     * \code
     * Attribute att = SelectRepresentation({SquareRoot,
     *                                       DiagonalCovarianceMatrix,
     *                                       CovarianceMatrix});
     * \endcode
     *
     * will assign att to DiagonalCovarianceMatrix since that is the first
     * available representation within the initializer-list
     * <tt>{#SquareRoot, #DiagonalCovarianceMatrix, #CovarianceMatrix}</tt>.
     *
     * This method is used to determine the best suitable representation
     * for conversion. It is recommanded to put the diagonal forms at the
     * beginning of the initialization-list. Diagonal forms can be converted
     * most efficiently other  representations.
     */
    template <int AttributeCount>
    Attribute select_first_representation(
        const std::array<Attribute, AttributeCount>& representations
    ) const noexcept
    {
        for (auto& rep: representations)  if (!is_dirty(rep)) return rep;
        return Attributes;
    }
    /** \endcond */

protected:
    /** \cond internal */
    Variate mean_;                     /**< \brief first moment vector */
    mutable SecondMoment covariance_;  /**< \brief cov. form */
    mutable SecondMoment precision_;   /**< \brief cov. inverse form */
    mutable SecondMoment square_root_; /**< \brief cov. square root form */
    mutable bool full_rank_;           /**< \brief full rank flag */
    mutable Real log_norm_;            /**< \brief log normalizing constant */
    mutable Real determinant_;         /**< \brief determinant of covariance */
    mutable FlagArray dirty_;          /**< \brief data validity flags */
    /** \endcond */
};

/** \} */

}


//   -----------------------------------------------------------------------------------------------
//    Copyright 2015 André Bergner. Distributed under the Boost Software License, Version 1.0.
//     (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//      --------------------------------------------------------------------------------------------

#include <iostream>
#include <boost/mpl/int.hpp>
#include <boost/proto/core.hpp>
#include <boost/proto/transform.hpp>

#include "callable_decltype.hpp"
#include "tuple_tools.hpp"

namespace mpl = boost::mpl;
namespace proto = boost::proto;

//  This demo shows how haskell like self-enumerating placeholders (i.e. having a
//  generic placeholder _ that increments the input argument index on each usage)
//  can be implemented in boost.proto by passing along different transform states
//  on each branch of a binary operator.

struct placeholder {};

namespace transforms
{
   using namespace proto;

   template < typename Op >
   struct count_node_impl
   {
      struct apply : or_
      <  when
         <  Op
         ,  fold<_, mpl::int_<1>(), mpl::plus<apply, _state>()>
         >
      ,  when
         <  nary_expr<_, vararg<_>>
         ,  fold<_, mpl::int_<0>(), mpl::plus<apply, _state>()>
         >
      ,  otherwise< mpl::int_<0>() >
      >
      {};
   };

   template < typename Node >
   struct count_node_impl< terminal<Node> >
   {
      struct apply : or_
      <  when
         <  terminal< placeholder >
         ,  mpl::int_<1>()
         >
      ,  when
         <  nary_expr<_, vararg<_>>
         ,  fold<_, mpl::int_<0>(), mpl::plus<apply, _state>()>
         >
      ,  otherwise< mpl::int_<0>() >
      >
      {};
   };

   template < typename Node >
   using count_node = typename count_node_impl<Node>::apply;

   struct place_the_holder : callable_decltype
   {
      template< typename State >
      auto operator()( placeholder const & , State const & ) const
      {
         return  State::value;
      }
   };

   struct nary_eval;

   struct evaluate : or_
   <  when
      <  terminal< placeholder >
      ,  place_the_holder( _value , _state )
      >
   ,  when
      <  nary_expr<_, vararg<_>>
      ,  nary_eval( _expr, _state )
      >
   ,  when
      <  _
      ,  _default< evaluate >
      >
   >
   {};

   struct nary_eval : callable_decltype
   {
      template< typename Expr , typename State , std::size_t... Ns >
      auto apply( std::index_sequence<Ns...> , Expr const & x , State const & s ) const
      {
         using  tag = typename tag_of<Expr>::type;
         using  node_t = terminal<placeholder>;

         auto       n = std::make_tuple( count_node<node_t>{}(child_c<Ns>(x))... );
         evaluate   e;
         _default<> f;

         auto add = [](auto s, auto x) { return typename mpl::plus<decltype(s),decltype(x)>::type{} ; };
         auto m = std::tuple_cat( std::make_tuple(s) , scan( n, s, add ));

         return f(make_expr<tag>( e( child_c<Ns>(x) , std::get<Ns>(m) )... ));
      }

      template< typename Expr, typename State >
      auto operator()( Expr const & e , State const & s ) const
      {
         using idx_seq = std::make_index_sequence< proto::arity_of<Expr>::value >;
         return apply( idx_seq{}, e , s );
      }
   };

}

using  transforms :: evaluate;

const proto::terminal< placeholder >::type   _  = {{}};

int main()
{
   using namespace std;

   auto e = [](auto expr) { return evaluate{}( expr , mpl::int_<0>{} ); };

   std::cout << transforms:: count_node< proto::multiplies<proto::_,proto::_> >{}( 0 +_* _+_ *_*_[0] ) << std::endl;
   std::cout << transforms:: count_node< proto::terminal<placeholder> >{}( 0 +_* _+_ *_*_[0] ) << std::endl;

   std::cout << "--- expecing 0 ---" << std::endl;
   cout << e( (1*_ + 0*_) + 0*_ ) << endl;
   cout << e( 1*_ + (0*_ + 0*_) ) << endl;

   std::cout << "--- expecing 1 ---" << std::endl;
   cout << e( (0*_ + 1*_) + 0*_ ) << endl;
   cout << e( 0*_ + (1*_ + 0*_) ) << endl;

   std::cout << "--- expecing 2 ---" << std::endl;
   cout << e( (0*_ + 0*_) + 1*_ ) << endl;
   cout << e( 0*_ + (0*_ + 1*_) ) << endl;

   std::cout << "--- expecing 3 ---" << std::endl;
   cout << e( (1*_ + 0*_) + (0*_ + 1*_) ) << endl;
   cout << e( (1*_ + 0*_ + 0*_) + 1*_ ) << endl;
   cout << e( 1*_ + (0*_ + 0*_ + 1*_) ) << endl;
}


#include <math.h>
#include <stdio.h>

#include <boost/math/quaternion.hpp>

extern "C" void externalForces(double t, double *f, unsigned int size_z, double *z)
{
  f[0]=0;
  f[1]=0;
  f[2]=0;

}

extern "C" void externalMoment(double t,double *m, unsigned int size_z, double *z)
{
  m[0]=0;
  m[1]=0;
  m[2]=0;
}

extern "C" void fInt_beam1(double t, double *q, double *v, double *f, unsigned int size_z, double *z)
{
  // printf("fInt_beam1\n");
  // printf("q[0] = %e\t q[1] = %e\t, q[2]=%e\n",q[0],q[1],q[2]);
  f[0]=1e4*q[0];
  f[1]=0.0;
  f[2]=1e4*q[2];
  // printf("f[0] = %e\t f[1] = %e\t, f[2]=%e\n",f[0],f[1],f[2]);

}
extern "C" void jacobianFIntq_beam1(double t, double *q, double *v, double *jac, unsigned int size_z, double *z)
{
  for(int i =0; i < 3; i++)
  {
    for(int j=0; j<7; j++)
      jac[i+j*3]=0.0;
  }
  jac[0+0*3]=1e4;
  jac[2+2*3]=1e4;
}

extern "C" void jacobianFIntv_beam1(double t, double *q, double *v, double *jac, unsigned int size_z, double *z)
{
  for(int i =0; i < 3; i++)
  {
    for(int j=0; j< 6; j++)
      jac[i+j*3]=0.0;
  }
}

extern "C" void mInt_beam1(double t, double *q, double *v, double *m, unsigned int size_z, double *z)
{
  // printf("mInt_beam1 :\n");

  // Simple torsional spring around y axis
  // printf("q[0] = %e\n", q[0]);
  // printf("q[1] = %e\n", q[1]);
  // printf("q[2] = %e\n", q[2]);
  // printf("q[3] = %e\n", q[3]);
  // printf("q[4] = %e\n", q[4]);
  // printf("q[5] = %e\n", q[5]);
  // printf("q[6] = %e\n", q[6]);

  double angle = 2*asin(q[5]);
  // printf("angle = %e\n", angle);
  m[0]=0.0;
  m[1]=1e3*(angle-1.0);
  m[2]=0.0;
  // printf("m[0] = %e\t m[1] = %e\t, m[2]=%e\n",m[0],m[1],m[2]);
}

extern "C" void jacobianMIntq_beam1(double t, double *q, double *v, double *jac, unsigned int size_z, double *z)
{
  // printf("jacobianMIntq_beam1:\n ");
  for(int i =0; i < 3; i++)
  {
    for(int j=0; j<7; j++)
      jac[i+j*3]=0.0;
  }
  // printf("q[0] = %e\n", q[0]);
  // printf("q[1] = %e\n", q[1]);
  // printf("q[2] = %e\n", q[2]);
  // printf("q[3] = %e\n", q[3]);
  // printf("q[4] = %e\n", q[4]);
  // printf("q[5] = %e\n", q[5]);
  // printf("q[6] = %e\n", q[6]);

  // double angle = 2*asin(q[5]);

  // printf("angle = %e\n", angle);
  jac[1+5*3]=1e3 * 2.0 / sqrt(1 - q[5]*q[5]) ;
  //printf("jac[3+3*3] = %e\n", jac[3+3*3]);
  // printf("[(");
  // for (int i =0; i < 3; i++)
  // {
  //   for (int j=0; j<7; j++) printf("%e,\t",jac[i+j*3]);
  //   printf("\n");
  // }
  // printf(")]\n");
  // // Computation by finite difference
  // double epsilon = 1e-8;
  // double vector[7];
  // double m1[3],m2[3];

  // for (int j =0; j < 7; j++)
  // {
  //   for (int k =0; k < 7; k++) vector[k] =q[k];
  //   vector[j] += epsilon;
  //   mInt_beam1(t, q, v, m1,  size_z, z);
  //   mInt_beam1(t, vector, v, m2,  size_z, z);
  //   jac[0+j*3]=(m2[0]-m1[0])/epsilon ;
  //   jac[1+j*3]=(m2[1]-m1[1])/epsilon ;
  //   jac[2+j*3]=(m2[2]-m1[2])/epsilon ;
  // }

  // for (int i =0; i < 3; i++)
  // {
  //   for (int j=0; j<7; j++) printf("%e\t",jac[i+j*3]);
  //   printf("\n");
  // }

}

extern "C" void jacobianMIntv_beam1(double t, double *q, double *v, double *jac, unsigned int size_z, double *z)
{
  for(int i =0; i < 3; i++)
  {
    for(int j=0; j< 6; j++)
      jac[i+j*3]=0.0;
  }
}


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.
// Simple tests for popsolver.
//
#include <popsolver/Model.hpp>
#define BOOST_TEST_MODULE Simple
#include <boost/test/unit_test.hpp>

using namespace popsolver;

BOOST_AUTO_TEST_CASE(NoConstraints) {
  Model m;
  auto a = m.addVariable(5, 10);
  auto s = m.minimize(a);
  BOOST_CHECK_EQUAL(s[a], DataType{5});
}

BOOST_AUTO_TEST_CASE(Unsatisfiable) {
  Model m;
  auto a = m.addVariable(2, 5);
  m.lessOrEqual(a, DataType{1});
  BOOST_CHECK_EQUAL(m.minimize(a).validSolution(), false);
}

BOOST_AUTO_TEST_CASE(MultiObjective) {
  Model m;
  auto a = m.addVariable(1, 10);
  auto b = m.addVariable(1, 10);
  m.lessOrEqual(DataType{5}, m.sum({a, b}));
  auto s1 = m.minimize({a, b});
  BOOST_CHECK_EQUAL(s1[a], DataType{1});
  BOOST_CHECK_EQUAL(s1[b], DataType{4});
  auto s2 = m.minimize({b, a});
  BOOST_CHECK_EQUAL(s2[a], DataType{4});
  BOOST_CHECK_EQUAL(s2[b], DataType{1});
}


/*
 * Copyright (c) CERN 2017
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "VoShares.h"
#include "db/generic/SingleDbInstance.h"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/discrete_distribution.hpp>
#include <common/Logger.h>

using namespace db;
using namespace fts3::common;

boost::mt19937 generator;

namespace fts3 {
namespace server {

/**
 * Given the pair, a list of vos for the pair, and a list of weights for VOs, pick one
 * based on those weights.
 * @note If a VO is not on the map, it will fallback to 'public', if it is there
 * @note If the weight for a VO/public is 0, it will never be picked!
 */
boost::optional<QueueId> selectQueueForPair(const Pair &pair,
    const std::vector<std::pair<std::string, unsigned>> &vos,
    const std::map<std::string, double> &weights,
    std::vector<QueueId> &unschedulable)
{
    // Weights per position in vos vector
    std::vector<double> finalWeights(vos.size());
    int unschedulableCount = 0;

    // Get the public (catchall weight)
    // If there is no config, this is the only weight!
    double publicWeight = 0;
    if (weights.empty()) {
        publicWeight = 1;
    }
    else {
        auto publicIter = weights.find("public");
        if (publicIter != weights.end()) {
            publicWeight = publicIter->second;
        }
    }

    // Need to calculate how many "public" there are, so we can split
    int publicCount = 0;
    for (auto i = vos.begin(); i != vos.end(); ++i) {
        if (weights.find(i->first) == weights.end()) {
            ++publicCount;
        }
    }
    if (publicCount > 0) {
        publicWeight /= static_cast<double>(publicCount);
    }

    // Second pass, fill up the weights
    int pos = 0;
    for (auto i = vos.begin(); i != vos.end(); ++i, ++pos) {
        auto wIter = weights.find(i->first);
        if (wIter == weights.end()) {
            finalWeights[pos] = publicWeight;
        }
        else {
            finalWeights[pos] = wIter->second;
        }
        if (finalWeights[pos] <= 0) {
            unschedulable.emplace_back(pair.source, pair.destination, i->first, i->second);
            ++unschedulableCount;
        }
    }

    if (unschedulableCount == vos.size()) {
        return boost::optional<QueueId>();
    }

    // And pick one at random
    boost::random::discrete_distribution<> dist(finalWeights);
    int chosen = dist(generator);
    return QueueId(pair.source, pair.destination, vos[chosen].first, vos[chosen].second);
}

std::vector<QueueId> applyVoShares(const std::vector<QueueId> queues, std::vector<QueueId> &unschedulable)
{
    // Vo list for each pair
    std::map<Pair, std::vector<std::pair<std::string, unsigned>>> vosPerPair;

    for (auto i = queues.begin(); i != queues.end(); ++i) {
        vosPerPair[Pair(i->sourceSe, i->destSe)].push_back(std::make_pair(i->voName, i->activeCount));
    }

    // One VO per pair
    std::vector<QueueId> result;
    for (auto j = vosPerPair.begin(); j != vosPerPair.end(); ++j) {
        const Pair &p = j->first;
        const std::vector<std::pair<std::string, unsigned>> &vos = j->second;
        std::vector<ShareConfig> shares = DBSingleton::instance().getDBObjectInstance()->getShareConfig(p.source, p.destination);

        std::map<std::string, double> weights;
        for (auto k = shares.begin(); k != shares.end(); ++k) {
            weights[k->vo] = k->weight;
        }

        boost::optional<QueueId> chosen = selectQueueForPair(p, vos, weights, unschedulable);

        if (chosen) {
            FTS3_COMMON_LOGGER_NEWLOG(DEBUG) << "Chosen " << chosen->voName << " for " << p << commit;
            FTS3_COMMON_LOGGER_NEWLOG(DEBUG) << "Options were " << commit;
            for (auto i = vos.begin(); i != vos.end(); ++i) {
                FTS3_COMMON_LOGGER_NEWLOG(DEBUG) << "\t" << i->first << commit;
            }

            result.emplace_back(chosen.get());
        }
        else {
            FTS3_COMMON_LOGGER_NEWLOG(INFO) << "None chosen for " << p << commit;
        }
    }
    return result;
}

}
}


#include <iostream>
#include <iomanip>
#include <map>
#include <string>
#include <fstream>
#include <locale>
#include <vector>
#include <thread>
#include <boost/program_options.hpp>
#include <boost/program_options/errors.hpp>
#include <boost/log/core.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/expressions.hpp>
//#include <boost/log/utility/setup/file.hpp>
#include <boost/dynamic_bitset.hpp>
#include <iostream>
#include "timer.h"
#include <mpi.h>

namespace po = boost::program_options;
namespace logging = boost::log;

uint8_t* A;
uint8_t* x;
uint8_t* my_A;
int* Ax;
int* my_Ax;

int main(int argc, const char *argv[]) {
    unsigned int totalCores = std::thread::hardware_concurrency();
    double start, finish;
    int seed = 0;
    int mod = 10;
    int dim_size = 5;
    int comm_sz;    // number of processes
    int my_rank;    // my process rank

    MPI_Init(NULL, NULL);
    MPI_Comm_size(MPI_COMM_WORLD, &comm_sz);
    MPI_Comm_rank(MPI_COMM_WORLD, &my_rank);

    // Set log level
	logging::core::get()->set_filter(logging::trivial::severity >= logging::trivial::severity_level::info);

    // let's get some of the data from the users
    try
    {
        po::options_description desc("Allowed options");
        desc.add_options()
            ("help,h", "Show the usage/help screen")
            ("seed,s", po::value<int>()->default_value(0), "Seed used to fill matrix and array")
            ("mod,m", po::value<int>()->default_value(10), "Mod used to bound upper size of values in matrix/array")
            ("size,n", po::value<int>()->default_value(5), "Size length of matrix sizes and vector length");

        po::positional_options_description pos;
        pos.add("seed", 1);
        pos.add("mod", 1);
        pos.add("size", 1);

        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(desc).positional(pos).run(), vm);
        po::notify(vm);

        if (vm.count("help")) {
            std::cout << desc << '\n';
            return EXIT_SUCCESS;
        }
        else {
            if (vm.count("seed")){
                seed = vm["seed"].as<int>();
            }
            if (vm.count("mod")) {
                mod = vm["mod"].as<int>();
            }
            if (vm.count("size")) {
                dim_size = vm["size"].as<int>();
            }
        }
    }
    catch (const po::error &ex)
    {
        std::cerr << ex.what() << '\n';
    }

    // allocating x
    x = (uint8_t*)malloc(sizeof(uint8_t)*dim_size);
    int total_size = dim_size * dim_size;
    my_A = (uint8_t*)malloc(sizeof(uint8_t)*total_size/comm_sz);
    int rank_dim_size = dim_size/comm_sz;

    if (my_rank == 0) {
        // ensure we can split evenly
        if (dim_size % comm_sz != 0) {
            BOOST_LOG_TRIVIAL(error) << "Size must be evenly divisible by the number of requested processors";
            BOOST_LOG_TRIVIAL(error) << "Size: " << dim_size << " Processors Requested: " << comm_sz;
            BOOST_LOG_TRIVIAL(error) << "Exiting.";
            MPI_Finalize();
            return EXIT_FAILURE;
        }

        BOOST_LOG_TRIVIAL(info) << "Matrix-Vector Multiplication (MPI)";
        BOOST_LOG_TRIVIAL(info) << "Cores available:  " << totalCores;
        BOOST_LOG_TRIVIAL(info) << "MPI World Size:  " << comm_sz;
        BOOST_LOG_TRIVIAL(info) << "MPI Process Rank:  " << my_rank;
        BOOST_LOG_TRIVIAL(info) << "Using Seed Value: " << seed;
        BOOST_LOG_TRIVIAL(info) << "Using Mod Value: " << mod;
        BOOST_LOG_TRIVIAL(info) << "Using Size Value: " << dim_size;

        BOOST_LOG_TRIVIAL(info) << "Creating Matrix...";
        A = (uint8_t*)malloc(sizeof(uint8_t)*total_size);
        int current = seed;
        for (int i = 0; i < total_size; ++i) {
            current++;
            A[i] = current % mod;
        }

        BOOST_LOG_TRIVIAL(info) << "Creating Vector...";
        current = seed;
        for (int i = 0; i < dim_size; ++i) {
            current++;
            x[i] = current % mod;
        }

        // prep the answer/result
        Ax = (int*)malloc(sizeof(int)*dim_size);
        for (int i = 0; i < dim_size; ++i) {
            Ax[i] = 0;
        }

        GET_TIME(start);
    }

    MPI_Barrier(MPI_COMM_WORLD);
    MPI_Bcast(x, dim_size, MPI_UINT8_T, 0, MPI_COMM_WORLD);
    MPI_Scatter(A, total_size/comm_sz, MPI_UINT8_T, my_A, total_size/comm_sz, MPI_UINT8_T, 0, MPI_COMM_WORLD);

    my_Ax = (int*)malloc(sizeof(int)*rank_dim_size);

    for (int i = 0; i < rank_dim_size; ++i) {
        int cell_ans = 0;
        for (int j = 0; j < dim_size; ++j) {
            cell_ans += (my_A[(i*dim_size)+j] * x[j]);
        }
        my_Ax[i] = cell_ans;
    }

    if (my_rank != 0) {
        // send my_Ax to rank 0
        MPI_Send(my_Ax, rank_dim_size, MPI_INT, 0, 0, MPI_COMM_WORLD);
    } else {
        // update Ax with rank 0 data (local)
        for (int i = 0; i < rank_dim_size; ++i) {
            Ax[i] = my_Ax[i];
        }

        // receive from each & append/update Ax
        for (int i = 1; i < comm_sz; ++i) {
            MPI_Recv(my_Ax, rank_dim_size, MPI_INT, i, 0, MPI_COMM_WORLD, MPI_STATUS_IGNORE);

            for (int j = 0; j < rank_dim_size; ++j) {
                Ax[(i*rank_dim_size) + j] = my_Ax[j];
            }
        }

        // sum
        BOOST_LOG_TRIVIAL(info) << "Summing Results...";
        long summed_answer = 0;
        for (int i = 0; i < dim_size; ++i) {
            summed_answer += Ax[i];
        }

        GET_TIME(finish);

        // be done.
        BOOST_LOG_TRIVIAL(info) << "Displaying Results:";
        BOOST_LOG_TRIVIAL(info) << "Summed Result: " << summed_answer;
        BOOST_LOG_TRIVIAL(info) << "Time Elapsed...";
        printf("Elapsed time = %e seconds\n", finish - start);
    }

    // clean up
    free(A);
    free(my_A);
    free(x);
    free(Ax);
    free(my_Ax);

    MPI_Finalize();

    return EXIT_SUCCESS;
}


// copyright (c) 2011-2013 the dzcoin core developers
// distributed under the mit software license, see the accompanying
// file copying or http://www.opensource.org/licenses/mit-license.php.
#include "arith_uint256.h"
#include "uint256.h"
#include "version.h"
#include "test/test_dzcoin.h"

#include <boost/test/unit_test.hpp>
#include <stdint.h>
#include <sstream>
#include <iomanip>
#include <limits>
#include <cmath>
#include <string>
#include <stdio.h>

boost_fixture_test_suite(uint256_tests, basictestingsetup)

const unsigned char r1array[] =
    "\x9c\x52\x4a\xdb\xcf\x56\x11\x12\x2b\x29\x12\x5e\x5d\x35\xd2\xd2"
    "\x22\x81\xaa\xb5\x33\xf0\x08\x32\xd5\x56\xb1\xf9\xea\xe5\x1d\x7d";
const char r1arrayhex[] = "7d1de5eaf9b156d53208f033b5aa8122d2d2355d5e12292b121156cfdb4a529c";
const uint256 r1l = uint256(std::vector<unsigned char>(r1array,r1array+32));
const uint160 r1s = uint160(std::vector<unsigned char>(r1array,r1array+20));

const unsigned char r2array[] =
    "\x70\x32\x1d\x7c\x47\xa5\x6b\x40\x26\x7e\x0a\xc3\xa6\x9c\xb6\xbf"
    "\x13\x30\x47\xa3\x19\x2d\xda\x71\x49\x13\x72\xf0\xb4\xca\x81\xd7";
const uint256 r2l = uint256(std::vector<unsigned char>(r2array,r2array+32));
const uint160 r2s = uint160(std::vector<unsigned char>(r2array,r2array+20));

const unsigned char zeroarray[] =
    "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"
    "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00";
const uint256 zerol = uint256(std::vector<unsigned char>(zeroarray,zeroarray+32));
const uint160 zeros = uint160(std::vector<unsigned char>(zeroarray,zeroarray+20));

const unsigned char onearray[] =
    "\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"
    "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00";
const uint256 onel = uint256(std::vector<unsigned char>(onearray,onearray+32));
const uint160 ones = uint160(std::vector<unsigned char>(onearray,onearray+20));

const unsigned char maxarray[] =
    "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff"
    "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff";
const uint256 maxl = uint256(std::vector<unsigned char>(maxarray,maxarray+32));
const uint160 maxs = uint160(std::vector<unsigned char>(maxarray,maxarray+20));

std::string arraytostring(const unsigned char a[], unsigned int width)
{
    std::stringstream stream;
    stream << std::hex;
    for (unsigned int i = 0; i < width; ++i)
    {
        stream<<std::setw(2)<<std::setfill('0')<<(unsigned int)a[width-i-1];
    }
    return stream.str();
}

inline uint160 uint160s(const char *str)
{
    uint160 rv;
    rv.sethex(str);
    return rv;
}
inline uint160 uint160s(const std::string& str)
{
    uint160 rv;
    rv.sethex(str);
    return rv;
}

boost_auto_test_case( basics ) // constructors, equality, inequality
{
    boost_check(1 == 0+1);
    // constructor uint256(vector<char>):
    boost_check(r1l.tostring() == arraytostring(r1array,32));
    boost_check(r1s.tostring() == arraytostring(r1array,20));
    boost_check(r2l.tostring() == arraytostring(r2array,32));
    boost_check(r2s.tostring() == arraytostring(r2array,20));
    boost_check(zerol.tostring() == arraytostring(zeroarray,32));
    boost_check(zeros.tostring() == arraytostring(zeroarray,20));
    boost_check(onel.tostring() == arraytostring(onearray,32));
    boost_check(ones.tostring() == arraytostring(onearray,20));
    boost_check(maxl.tostring() == arraytostring(maxarray,32));
    boost_check(maxs.tostring() == arraytostring(maxarray,20));
    boost_check(onel.tostring() != arraytostring(zeroarray,32));
    boost_check(ones.tostring() != arraytostring(zeroarray,20));

    // == and !=
    boost_check(r1l != r2l && r1s != r2s);
    boost_check(zerol != onel && zeros != ones);
    boost_check(onel != zerol && ones != zeros);
    boost_check(maxl != zerol && maxs != zeros);

    // string constructor and copy constructor
    boost_check(uint256s("0x"+r1l.tostring()) == r1l);
    boost_check(uint256s("0x"+r2l.tostring()) == r2l);
    boost_check(uint256s("0x"+zerol.tostring()) == zerol);
    boost_check(uint256s("0x"+onel.tostring()) == onel);
    boost_check(uint256s("0x"+maxl.tostring()) == maxl);
    boost_check(uint256s(r1l.tostring()) == r1l);
    boost_check(uint256s("   0x"+r1l.tostring()+"   ") == r1l);
    boost_check(uint256s("") == zerol);
    boost_check(r1l == uint256s(r1arrayhex));
    boost_check(uint256(r1l) == r1l);
    boost_check(uint256(zerol) == zerol);
    boost_check(uint256(onel) == onel);

    boost_check(uint160s("0x"+r1s.tostring()) == r1s);
    boost_check(uint160s("0x"+r2s.tostring()) == r2s);
    boost_check(uint160s("0x"+zeros.tostring()) == zeros);
    boost_check(uint160s("0x"+ones.tostring()) == ones);
    boost_check(uint160s("0x"+maxs.tostring()) == maxs);
    boost_check(uint160s(r1s.tostring()) == r1s);
    boost_check(uint160s("   0x"+r1s.tostring()+"   ") == r1s);
    boost_check(uint160s("") == zeros);
    boost_check(r1s == uint160s(r1arrayhex));

    boost_check(uint160(r1s) == r1s);
    boost_check(uint160(zeros) == zeros);
    boost_check(uint160(ones) == ones);
}

boost_auto_test_case( comparison ) // <= >= < >
{
    uint256 lastl;
    for (int i = 255; i >= 0; --i) {
        uint256 tmpl;
        *(tmpl.begin() + (i>>3)) |= 1<<(7-(i&7));
        boost_check( lastl < tmpl );
        lastl = tmpl;
    }

    boost_check( zerol < r1l );
    boost_check( r2l < r1l );
    boost_check( zerol < onel );
    boost_check( onel < maxl );
    boost_check( r1l < maxl );
    boost_check( r2l < maxl );

    uint160 lasts;
    for (int i = 159; i >= 0; --i) {
        uint160 tmps;
        *(tmps.begin() + (i>>3)) |= 1<<(7-(i&7));
        boost_check( lasts < tmps );
        lasts = tmps;
    }
    boost_check( zeros < r1s );
    boost_check( r2s < r1s );
    boost_check( zeros < ones );
    boost_check( ones < maxs );
    boost_check( r1s < maxs );
    boost_check( r2s < maxs );
}

boost_auto_test_case( methods ) // gethex sethex begin() end() size() getlow64 getserializesize, serialize, unserialize
{
    boost_check(r1l.gethex() == r1l.tostring());
    boost_check(r2l.gethex() == r2l.tostring());
    boost_check(onel.gethex() == onel.tostring());
    boost_check(maxl.gethex() == maxl.tostring());
    uint256 tmpl(r1l);
    boost_check(tmpl == r1l);
    tmpl.sethex(r2l.tostring());   boost_check(tmpl == r2l);
    tmpl.sethex(zerol.tostring()); boost_check(tmpl == uint256());

    tmpl.sethex(r1l.tostring());
    boost_check(memcmp(r1l.begin(), r1array, 32)==0);
    boost_check(memcmp(tmpl.begin(), r1array, 32)==0);
    boost_check(memcmp(r2l.begin(), r2array, 32)==0);
    boost_check(memcmp(zerol.begin(), zeroarray, 32)==0);
    boost_check(memcmp(onel.begin(), onearray, 32)==0);
    boost_check(r1l.size() == sizeof(r1l));
    boost_check(sizeof(r1l) == 32);
    boost_check(r1l.size() == 32);
    boost_check(r2l.size() == 32);
    boost_check(zerol.size() == 32);
    boost_check(maxl.size() == 32);
    boost_check(r1l.begin() + 32 == r1l.end());
    boost_check(r2l.begin() + 32 == r2l.end());
    boost_check(onel.begin() + 32 == onel.end());
    boost_check(maxl.begin() + 32 == maxl.end());
    boost_check(tmpl.begin() + 32 == tmpl.end());
    boost_check(r1l.getserializesize(0,protocol_version) == 32);
    boost_check(zerol.getserializesize(0,protocol_version) == 32);

    std::stringstream ss;
    r1l.serialize(ss,0,protocol_version);
    boost_check(ss.str() == std::string(r1array,r1array+32));
    tmpl.unserialize(ss,0,protocol_version);
    boost_check(r1l == tmpl);
    ss.str("");
    zerol.serialize(ss,0,protocol_version);
    boost_check(ss.str() == std::string(zeroarray,zeroarray+32));
    tmpl.unserialize(ss,0,protocol_version);
    boost_check(zerol == tmpl);
    ss.str("");
    maxl.serialize(ss,0,protocol_version);
    boost_check(ss.str() == std::string(maxarray,maxarray+32));
    tmpl.unserialize(ss,0,protocol_version);
    boost_check(maxl == tmpl);
    ss.str("");

    boost_check(r1s.gethex() == r1s.tostring());
    boost_check(r2s.gethex() == r2s.tostring());
    boost_check(ones.gethex() == ones.tostring());
    boost_check(maxs.gethex() == maxs.tostring());
    uint160 tmps(r1s);
    boost_check(tmps == r1s);
    tmps.sethex(r2s.tostring());   boost_check(tmps == r2s);
    tmps.sethex(zeros.tostring()); boost_check(tmps == uint160());

    tmps.sethex(r1s.tostring());
    boost_check(memcmp(r1s.begin(), r1array, 20)==0);
    boost_check(memcmp(tmps.begin(), r1array, 20)==0);
    boost_check(memcmp(r2s.begin(), r2array, 20)==0);
    boost_check(memcmp(zeros.begin(), zeroarray, 20)==0);
    boost_check(memcmp(ones.begin(), onearray, 20)==0);
    boost_check(r1s.size() == sizeof(r1s));
    boost_check(sizeof(r1s) == 20);
    boost_check(r1s.size() == 20);
    boost_check(r2s.size() == 20);
    boost_check(zeros.size() == 20);
    boost_check(maxs.size() == 20);
    boost_check(r1s.begin() + 20 == r1s.end());
    boost_check(r2s.begin() + 20 == r2s.end());
    boost_check(ones.begin() + 20 == ones.end());
    boost_check(maxs.begin() + 20 == maxs.end());
    boost_check(tmps.begin() + 20 == tmps.end());
    boost_check(r1s.getserializesize(0,protocol_version) == 20);
    boost_check(zeros.getserializesize(0,protocol_version) == 20);

    r1s.serialize(ss,0,protocol_version);
    boost_check(ss.str() == std::string(r1array,r1array+20));
    tmps.unserialize(ss,0,protocol_version);
    boost_check(r1s == tmps);
    ss.str("");
    zeros.serialize(ss,0,protocol_version);
    boost_check(ss.str() == std::string(zeroarray,zeroarray+20));
    tmps.unserialize(ss,0,protocol_version);
    boost_check(zeros == tmps);
    ss.str("");
    maxs.serialize(ss,0,protocol_version);
    boost_check(ss.str() == std::string(maxarray,maxarray+20));
    tmps.unserialize(ss,0,protocol_version);
    boost_check(maxs == tmps);
    ss.str("");
}

boost_auto_test_case( conversion )
{
    boost_check(arithtouint256(uinttoarith256(zerol)) == zerol);
    boost_check(arithtouint256(uinttoarith256(onel)) == onel);
    boost_check(arithtouint256(uinttoarith256(r1l)) == r1l);
    boost_check(arithtouint256(uinttoarith256(r2l)) == r2l);
    boost_check(uinttoarith256(zerol) == 0);
    boost_check(uinttoarith256(onel) == 1);
    boost_check(arithtouint256(0) == zerol);
    boost_check(arithtouint256(1) == onel);
    boost_check(arith_uint256(r1l.gethex()) == uinttoarith256(r1l));
    boost_check(arith_uint256(r2l.gethex()) == uinttoarith256(r2l));
    boost_check(r1l.gethex() == uinttoarith256(r1l).gethex());
    boost_check(r2l.gethex() == uinttoarith256(r2l).gethex());
}

boost_auto_test_suite_end()