#include <algorithm>
#include <boost/foreach.hpp>

#include "constants.hpp"
#include "fragment_model.hpp"
#include "fragbias.hpp"

FragBias::FragBias(FragmentModel* fm)
{
    // lengths used for interpolation
    const pos_t lengths[] = {
        25, 50, 75, 100, 125, 150, 175, 200, 225, 250, 275, 300,
        325, 350, 375, 400, 425, 450, 475, 500, 550, 600, 650, 700,
        750, 800, 850, 900, 950, 1000, 1100, 1200, 1300, 1500, 1600,
        1700, 1800, 1900, 2000, 2500, 3000, 3500, 4000, 4500, 5000
    };

    this->lengths.resize(sizeof(lengths) / sizeof(pos_t));
    std::copy(lengths, lengths + this->lengths.size(), this->lengths.begin());

    // assumed fragmentation probability
    this->fm = fm;
    p = (1.0 / fm->frag_len_med()) / 2.0;
    double omp = 1.0 - p;
    max_non_frag_run = std::max<pos_t>(1, lround(log(1e-5) / log(omp)));

    non_frag_run_pr_sum.resize(max_non_frag_run);
    non_frag_run_pr.resize(max_non_frag_run);

    non_frag_run_pr[0] = 1.0;
    non_frag_run_pr_sum[0] = 1.0;
    for (pos_t m = 1; m < max_non_frag_run; ++m) {
        non_frag_run_pr_sum[m] = non_frag_run_pr_sum[m - 1] + pow(omp, m - 1) / (double) m;
        non_frag_run_pr[m] = pow(omp, m - 1);
    }

    BOOST_FOREACH (pos_t l, this->lengths) {
        std::vector<float> fb(l);
        compute(&fb.at(0), 0, l, l);
        bias.push_back(fb);
    }
}

void FragBias::compute(float* out, pos_t from, pos_t to, pos_t l)
{
        // Four cases:
        for (pos_t u = from; u < to; ++u) out[u] = 0.0;

#if 0
        // enrichment of end positions assuming the end position is
        // primed first
        // Case 1: no fragmentation
        double c = (1.0 / l) * (l - 1 < max_non_frag_run ? non_frag_run_pr[l - 1] : 0.0);
        for (pos_t v = from; v < to; ++v) {
            double select_pr = fm->frag_len_c(v) / v;
            out[v] = c * select_pr;
        }

        // Case 2: read contained in a fragment at the left end of the
        //         transcript
        for (pos_t v = from; v < to; ++v) {
            for (pos_t j = v; j < std::min<pos_t>(l, v + max_non_frag_run); ++j) {
                double select_pr = fm->frag_len_c(v) / v;
                double run_pr = j - 1 < max_non_frag_run ? non_frag_run_pr[j - 1] : 0.0;
                out[v] += p * run_pr * (1.0 / j) * select_pr;
            }
        }

        // Case 3: read contained a fragment at the right end
        for (pos_t v = from; v < to; ++v) {
            for (pos_t i = std::max<pos_t>(0, v - max_non_frag_run); i < v; ++i) {
                double select_pr = fm->frag_len_c(v - i) / (v - i);
                double run_pr = l - i - 1 < max_non_frag_run ? non_frag_run_pr[l - i - 1] : 0.0;
                out[v] += p * run_pr * (1.0 / ((l - i))) * select_pr;
            }
        }

        // Case 4: read flanked by fragmentation breakpoints
        for (pos_t v = from; v < to; ++v) {
            for (pos_t h = 1; h < std::min<pos_t>(max_non_frag_run, v); ++h) {
                double run_pr1 = non_frag_run_pr_sum[
                    std::min<pos_t>((l - v)+ h, max_non_frag_run - 1)];
                double run_pr2 = non_frag_run_pr_sum[
                    std::min<pos_t>(h, max_non_frag_run - 1)];

                out[v] += p * p * (fm->frag_len_c(h) / h) *
                    (run_pr1 - run_pr2);
            }
        }
#endif

        // enrichment of start positions assuming the start position
        // is primed first.
        // Case 1: no fragmentation
        double c = (1.0 / l) * (l - 1 < max_non_frag_run ? non_frag_run_pr[l - 1] : 0.0);
        for (pos_t u = from; u < to; ++u) {
            double select_pr = fm->frag_len_c(l - u + 1) / (l - u + 1);
            out[u] = c * select_pr;
        }

        // Case 2: read contained in a fragment at the left end of the
        //         transcript
        for (pos_t u = from; u < to; ++u) {
            for (pos_t j = u + 1; j < std::min<pos_t>(l, u + max_non_frag_run); ++j) {
                double select_pr = fm->frag_len_c(j - u) / (j - u);
                double run_pr = j - 1 < max_non_frag_run ? non_frag_run_pr[j - 1] : 0.0;
                out[u] += p * run_pr * (1.0 / j) * select_pr;
            }
        }

        // Case 3: read contained a fragment at the right end
        for (pos_t u = from; u < to; ++u) {
            for (pos_t i = std::max<pos_t>(0, u - max_non_frag_run); i < u; ++i) {
                double select_pr = fm->frag_len_c(l - u + 1) / (l - u + 1);
                double run_pr = l - i - 1 < max_non_frag_run ? non_frag_run_pr[l - i - 1] : 0.0;
                out[u] += p * run_pr * (1.0 / ((l - i))) * select_pr;
            }
        }

        // Case 4: read flanked by fragmentation breakpoints
        for (pos_t u = from; u < to; ++u) {
            for (pos_t h = 1; h < std::min<pos_t>(max_non_frag_run, l - u); ++h) {
                double run_pr1 = non_frag_run_pr_sum[
                    std::min<pos_t>(u + h, max_non_frag_run - 1)];
                double run_pr2 = non_frag_run_pr_sum[
                    std::min<pos_t>(h, max_non_frag_run - 1)];

                out[u] += p * p * (fm->frag_len_c(h) / h) *
                    (run_pr1 - run_pr2);
            }
        }

        for (pos_t u = from; u < to; ++u) {
            out[u] *= constants::fragbias_scale;
        }
}

void FragBias::get_bias(float* out, pos_t l)
{
    unsigned int a;
    // extrapolate downwards
    if (l < lengths.front()) {
        a = 0;
    }
    // extrapolate upwards
    else if (l >= lengths.back()) {
        a = lengths.size() - 2;
    }
    // interpolate
    else {
        std::vector<pos_t>::iterator a_it =
            std::upper_bound(lengths.begin(), lengths.end(), l);
        a = a_it - lengths.begin() - 1;
    }

    unsigned int b = a + 1;

    pos_t al = lengths[a];
    pos_t bl = lengths[b];

    double z = (double) (l - al) / (double) (bl - al);

    // extrapolation can be very very wrong, so we just explicitly compute the
    // ends and copy the middles.
    pos_t endlen = 0;
    if (b == lengths.size() - 1) {
        z = 0.0;
        endlen = constants::fragbias_endlen;
    }
    pos_t to = std::min<pos_t>(l, endlen);
    compute(out, 0, to, l);
    compute(out, std::max<pos_t>(to, l - endlen), l, l);

    for (pos_t u = endlen; u < l - endlen; ++u) {
        double r = (double) u / (double) (l - 1);
        pos_t au = lround(r * (al - 1));
        pos_t bu = lround(r * (bl - 1));
        double slope = bias[b][bu] - bias[a][au];

        out[u] = bias[a][au] + z * slope;
    }
}


#include <cstdlib>
#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>

#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "filelib.h"
#include "weights.h"
#include "sparse_vector.h"
#include "optimize.h"
#include "liblbfgs/lbfgs++.h"

using namespace std;
namespace po = boost::program_options;

// since this is a ranking model, there should be equal numbers of
// positive and negative examples, so the bias should be 0

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("weights,w", po::value<string>(), "Weights from previous iteration (used as initialization and interpolation")
        ("regularization_strength,C",po::value<double>()->default_value(500.0), "l2 regularization strength")
        ("l1",po::value<double>()->default_value(0.0), "l1 regularization strength")
        ("regularize_to_weights,y",po::value<double>()->default_value(5000.0), "Differences in learned weights to previous weights are penalized with an l2 penalty with this strength; 0.0 = no effect")
        ("memory_buffers,m",po::value<unsigned>()->default_value(100), "Number of memory buffers (LBFGS)")
        ("min_reg,r",po::value<double>()->default_value(0.01), "When tuning (-T) regularization strength, minimum regularization strenght")
        ("max_reg,R",po::value<double>()->default_value(1e6), "When tuning (-T) regularization strength, maximum regularization strenght")
        ("testset,t",po::value<string>(), "Optional held-out test set")
        ("tune_regularizer,T", "Use the held out test set (-t) to tune the regularization strength")
        ("interpolate_with_weights,p",po::value<double>()->default_value(1.0), "[deprecated] Output weights are p*w + (1-p)*w_prev; 1.0 = no effect")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

void ParseSparseVector(string& line, size_t cur, SparseVector<weight_t>* out) {
  SparseVector<weight_t>& x = *out;
  size_t last_start = cur;
  size_t last_comma = string::npos;
  while(cur <= line.size()) {
    if (line[cur] == ' ' || cur == line.size()) {
      if (!(cur > last_start && last_comma != string::npos && cur > last_comma)) {
        cerr << "[ERROR] " << line << endl << "  position = " << cur << endl;
        exit(1);
      }
      const int fid = FD::Convert(line.substr(last_start, last_comma - last_start));
      if (cur < line.size()) line[cur] = 0;
      const weight_t val = strtod(&line[last_comma + 1], NULL);
      x.set_value(fid, val);

      last_comma = string::npos;
      last_start = cur+1;
    } else {
      if (line[cur] == '=')
        last_comma = cur;
    }
    ++cur;
  }
}

void ReadCorpus(istream* pin, vector<pair<bool, SparseVector<weight_t> > >* corpus) {
  istream& in = *pin;
  corpus->clear();
  bool flag = false;
  int lc = 0;
  string line;
  SparseVector<weight_t> x;
  while(getline(in, line)) {
    ++lc;
    if (lc % 1000 == 0) { cerr << '.'; flag = true; }
    if (lc % 40000 == 0) { cerr << " [" << lc << "]\n"; flag = false; }
    if (line.empty()) continue;
    const size_t ks = line.find("\t");
    assert(string::npos != ks);
    assert(ks == 1);
    const bool y = line[0] == '1';
    x.clear();
    ParseSparseVector(line, ks + 1, &x);
    corpus->push_back(make_pair(y, x));
  }
  if (flag) cerr << endl;
}

void GradAdd(const SparseVector<weight_t>& v, const double scale, weight_t* acc) {
  for (SparseVector<weight_t>::const_iterator it = v.begin();
       it != v.end(); ++it) {
    acc[it->first] += it->second * scale;
  }
}

double ApplyRegularizationTerms(const double C,
                                const double T,
                                const vector<weight_t>& weights,
                                const vector<weight_t>& prev_weights,
                                weight_t* g) {
  double reg = 0;
  for (size_t i = 0; i < weights.size(); ++i) {
    const double prev_w_i = (i < prev_weights.size() ? prev_weights[i] : 0.0);
    const double& w_i = weights[i];
    reg += C * w_i * w_i;
    g[i] += 2 * C * w_i;

    const double diff_i = w_i - prev_w_i;
    reg += T * diff_i * diff_i;
    g[i] += 2 * T * diff_i;
  }
  return reg;
}

double TrainingInference(const vector<weight_t>& x,
                         const vector<pair<bool, SparseVector<weight_t> > >& corpus,
                         weight_t* g = NULL) {
  double cll = 0;
  for (int i = 0; i < corpus.size(); ++i) {
    const double dotprod = corpus[i].second.dot(x) + (x.size() ? x[0] : weight_t()); // x[0] is bias
    double lp_false = dotprod;
    double lp_true = -dotprod;
    if (0 < lp_true) {
      lp_true += log1p(exp(-lp_true));
      lp_false = log1p(exp(lp_false));
    } else {
      lp_true = log1p(exp(lp_true));
      lp_false += log1p(exp(-lp_false));
    }
    lp_true*=-1;
    lp_false*=-1;
    if (corpus[i].first) {  // true label
      cll -= lp_true;
      if (g) {
        // g -= corpus[i].second * exp(lp_false);
        GradAdd(corpus[i].second, -exp(lp_false), g);
        g[0] -= exp(lp_false); // bias
      }
    } else {                  // false label
      cll -= lp_false;
      if (g) {
        // g += corpus[i].second * exp(lp_true);
        GradAdd(corpus[i].second, exp(lp_true), g);
        g[0] += exp(lp_true); // bias
      }
    }
  }
  return cll;
}

struct ProLoss {
  ProLoss(const vector<pair<bool, SparseVector<weight_t> > >& tr,
          const vector<pair<bool, SparseVector<weight_t> > >& te,
          const double c,
          const double t,
          const vector<weight_t>& px) : training(tr), testing(te), C(c), T(t), prev_x(px){}
  double operator()(const vector<double>& x, double* g) const {
    fill(g, g + x.size(), 0.0);
    double cll = TrainingInference(x, training, g);
    tppl = 0;
    if (testing.size())
      tppl = pow(2.0, TrainingInference(x, testing, g) / (log(2) * testing.size()));
    double ppl = cll / log(2);
    ppl /= training.size();
    ppl = pow(2.0, ppl);
    double reg = ApplyRegularizationTerms(C, T, x, prev_x, g);
    return cll + reg;
  }
  const vector<pair<bool, SparseVector<weight_t> > >& training, testing;
  const double C, T;
  const vector<double>& prev_x;
  mutable double tppl;
};

// return held-out log likelihood
double LearnParameters(const vector<pair<bool, SparseVector<weight_t> > >& training,
                       const vector<pair<bool, SparseVector<weight_t> > >& testing,
                       const double C,
                       const double C1,
                       const double T,
                       const unsigned memory_buffers,
                       const vector<weight_t>& prev_x,
                       vector<weight_t>* px) {
  assert(px->size() == prev_x.size());
  ProLoss loss(training, testing, C, T, prev_x);
  LBFGS<ProLoss> lbfgs(px, loss, memory_buffers, C1);
  lbfgs.MinimizeFunction();
  return loss.tppl;
}

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  string line;
  vector<pair<bool, SparseVector<weight_t> > > training, testing;
  const bool tune_regularizer = conf.count("tune_regularizer");
  if (tune_regularizer && !conf.count("testset")) {
    cerr << "--tune_regularizer requires --testset to be set\n";
    return 1;
  }
  const double min_reg = conf["min_reg"].as<double>();
  const double max_reg = conf["max_reg"].as<double>();
  double C = conf["regularization_strength"].as<double>(); // will be overridden if parameter is tuned
  double C1 = conf["l1"].as<double>(); // will be overridden if parameter is tuned
  const double T = conf["regularize_to_weights"].as<double>();
  assert(C >= 0.0);
  assert(min_reg >= 0.0);
  assert(max_reg >= 0.0);
  assert(max_reg > min_reg);
  const double psi = conf["interpolate_with_weights"].as<double>();
  if (psi < 0.0 || psi > 1.0) { cerr << "Invalid interpolation weight: " << psi << endl; return 1; }
  ReadCorpus(&cin, &training);
  if (conf.count("testset")) {
    ReadFile rf(conf["testset"].as<string>());
    ReadCorpus(rf.stream(), &testing);
  }
  cerr << "Number of features: " << FD::NumFeats() << endl;

  vector<weight_t> x, prev_x;  // x[0] is bias
  if (conf.count("weights")) {
    Weights::InitFromFile(conf["weights"].as<string>(), &x);
    x.resize(FD::NumFeats());
    prev_x = x;
  } else {
    x.resize(FD::NumFeats());
    prev_x = x;
  }
  cerr << "         Number of features: " << x.size() << endl;
  cerr << "Number of training examples: " << training.size() << endl;
  cerr << "Number of  testing examples: " << testing.size() << endl;
  double tppl = 0.0;
  vector<pair<double,double> > sp;
  vector<double> smoothed;
  if (tune_regularizer) {
    C = min_reg;
    const double steps = 18;
    double sweep_factor = exp((log(max_reg) - log(min_reg)) / steps);
    cerr << "SWEEP FACTOR: " << sweep_factor << endl;
    while(C < max_reg) {
      cerr << "C=" << C << "\tT=" <<T << endl;
      tppl = LearnParameters(training, testing, C, C1, T, conf["memory_buffers"].as<unsigned>(), prev_x, &x);
      sp.push_back(make_pair(C, tppl));
      C *= sweep_factor;
    }
    smoothed.resize(sp.size(), 0);
    smoothed[0] = sp[0].second;
    smoothed.back() = sp.back().second;
    for (int i = 1; i < sp.size()-1; ++i) {
      double prev = sp[i-1].second;
      double next = sp[i+1].second;
      double cur = sp[i].second;
      smoothed[i] = (prev*0.2) + cur * 0.6 + (0.2*next);
    }
    double best_ppl = 9999999;
    unsigned best_i = 0;
    for (unsigned i = 0; i < sp.size(); ++i) {
      if (smoothed[i] < best_ppl) {
        best_ppl = smoothed[i];
        best_i = i;
      }
    }
    C = sp[best_i].first;
  }  // tune regularizer
  tppl = LearnParameters(training, testing, C, C1, T, conf["memory_buffers"].as<unsigned>(), prev_x, &x);
  if (conf.count("weights")) {
    for (int i = 1; i < x.size(); ++i) {
      x[i] = (x[i] * psi) + prev_x[i] * (1.0 - psi);
    }
  }
  cout.precision(15);
  cout << "# C=" << C << "\theld out perplexity=";
  if (tppl) { cout << tppl << endl; } else { cout << "N/A\n"; }
  if (sp.size()) {
    cout << "# Parameter sweep:\n";
    for (int i = 0; i < sp.size(); ++i) {
      cout << "# " << sp[i].first << "\t" << sp[i].second << "\t" << smoothed[i] << endl;
    }
  }
  Weights::WriteToFile("-", x);
  return 0;
}


/*!
  \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 */
};

/** \} */

}


#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;
  }
}