/*!
  \file gpp_knowledge_gradient_optimization.hpp
  \rst
  1. OVERVIEW OF KNOWLEDGE GRADIENT WHAT ARE WE TRYING TO DO?
  2. IMPLEMENTATION NOTES
  3. CITATIONS

    **1. OVERVIEW OF KNOWLEDGE GRADIENT; WHAT ARE WE TRYING TO DO?**

    .. Note:: these comments are copied in Python: interfaces/__init__.py

    The optimization process models the objective using a Gaussian process (GP) prior
    (also called a GP predictor) based on the specified covariance and the input
    data (e.g., through member functions ComputeMeanOfPoints, ComputeVarianceOfPoints).  Using the GP,
    we can compute the knowledge gradient (KG) from sampling any particular point.  KG
    is defined relative to the best currently known value, and it represents what the
    algorithm believes is the most likely outcome from sampling a particular point in parameter
    space (aka conducting a particular experiment).

    See KnowledgeGradientEvaluator class docs for further details on computing KG.
    Both support ComputeKnowledgeGradient() and ComputeGradKnowledgeGradient().

    The behavior of the GP is controlled by its underlying
    covariance function and the data/uncertainty of prior points (experiments).

    With the ability of the compute KG, the final step is to optimize
    to find the best KG.  This is done using multistart gradient descent (MGD), in
    ComputeKGOptimalPointsToSample(). This method wraps a MGD call and falls back on random search
    if that fails. See gpp_optimization.hpp for multistart/optimization templates. This method
    can evaluate and optimize KG at serval points simultaneously; e.g., if we wanted to run 4 simultaneous
    experiments, we can use KG to select all 4 points at once.

    Additionally, there are use cases where we have existing experiments that are not yet complete but
    we have an opportunity to start some new trials. For example, maybe we are a drug company currently
    testing 2 combinations of dosage levels. We got some new funding, and can now afford to test
    3 more sets of dosage parameters. Ideally, the decision on the new experiments should depend on
    the existence of the 2 ongoing tests. We may not have any data from the ongoing experiments yet;
    e.g., they are [double]-blind trials. If nothing else, we would not want to duplicate any
    existing experiments! So we want to solve 3-KG using the knowledge of the 2 ongoing experiments.

    We call this q,p-KG, so the previous example would be 3,2-KG. So q is the number of new
    (simultaneous) experiments to select. In code, this would be the size of the output from KG
    optimization (i.e., ``best_points_to_sample``, of which there are ``q = num_to_sample points``).
    p is the number of ongoing/incomplete experiments to take into account (i.e., ``points_being_sampled``
    of which there are ``p = num_being_sampled`` points).

    Back to optimization: the idea behind gradient descent is simple.  The gradient gives us the
    direction of steepest ascent (negative gradient is steepest descent).  So each iteration, we
    compute the gradient and take a step in that direction.  The size of the step is not specified
    by GD and is left to the specific implementation.  Basically if we take steps that are
    too large, we run the risk of over-shooting the solution and even diverging.  If we
    take steps that are too small, it may take an intractably long time to reach the solution.
    Thus the magic is in choosing the step size; we do not claim that our implementation is
    perfect, but it seems to work reasonably.  See ``gpp_optimization.hpp`` for more details about
    GD as well as the template definition.

    For particularly difficult problems or problems where gradient descent's parameters are not
    well-chosen, GD can fail to converge.  If this happens, we can fall back on heuristics;
    e.g., 'dumb' search (i.e., evaluate EI at a large number of random points and take the best
    one). Naive search lives in: ComputeOptimalPointsToSampleViaLatinHypercubeSearch<>().

    **2. IMPLEMENTATION NOTES**

    a. This file has a few primary endpoints for KG optimization:

       i. ComputeKGOptimalPointsToSampleWithRandomStarts<>():

          Solves the q,p-KG problem.

          Takes in a gaussian_process describing the prior, domain, config, etc.; outputs the next best point(s) (experiment)
          to sample (run). Uses gradient descent.

       ii. ComputeKGOptimalPointsToSampleViaLatinHypercubeSearch<>():

           Estimates the q,p-KG problem.

           Takes in a gaussian_process describing the prior, domain, etc.; outputs the next best point(s) (experiment)
           to sample (run). Uses 'dumb' search.

       iii. ComputeKGOptimalPointsToSample<>() (Recommended):

            Solves the q,p-KG problem.

            Wraps the previous two items; relies on gradient descent and falls back to "dumb" search if it fails.

       .. NOTE::
           See ``gpp_knowledge_gradient_optimization.cpp``'s header comments for more detailed implementation notes.

           There are also several other functions with external linkage in this header; these
           are provided primarily to ease testing and to permit lower level access from python.

    b. See ``gpp_common.hpp`` header comments for additional implementation notes.

    **3. CITATIONS**

    a. Gaussian Processes for Machine Learning.
       Carl Edward Rasmussen and Christopher K. I. Williams. 2006.
       Massachusetts Institute of Technology.  55 Hayward St., Cambridge, MA 02142.
       http://www.gaussianprocess.org/gpml/ (free electronic copy)

    b. The Knowledge-Gradient Policy for Correlated Normal Beliefs.
       P.I. Frazier, W.B. Powell & S. Dayanik.
       INFORMS Journal on Computing, 2009.

    c. Differentiation of the Cholesky Algorithm.
       S. P. Smith. 1995.
       Journal of Computational and Graphical Statistics. Volume 4. Number 2. p134-147

    d. A Multi-points Criterion for Deterministic Parallel Global Optimization based on Gaussian Processes.
       David Ginsbourger, Rodolphe Le Riche, and Laurent Carraro.  2008.
       D´epartement 3MI. Ecole Nationale Sup´erieure des Mines. 158 cours Fauriel, Saint-Etienne, France.
       ginsbourger@emse.fr, leriche@emse.fr, carraro@emse.fr
\endrst*/

#ifndef MOE_OPTIMAL_LEARNING_CPP_GPP_EXPECTED_IMPROVEMENT_MCMC_OPTIMIZATION_HPP_
#define MOE_OPTIMAL_LEARNING_CPP_GPP_EXPECTED_IMPROVEMENT_MCMC_OPTIMIZATION_HPP_

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

#include <stdlib.h>
#include <queue>

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

#include "gpp_common.hpp"
#include "gpp_domain.hpp"
#include "gpp_exception.hpp"
#include "gpp_covariance.hpp"
#include "gpp_logging.hpp"
#include "gpp_math.hpp"
#include "gpp_optimization.hpp"
#include "gpp_optimizer_parameters.hpp"
#include "gpp_knowledge_gradient_mcmc_optimization.hpp"
#include "gpp_random.hpp"

namespace optimal_learning {

struct ExpectedImprovementMCMCState;
/*!\rst
  A class to encapsulate the computation of knowledge gradient and its spatial gradient. This class handles the
  general KG computation case using monte carlo integration; it can support q,p-KG optimization. It is designed to work
  with any GaussianProcess.  Additionally, this class has no state and within the context of KG optimization, it is
  meant to be accessed by const reference only.

  The random numbers needed for KG computation will be passed as parameters instead of contained as members to make
  multithreading more straightforward.
\endrst*/
class ExpectedImprovementMCMCEvaluator final {
 public:
  using StateType = ExpectedImprovementMCMCState;
  /*!\rst
    Constructs a KnowledgeGradientEvaluator object.  All inputs are required; no default constructor nor copy/assignment are allowed.

    \param
      :gaussian_process: GaussianProcess object (holds ``points_sampled``, ``values``, ``noise_variance``, derived quantities)
        that describes the underlying GP
      :discrete_pts[dim][num_pts]: the set of points to approximate the KG factor
      :num_pts: number of points in discrete_pts
      :num_mc_iterations: number of monte carlo iterations
      :best_so_far: best (minimum) objective function value (in ``points_sampled_value``)
  \endrst*/
  ExpectedImprovementMCMCEvaluator(const GaussianProcessMCMC& gaussian_process_mcmc,
                                   int num_mc_iterations, double const * best_so_far,
                                   std::vector<typename ExpectedImprovementState::EvaluatorType> * evaluator_vector);

  int dim() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return dim_;
  }

  int num_mcmc() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return num_mcmc_hypers_;
  }

  std::vector<ExpectedImprovementEvaluator> * expected_improvement_evaluator_list() const noexcept OL_WARN_UNUSED_RESULT {
    return expected_improvement_evaluator_lst;
  }

  std::vector<double> best_so_far_list(double const * best_so_far) const noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> result(num_mcmc_hypers_);
    std::copy(best_so_far, best_so_far + num_mcmc_hypers_, result.data());
    return result;
  }

  /*!\rst
    Wrapper for ComputeKnowledgeGradient(); see that function for details.
  \endrst*/
  double ComputeObjectiveFunction(StateType * ei_state) const OL_NONNULL_POINTERS OL_WARN_UNUSED_RESULT {
    return ComputeExpectedImprovement(ei_state);
  }

  /*!\rst
    Wrapper for ComputeGradKnowledgeGradient(); see that function for details.
  \endrst*/
  void ComputeGradObjectiveFunction(StateType * ei_state, double * restrict grad_EI) const OL_NONNULL_POINTERS {
    ComputeGradExpectedImprovement(ei_state, grad_EI);
  }

  /*!\rst
    Computes the knowledge gradient
    \param
      :kg_state[1]: properly configured state object
    \output
      :kg_state[1]: state with temporary storage modified; ``normal_rng`` modified
    \return
      the knowledge gradient from sampling ``points_to_sample`` with ``points_being_sampled`` concurrent experiments
  \endrst*/
  double ComputeExpectedImprovement(StateType * ei_state) const OL_NONNULL_POINTERS OL_WARN_UNUSED_RESULT;

  /*!\rst
    Computes the (partial) derivatives of the knowledge gradient with respect to each point of ``points_to_sample``.
    As with ComputeKnowledgeGradient(), this computation accounts for the effect of ``points_being_sampled``
    concurrent experiments.

    ``points_to_sample`` is the "q" and ``points_being_sampled`` is the "p" in q,p-KG.

    \param
      :kg_state[1]: properly configured state object
    \output
      :kg_state[1]: state with temporary storage modified; ``normal_rng`` modified
      :grad_KG[dim][num_to_sample]: gradient of KG, ``\pderiv{KG(Xq \cup Xp)}{Xq_{d,i}}`` where ``Xq`` is ``points_to_sample``
      and ``Xp`` is ``points_being_sampled`` (grad KG from sampling ``points_to_sample`` with
      ``points_being_sampled`` concurrent experiments wrt each dimension of the points in ``points_to_sample``)
  \endrst*/
  void ComputeGradExpectedImprovement(StateType * ei_state, double * restrict grad_EI) const OL_NONNULL_POINTERS;

  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(ExpectedImprovementMCMCEvaluator);

 private:
  //! spatial dimension (e.g., entries per point of points_sampled)
  const int dim_;
  //! number of mcmc hyperparameters
  int num_mcmc_hypers_;
  //! number of monte carlo iterations
  int num_mc_iterations_;
  //! best (minimum) objective function value (in points_sampled_value)
  std::vector<double> best_so_far_;
  //! pointer to gaussian process used in KG computations
  const GaussianProcessMCMC * gaussian_process_mcmc_;
  //! pointer to gaussian process used in KG computations
  std::vector<typename ExpectedImprovementState::EvaluatorType> * expected_improvement_evaluator_lst;
};

/*!\rst
  State object for KnowledgeGradientEvaluator.  This tracks the points being sampled in concurrent experiments
  (``points_being_sampled``) ALONG with the points currently being evaluated via knowledge gradient for future experiments
  (called ``points_to_sample``); these are the p and q of q,p-KG, respectively.  ``points_to_sample`` joined with
  ``points_being_sampled`` is stored in ``union_of_points`` in that order.

  This struct also tracks the state of the GaussianProcess that underlies the knowledge gradient computation: the GP state
  is built to handle the initial ``union_of_points``, and subsequent updates to ``points_to_sample`` in this object also update
  the GP state.

  This struct also holds a pointer to a random number generator needed for Monte Carlo integrated KG computations.

  .. WARNING::
       Users MUST guarantee that multiple state objects DO NOT point to the same RNG (in a multithreaded env).

  See general comments on State structs in ``gpp_common.hpp``'s header docs.
\endrst*/
struct ExpectedImprovementMCMCState final {
  using EvaluatorType = ExpectedImprovementMCMCEvaluator;

  /*!\rst
    Constructs an KnowledgeGradientMCMCState object with a specified source of randomness for the purpose of computing KG
    (and its gradient) over the specified set of points to sample.
    This establishes properly sized/initialized temporaries for KG computation, including dependent state from the
    associated Gaussian Process (which arrives as part of the kg_evaluator).

    .. WARNING:: This object is invalidated if the associated kg_evaluator is mutated.  SetupState() should be called to reset.

    .. WARNING::
         Using this object to compute gradients when ``configure_for_gradients`` := false results in UNDEFINED BEHAVIOR.

    \param
      :kg_evaluator: knowledge gradient evaluator object that specifies the parameters & GP for KG evaluation
      :points_to_sample[dim][num_to_sample]: points at which to evaluate KG and/or its gradient to check their value in future experiments (i.e., test points for GP predictions)
      :points_being_sampled[dim][num_being_sampled]: points 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-KG)
      :num_being_sampled: number of points being sampled in concurrent experiments (i.e., the "p" in q,p-KG)
      :configure_for_gradients: true if this object will be used to compute gradients, false otherwise
      :normal_rng[1]: pointer to a properly initialized\* NormalRNG object

    .. NOTE::
         \* The NormalRNG object must already be seeded.  If multithreaded computation is used for KG, then every state object
         must have a different NormalRNG (different seeds, not just different objects).
  \endrst*/
  ExpectedImprovementMCMCState(const EvaluatorType& ei_evaluator, double const * restrict points_to_sample,
                               double const * restrict points_being_sampled, int num_to_sample_in,
                               int num_being_sampled_in, int const * restrict gradients_in, int num_gradients_in,
                               bool configure_for_gradients, NormalRNGInterface * normal_rng_in,
                               std::vector<typename ExpectedImprovementEvaluator::StateType> * ei_state_vector);

  ExpectedImprovementMCMCState(ExpectedImprovementMCMCState&& other);

  /*!\rst
    Create a vector with the union of points_to_sample and points_being_sampled (the latter is appended to the former).

    Note the l-value return. Assigning the return to a std::vector<double> or passing it as an argument to the ctor
    will result in copy-elision or move semantics; no copying/performance loss.

    \param:
      :points_to_sample[dim][num_to_sample]: points at which to evaluate KG and/or its gradient to check their value in future experiments (i.e., test points for GP predictions)
      :points_being_sampled[dim][num_being_sampled]: points 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-KG)
      :num_being_sampled: number of points being sampled in concurrent experiments (i.e., the "p" in q,p-KG)
      :dim: the number of spatial dimensions of each point array
    \return
      std::vector<double> with the union of the input arrays: points_being_sampled is *appended* to points_to_sample
  \endrst*/
  static std::vector<double> BuildUnionOfPoints(double const * restrict points_to_sample,
                                                double const * restrict points_being_sampled,
                                                int num_to_sample, int num_being_sampled,
                                                int dim) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> union_of_points(dim*(num_to_sample + num_being_sampled));
    std::copy(points_to_sample, points_to_sample + dim*num_to_sample, union_of_points.data());
    std::copy(points_being_sampled, points_being_sampled + dim*num_being_sampled,
              union_of_points.data() + dim*num_to_sample);
    return union_of_points;
  }

  int GetProblemSize() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return dim*num_to_sample;
  }

  /*!\rst
    Get the ``points_to_sample``: potential future samples whose KG (and/or gradients) are being evaluated

    \output
      :points_to_sample[dim][num_to_sample]: potential future samples whose KG (and/or gradients) are being evaluated
  \endrst*/
  void GetCurrentPoint(double * restrict points_to_sample) const noexcept OL_NONNULL_POINTERS {
    std::copy(union_of_points.data(), union_of_points.data() + num_to_sample*dim, points_to_sample);
  }

  /*!\rst
    Change the potential samples whose KG (and/or gradient) are being evaluated.
    Update the state's derived quantities to be consistent with the new points.

    \param
      :kg_evaluator: expected improvement evaluator object that specifies the parameters & GP for KG evaluation
      :points_to_sample[dim][num_to_sample]: potential future samples whose KG (and/or gradients) are being evaluated
  \endrst*/
  void SetCurrentPoint(const EvaluatorType& kg_evaluator,
                       double const * restrict points_to_sample_in) OL_NONNULL_POINTERS;

  /*!\rst
    Configures this state object with new ``points_to_sample``, the location of the potential samples whose KG is to be evaluated.
    Ensures all state variables & temporaries are properly sized.
    Properly sets all dependent state variables (e.g., GaussianProcess's state) for KG evaluation.

    .. WARNING::
         This object's state is INVALIDATED if the ``kg_evaluator`` (including the GaussianProcess it depends on) used in
         SetupState is mutated! SetupState() should be called again in such a situation.

    \param
      :kg_evaluator: knowledge gradient evaluator object that specifies the parameters & GP for KG evaluation
      :points_to_sample[dim][num_to_sample]: potential future samples whose KG (and/or gradients) are being evaluated
  \endrst*/
  void SetupState(const EvaluatorType& ei_evaluator, double const * restrict points_to_sample);

  // size information
  //! spatial dimension (e.g., entries per point of ``points_sampled``)
  const int dim;
  //! number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-KG)
  const int num_to_sample;
  //! number of points being sampled concurrently (i.e., the "p" in q,p-KG)
  const int num_being_sampled;
  //! number of derivative terms desired (usually 0 for no derivatives or num_to_sample)
  const int num_derivatives;
  //! number of points in union_of_points: num_to_sample + num_being_sampled
  const int num_union;

  // gradients index
  std::vector<int> gradients;
  // the number of gradients observations
  int num_gradients_to_sample;

  //! points currently being sampled; this is the union of the points represented by "q" and "p" in q,p-KG
  //! ``points_to_sample`` is stored first in memory, immediately followed by ``points_being_sampled``
  std::vector<double> union_of_points;

  //! gaussian process state
  std::vector<typename ExpectedImprovementEvaluator::StateType> * ei_state_list;

  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(ExpectedImprovementMCMCState);
};

struct OnePotentialSampleExpectedImprovementMCMCState;
/*!\rst
  A class to encapsulate the computation of knowledge gradient and its spatial gradient. This class handles the
  general KG computation case using monte carlo integration; it can support q,p-KG optimization. It is designed to work
  with any GaussianProcess.  Additionally, this class has no state and within the context of KG optimization, it is
  meant to be accessed by const reference only.

  The random numbers needed for KG computation will be passed as parameters instead of contained as members to make
  multithreading more straightforward.
\endrst*/
class OnePotentialSampleExpectedImprovementMCMCEvaluator final {
 public:
  using StateType = OnePotentialSampleExpectedImprovementMCMCState;
  /*!\rst
    Constructs a KnowledgeGradientEvaluator object.  All inputs are required; no default constructor nor copy/assignment are allowed.

    \param
      :gaussian_process: GaussianProcess object (holds ``points_sampled``, ``values``, ``noise_variance``, derived quantities)
        that describes the underlying GP
      :discrete_pts[dim][num_pts]: the set of points to approximate the KG factor
      :num_pts: number of points in discrete_pts
      :num_mc_iterations: number of monte carlo iterations
      :best_so_far: best (minimum) objective function value (in ``points_sampled_value``)
  \endrst*/
  OnePotentialSampleExpectedImprovementMCMCEvaluator(const GaussianProcessMCMC& gaussian_process_mcmc, double const * best_so_far,
                                                     std::vector<typename OnePotentialSampleExpectedImprovementState::EvaluatorType> * evaluator_vector);

  int dim() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return dim_;
  }

  int num_mcmc() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return num_mcmc_hypers_;
  }

  std::vector<OnePotentialSampleExpectedImprovementEvaluator> * expected_improvement_evaluator_list() const noexcept OL_WARN_UNUSED_RESULT {
    return expected_improvement_evaluator_lst;
  }

  std::vector<double> best_so_far_list(double const * best_so_far) const noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> result(num_mcmc_hypers_);
    std::copy(best_so_far, best_so_far + num_mcmc_hypers_, result.data());
    return result;
  }

  /*!\rst
    Wrapper for ComputeKnowledgeGradient(); see that function for details.
  \endrst*/
  double ComputeObjectiveFunction(StateType * ei_state) const OL_NONNULL_POINTERS OL_WARN_UNUSED_RESULT {
    return ComputeExpectedImprovement(ei_state);
  }

  /*!\rst
    Wrapper for ComputeGradKnowledgeGradient(); see that function for details.
  \endrst*/
  void ComputeGradObjectiveFunction(StateType * ei_state, double * restrict grad_EI) const OL_NONNULL_POINTERS {
    ComputeGradExpectedImprovement(ei_state, grad_EI);
  }

  /*!\rst
    Computes the knowledge gradient
    \param
      :kg_state[1]: properly configured state object
    \output
      :kg_state[1]: state with temporary storage modified; ``normal_rng`` modified
    \return
      the knowledge gradient from sampling ``points_to_sample`` with ``points_being_sampled`` concurrent experiments
  \endrst*/
  double ComputeExpectedImprovement(StateType * ei_state) const OL_NONNULL_POINTERS OL_WARN_UNUSED_RESULT;

  /*!\rst
    Computes the (partial) derivatives of the knowledge gradient with respect to each point of ``points_to_sample``.
    As with ComputeKnowledgeGradient(), this computation accounts for the effect of ``points_being_sampled``
    concurrent experiments.

    ``points_to_sample`` is the "q" and ``points_being_sampled`` is the "p" in q,p-KG.

    \param
      :kg_state[1]: properly configured state object
    \output
      :kg_state[1]: state with temporary storage modified; ``normal_rng`` modified
      :grad_KG[dim][num_to_sample]: gradient of KG, ``\pderiv{KG(Xq \cup Xp)}{Xq_{d,i}}`` where ``Xq`` is ``points_to_sample``
      and ``Xp`` is ``points_being_sampled`` (grad KG from sampling ``points_to_sample`` with
      ``points_being_sampled`` concurrent experiments wrt each dimension of the points in ``points_to_sample``)
  \endrst*/
  void ComputeGradExpectedImprovement(StateType * ei_state, double * restrict grad_EI) const OL_NONNULL_POINTERS;

  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(OnePotentialSampleExpectedImprovementMCMCEvaluator);

 private:
  //! spatial dimension (e.g., entries per point of points_sampled)
  const int dim_;
  //! number of mcmc hyperparameters
  int num_mcmc_hypers_;
  //! best (minimum) objective function value (in points_sampled_value)
  std::vector<double> best_so_far_;
  //! pointer to gaussian process used in KG computations
  const GaussianProcessMCMC * gaussian_process_mcmc_;
  //! pointer to gaussian process used in KG computations
  std::vector<typename OnePotentialSampleExpectedImprovementState::EvaluatorType> * expected_improvement_evaluator_lst;
};

/*!\rst
  State object for KnowledgeGradientEvaluator.  This tracks the points being sampled in concurrent experiments
  (``points_being_sampled``) ALONG with the points currently being evaluated via knowledge gradient for future experiments
  (called ``points_to_sample``); these are the p and q of q,p-KG, respectively.  ``points_to_sample`` joined with
  ``points_being_sampled`` is stored in ``union_of_points`` in that order.

  This struct also tracks the state of the GaussianProcess that underlies the knowledge gradient computation: the GP state
  is built to handle the initial ``union_of_points``, and subsequent updates to ``points_to_sample`` in this object also update
  the GP state.

  This struct also holds a pointer to a random number generator needed for Monte Carlo integrated KG computations.

  .. WARNING::
       Users MUST guarantee that multiple state objects DO NOT point to the same RNG (in a multithreaded env).

  See general comments on State structs in ``gpp_common.hpp``'s header docs.
\endrst*/
struct OnePotentialSampleExpectedImprovementMCMCState final {
  using EvaluatorType = OnePotentialSampleExpectedImprovementMCMCEvaluator;

  /*!\rst
    Constructs an KnowledgeGradientMCMCState object with a specified source of randomness for the purpose of computing KG
    (and its gradient) over the specified set of points to sample.
    This establishes properly sized/initialized temporaries for KG computation, including dependent state from the
    associated Gaussian Process (which arrives as part of the kg_evaluator).

    .. WARNING:: This object is invalidated if the associated kg_evaluator is mutated.  SetupState() should be called to reset.

    .. WARNING::
         Using this object to compute gradients when ``configure_for_gradients`` := false results in UNDEFINED BEHAVIOR.

    \param
      :kg_evaluator: knowledge gradient evaluator object that specifies the parameters & GP for KG evaluation
      :points_to_sample[dim][num_to_sample]: points at which to evaluate KG and/or its gradient to check their value in future experiments (i.e., test points for GP predictions)
      :points_being_sampled[dim][num_being_sampled]: points 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-KG)
      :num_being_sampled: number of points being sampled in concurrent experiments (i.e., the "p" in q,p-KG)
      :configure_for_gradients: true if this object will be used to compute gradients, false otherwise
      :normal_rng[1]: pointer to a properly initialized\* NormalRNG object

    .. NOTE::
         \* The NormalRNG object must already be seeded.  If multithreaded computation is used for KG, then every state object
         must have a different NormalRNG (different seeds, not just different objects).
  \endrst*/
  OnePotentialSampleExpectedImprovementMCMCState(const EvaluatorType& ei_evaluator, double const * restrict point_to_sample,
                                                 double const * restrict OL_UNUSED(points_being_sampled),
                                                 int OL_UNUSED(num_to_sample_in), int OL_UNUSED(num_being_sampled_in),
                                                 bool configure_for_gradients, NormalRNGInterface * OL_UNUSED(normal_rng_in),
                                                 std::vector<typename OnePotentialSampleExpectedImprovementEvaluator::StateType> * ei_state_vector);

  OnePotentialSampleExpectedImprovementMCMCState(OnePotentialSampleExpectedImprovementMCMCState&& other);

  int GetProblemSize() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return dim*num_to_sample;
  }

  /*!\rst
    Get the ``points_to_sample``: potential future samples whose KG (and/or gradients) are being evaluated

    \output
      :points_to_sample[dim][num_to_sample]: potential future samples whose KG (and/or gradients) are being evaluated
  \endrst*/
  void GetCurrentPoint(double * restrict points_to_sample) const noexcept OL_NONNULL_POINTERS {
    std::copy(point_to_sample.data(), point_to_sample.data() + dim, points_to_sample);
  }

  /*!\rst
    Change the potential samples whose KG (and/or gradient) are being evaluated.
    Update the state's derived quantities to be consistent with the new points.

    \param
      :kg_evaluator: expected improvement evaluator object that specifies the parameters & GP for KG evaluation
      :points_to_sample[dim][num_to_sample]: potential future samples whose KG (and/or gradients) are being evaluated
  \endrst*/
  void SetCurrentPoint(const EvaluatorType& kg_evaluator,
                       double const * restrict points_to_sample_in) OL_NONNULL_POINTERS;

  /*!\rst
    Configures this state object with new ``points_to_sample``, the location of the potential samples whose KG is to be evaluated.
    Ensures all state variables & temporaries are properly sized.
    Properly sets all dependent state variables (e.g., GaussianProcess's state) for KG evaluation.

    .. WARNING::
         This object's state is INVALIDATED if the ``kg_evaluator`` (including the GaussianProcess it depends on) used in
         SetupState is mutated! SetupState() should be called again in such a situation.

    \param
      :kg_evaluator: knowledge gradient evaluator object that specifies the parameters & GP for KG evaluation
      :points_to_sample[dim][num_to_sample]: potential future samples whose KG (and/or gradients) are being evaluated
  \endrst*/
  void SetupState(const EvaluatorType& ei_evaluator, double const * restrict points_to_sample);

  // size information
  //! spatial dimension (e.g., entries per point of ``points_sampled``)
  const int dim;
  //! number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-KG)
  const int num_to_sample = 1;

  //! number of derivative terms desired (usually 0 for no derivatives or num_to_sample)
  const int num_derivatives;

  //! point at which to evaluate EI and/or its gradient (e.g., to check its value in future experiments)
  std::vector<double> point_to_sample;

  //! gaussian process state
  std::vector<typename OnePotentialSampleExpectedImprovementEvaluator::StateType> * ei_state_list;

  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(OnePotentialSampleExpectedImprovementMCMCState);
};

/*!\rst
  Set up vector of ExpectedImprovementEvaluator::StateType.

  This is a utility function just for reducing code duplication.

  \param
    :kg_evaluator: evaluator object associated w/the state objects being constructed
    :points_to_sample[dim][num_to_sample]: initial points to load into state (must be a valid point for the problem);
      i.e., points at which to evaluate KG and/or its gradient
    :points_being_sampled[dim][num_being_sampled]: points that are being sampled in concurrently experiments
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-KG)
    :num_being_sampled: number of points being sampled concurrently (i.e., the p in q,p-KG)
    :max_num_threads: maximum number of threads for use by OpenMP (generally should be <= # cores)
    :configure_for_gradients: true if these state objects will be used to compute gradients, false otherwise
    :state_vector[arbitrary]: vector of state objects, arbitrary size (usually 0)
    :normal_rng[max_num_threads]: a vector of NormalRNG objects that provide the (pesudo)random source for MC integration
  \output
    :state_vector[max_num_threads]: vector of states containing ``max_num_threads`` properly initialized state objects
\endrst*/
inline OL_NONNULL_POINTERS void SetupExpectedImprovementMCMCState(
    const OnePotentialSampleExpectedImprovementMCMCEvaluator& ei_evaluator,
    double const * restrict points_to_sample,
    double const * restrict points_being_sampled,
    int num_to_sample,
    int num_being_sampled,
    int const * restrict gradients, int num_gradients,
    int max_num_threads,
    bool configure_for_gradients,
    NormalRNG * normal_rng,
    std::vector<typename OnePotentialSampleExpectedImprovementEvaluator::StateType> * ei_state_vector,
    std::vector<typename OnePotentialSampleExpectedImprovementMCMCEvaluator::StateType> * state_vector) {
  state_vector->reserve(max_num_threads);
  for (int i = 0; i < max_num_threads; ++i) {
    //kg_state_vector.reserve(0);
    state_vector->emplace_back(ei_evaluator, points_to_sample, points_being_sampled, num_to_sample,
                               num_being_sampled, configure_for_gradients,
                               normal_rng + i, ei_state_vector+i);
  }
}

/*!\rst
  Set up vector of ExpectedImprovementEvaluator::StateType.

  This is a utility function just for reducing code duplication.

  \param
    :kg_evaluator: evaluator object associated w/the state objects being constructed
    :points_to_sample[dim][num_to_sample]: initial points to load into state (must be a valid point for the problem);
      i.e., points at which to evaluate KG and/or its gradient
    :points_being_sampled[dim][num_being_sampled]: points that are being sampled in concurrently experiments
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-KG)
    :num_being_sampled: number of points being sampled concurrently (i.e., the p in q,p-KG)
    :max_num_threads: maximum number of threads for use by OpenMP (generally should be <= # cores)
    :configure_for_gradients: true if these state objects will be used to compute gradients, false otherwise
    :state_vector[arbitrary]: vector of state objects, arbitrary size (usually 0)
    :normal_rng[max_num_threads]: a vector of NormalRNG objects that provide the (pesudo)random source for MC integration
  \output
    :state_vector[max_num_threads]: vector of states containing ``max_num_threads`` properly initialized state objects
\endrst*/
inline OL_NONNULL_POINTERS void SetupExpectedImprovementMCMCState(
    const ExpectedImprovementMCMCEvaluator& ei_evaluator,
    double const * restrict points_to_sample,
    double const * restrict points_being_sampled,
    int num_to_sample,
    int num_being_sampled,
    int const * restrict gradients, int num_gradients,
    int max_num_threads,
    bool configure_for_gradients,
    NormalRNG * normal_rng,
    std::vector<typename ExpectedImprovementEvaluator::StateType> * ei_state_vector,
    std::vector<typename ExpectedImprovementMCMCEvaluator::StateType> * state_vector) {
  state_vector->reserve(max_num_threads);
  for (int i = 0; i < max_num_threads; ++i) {
    //kg_state_vector.reserve(0);
    state_vector->emplace_back(ei_evaluator, points_to_sample, points_being_sampled, num_to_sample,
                               num_being_sampled, gradients, num_gradients, configure_for_gradients,
                               normal_rng + i, ei_state_vector+i);
  }
}

/*!\rst
  Solve the q,p-KG problem (see ComputeKGOptimalPointsToSample and/or header docs) by optimizing the knowledge gradient.
  Optimization is done using restarted Gradient Descent, via GradientDescentOptimizer<...>::Optimize() from
  ``gpp_optimization.hpp``.  Please see that file for details on gradient descent and see gpp_optimizer_parameters.hpp
  for the meanings of the GradientDescentParameters.

  This function is just a simple wrapper that sets up the Evaluator's State and calls a general template for restarted GD.

  This function does not perform multistarting or employ any other robustness-boosting heuristcs; it only
  converges if the ``initial_guess`` is close to the solution. In general,
  ComputeOptimalPointsToSample() (see below) is preferred. This function is meant for:

  1. easier testing;
  2. if you really know what you're doing.

  Solution is guaranteed to lie within the region specified by ``domain``; note that this may not be a
  true optima (i.e., the gradient may be substantially nonzero).

  \param
    :kg_evaluator: reference to object that can compute ExpectedImprovement and its spatial gradient
    :optimizer_parameters: GradientDescentParameters object that describes the parameters controlling KG optimization
      (e.g., number of iterations, tolerances, learning rate)
    :domain: object specifying the domain to optimize over (see ``gpp_domain.hpp``)
    :initial_guess[dim][num_to_sample]: initial guess for gradient descent
    :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-KG)
    :num_being_sampled: number of points being sampled concurrently (i.e., the "p" in q,p-KG)
    :normal_rng[1]: a NormalRNG object that provides the (pesudo)random source for MC integration
  \output
    :normal_rng[1]: NormalRNG object will have its state changed due to random draws
    :next_point[dim][num_to_sample]: points yielding the best KG according to gradient descent
\endrst*/
template <typename ExpectedImprovementMCMCEvaluator, typename DomainType>
void RestartedGradientDescentEIMCMCOptimization(const ExpectedImprovementMCMCEvaluator& ei_evaluator,
                                                const GradientDescentParameters& optimizer_parameters,
                                                const DomainType& domain, double const * restrict initial_guess,
                                                double const * restrict points_being_sampled, int num_to_sample,
                                                int num_being_sampled, NormalRNG * normal_rng,
                                                double * restrict next_point) {
  if (unlikely(optimizer_parameters.max_num_restarts <= 0)) {
    return;
  }
  int dim = ei_evaluator.dim();

  int num_derivatives = (*ei_evaluator.expected_improvement_evaluator_list())[0].gaussian_process()->num_derivatives();
  std::vector<int> derivatives((*ei_evaluator.expected_improvement_evaluator_list())[0].gaussian_process()->derivatives());

  std::vector<typename ExpectedImprovementEvaluator::StateType> ei_state_vector;

  OL_VERBOSE_PRINTF("Expected Improvement Optimization via %s:\n", OL_CURRENT_FUNCTION_NAME);

  bool configure_for_gradients = true;
  typename ExpectedImprovementEvaluator::StateType ei_state(ei_evaluator, initial_guess,
                                                            points_being_sampled, num_to_sample,
                                                            num_being_sampled,
                                                            derivatives.data(), num_derivatives,
                                                            configure_for_gradients,
                                                            normal_rng, &ei_state_vector);

  using RepeatedDomain = RepeatedDomain<DomainType>;
  RepeatedDomain repeated_domain(domain, num_to_sample);
  GradientDescentOptimizer<ExpectedImprovementMCMCEvaluator, RepeatedDomain> gd_opt;
  gd_opt.Optimize(ei_evaluator, optimizer_parameters, repeated_domain, &ei_state);
  ei_state.GetCurrentPoint(next_point);
}

/*!\rst
  Perform multistart gradient descent (MGD) to solve the q,p-KG problem (see ComputeKGOptimalPointsToSample and/or
  header docs).  Starts a GD run from each point in ``start_point_set``.  The point corresponding to the
  optimal KG\* is stored in ``best_next_point``.

  \* Multistarting is heuristic for global optimization. KG is not convex so this method may not find the true optimum.

  This function wraps MultistartOptimizer<>::MultistartOptimize() (see ``gpp_optimization.hpp``), which provides the multistarting
  component. Optimization is done using restarted Gradient Descent, via GradientDescentOptimizer<...>::Optimize() from
  ``gpp_optimization.hpp``. Please see that file for details on gradient descent and see ``gpp_optimizer_parameters.hpp``
  for the meanings of the GradientDescentParameters.

  This function (or its wrappers, e.g., ComputeOptimalPointsToSampleWithRandomStarts) are the primary entry-points for
  gradient descent based KG optimization in the ``optimal_learning`` library.

  Users may prefer to call ComputeKGOptimalPointsToSample(), which applies other heuristics to improve robustness.

  Currently, during optimization, we recommend that the coordinates of the initial guesses not differ from the
  coordinates of the optima by more than about 1 order of magnitude. This is a very (VERY!) rough guideline for
  sizing the domain and num_multistarts; i.e., be wary of sets of initial guesses that cover the space too sparsely.

  Solution is guaranteed to lie within the region specified by ``domain``; note that this may not be a
  true optima (i.e., the gradient may be substantially nonzero).

  .. WARNING::
       This function fails ungracefully if NO improvement can be found!  In that case,
       ``best_next_point`` will always be the first point in ``start_point_set``.
       ``found_flag`` will indicate whether this occured.

  \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).
    :start_point_set[dim][num_to_sample][num_multistarts]: set of initial guesses for MGD (one block of num_to_sample points per multistart)
    :points_being_sampled[dim][num_being_sampled]: points that are being sampled in concurrent experiments
    :discrete_pts[dim][num_pts]: points to approximate KG
    :num_multistarts: number of points in set of initial guesses
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-KG)
    :num_being_sampled: number of points being sampled concurrently (i.e., the "p" in q,p-KG)
    :num_pts: number of points in discrete_pts
    :best_so_far: value of the best mean value so far in discrete_pts
    :max_int_steps: maximum number of MC iterations
    :normal_rng[thread_schedule.max_num_threads]: a vector of NormalRNG objects that provide
      the (pesudo)random source for MC integration
    :noise: variance of measurement noise
  \output
    :normal_rng[thread_schedule.max_num_threads]: NormalRNG objects will have their state changed due to random draws
    :found_flag[1]: true if ``best_next_point`` corresponds to a nonzero KG
    :best_next_point[dim][num_to_sample]: points yielding the best KG according to MGD
\endrst*/
template <typename DomainType>
OL_NONNULL_POINTERS void ComputeEIMCMCOptimalPointsToSampleViaMultistartGradientDescent(
    GaussianProcessMCMC& gaussian_process_mcmc,
    const GradientDescentParameters& optimizer_parameters,
    const DomainType& domain,
    const ThreadSchedule& thread_schedule,
    double const * restrict start_point_set,
    double const * restrict points_being_sampled,
    int num_multistarts,
    int num_to_sample,
    int num_being_sampled,
    double const * best_so_far,
    int max_int_steps,
    NormalRNG * normal_rng,
    bool * restrict found_flag,
    double * restrict best_next_point) {
  if (unlikely(num_multistarts <= 0)) {
    OL_THROW_EXCEPTION(LowerBoundException<int>, "num_multistarts must be > 1", num_multistarts, 1);
  }

  bool configure_for_gradients = true;
  if (num_to_sample == 1 && num_being_sampled == 0) {
    std::vector<typename OnePotentialSampleExpectedImprovementState::EvaluatorType> ei_evaluator_lst;
    OnePotentialSampleExpectedImprovementMCMCEvaluator ei_evaluator(gaussian_process_mcmc, best_so_far, &ei_evaluator_lst);

    int num_derivatives = (*ei_evaluator.expected_improvement_evaluator_list())[0].gaussian_process()->num_derivatives();
    std::vector<int> derivatives((*ei_evaluator.expected_improvement_evaluator_list())[0].gaussian_process()->derivatives());

    std::vector<typename OnePotentialSampleExpectedImprovementMCMCEvaluator::StateType> state_vector;
    std::vector<std::vector<typename OnePotentialSampleExpectedImprovementEvaluator::StateType>> ei_state_vector(thread_schedule.max_num_threads);
    SetupExpectedImprovementMCMCState(ei_evaluator, start_point_set, points_being_sampled,
                                      num_to_sample, num_being_sampled, derivatives.data(), num_derivatives,
                                      thread_schedule.max_num_threads, configure_for_gradients,
                                      normal_rng, ei_state_vector.data(), &state_vector);

    std::vector<double> EI_starting(num_multistarts);
    for (int i=0; i<num_multistarts; ++i){
      state_vector[0].SetCurrentPoint(ei_evaluator, start_point_set + i*num_to_sample*gaussian_process_mcmc.dim());
      EI_starting[i] = ei_evaluator.ComputeExpectedImprovement(&state_vector[0]);
    }

    std::priority_queue<std::pair<double, int>> q;
    int k = 20; // number of indices we need
    for (int i = 0; i < EI_starting.size(); ++i) {
      if (i < k){
        q.push(std::pair<double, int>(-EI_starting[i], i));
      }
      else{
        if (q.top().first > -EI_starting[i]){
          q.pop();
          q.push(std::pair<double, int>(-EI_starting[i], i));
        }
      }
    }

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

    // init winner to be first point in set and 'force' its value to be 0.0; we cannot do worse than this
    OptimizationIOContainer io_container(state_vector[0].GetProblemSize(), 0.0, top_k_starting.data());

    using RepeatedDomain = RepeatedDomain<DomainType>;
    RepeatedDomain repeated_domain(domain, num_to_sample);
    GradientDescentOptimizer<OnePotentialSampleExpectedImprovementMCMCEvaluator, RepeatedDomain> gd_opt;
    MultistartOptimizer<GradientDescentOptimizer<OnePotentialSampleExpectedImprovementMCMCEvaluator, RepeatedDomain> > multistart_optimizer;
    multistart_optimizer.MultistartOptimize(gd_opt, ei_evaluator, optimizer_parameters,
                                            repeated_domain, thread_schedule, top_k_starting.data(),
                                            k, state_vector.data(), nullptr, &io_container);
    *found_flag = io_container.found_flag;
    std::copy(io_container.best_point.begin(), io_container.best_point.end(), best_next_point);
  } else {
    std::vector<typename ExpectedImprovementState::EvaluatorType> ei_evaluator_lst;
    ExpectedImprovementMCMCEvaluator ei_evaluator(gaussian_process_mcmc, max_int_steps, best_so_far, &ei_evaluator_lst);

    int num_derivatives = (*ei_evaluator.expected_improvement_evaluator_list())[0].gaussian_process()->num_derivatives();
    std::vector<int> derivatives((*ei_evaluator.expected_improvement_evaluator_list())[0].gaussian_process()->derivatives());

    std::vector<typename ExpectedImprovementMCMCEvaluator::StateType> state_vector;
    std::vector<std::vector<typename ExpectedImprovementEvaluator::StateType>> ei_state_vector(thread_schedule.max_num_threads);
    SetupExpectedImprovementMCMCState(ei_evaluator, start_point_set, points_being_sampled,
                                      num_to_sample, num_being_sampled, derivatives.data(), num_derivatives,
                                      thread_schedule.max_num_threads, configure_for_gradients,
                                      normal_rng, ei_state_vector.data(), &state_vector);

    std::vector<double> EI_starting(num_multistarts);
    for (int i=0; i<num_multistarts; ++i){
      state_vector[0].SetCurrentPoint(ei_evaluator, start_point_set + i*num_to_sample*gaussian_process_mcmc.dim());
      EI_starting[i] = ei_evaluator.ComputeExpectedImprovement(&state_vector[0]);
    }

    std::priority_queue<std::pair<double, int>> q;
    int k = 20; // number of indices we need
    for (int i = 0; i < EI_starting.size(); ++i) {
      if (i < k){
        q.push(std::pair<double, int>(-EI_starting[i], i));
      }
      else{
        if (q.top().first > -EI_starting[i]){
          q.pop();
          q.push(std::pair<double, int>(-EI_starting[i], i));
        }
      }
    }

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

    // init winner to be first point in set and 'force' its value to be 0.0; we cannot do worse than this
    OptimizationIOContainer io_container(state_vector[0].GetProblemSize(), 0.0, top_k_starting.data());

    using RepeatedDomain = RepeatedDomain<DomainType>;
    RepeatedDomain repeated_domain(domain, num_to_sample);
    GradientDescentOptimizer<ExpectedImprovementMCMCEvaluator, RepeatedDomain> gd_opt;
    MultistartOptimizer<GradientDescentOptimizer<ExpectedImprovementMCMCEvaluator, RepeatedDomain> > multistart_optimizer;
    multistart_optimizer.MultistartOptimize(gd_opt, ei_evaluator, optimizer_parameters,
                                            repeated_domain, thread_schedule, top_k_starting.data(),
                                            k, state_vector.data(), nullptr, &io_container);
    *found_flag = io_container.found_flag;
    std::copy(io_container.best_point.begin(), io_container.best_point.end(), best_next_point);
  }
}

/*!\rst
  Function to evaluate Knowledge Gradient (q,p-KG) over a specified list of ``num_multistarts`` points.
  Optionally outputs the KG at each of these points.
  Outputs the point of the set obtaining the maximum KG value.

  Generally gradient descent is preferred but when they fail to converge this may be the only "robust" option.
  This function is also useful for plotting or debugging purposes (just to get a bunch of KG values).

  This function is just a wrapper that builds the required state objects and a NullOptimizer object and calls
  MultistartOptimizer<...>::MultistartOptimize(...); see gpp_optimization.hpp.

  \param
    :gaussian_process: GaussianProcess object (holds ``points_sampled``, ``values``, ``noise_variance``, derived quantities)
      that describes the underlying GP
    :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_static), chunk_size (0).
    :initial_guesses[dim][num_to_sample][num_multistarts]: list of points at which to compute KG
    :points_being_sampled[dim][num_being_sampled]: points that are being sampled in concurrent experiments
    :discrete_pts[dim][num_pts]: points to approximate KG
    :num_multistarts: number of points to check
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-KG)
    :num_being_sampled: number of points being sampled concurrently (i.e., the "p" in q,p-KG)
    :num_pts: number of points in discrete_pts
    :best_so_far: value of the best mean value so far in discrete_pts
    :max_int_steps: maximum number of MC iterations
    :normal_rng[thread_schedule.max_num_threads]: a vector of NormalRNG objects that provide
      the (pesudo)random source for MC integration
    :noise: variance of measurement noise
  \output
    :found_flag[1]: true if best_next_point corresponds to a nonzero KG
    :normal_rng[thread_schedule.max_num_threads]: NormalRNG objects will have their state changed due to random draws
    :function_values[num_multistarts]: KG evaluated at each point of ``initial_guesses``, in the same order as
      ``initial_guesses``; never dereferenced if nullptr
    :best_next_point[dim][num_to_sample]: points yielding the best KG according to dumb search
\endrst*/
void EvaluateEIMCMCAtPointList(GaussianProcessMCMC& gaussian_process_mcmc,
                               const ThreadSchedule& thread_schedule,
                               double const * restrict initial_guesses,
                               double const * restrict points_being_sampled,
                               int num_multistarts, int num_to_sample,
                               int num_being_sampled, double const * best_so_far,
                               int max_int_steps, bool * restrict found_flag, NormalRNG * normal_rng,
                               double * restrict function_values,
                               double * restrict best_next_point);

/*!\rst
  Perform multistart gradient descent (MGD) to solve the q,p-KG problem (see ComputeKGOptimalPointsToSample and/or
  header docs), starting from ``num_multistarts`` points selected randomly from the within the 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 ComputeKGOptimalPointsToSampleViaMultistartGradientDescent() 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 KG 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
    :discrete_pts[dim][num_pts]: points to approximate KG
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-KG)
    :num_being_sampled: number of points being sampled concurrently (i.e., the "p" in q,p-KG)
    :num_pts: number of points in discrete_pts
    :best_so_far: value of the best mean value so far in discrete_pts
    :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
    :noise: variance of measurement noise
  \output
    :found_flag[1]: true if best_next_point corresponds to a nonzero KG
    :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 KG according to MGD
\endrst*/
template <typename DomainType>
void ComputeEIMCMCOptimalPointsToSampleWithRandomStarts(GaussianProcessMCMC& gaussian_process_mcmc,
                                                        const GradientDescentParameters& optimizer_parameters,
                                                        const DomainType& domain, const ThreadSchedule& thread_schedule,
                                                        double const * restrict points_being_sampled,
                                                        int num_to_sample, int num_being_sampled,
                                                        double const * best_so_far,
                                                        int max_int_steps, bool * restrict found_flag,
                                                        UniformRandomGenerator * uniform_generator, NormalRNG * normal_rng,
                                                        double * restrict best_next_point) {
/*
  int grid_size = 100;
  std::vector<double> starting_points(gaussian_process_mcmc.dim()*optimizer_parameters.num_multistarts*num_to_sample);
  std::vector<double> temp_points(gaussian_process_mcmc.dim()*grid_size*num_to_sample);
  std::vector<double> function_values(grid_size, 0.0);

  // GenerateUniformPointsInDomain() is allowed to return fewer than the requested number of multistarts
  RepeatedDomain<DomainType> repeated_domain(domain, num_to_sample);
  int num_multistarts = optimizer_parameters.num_multistarts;

  for (int i = 0; i < num_multistarts; ++i){
      repeated_domain.GenerateUniformPointsInDomain(grid_size, uniform_generator, temp_points.data());
      EvaluateEIMCMCAtPointList(gaussian_process_mcmc, thread_schedule, temp_points.data(),
                                points_being_sampled, grid_size, num_to_sample,
                                num_being_sampled, best_so_far, max_int_steps, found_flag, normal_rng,
                                function_values.data(), starting_points.data() + i*num_to_sample*gaussian_process_mcmc.dim());
  }
*/
  std::vector<double> starting_points(gaussian_process_mcmc.dim()*optimizer_parameters.num_multistarts*num_to_sample);
  RepeatedDomain<DomainType> repeated_domain(domain, num_to_sample);
  int num_multistarts = repeated_domain.GenerateUniformPointsInDomain(optimizer_parameters.num_multistarts,
                                                                      uniform_generator, starting_points.data());
  ComputeEIMCMCOptimalPointsToSampleViaMultistartGradientDescent(gaussian_process_mcmc, optimizer_parameters, domain,
                                                                 thread_schedule, starting_points.data(),
                                                                 points_being_sampled, num_multistarts,
                                                                 num_to_sample, num_being_sampled,
                                                                 best_so_far, max_int_steps,
                                                                 normal_rng, found_flag, best_next_point);
#ifdef OL_WARNING_PRINT
  if (false == *found_flag) {
    OL_WARNING_PRINTF("WARNING: %s DID NOT CONVERGE\n", OL_CURRENT_FUNCTION_NAME);
    OL_WARNING_PRINTF("First multistart point was returned:\n");
    PrintMatrixTrans(starting_points.data(), num_to_sample, gaussian_process_mcmc.dim());
  }
#endif
}

/*!\rst
  Perform a random, naive search to "solve" the q,p-KG problem (see ComputeKGOptimalPointsToSample and/or
  header docs).  Evaluates KG at ``num_multistarts`` points (e.g., on a latin hypercube) to find the
  point with the best KG value.

  Generally gradient descent is preferred but when they fail to converge this may be the only "robust" option.

  Solution is guaranteed to lie within the region specified by ``domain``; note that this may not be a
  true optima (i.e., the gradient may be substantially nonzero).

  Wraps EvaluateKGAtPointList(); constructs the input point list with a uniform random sampling from the given Domain object.

  \param
    :gaussian_process: GaussianProcess object (holds ``points_sampled``, ``values``, ``noise_variance``, derived quantities)
      that describes the underlying GP
    :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_static), chunk_size (0).
    :points_being_sampled[dim][num_being_sampled]: points that are being sampled in concurrent experiments
    :discrete_pts[dim][num_pts]: points to approximate KG
    :num_multistarts: number of random points to check
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-KG)
    :num_being_sampled: number of points being sampled concurrently (i.e., the "p" in q,p-KG)
    :num_pts: number of points in discrete_pts
    :best_so_far: value of the best mean value so far in discrete_pts
    :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
    :noise: variance of measurement noise
  \output
    found_flag[1]: true if best_next_point corresponds to a nonzero KG
    :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 KG according to dumb search
\endrst*/
template <typename DomainType>
void ComputeEIMCMCOptimalPointsToSampleViaLatinHypercubeSearch(GaussianProcessMCMC& gaussian_process_mcmc,
                                                               const DomainType& domain,
                                                               const ThreadSchedule& thread_schedule,
                                                               double const * restrict points_being_sampled,
                                                               int num_multistarts, int num_to_sample,
                                                               int num_being_sampled, double const * best_so_far,
                                                               int max_int_steps,
                                                               bool * restrict found_flag,
                                                               UniformRandomGenerator * uniform_generator,
                                                               NormalRNG * normal_rng,
                                                               double * restrict best_next_point) {
  std::vector<double> initial_guesses(gaussian_process_mcmc.dim()*num_multistarts*num_to_sample);
  RepeatedDomain<DomainType> repeated_domain(domain, num_to_sample);
  num_multistarts = repeated_domain.GenerateUniformPointsInDomain(num_multistarts, uniform_generator,
                                                                  initial_guesses.data());

  EvaluateEIMCMCAtPointList(gaussian_process_mcmc, thread_schedule, initial_guesses.data(),
                            points_being_sampled, num_multistarts, num_to_sample,
                            num_being_sampled, best_so_far, max_int_steps,
                            found_flag, normal_rng, nullptr, best_next_point);
}

/*!\rst
  Solve the q,p-KG problem (see header docs) by optimizing the knowledge gradient.
  Uses multistart gradient descent, "dumb" search, and/or other heuristics to perform the optimization.

  This is the primary entry-point for KG optimization in the optimal_learning library. It offers our best shot at
  improving robustness by combining higher accuracy methods like gradient descent with fail-safes like random/grid search.

  Returns the optimal set of q points to sample CONCURRENTLY by solving the q,p-KG problem.  That is, we may want to run 4
  experiments at the same time and maximize the KG across all 4 experiments at once while knowing of 2 ongoing experiments
  (4,2-KG). This function handles this use case. Evaluation of q,p-KG (and its gradient) for q > 1 or p > 1 is expensive
  (requires monte-carlo iteration), so this method is usually very expensive.

  Wraps ComputeKGOptimalPointsToSampleWithRandomStarts() and ComputeKGOptimalPointsToSampleViaLatinHypercubeSearch().

  Compared to ComputeHeuristicPointsToSample() (``gpp_heuristic_expected_improvement_optimization.hpp``), this function
  makes no external assumptions about the underlying objective function. Instead, it utilizes a feature of the
  GaussianProcess that allows the GP to account for ongoing/incomplete experiments.

  .. NOTE:: These comments were copied into multistart_knowledge_gradient_optimization() in cpp_wrappers/knowledge_gradient.py.

  \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 KG 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
    :discrete_pts[dim][num_pts]: points to approximate KG
    :num_to_sample: how many simultaneous experiments you would like to run (i.e., the q in q,p-KG)
    :num_being_sampled: number of points being sampled concurrently (i.e., the p in q,p-KG)
    :num_pts: number of points in discrete_pts
    :best_so_far: value of the best mean value so far in discrete_pts
    :max_int_steps: maximum number of MC iterations
    :lhc_search_only: whether to ONLY use latin hypercube search (and skip gradient descent EI opt)
    :num_lhc_samples: number of samples to draw if/when doing latin hypercube search
    :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
    :noise: variance of measurement noise
  \output
    :found_flag[1]: true if best_points_to_sample corresponds to a nonzero KG if sampled simultaneously
    :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_points_to_sample[num_to_sample*dim]: point yielding the best KG according to MGD
\endrst*/
template <typename DomainType>
void ComputeEIMCMCOptimalPointsToSample(GaussianProcessMCMC& gaussian_process_mcmc,
                                        const GradientDescentParameters& optimizer_parameters,
                                        const DomainType& domain, const ThreadSchedule& thread_schedule,
                                        double const * restrict points_being_sampled,
                                        int num_to_sample, int num_being_sampled,
                                        double const * 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 explicit instantiation declarations, see gpp_common.hpp header comments, item 6
extern template void ComputeEIMCMCOptimalPointsToSample(
    GaussianProcessMCMC& gaussian_process_mcmc, const GradientDescentParameters& optimizer_parameters,
    const TensorProductDomain& domain, const ThreadSchedule& thread_schedule,
    double const * restrict points_being_sampled,
    int num_to_sample, int num_being_sampled,
    double const * 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);
extern template void ComputeEIMCMCOptimalPointsToSample(
    GaussianProcessMCMC& gaussian_process_mcmc, const GradientDescentParameters& optimizer_parameters,
    const SimplexIntersectTensorProductDomain& domain, const ThreadSchedule& thread_schedule,
    double const * restrict points_being_sampled,
    int num_to_sample, int num_being_sampled,
    double const * 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

#endif  // MOE_OPTIMAL_LEARNING_CPP_GPP_HEURISTIC_EXPECTED_IMPROVEMENT_OPTIMIZATION_HPP_


/**
   $ g++ -I/path/to/boost sieve.cpp -o sieve && sieve 10000000
 */
#include <inttypes.h> // uintmax_t
#include <limits>
#include <cmath>
#include <iostream>
#include <sstream>
#include <vector>

#include <boost/lambda/lambda.hpp>

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

  int limit = 10000;
  if (argc == 2) {
    stringstream ss(argv[--argc]);
    ss >> limit;

    if (limit < 1 or ss.fail()) {
      cerr << "USAGE:\n  sieve LIMIT\n\nwhere LIMIT in the range [1, "
	   << numeric_limits<int>::max() << ")" << endl;
      return 2;
    }
  }

  // print primes less then 100
  primesupto(100, cout << _1 << " ");
  cout << endl;

  // find number of primes less then limit and their sum
  int count = 0;
  uintmax_t sum = 0;
  primesupto(limit, (var(sum) += _1, var(count) += 1));

  cout << "limit sum pi(n)\n"
       << limit << " " << sum << " " << count << endl;
}


#include <Eigen/Dense>
#include <Eigen/Core>

class Belief {
public:
	void propagateBelief(Mat &P1, Point &qNear, Point &qNew, double &sigma, Mat &P2)
	{
		dx1 = qNear.x - qNew.x;
		dy1 = qNear.y - qNew.y;
		dz1 = qNear.z - qNew.z;

		r = sqrt(pow(dx1, 2) + pow(dy1, 2) + pow(dz1, 2));

		Eigen::MatrixXd H(1, DIM);

		H.row(0) << (1 / r)*dx1, (1 / r)*dy1, (1 / r)*dz1;

		z = 1 / (1 + r * r);
		R = sigma * sigma;

		P_prd = P1 + GQG;
		S = (H * P_prd * H.transpose()).value() + M * R * M;

		K = (1 / S) * (P_prd * H.transpose());
		P2 = (I - (K*H)) * P_prd;
	}

private:
	Mat I = Eigen::MatrixXd::Identity(DIM, DIM);
	Mat A = I;
	Mat B = I;
	Mat G = I;

	Mat GQG = G * Q * G.transpose();

	Mat P_prd, P2;

	float processNoise = 0.028;
	Mat Q = pow(processNoise, 2) * I;
	double M = 1;
	double R, z, S, r;
	double dx1, dy1, dz1;
	Eigen::MatrixXd H = Eigen::MatrixXd(1, DIM);
	Eigen::MatrixXd K;
};


#pragma once
#include <fmt/core.h>
#include <Eigen/Dense>
#include <random>

template<typename T>
using PCoord = Eigen::Matrix<T, 1, 3>;

enum JastrowType {
    SIMPLE_JASTROW,
};

enum AtomicWfnType {
    VB,
    MO,
};

template<typename T>
class WaveFn {
public:
    virtual ~WaveFn(){};
    // return the value of wave function
    virtual T value(const PCoord<T>&)=0;

    // return three components of grad wave function
    virtual PCoord<T> grad(const PCoord<T>&)=0;

    // return laplacian of the wave function
    virtual T laplace(const PCoord<T>&)=0;
};

// Simple Wave function $$\phi(r) = (1+cr)e^{-\alpha r}$$
template <typename T>
class AtomicWaveFn: public WaveFn<T> {
public:
    AtomicWaveFn(T c, T alpha): c(c), alpha(alpha) {};
    ~AtomicWaveFn(){};

    T value(const PCoord<T>& coord) {
        auto r = coord.norm();
        auto ar = alpha*r;
        return (1+c*r)*std::exp(-ar);
    }

    PCoord<T> grad(const PCoord<T>& coord) {
        auto r = coord.norm();
        auto ar = alpha*r;
        auto coeff = (c-alpha*(c*r+1))*std::exp(-ar);
        return coeff*coord.normalized();
    }

    T laplace(const PCoord<T>& coord) {
        auto r = coord.norm();
        auto ar = alpha*r;
        return (c*(ar*(ar-4)+2) + alpha*(ar-2))*std::exp(-ar)/r;
    }

private:
    T c, alpha;
};

// Composed Wave functions: VB
template<typename T>
class VBWaveFn: public WaveFn<T> {
public:
    VBWaveFn(T c, T alpha, const PCoord<T>& R1, const PCoord<T>& R2):
        c(c), alpha(alpha), R1(R1), R2(R2) {
        phi1 = new AtomicWaveFn<T>(c, alpha);
        phi2 = new AtomicWaveFn<T>(c, alpha);
    }

    ~VBWaveFn() {
        delete phi1, phi2;
    }

    T value(const PCoord<T>& r) {
        return phi1->value(r-R1)*phi2->value(r-R2);
    }

    PCoord<T> grad(const PCoord<T>& r) {
        return phi2->value(r-R2)*phi1->grad(r-R1) +
               phi1->value(r-R1)*phi2->grad(r-R2);
    }

    T laplace(const PCoord<T>& r) {
        return phi2->value(r-R2)*phi1->laplace(r-R1) +
               phi1->value(r-R1)*phi2->laplace(r-R2) +
               2*(phi1->grad(r-R1)).dot(phi2->grad(r-R2));
    }

private:
    T c, alpha;
    PCoord<T> R1, R2;
    AtomicWaveFn<T>* phi1;
    AtomicWaveFn<T>* phi2;
};

// Composed Wave functions: MO
template <typename T>
class MOWaveFn: public WaveFn<T> {
public:
    MOWaveFn(T c, T alpha, const PCoord<T>& R1, const PCoord<T>& R2):
        c(c), alpha(alpha), R1(R1), R2(R2) {
        phi1 = new AtomicWaveFn<T>(c, alpha);
        phi2 = new AtomicWaveFn<T>(c, alpha);
    }

    T value(const PCoord<T>& r) {
        return phi1->value(r-R1)+phi2->value(r-R2);
    }

    PCoord<T> grad(const PCoord<T>& r) {
        return phi1->grad(r-R1) + phi2->grad(r-R2);
    }

    T laplace(const PCoord<T>& r) {
        return phi1->laplace(r-R1) + phi2->laplace(r-R2);
    }

private:
    T c, alpha;
    PCoord<T> R1, R2;
    AtomicWaveFn<T>* phi1;
    AtomicWaveFn<T>* phi2;
};

// Define Jastrow wavefunction
template<typename T>
// class JastrowWfn: public WaveFn<T> {
class JastrowWfn {
public:
    JastrowWfn(T factor): factor(factor) {}

    ~JastrowWfn() {}

    inline T value(const PCoord<T>& r1, const PCoord<T>& r2) {
        return std::exp(-ufunc(r1, r2));
    }

    // The gradient is symmetric w.r.t r1 and r2
    std::pair<PCoord<T>, PCoord<T>>
    grad(const PCoord<T>& r1, const PCoord<T>& r2) {
        auto val = value(r1, r2);
        auto r12 = r1 - r2;
        auto r = r12.norm();
        auto dv = -r12/(2*r*std::pow(1+r/factor, 2));
        return {-dv*val, dv*val};
    }

    std::pair<T, T>
    laplace(const PCoord<T>& r1, const PCoord<T>& r2) {
        auto val = value(r1, r2);
        auto r = (r1 - r2).norm();
        auto dv = -1/(2*std::pow(1+r/factor, 2));
        auto d2v = -1/(r*std::pow(1+r/factor, 3));
        auto ret = (dv*dv - d2v)*val;
        return {ret, ret};
    }
private:
    T factor;
    inline T ufunc(const PCoord<T>& r1, const PCoord<T> r2) {
        auto r = (r1 - r2).norm();
        return factor/(2 + 2*r/factor);
    }
};

template<typename T, JastrowType Jastrow, AtomicWfnType AtomicWfn>
class H2Mol {

public:
    const JastrowType jastrow_type = Jastrow;
    const AtomicWfnType atomicwfn_type = AtomicWfn;

    H2Mol(T factor, T c, T alpha, const PCoord<T>& R1, const PCoord<T>& R2):
        R1(R1), R2(R2) {
        switch (Jastrow) {
            case JastrowType::SIMPLE_JASTROW:
                jastrow = new JastrowWfn<T>(factor);
                break;
            default:
                throw std::runtime_error("Invalid jastrow function.");
                break;
        }

        switch(AtomicWfn) {
            case AtomicWfnType::MO:
                atomicwfn = new MOWaveFn<T>(c, alpha, R1, R2);
                break;
            case AtomicWfnType::VB:
                atomicwfn = new VBWaveFn<T>(c, alpha, R1, R2);
                break;
            default:
                throw std::runtime_error("Invalid atomic wave function");
                break;
        }
    }

    ~H2Mol() {
        delete jastrow;
        delete atomicwfn;
    }

    // Notice this calculate \psi^* \psi
    T density(const PCoord<T>& r1, const PCoord<T>& r2) {
        return std::pow(jastrow->value(r1, r2)* \
               atomicwfn->value(r1)*atomicwfn->value(r2), 2);
    }

    // Calculate the energy with current density
    T energy(const PCoord<T>& r1, const PCoord<T>& r2) {
        T ret = 0.0;
        PCoord<T> dJdr1, dJdr2;
        T d2Jdr1, d2Jdr2;
        auto jval = jastrow->value(r1, r2);
        std::tie(d2Jdr1, d2Jdr2) = jastrow->laplace(r1, r2);
        std::tie(dJdr1, dJdr2) = jastrow->grad(r1, r2);
        ret += (d2Jdr1+d2Jdr2)/jval;
        auto val1 = atomicwfn->value(r1);
        auto val2 = atomicwfn->value(r2);
        ret += atomicwfn->laplace(r1)/val1;
        ret += atomicwfn->laplace(r2)/val2;

        ret += 2*dJdr1.dot(atomicwfn->grad(r1))/(jval*val1);
        ret += 2*dJdr2.dot(atomicwfn->grad(r2))/(jval*val2);

        ret = -0.5*ret; // kintetic energy

        ret += -1.0/(r1-R1).norm();
        ret += -1.0/(r1-R2).norm();
        ret += -1.0/(r2-R1).norm();
        ret += -1.0/(r2-R2).norm();
        ret += 1.0/(r1-r2).norm();
        ret += 1.0/(R1-R2).norm();

        return ret;
    }

private:
    JastrowWfn<T>* jastrow = nullptr;
    WaveFn<T>* atomicwfn = nullptr;
    PCoord<T> R1, R2;
};

template<typename T> //, typename wfn>
class H2MolQMC {
public:
    H2MolQMC(T factor, T c, T alpha, const PCoord<T>& R1, const PCoord<T>& R2, T dr): dr(dr){
        mol = new H2Mol<T, JastrowType::SIMPLE_JASTROW, AtomicWfnType::MO>(factor, c, alpha, R1, R2);
    }

    ~H2MolQMC() {
        delete mol;
    }

    std::pair<T, T> sample(int maxstep=10000) {

        // Thsi method return the number between [-1, 1]
        PCoord<T> r1 = PCoord<T>::Random();
        PCoord<T> r2 = PCoord<T>::Random();

        PCoord<T> r1_new, r2_new;
        T energy = mol->energy(r1, r2);
        T density = mol->density(r1, r2);
        std::uniform_real_distribution<T> rnum(0, 1);

        //energy_old = mol->energy();
        T energy_tot = 0.0;
        T energy_sq_tot = 0.0;
        int accept = 0;
        for(int i=0; i<maxstep; i++) {

            dr = std::max(dr, 0.1);
            dr = std::min(dr, 10.0);

            r1_new = r1 + dr*PCoord<T>::Random();
            r2_new = r2 + dr*PCoord<T>::Random();

            auto dtmp = mol->density(r1_new, r2_new);
            auto ratio = dtmp/density;

            if(ratio > 1 || ratio > rnum(rgen)) {
                r1 = r1_new;
                r2 = r2_new;
                density = dtmp;
                energy = mol->energy(r1, r2);
                accept += 1;
            }

            if(static_cast<T>(accept)/(i+1) > 0.5) dr*=scale;
            else dr/=scale;

            energy_tot += energy;
            energy_sq_tot += energy*energy;
        }
        auto energy_avg = energy_tot/maxstep;
        auto energy_std = std::sqrt(energy_sq_tot/maxstep - energy_avg*energy_avg);
        fmt::print("Accept ratio: {}\n", (double) accept/maxstep);
        return {energy_avg, energy_std};
    }

private:
    H2Mol<T, JastrowType::SIMPLE_JASTROW, AtomicWfnType::MO>* mol;
    PCoord<T> R1, R2;
    T dr;
    std::random_device rd;
    std::mt19937 rgen{rd()};
    const T scale = 1.01;
    // T c, alpha;
};


#pragma once
#include <cstddef>
#include <random>
#include <functional>
#include <vector>
#include <limits>
#include <iostream>
#include <boost/multiprecision/number.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/math/special_functions/prime.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/multiprecision/miller_rabin.hpp>

namespace mp = boost::multiprecision;
namespace math = boost::math;
namespace rnd = boost::random;

namespace rsa {

    struct random {

        boost::mt19937_64 generator;

        void init_generator(const uint64_t seed) {
            generator = boost::mt19937_64(seed);
        }

        static auto& default_random() {
            static random rand;
            return rand;
        }

    };

    template<unsigned int N>
    struct num_utils {

        using number = mp::number<mp::backends::cpp_int_backend<N, N, mp::unsigned_magnitude, mp::unchecked, void>>;
        using signed_number = mp::number<mp::backends::cpp_int_backend<2 * N, 2 * N, mp::signed_magnitude, mp::unchecked, void>>;

        static auto get_int_random(const number min = std::numeric_limits<number>::min(),
                                   const number max = std::numeric_limits<number>::max()) {
            rnd::uniform_int_distribution<number> dist(min, max);
            return std::bind(dist, std::ref(random::default_random().generator));

        }

        static auto generate_random_prime(const unsigned int k = 100) {
            const auto rand = get_int_random();
            while(true) {
                const auto n = rand();
                for (unsigned int i = 0; i < math::max_prime; ++i)
                    if (n % math::prime(i) == 0)
                        goto next;
                if (mp::miller_rabin_test(n, k))
                    return n;
                next:
                continue;
            }
        }

        static auto bezout_identity(const signed_number& a, const signed_number& b) {
            if (a == 0)
                return std::make_pair(signed_number(0), signed_number(1));
            const auto buff = bezout_identity(b % a, a);
            return std::make_pair(buff.second - (b / a) * buff.first, buff.first);
        }

        template<typename It>
        static auto bytes_to_number(It begin, const It& end) {
            number result;
            mp::import_bits(result, begin, end, 8);
            return result;
        }

        static auto number_to_bytes(const number& number) {
            std::array<char, N / 8> result;
            result.fill(0);
            mp::export_bits(number, result.rbegin(), 8, false);
            return result;
        }

    };

}// namespace rsa


//
//  Neural.cpp
//
//
//  Created by Zac Schulwolf on 12/27/16.
//
// Based on http://neuralnetworksanddeeplearning.com/chap1.html
// Compile by g++ -I "$(brew --prefix eigen)/include/eigen3" Neural.cpp -o Neural

#include <iostream>
#include <Eigen/Dense>
#include <ctime> //for random stuff
#include <list> //for list of Matrices for biases and weights
using namespace Eigen;
using namespace std;

void Biases(list<MatrixXd> &biases, VectorXi sizes){ //NEEDS TO BE OPTIMIZED inline?
    int r;
    for(int i=0; i <sizes.rows()-1;i++){
        r=sizes[i+1];
        biases.push_back(MatrixXd::Random(r,1));
    }

    cout << endl << "Biases:" << endl; //print biases
    for (list<MatrixXd>::iterator itb = biases.begin(); itb != biases.end(); itb++) //print biases
        cout << *itb << endl <<endl; //print biases
}

void Weights(list<MatrixXd> &weights, VectorXi sizes){ //NEEDS TO BE OPTIMIZED inline?
    int x,y;
    for(int i=0; i <sizes.rows()-1;i++){

        y=sizes[i+1];
        x=sizes[i];
        weights.push_back(MatrixXd::Random(y,x));
    }

    cout << endl << "Weights:" << endl; //print weights
    for (list<MatrixXd>::iterator itw = weights.begin(); itw != weights.end(); itw++) //print weights
        cout << *itw << endl <<endl; //print weights
}

class Network{
    int num_layers;
    VectorXi sizes;
    list<MatrixXd> biases;
    list<MatrixXd> weights;

    VectorXd sigmoid(ArrayXd z){
        return (1/(1+exp(-z))).vector();
    }

public:
    Network(VectorXi Sizes){ //constructor
        struct timespec tm; //time for random seed
        clock_gettime(CLOCK_REALTIME,&tm); //time for random seed
        srand(tm.tv_nsec); //This makes the Eigen random different each time        NEEDS TO BE OPTIMIZED

        num_layers = Sizes.rows();
        sizes = Sizes;
        Biases(biases,sizes);
        Weights(weights,sizes);

    }

    VectorXd feedforward(VectorXd a){ //probably needs to be private
		MatrixXd b;
        MatrixXd w;
        list<MatrixXd>::iterator itb = biases.begin();
        list<MatrixXd>::iterator itw = weights.begin();
        cout << "sizes.rows() = " << sizes.rows()<<endl;

        /*for (list<MatrixXd>::iterator itb = biases.begin(); itb != biases.end(); itb++){ //print biases
            b = *itb;
        }*/

        for (int i=0; i<sizes.rows()-1;i++,itb++,itw++){
            ///cout << "asdjkasldjsakldjaklsdjlkas\n";
            //b.resize((*itb).rows(),(*itb).cols());
            //w.resize((*itw).rows(),(*itw).cols());

            cout << (*itb).rows() << endl << (*itb).cols() << endl << endl;
            cout << (*itw).rows() << endl << (*itw).cols() << endl << endl;
            b= *itb;
            w= *itw;
            cout << "*itb:\n"<< *itb << endl <<endl; //print biases
            cout << "*itw:\n"<< *itw << endl <<endl; //print weights

            //cout << "itb at " << i << " is:\n" << *itb << endl;
            //cout << "itw at " << i << " is:\n" << *itw << endl;
            cout << "a:\n" << a << endl << "w:\n" << w << endl <<"a.matrix()\n" << a.matrix() << endl << "b.array():\n" << b.array() << endl;

            a = sigmoid((a * w).array() + b.array()); //w.row(0) is there because weights is always a vector but its easier to store it as a matrix
            cout << endl << endl << a << endl << endl;

        }
        return a;
    }

};

int main(){
    VectorXi s(3);
    s << 1,2,3;
    Network a(s);
    ArrayXd arr(2);
    arr << 1,2;
    a.feedforward(arr);

    return 0;
}


#include <fstream>

#include <boost/format.hpp>

#include "DelaunayTriangulation.h"
#include "DT_Circle.h"

bool isContained(Triangle * t, Vertex * p);
bool inCircle(Triangle *a, Vertex * pr);
bool inCircle(Vertex * a, Vertex * b, Vertex * c, Vertex * d);
Circle circleForPoints(Vertex * a, Vertex * b, Vertex * c);
Vertex pointAlongLine2D(Vertex * a, Vertex * b, double tn = 0.5);
bool onEdge(Vertex * a, Vertex * b, Vertex * pr);
double distance2Point(Vertex * a, Vertex * b, Vertex * pr);

//
// Friend functions of Triangle & Vertex
//
bool isContained(Triangle * t, Vertex * p) {
    double x = p->getX();
    double y = p->getY();

    double x1 = t->data[0]->getX();
    double y1 = t->data[0]->getY();

    double x2 = t->data[1]->getX();
    double y2 = t->data[1]->getY();

    double x3 = t->data[2]->getX();
    double y3 = t->data[2]->getY();

    double denom = 1.0 / (x1*(y2 - y3) + y1*(x3 - x2) + x2*y3 - y2*x3);

    double t1 = (x*(y3 - y1) + y*(x1 - x3) - x1*y3 + y1*x3) *  denom;
    double t2 = (x*(y2 - y1) + y*(x1 - x2) - x1*y2 + y1*x2) * -denom;

    if (0.0 <= t1 && t1 <= 1.0 &&
        0.0 <= t2 && t2 <= 1.0 &&
        t1 + t2 <= 1.0)
        return true;
    else
        return false;
}

bool inCircle(Triangle *a, Vertex * pr)
{
    Vertex ** p = a->getVertices();
    return inCircle(p[0],p[1],p[2],pr);
}

bool inCircle(Vertex * a, Vertex * b, Vertex * c, Vertex * d)
{
    double xa = a->getX();
    double xb = b->getX();
    double xc = c->getX();
    double xd = d->getX();

    double ya = a->getY();
    double yb = b->getY();
    double yc = c->getY();
    double yd = d->getY();

    double sa = xa * xa + ya * ya;
    double sb = xb * xb + yb * yb;
    double sc = xc * xc + yc * yc;
    double sd = xd * xd + yd * yd;

    double det = 0.0;
    det  = xa * ( yb * (sc - sd) - sb * (yc - yd) + (yc * sd - yd * sc) );
    det -= ya * ( xb * (sc - sd) - sb * (xc - xd) + (xc * sd - xd * sc) );
    det += sa * ( xb * (yc - yd) - yb * (xc - xd) + (xc * yd - xd * yc) );
    det -=      ( xb * (yc * sd - yd * sc) - yb * (xc * sd - xd * sc) + sb * (xc * yd - xd * yc) );

    if (det > 0.0)
        return true;
    else
        return false;
}

Circle circleForPoints(Vertex * a, Vertex * b, Vertex * c)
{
    double x1 = a->getX();
    double x2 = b->getX();
    double x3 = c->getX();

    double y1 = a->getY();
    double y2 = b->getY();
    double y3 = c->getY();

    double X2 = (double)(x2-x1);
    double X3 = (double)(x3-x1);
    double Y2 = (double)(y2-y1);
    double Y3 = (double)(y3-y1);

    double alpha = X3 / X2;

    double bx2 = (x2+x1) * X2;
    double bx3 = (x3+x1) * X3;
    double by2 = (y2+y1) * Y2;
    double by3 = (y3+y1) * Y3;

    double h = 0.0;
    double k = 0.0;
    double r = 0.0;

    k = bx3 + by3 - alpha * (bx2 + by2);
    k /= 2 * (Y3 - alpha * Y2);

    h = bx2 + by2 - 2 * k * Y2;
    h /= 2 * X2;

    r = sqrt( (x1 - h)*(x1 - h) + (y1 - k)*(y1 - k) );

    return Circle(h,k,r);
}

Vertex pointAlongLine2D(Vertex * a, Vertex * b, double tn)
{
    double nx = a->getX() + tn * (b->getX() - a->getX());
    double ny = a->getY() + tn * (b->getY() - a->getY());
    return Vertex(nx,ny);
}

bool onEdge(Vertex * a, Vertex * b, Vertex * pr) {
    double minx = (a->getX() <= b->getX())? a->getX() : b->getX();
    double maxx = (a->getX() >= b->getX())? a->getX() : b->getX();

    double miny = (a->getY() <= b->getY())? a->getY() : b->getY();
    double maxy = (a->getY() >= b->getY())? a->getY() : b->getY();

    if (pr->getX() < minx)
        return false;
    else if (pr->getX() > maxx)
        return false;
    else if (pr->getY() < miny)
        return false;
    else if (pr->getY() > maxy)
        return false;
    else
        return distance2Point(a,b,pr) < Constants::DIST_THRESH;
}

double distance2Point(Vertex * a, Vertex * b, Vertex * pr) {
    double nx = b->getX() - a->getX();
    double ny = b->getY() - a->getY();

    double nmag = sqrt(nx * nx + ny * ny);

    double px = pr->getX() - a->getX();
    double py = pr->getY() - a->getY();

    //double pmag = sqrt(px * px + py * py);

    double dnp = nx * px + ny * py; // dot(n,p)

    double dnpx = nx * dnp / (nmag * nmag);
    double dnpy = ny * dnp / (nmag * nmag);

    double perx = dnpx - px;
    double pery = dnpy - py;

    return sqrt(perx * perx + pery * pery);
}

void DelaunayTriangulation::permute()
{
    std::srand(pts.len());
    for (int i = 0; i < pts.len(); i++) {
        rPerm[i] = i;
    }
    for (int i = pts.len() - 1; i >= 0; i--) {
        int j = std::rand() % (i + 1);
        int temp = rPerm[i];
        rPerm[i] = rPerm[j];
        rPerm[j] = temp;
    }
}

void DelaunayTriangulation::init()
{
    omg1.set( 3.0 * M, 0.0);
    omg2.set( 0.0, 3.0 * M);
    omg3.set(-3.0 * M, -3.0 * M);
    Triangle * itri = dag.get();
    itri->setVertices(&omg1, &omg2, &omg3);
}

DelaunayTriangulation::DelaunayTriangulation() : M(0)
{
}

DelaunayTriangulation::~DelaunayTriangulation()
{
}

void DelaunayTriangulation::addPt(double x, double y, double z = 0.0)
{
	Vertex * v = pts.get();
	if (nullptr != v) {
		v->set(x, y, z);

        int max = (x >= y) ? x : y;
        if (M <= max)
            M = max;
	}
	else {
		fprintf(stdout,"WARNING: Pool of points is empty!\n");
	}
}

Vertex * DelaunayTriangulation::getPoint(int i)
{
    int rPerm_[10] = {2, 9, 7, 8, 4, 1, 3, 0, 6, 5};
    return pts[rPerm_[i]];
    //return pts[rPerm[i]];
}

///
// ValidEdge(∆, pr,D(P))
// Let ∆adj be the triangle opposite to pr and adjacent to ∆
// if InCircle(∆adj , pr) then
//     (* We have to make an edge flip *)
//     flip(∆,∆adj, pr,D(P))
//     Let ∆′ and ∆′′ be the two new triangles, recursively legalize them
//     ValidEdge(∆′, pr,D(P))
//     ValidEdge(∆′′, pr,D(P))
// end if
///
void DelaunayTriangulation::validEdge(Triangle *a, Vertex *pr)
{
    TriangleList<Constants::adjListSize> copy;

    // Let ∆adj be the triangle opposite to pr and adjacent to ∆
    dag.findAdjacentTriangle(a, pr, false);
    copy.copy(dag.adjList);
    if (copy.len > 1) {

        Vertex ** points = copy[1]->getVertices();
        Circle c = circleForPoints(points[0], points[1], points[2]);
        bool answer1 = c.pointInside(pr->getX(), pr->getY());
        bool answer2 = inCircle(copy[1], pr);

        if (answer1 != answer2) {
            fprintf(stdout, "WARNING: inCircle produced wrong answer.\n");
            fprintf(stdout, "\t%s\n", c.to_json().dump().c_str());
            fprintf(stdout, "\t%s\n", a->to_json().dump().c_str());
            fprintf(stdout, "\t%s\n", pr->to_json().dump().c_str());
        }

        if (answer1) {
            Triangle *n[2];
            dag.flip(copy[0],copy[1],pr,n);
            validEdge(n[0], pr);
            validEdge(n[1], pr);
        }
    }
}

void DelaunayTriangulation::compute()
{
	if (pts.len() < 3) {
		fprintf(stdout, "WARNING: Not enough points added to Pool\n");
		return;
	}

	init();
	permute();

    logPoints();
    json j;

	for (int i = 0; i < pts.len(); i++) {
        Vertex * p = getPoint(i);

        logStep(i, p);

        dag.findTriangleContainingPoint(p);
        if (2 == dag.adjList.len) { // on edge
            dag.divideOnEdge(dag.adjList[0], dag.adjList[1], p);
        } else { // on interior
            dag.divideOnInterior(dag.adjList[0], p);
        }

        TriangleList<Constants::splitListSize> copy;
        copy.copy(dag.splitList);
        for (int i = 0; i < copy.len; i++) {
            assert(true == copy[i]->isValid());
            validEdge(copy[i], p);
        }
	}

    //
    // removing all triangles with the initial fake points added to DAG.
    //
    dag.removeTriangleContainingPoint(omg1);
    dag.removeTriangleContainingPoint(omg2);
    dag.removeTriangleContainingPoint(omg3);
    json sol = dag.to_json();
    std::ofstream log;
    log.open ("solution.json");
    log << sol.dump();
    log.close();
}

void DelaunayTriangulation::logStep(int loop, Vertex * p)
{
    json j;
    j["loop"] = loop;
    j["point"] = p->to_json();
    j["dag"] = dag.len();

    std::ofstream log;
    log.open (str(boost::format("loop_%05d.json") % dag.len()));
    log << j.dump();
    log.close();
}

void DelaunayTriangulation::logPoints()
{
    std::ofstream log;
    log.open ("points.json");
    json j;
    j.push_back(omg1.to_json());
    j.push_back(omg2.to_json());
    j.push_back(omg3.to_json());
    for (int i = 0; i < pts.len(); i++) {
        j.push_back( pts[i]->to_json() );
    }
    log << j.dump();
    log.close();
}


//  Routines for computing p-values of occurrence overlaps using the hypergeometric distribution
//

#ifndef INCLUDED_overlap_probs_HH
#define INCLUDED_overlap_probs_HH

#include "misc.hh"
#include <boost/math/distributions/hypergeometric.hpp>

inline
Real
product_cdf( Real const x )
{
	return x - x*log(x);
}

inline
Real
compute_overlap_pvalue(
	Size const overlap,
	Size const count1,
	Size const count2,
	Size const total
)
{
	using namespace boost::math;
	// whats the smallest possible overlap? max( 0, count1+count2-total )
	if ( overlap==0 || count1==total || count2==total || count1+count2 >= total+overlap ) return 1.0;
	hypergeometric_distribution<> hgd( count1, count2, total );
	return cdf( complement( hgd, overlap-1 ) ); // need overlap-1 to be a valid overlap value!
}

inline // silly?
Real
compute_overlap_pvalue_with_bias(
	bools const & occs1,
	bools const & occs2,
	Reals const & subject_bias_factor
)
{
	Real pval(1.);

	Size table[2][2];

	table[0][0] = table[0][1] = table[1][0] = table[1][1] = 0;

	Size const total( occs1.size() );
	runtime_assert( occs2.size() == total );
	runtime_assert( subject_bias_factor.size() == total );

	for ( Size i=0; i< total; ++i ) { ++table[ occs1[i] ][ occs2[i] ]; }

	Size const overlap( table[1][1] ), total1( table[1][0] + table[1][1] ), total2( table[0][1] + table[1][1] );

	if ( overlap > 0 && total1 < total && total2 < total ) {
		Real bias(1.0);
		for ( Size i=0; i< total; ++i ) {
			if ( occs1[i] && occs2[i] ) bias *= subject_bias_factor[i];
		}
		pval = min( 1.0, bias * compute_overlap_pvalue( overlap, total1, total2, total ) );
	}

	return pval;
}

#endif