/*!
  \file gpp_knowledge_gradient_inner_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_KNOWLEDGE_GRADIENT_INNER_OPTIMIZATION_HPP_
#define MOE_OPTIMAL_LEARNING_CPP_GPP_KNOWLEDGE_GRADIENT_INNER_OPTIMIZATION_HPP_

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

#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_random.hpp"

namespace optimal_learning {

struct FuturePosteriorMeanState;
/*!\rst
  This is a specialization of the ExpectedImprovementEvaluator class for when the number of potential samples is 1; i.e.,
  ``num_to_sample == 1`` and the number of concurrent samples is 0; i.e. ``num_being_sampled == 0``.
  In other words, this class only supports the computation of 1,0-EI.  In this case, we have analytic formulas
  for computing EI and its gradient.
  Thus this class does not perform any explicit numerical integration, nor do its EI functions require access to a
  random number generator.
  This class's methods have some parameters that are unused or redundant.  This is so that the interface matches that of
  the more general ExpectedImprovementEvaluator.
  For other details, see ExpectedImprovementEvaluator for more complete description of what EI is and the outputs of
  EI and grad EI computations.
\endrst*/
class FuturePosteriorMeanEvaluator final {
 public:
  using StateType = FuturePosteriorMeanState;

  /*!\rst
    Constructs a OnePotentialSampleExpectedImprovementEvaluator 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
      :best_so_far: best (minimum) objective function value (in ``points_sampled_value``)
  \endrst*/
  FuturePosteriorMeanEvaluator(const GaussianProcess& gaussian_process_in,
                               double const * coefficient,
                               double const * to_sample,
                               const int num_to_sample,
                               int const * to_sample_derivatives,
                               int num_derivatives,
                               double const * chol,
                               double const * train_sample);

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

  const GaussianProcess * gaussian_process() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return gaussian_process_;
  }

  std::vector<double> to_sample_points(double const * to_sample,
                                       int num_to_sample) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> result(num_to_sample*dim_);
    std::copy(to_sample, to_sample + num_to_sample*dim_, result.data());
    return result;
  }

  std::vector<int> derivatives(int const * to_sample_derivatives,
                                  int num_derivatives) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<int> result(num_derivatives);
    std::copy(to_sample_derivatives, to_sample_derivatives + num_derivatives, result.data());
    return result;
  }

  std::vector<double> coeff(double const * coefficient, double const * chol,
                            int num_to_sample, int num_derivatives) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> result(num_to_sample*(1+num_derivatives));
    std::copy(coefficient, coefficient + num_to_sample*(1+num_derivatives), result.data());
    TriangularMatrixVectorSolve(chol, 'T', num_to_sample_*(1+num_derivatives_),
                                num_to_sample_*(1+num_derivatives_), result.data());
    return result;
  }

  std::vector<double> coeff_combine(double const * coefficient, double const * train_sample,
                                    int num_to_sample, int num_derivatives) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> temp(num_to_sample*(1+num_derivatives));
    std::copy(coefficient, coefficient + num_to_sample*(1+num_derivatives), temp.data());

    std::vector<double> result(gaussian_process_->get_K_inv_y());
    int num_observations = gaussian_process_->num_sampled() * (1 + gaussian_process_->num_derivatives());
    GeneralMatrixVectorMultiply(train_sample, 'N', temp.data(), -1.0, 1.0,
                                num_observations, num_to_sample*(1+num_derivatives),
                                num_observations, result.data());
    return result;
  }

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

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

  /*!\rst
    Computes the expected improvement ``EI(Xs) = E_n[[f^*_n(X) - min(f(Xs_1),...,f(Xs_m))]^+]``
    Uses analytic formulas to evaluate the expected improvement.
    \param
      :ei_state[1]: properly configured state object
    \output
      :ei_state[1]: state with temporary storage modified
    \return
      the expected improvement from sampling ``point_to_sample``
  \endrst*/
  double ComputePosteriorMean(StateType * ps_state) const;

  /*!\rst
    Computes the (partial) derivatives of the expected improvement with respect to the point to sample.
    Uses analytic formulas to evaluate the spatial gradient of the expected improvement.
    \param
      :ei_state[1]: properly configured state object
    \output
      :ei_state[1]: state with temporary storage modified
      :grad_EI[dim]: gradient of EI, ``\pderiv{EI(x)}{x_d}``, where ``x`` is ``points_to_sample``
  \endrst*/
  void ComputeGradPosteriorMean(StateType * ps_state, double * restrict grad_PS) const;

  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(FuturePosteriorMeanEvaluator);

 private:
  //! spatial dimension (e.g., entries per point of ``points_sampled``)
  const int dim_;

  //! pointer to gaussian process used in computations
  const GaussianProcess * gaussian_process_;

  //! points to sample in the next sampling step
  const std::vector<double> to_sample_;

  //! number of points in the next sampling step
  const int num_to_sample_;

  //! the dims with derivatives
  const std::vector<int> to_sample_derivatives_;

  //! number of derivatives
  const int num_derivatives_;

  //! transpose(Chol)^-1 * (sampled Z)
  const std::vector<double> coeff_;
  const std::vector<double> coeff_combined_;
};

/*!\rst
  State object for OnePotentialSampleExpectedImprovementEvaluator.  This tracks the *ONE* ``point_to_sample``
  being evaluated via expected improvement.
  This is just a special case of ExpectedImprovementState; see those class docs for more details.
  See general comments on State structs in ``gpp_common.hpp``'s header docs.
\endrst*/
struct FuturePosteriorMeanState final {
  using EvaluatorType = FuturePosteriorMeanEvaluator;

  /*!\rst
    Constructs an OnePotentialSampleExpectedImprovementState object for the purpose of computing EI
    (and its gradient) over the specified point to sample.
    This establishes properly sized/initialized temporaries for EI computation, including dependent state from the
    associated Gaussian Process (which arrives as part of the ``ei_evaluator``).
    .. WARNING::
         This object is invalidated if the associated ei_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
      :ei_evaluator: expected improvement evaluator object that specifies the parameters & GP for EI evaluation
      :point_to_sample[dim]: point at which to evaluate EI and/or its gradient to check their value in future experiments (i.e., test point for GP predictions)
      :configure_for_gradients: true if this object will be used to compute gradients, false otherwise
  \endrst*/
  FuturePosteriorMeanState(const EvaluatorType& ps_evaluator, const int num_fidelity_in, double const * restrict point_to_sample_in, bool configure_for_gradients);

  FuturePosteriorMeanState(FuturePosteriorMeanState&& other);

  int GetProblemSize() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return dim - num_fidelity;
  }

  std::vector<double> BuildUnionOfPoints(double const * restrict points_to_sample) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> union_of_points(dim);
    std::copy(points_to_sample, points_to_sample + dim - num_fidelity, union_of_points.data());
    std::fill(union_of_points.data() + dim - num_fidelity, union_of_points.data() + dim, 1.0);
    return union_of_points;
  }

  /*!\rst
    Get ``point_to_sample``: the potential future sample whose EI (and/or gradients) is being evaluated
    \output
      :point_to_sample[dim]: potential sample whose EI is being evaluted
  \endrst*/
  void GetCurrentPoint(double * restrict point_to_sample_out) const noexcept OL_NONNULL_POINTERS {
    std::copy(point_to_sample.data(), point_to_sample.data() + dim - num_fidelity, point_to_sample_out);
  }

  void Initialize(const EvaluatorType& ps_evaluator);

  /*!\rst
    Change the potential sample whose EI (and/or gradient) is being evaluated.
    Update the state's derived quantities to be consistent with the new point.
    \param
      :ei_evaluator: expected improvement evaluator object that specifies the parameters & GP for EI evaluation
      :point_to_sample[dim]: potential future sample whose EI (and/or gradients) is being evaluated
  \endrst*/
  void SetCurrentPoint(const EvaluatorType& ps_evaluator,
                       double const * restrict point_to_sample_in) OL_NONNULL_POINTERS;

  /*!\rst
    Configures this state object with a new ``point_to_sample``, the location of the potential sample whose EI is to be evaluated.
    Ensures all state variables & temporaries are properly sized.
    Properly sets all dependent state variables (e.g., GaussianProcess's state) for EI evaluation.
    .. WARNING::
         This object's state is INVALIDATED if the ei_evaluator (including the GaussianProcess it depends on) used in
         SetupState is mutated! SetupState() should be called again in such a situation.
    \param
      :ei_evaluator: expected improvement evaluator object that specifies the parameters & GP for EI evaluation
      :point_to_sample[dim]: potential future sample whose EI (and/or gradients) is being evaluated
  \endrst*/
  void SetupState(const EvaluatorType& ps_evaluator,
                  double const * restrict point_to_sample_in) OL_NONNULL_POINTERS;

  // size information
  //! spatial dimension (e.g., entries per point of ``points_sampled``)
  const int dim;
  //! dim of the fidelity
  const int num_fidelity;
  //! number of points to sample (i.e., the "q" in q,p-EI); MUST be 1
  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;

  std::vector<double> K_star;
  std::vector<double> grad_K_star;

  UniformRandomGenerator randomGenerator;
  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(FuturePosteriorMeanState);
};

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

  This is a utility function just for reducing code duplication.

  \param
    :ei_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 EI 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-EI)
    :num_being_sampled: number of points being sampled concurrently (i.e., the p in q,p-EI)
    :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 SetupFuturePosteriorMeanState(
    const FuturePosteriorMeanEvaluator& fpm_evaluator,
    double const * restrict points_to_sample,
    int max_num_threads,
    bool configure_for_gradients,
    const int num_fidelity,
    std::vector<typename FuturePosteriorMeanEvaluator::StateType> * state_vector) {
  state_vector->reserve(max_num_threads);
  for (int i = 0; i < max_num_threads; ++i) {
    state_vector->emplace_back(fpm_evaluator, num_fidelity, points_to_sample, configure_for_gradients);
  }
}

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

  // special analytic case when we are not using (or not accounting for) multiple, simultaneous experiments
  FuturePosteriorMeanEvaluator ps_evaluator(gaussian_process, coefficient, to_sample,
                                            num_to_sample, to_sample_derivatives,
                                            num_derivatives, chol, train_sample);

  typename FuturePosteriorMeanEvaluator::StateType ps_state(ps_evaluator, num_fidelity, initial_guess, configure_for_gradients);

  GradientDescentOptimizer<FuturePosteriorMeanEvaluator, DomainType> gd_opt;
  gd_opt.Optimize(ps_evaluator, optimizer_parameters, domain, &ps_state);
  ps_state.GetCurrentPoint(best_next_point);
  best_objective_value = ps_evaluator.ComputeObjectiveFunction(&ps_state);
}

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

// template explicit instantiation declarations, see gpp_common.hpp header comments, item 6
extern template void ComputeOptimalFuturePosteriorMean(const GaussianProcess& gaussian_process, const int num_fidelity, double const * coefficient,
                                                       double const * to_sample, const int num_to_sample, int const * to_sample_derivatives,
                                                       int num_derivatives, double const * chol, double const * train_sample,
                                                       const GradientDescentParameters& optimizer_parameters, const TensorProductDomain& domain,
                                                       int max_num_threads, double const * restrict start_point_set,
                                                       int num_multistarts, double * restrict best_function_value,
                                                       double * restrict best_next_point);
extern template void ComputeOptimalFuturePosteriorMean(const GaussianProcess& gaussian_process, const int num_fidelity, double const * coefficient,
                                                       double const * to_sample, const int num_to_sample, int const * to_sample_derivatives,
                                                       int num_derivatives, double const * chol, double const * train_sample,
                                                       const GradientDescentParameters& optimizer_parameters, const SimplexIntersectTensorProductDomain& domain,
                                                       int max_num_threads, double const * restrict start_point_set,
                                                       int num_multistarts, double * restrict best_function_value,
                                                       double * restrict best_next_point);
}  // end namespace optimal_learning
#endif  // MOE_OPTIMAL_LEARNING_CPP_GPP_KNOWLEDGE_GRADIENT_INNER_OPTIMIZATION_HPP_


#pragma once

#include <vector>

#include "nifty/math/math.hxx"
#include "nifty/histogram/histogram.hxx"
#include "nifty/cgp/geometry.hxx"
#include "nifty/cgp/bounds.hxx"
#include "nifty/marray/marray.hxx"
#include "nifty/filters/gaussian_curvature.hxx"
#include "nifty/features/accumulated_features.hxx"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

namespace nifty{
namespace cgp{

    class Cell1CurvatureFeatures2D{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1CurvatureFeatures2D(
            const std::vector<float> & sigmas  = std::vector<float>({1.0f, 2.0f, 4.0f})
        )
        :   sigmas_(sigmas)
        {

        }

        size_t numberOfFeatures()const{
            return sigmas_.size() * AccType::NFeatures::value;
        }

        std::vector<std::string> names()const{

            std::string accNames [] = {
                std::string("Mean"),
                std::string("Sum"),
                std::string("Min"),
                std::string("Max"),
                std::string("Moment2"),
                std::string("Moment3"),
                std::string("Q0.10"),
                std::string("Q0.25"),
                std::string("Q0.50"),
                std::string("Q0.75"),
                std::string("Q0.90")
            };
            std::vector<std::string> res;
            auto baseName = std::string("GaussianCurvatureSigma");
            for(auto sigmaIndex=0; sigmaIndex<sigmas_.size(); ++sigmaIndex){
                auto name  = baseName + std::to_string(sigmas_[sigmaIndex]);
                for(const auto & accName : accNames){
                    res.push_back(name + accName);
                }

            }
            return res;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>  & cell1GeometryVector,
            const CellBoundedByVector<2,1> & cell1BoundedByVector,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> curvature;
            std::vector<float> buffer(AccType::NFeatures::value);

            for(auto sigmaIndex=0; sigmaIndex<sigmas_.size(); ++sigmaIndex){
                const auto sigma = sigmas_[sigmaIndex];
                nifty::filters::GaussianCurvature2D<> op(sigma, -1, 2.5);

                for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){
                    const auto & geo = cell1GeometryVector[cell1Index];

                    if(geo.size()>=4){

                        // we use a larger size?
                        if(curvature.capacity() < geo.size()){
                            curvature.resize(geo.size()*2);
                        }
                        else{
                            curvature.resize(geo.size());
                        }

                        // calculate curvature
                        //std::cout<<"    is closed "<<"\n";
                        const auto closedLine = cell1BoundedByVector[cell1Index].size() == 0;
                        //std::cout<<"    calculate curvature "<<"\n";
                        op(geo.begin(), geo.end(), curvature.begin(), closedLine);

                        // accumulate the values
                        AccType acc;
                        for(auto pass=0; pass < acc.requiredPasses(); ++pass){
                            for(const auto & c : curvature){
                                acc.acc(c, pass);
                            }
                        }
                        // write to buffer
                        acc.result(buffer.begin(), buffer.end());

                        // write to features out
                        const auto fIndex = sigmaIndex*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = buffer[afi];
                        }

                    }
                    else{
                        // write to features out
                        // a zero value seems legit since we
                        // assume a constant zero curvature
                        const auto fIndex = sigmaIndex*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = 0.0;
                        }

                    }
                }
            }
        }
    private:
        std::vector<float> sigmas_;
    };

    class Cell1LineSegmentDist2D{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1LineSegmentDist2D(
            const std::vector<size_t> & dists  = std::vector<size_t>({size_t(3),size_t(5),size_t(7)})
        )
        :   dists_(dists)
        {
        }

        size_t numberOfFeatures()const{
            return  dists_.size()*AccType::NFeatures::value;
        }

       std::vector<std::string> names()const{

            std::string accNames [] = {
                std::string("Mean"),
                std::string("Sum"),
                std::string("Min"),
                std::string("Max"),
                std::string("Moment2"),
                std::string("Moment3"),
                std::string("Q0.10"),
                std::string("Q0.25"),
                std::string("Q0.50"),
                std::string("Q0.75"),
                std::string("Q0.90")
            };
            std::vector<std::string> res;
            auto baseName = std::string("Cell1LineSegmentDistD");
            for(auto i=0; i<dists_.size(); ++i){
                auto name  = baseName + std::to_string(dists_[i]);
                for(const auto & accName : accNames){
                    res.push_back(name + accName);
                }

            }
            return res;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>  & cell1GeometryVector,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> buffer(AccType::NFeatures::value);

            typedef boost::geometry::model::d2::point_xy<double> point_type;
            typedef boost::geometry::model::linestring<point_type> linestring_type;

            for(auto di=0; di<dists_.size(); ++di){
                const auto ld = dists_[di];
                for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){
                    const auto & geo = cell1GeometryVector[cell1Index];
                    if(geo.size()>=4){

                        AccType acc;

                        for(auto pass=0; pass < acc.requiredPasses(); ++pass){

                            for(auto i=0; i<geo.size()-1; ++i){
                                const auto j = std::min(int(i + ld), int(geo.size()-1));
                                const auto & pS = geo[i];
                                const auto & pE = geo[j];

                                linestring_type line;
                                line.push_back(point_type(pS[0], pS[1]));
                                line.push_back(point_type(pE[0], pE[1]));

                                for(auto ii=i+1; ii<j-1; ++ii){
                                    const point_type p(geo[ii][0], geo[ii][1]);
                                    const auto d = boost::geometry::distance(p, line);
                                    acc.acc(d, pass);
                                }
                            }
                        }
                        // write to buffer
                        acc.result(buffer.begin(), buffer.end());

                        // write to features out
                        const auto fIndex = di*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = buffer[afi];
                        }

                    }
                    else{
                        // write to features out
                        // a zero value seems legit since we
                        // assume a constant zero curvature
                        const auto fIndex = di*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = 0.0;
                        }
                    }
                }
            }
        }
    private:
        std::vector<size_t> dists_;
    };

    class Cell1BasicGeometricFeatures2D{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1BasicGeometricFeatures2D(){
        }

        size_t numberOfFeatures()const{
            return 4 * 4 +  2*AccType::NFeatures::value + 4;
        }

        std::vector<std::string> names()const{

            std::string accNames [] = {
                std::string("Mean"),
                std::string("Sum"),
                std::string("Min"),
                std::string("Max"),
                std::string("Moment2"),
                std::string("Moment3"),
                std::string("Q0.10"),
                std::string("Q0.25"),
                std::string("Q0.50"),
                std::string("Q0.75"),
                std::string("Q0.90")
            };

            std::vector<std::string> res;
            const auto baseName = std::string("BasicGeometricFeatures");
            auto insertUVFeat = [&](const std::string & name){
                res.push_back(baseName+name+std::string("UV-Min"));
                res.push_back(baseName+name+std::string("UV-Max"));
                res.push_back(baseName+name+std::string("UV-Sum"));
                res.push_back(baseName+name+std::string("UV-AbsDiff"));
            };
            auto insertStatFeat = [&](const std::string & name){
                for(const auto & an : accNames){
                    res.push_back(baseName+name+an);
                }
            };

            res.push_back(baseName+std::string("EdgeSize"));
            insertUVFeat("NodeSize");
            insertUVFeat("NodeEdgeSizeRatio");

            res.push_back(baseName+std::string("EdgeEndpointDistance"));
            res.push_back(baseName+std::string("EdgeRelativeEndpointDistance"));
            res.push_back(baseName+std::string("NodeEndpointDistance"));

            insertUVFeat("EdgeNodeCenterOfMassDist");
            insertUVFeat("EdgeNodeCenterOfMassRatio");

            insertStatFeat("CenterOfMassCell1PointsDist");
            insertStatFeat("CenterOfMassCell2PointsDist");

            return res;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>   & cell1GeometryVector,
            const CellGeometryVector<2,2>   & cell2GeometryVector,
            const CellBoundsVector<2,1>     & cell1BoundsVector,
            nifty::marray::View<T> & features
        )const{

            using namespace nifty::math;
            std::vector<float> buffer(AccType::NFeatures::value);
            for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){

                const auto & cell1Bounds = cell1BoundsVector[cell1Index];
                const auto cell2UIndex = cell1Bounds[0]-1;
                const auto cell2VIndex = cell1Bounds[1]-1;

                auto fIndex = 0;

                auto insertCell2ValFeats = [&](const float uVal, const float vVal){
                    features(cell1Index, fIndex++) = std::min(uVal, vVal);
                    features(cell1Index, fIndex++) = std::max(uVal, vVal);
                    features(cell1Index, fIndex++) = uVal + vVal;
                    features(cell1Index, fIndex++) = std::abs(uVal-vVal);
                };

                const auto & geoE = cell1GeometryVector[cell1Index];
                const auto & geoU = cell2GeometryVector[cell2UIndex];
                const auto & geoV = cell2GeometryVector[cell2VIndex];

                // size based features
                const auto eSize = float(geoE.size());
                const auto uSize = float(geoU.size());
                const auto vSize = float(geoV.size());

                features(cell1Index, fIndex++) = eSize;
                insertCell2ValFeats(uSize, vSize);

                // size ratios
                const auto uNSize = std::sqrt(uSize);
                const auto vNSize = std::sqrt(vSize);
                {
                    const auto ratU =  uNSize/eSize;
                    const auto ratV =  vNSize/eSize;
                    insertCell2ValFeats(ratU, ratV);
                }

                // endpoint distance and ratios
                const auto endpointDistance = euclideanDistance(geoE.front(), geoE.back());
                features(cell1Index, fIndex++) = endpointDistance;
                features(cell1Index, fIndex++) = endpointDistance/eSize;

                // distance between cell2CenterOfMass  and cell1CenterOfMass
                const auto  comE = geoE.centerOfMass();
                const auto  comU = geoU.centerOfMass();
                const auto  comV = geoV.centerOfMass();
                {
                    const auto dUV  = euclideanDistance(comU, comV);
                    const auto dUE  = euclideanDistance(comU, comE);
                    const auto dVE  = euclideanDistance(comV, comE);

                    features(cell1Index, fIndex++) = dUV;
                    insertCell2ValFeats(dUE, dVE);

                    // distance ratios between cell2CenterOfMass  and cell1CenterOfMass

                    const auto ratU =  uNSize/dUE;
                    const auto ratV =  vNSize/dVE;
                    insertCell2ValFeats(ratU, ratV);
                }

                // statistic  over ||centerOfMass-points||  for cell 1
                {
                    AccType acc;
                    for(auto i=0; i<geoE.size(); ++i){
                        const auto & coord = geoE[i];
                        const auto d  = nifty::math::euclideanDistance(coord, comE);
                        acc.acc(d);
                    }
                    // write to buffer
                    acc.result(buffer.begin(), buffer.end());
                    // write results
                    for(auto i=0; i<buffer.size(); ++i){
                        features(cell1Index,  fIndex++) = buffer[i];
                    }
                }

                // statistic  over ||centerOfMass_cell1-points_cell2||
                {
                    AccType acc;

                    for(auto i=0; i<geoU.size(); ++i){
                        const auto & coord = geoU[i];
                        const auto d  = nifty::math::euclideanDistance(coord, comE);
                        acc.acc(d);
                    }
                    for(auto i=0; i<geoV.size(); ++i){
                        const auto & coord = geoV[i];
                        const auto d  = nifty::math::euclideanDistance(coord, comE);
                        acc.acc(d);
                    }

                    // write to buffer and write results
                    acc.result(buffer.begin(), buffer.end());

                    for(auto i=0; i<buffer.size(); ++i){
                        features(cell1Index,  fIndex++) = buffer[i];
                    }

                }

                // angle between cell2CenterOfMass  and cell1CenterOfMass
                // ...

                NIFTY_CHECK_OP(fIndex,==,numberOfFeatures(),"internal error");

            }

        }
    private:
        std::vector<size_t> dists_;
    };

    /**
    class GeometricAccumulator{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        GeometricAccumulator(
            const std::vector<size_t> & dists  = std::vector<size_t>({size_t(3),size_t(5),size_t(7)})
        )
        :   dists_(dists)
        {
        }

        size_t numberOfFeatures()const{
            return  dists_.size()*AccType::NFeatures::value;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>  & cell1GeometryVector,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> buffer(AccType::NFeatures::value);

            typedef boost::geometry::model::d2::point_xy<double> point_type;
            typedef boost::geometry::model::linestring<point_type> linestring_type;

            for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){

                const auto & geo = cell1GeometryVector[cell1Index];

            }

        }
    private:
        std::vector<size_t> dists_;
    };
    */

}
}


// Copyright (c) 2018-2021 FRC Team 3512. All Rights Reserved.

#pragma once

#include <Eigen/Core>
#include <frc/controller/LinearQuadraticRegulator.h>
#include <frc/estimator/KalmanFilter.h>
#include <frc/logging/CSVLogFile.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/LinearSystemLoop.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <units/length.h>
#include <units/velocity.h>

#include "Constants.hpp"

namespace frc3512 {

class ClimberController {
public:
    // State tolerances in meters and meters/sec respectively.
    static constexpr double kPositionTolerance = 0.05;
    static constexpr double kVelocityTolerance = 2.0;

    ClimberController();

    ClimberController(const ClimberController&) = delete;
    ClimberController& operator=(const ClimberController&) = delete;

    /**
     * Enables the control loop.
     */
    void Enable();

    /**
     * Disables the control loop.
     */
    void Disable();

    /**
     * Sets the end goal of the controller profile.
     *
     * @param goal Position in meters to set the goal to.
     */
    void SetGoal(double goal);

    /**
     * Sets the references.
     *
     * @param position Position of the carriage in meters.
     * @param velocity Velocity of the carriage in meters per second.
     */
    void SetReferences(units::meter_t position,
                       units::meters_per_second_t velocity);

    /**
     * Returns whether or not the goal has been reached.
     */
    bool AtGoal() const;

    /**
     * Returns whether or not position and velocity are tracking the profile.
     */
    bool AtReferences() const;

    /**
     * Sets the current encoder measurement.
     *
     * @param measuredPosition Position of the carriage in meters.
     */
    void SetMeasuredPosition(double measuredPosition);

    /**
     * Returns the control loop calculated voltage.
     */
    double ControllerVoltage();

    /**
     * Returns the estimated position.
     */
    double EstimatedPosition() const;

    /**
     * Returns the estimated velocity.
     */
    double EstimatedVelocity() const;

    /**
     * Returns the error between the position reference and the position
     * estimate.
     */
    double PositionError() const;

    /**
     * Returns the error between the velocity reference and the velocity
     * estimate.
     */
    double VelocityError() const;

    /**
     * Returns the current position reference set by the profile.
     */
    double PositionReference();

    /**
     * Returns the current velocity reference set by the profile.
     */
    double VelocityReference();

    /**
     * Executes the control loop for a cycle.
     */
    void Update();

    /**
     * Resets any internal state.
     */
    void Reset();

private:
    // The current sensor measurement.
    Eigen::Matrix<double, 1, 1> m_y;
    frc::TrapezoidProfile<units::meters>::State m_goal;

    frc::TrapezoidProfile<units::meters>::Constraints constraints{
        Constants::Climber::kMaxV, Constants::Climber::kMaxA};
    frc::TrapezoidProfile<units::meters> m_positionProfile{constraints,
                                                           {0_m, 0_mps}};

    // The current references from the profile.
    frc::TrapezoidProfile<units::meters>::State m_profiledReference;

    frc::LinearSystem<2, 1, 1> m_plant = [=] {
        constexpr auto motor = frc::DCMotor::Vex775Pro();

        // Robot mass
        constexpr auto m = 63.503_kg;

        // Radius of axle
        constexpr auto r = 0.003175_m;

        // Gear ratio
        constexpr double G = 50.0 / 1.0;

        return frc::LinearSystemId::ElevatorSystem(motor, m, r, G);
    }();
    frc::LinearQuadraticRegulator<2, 1> m_controller{
        m_plant, {0.02, 0.4}, {12.0}, Constants::kDt};
    frc::KalmanFilter<2, 1, 1> m_observer{
        m_plant, {0.05, 1.0}, {0.0001}, Constants::kDt};
    frc::LinearSystemLoop<2, 1, 1> m_loop{m_plant, m_controller, m_observer,
                                          12_V, Constants::kDt};

    bool m_isEnabled = false;

    bool m_atReferences = false;

    frc::CSVLogFile climberLogger{"Climber",      "EstPos (m)",
                                  "PosRef (m)",   "Voltage (V)",
                                  "EstVel (m/s)", "VelRef (m/s)"};
};
}  // namespace frc3512


#include <MetaScale.h>
#include <boost/math/common_factor_rt.hpp>

void MetaScale::normalize() {
  uint32_t a = mNum<0?-mNum:mNum;
  uint32_t gcd = boost::math::gcd(a,mDenom);
  mNum   /= gcd;
  mDenom /= gcd;
}

MetaScale& MetaScale::operator*=(const MetaScale& b) {
  mNum*=b.num();
  mDenom*=b.denom();
  normalize();
  return *this;
}

MetaScale& MetaScale::operator/=(const MetaScale& b) {
  uint32_t temp = mNum*b.denom();
  mDenom*=b.num();
  mNum = temp;
  normalize();
  return *this;
}
MetaScale MetaScale::operator*(const MetaScale& b) const {
  return MetaScale(*this) *= b;
}

MetaScale MetaScale::operator/(const MetaScale& b) const {
  return MetaScale(*this) /= b;
}


#include <gtest/gtest.h>
#include "../include/mathWrapper/double.h"
#include "../include/mathWrapper/boost.h"
#include "../include/Eigen.h"
#include "../include/mathWrapper/eigen.h"
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include <iostream>

TEST(discreteDiscreteKalmanFilter, test){
	int totalCount = 10000;

	vectorDouble * vdR_arr = new vectorDouble[totalCount];
	Eigen::VectorXd * vesR_arr = new Eigen::VectorXd[totalCount];
	vectorBoost * vbR_arr = new vectorBoost[totalCount];
	vectorEigen * veR_arr = new vectorEigen[totalCount];

	//covariance: [1.00000,0.20000;0.20000,2.00000]
	//chol(covariance): [1.0000,0.2000;0.0000,1.4000]

	Eigen::MatrixXd cholCov(2,2);cholCov<<1.0,0.2,0.0,1.4;Eigen::VectorXd mean1(2);mean1<<2.0,1.3;
	boost::numeric::ublas::matrix<double> cholCov2(2,2);boost::numeric::ublas::vector<double> mean2(2);
	mean2(0) = 2.0; mean2(1) = 1.3;
	cholCov2(0,0)=1;cholCov2(0,1)=0.2;cholCov2(1,0)=0.0;cholCov2(1,1)=1.4;
	matrixEigen cholCov3(cholCov);vectorEigen mean3(mean1);
	for(int i = 0; i < totalCount; i++){
		vectorDouble vdR = vectorDouble::randomVector(1,i*i);
		vdR_arr[i] = vdR*matrixDouble(0.1) + vectorDouble(5.0);

		Eigen::VectorXd vesR = Eigen::VectorXd::randomVector(2,i*i);
		vesR_arr[i] = cholCov*vesR + mean1;

		vectorBoost vbR = vectorBoost::randomVector(2,i*i);
		vbR_arr[i] = matrixBoost(cholCov2)*vbR + mean2;

		vectorEigen veR = vectorEigen::randomVector(2,i*i);
		veR_arr[i] = cholCov3*veR + mean3;
	}

	vectorDouble vdSum(0);
	Eigen::VectorXd evsSum(2);evsSum<<0.0,0.0;
	boost::numeric::ublas::vector<double> zero(2);zero(0)=0;zero(1)=0;
	vectorBoost vbSum(zero);
	vectorEigen evSum(evsSum);
	for(int i = 0; i < totalCount; i++){
		vdSum = vdSum + vdR_arr[i];
		evsSum = evsSum + vesR_arr[i];
		vbSum = vbSum + vbR_arr[i];
		evSum = evSum + veR_arr[i];
	}
	double vdSumMean = vdSum.getSystemValue()/double(totalCount);
	ASSERT_NEAR((vdSumMean)/5.0,1.0,0.001);

	double vdCovariance = 0;

	for(int i = 0; i < totalCount; i++){
		vdCovariance+=(vdR_arr[i].getSystemValue()-vdSumMean)*(vdR_arr[i].getSystemValue() - vdSumMean);
	}
	vdCovariance = vdCovariance/totalCount;
	ASSERT_NEAR(vdCovariance/0.01,1,0.01);
}

int main(int argc, char **argv){
	testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      121128    R.C.A. Boon       File created (in progress).
 *      130124    R.C.A. Boon       Removed retrograde function, reworked Kepler to MEE conversion
 *                                  to properly convert inclination to range [0,180) and use
 *                                  retrograde factor internally
 *      130131    R.C.A. Boon       Added Cartesian conversion cases, added avoidSingularityAtPi~
 *                                  boolean flag to Kepler conversions, optimized computation
 *                                  (partially)
 *      130225    D. Dirkx          Added overloaded function for Kepler to MEE that determines
 *                                  retrogradeness based on Kepler state
 *      130301    R.C.A. Boon       Updated use of mathematics::PI to basic_mathematics::
 *                                  mathematical_constants::PI, minor textual changes.
 *      130305    R.C.A. Boon       Replaced Eigen::VectorXd by basic_mathematics::Vector6d
 *
 *    References
 *      Verified Interval Propagation, Bart Rˆmgens; Delft (2011). Code archive.
 *      Modified Equinoctial Orbital Elements, author unknown;
 *          http://www.cdeagle.com/pdf/mee.pdf (2010?).
 *      Survey of Orbital Element Sets, Gerald R. Hintz; Journal of Guidance, Control and
 *          Dynamics (2008, Vol. 31 - Nr. 3).
 *      Code archive, E. Heeren (fellow Tudat developer).
 *
 *    Notes
 *
 */

#include <cmath>

#include <boost/exception/all.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/missionGeometry.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/modifiedEquinoctialElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"

namespace tudat
{

namespace orbital_element_conversions
{

//! Convert Keplerian to modified equinoctial orbital elements using implicit MEE equation set.
basic_mathematics::Vector6d convertKeplerianToModifiedEquinoctialElements(
        const basic_mathematics::Vector6d& keplerianElements )
// Based on Hintz, 2008.
{
    // Check if orbit is retrograde
    bool avoidSingularityAtPiInclination =
            mission_geometry::isOrbitRetrograde( keplerianElements );

    // Convert to modified equinoctial elements.
    return convertKeplerianToModifiedEquinoctialElements( keplerianElements,
                                                          avoidSingularityAtPiInclination );
}

//! Convert Keplerian to modified equinoctial orbital elements using MEE explicit equation set.
basic_mathematics::Vector6d convertKeplerianToModifiedEquinoctialElements(
        const basic_mathematics::Vector6d& keplerianElements,
        const bool avoidSingularityAtPiInclination )
// Based on Hintz, 2008.
{
    using mathematical_constants::PI;

    // Declaring eventual output vector.
    basic_mathematics::Vector6d modifiedEquinoctialState( 6 );

    // Compute semi-latus rectum.
    double singularityTolerance = 1.0e-15; // Based on tolerance chosen in
                                           // orbitalElementConversions.cpp in Tudat Core.

    // Extracting eccentricity for ease of referencing.
    double eccentricity = keplerianElements( eccentricityIndex );

    // If e is (very near) one, then semi-major axis is undefined and thus the first kepler
    // element is the semi-latus rectum.
    if ( std::fabs( eccentricity - 1.0 ) < singularityTolerance )
    {
        modifiedEquinoctialState( semiLatusRectumIndex )
                = keplerianElements( semiLatusRectumIndex );
    }
    else // eccentricity is significantly away from singularity and can be computed with p=a(1-e≤).
    {
        modifiedEquinoctialState( semiLatusRectumIndex ) = keplerianElements( semiMajorAxisIndex )
                                                           * ( 1.0 - eccentricity * eccentricity );
    }

    // Determine prograde-ness, as other five parameters depend on that fact.
    double inclination = keplerianElements( inclinationIndex );

    // If inclination is outside range [0,PI].
    if ( ( inclination < 0.0 ) || ( inclination > PI ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "Inclination is expected in range [0," << PI << "]\n"
                     << "Specified inclination: " << inclination << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }
    //Else, nothing wrong and continue.

    // Compute set dependant helper parameters.
    double argumentOfPeriapsisAndAscendingNode = 0.0;
    double tangentOfHalfInclination = 0.0;

    // If normal set of equations is to be used (i.e. not inverse set for singularity).
    if ( !avoidSingularityAtPiInclination )
    {
        // Add (+) argument of periapsis and longitude of ascending node.
        argumentOfPeriapsisAndAscendingNode = keplerianElements( argumentOfPeriapsisIndex )
                + keplerianElements( longitudeOfAscendingNodeIndex );

        // Take the tangent of half the inclination.
        tangentOfHalfInclination = std::tan( inclination / 2.0 );
    }
    else // the orbit is retrograde.
    {
        // Subtract (-) longitude of ascending node from argument of periapsis.
        argumentOfPeriapsisAndAscendingNode = keplerianElements( argumentOfPeriapsisIndex )
                - keplerianElements( longitudeOfAscendingNodeIndex );

        // Take the inverse of the tangent of half the inclination to avoid singularity at tan
        // PI/2.
        tangentOfHalfInclination = 1.0 / std::tan( inclination / 2.0 );
    }

    // Compute f-element.
    modifiedEquinoctialState( fElementIndex ) = eccentricity
            * std::cos( argumentOfPeriapsisAndAscendingNode );

    // Compute g-element.
    modifiedEquinoctialState( gElementIndex ) = eccentricity
            * std::sin( argumentOfPeriapsisAndAscendingNode );

    // Compute h-element.
    modifiedEquinoctialState( hElementIndex ) = tangentOfHalfInclination
            * std::cos( keplerianElements( longitudeOfAscendingNodeIndex ) );

    // Compute k-element.
    modifiedEquinoctialState( kElementIndex ) = tangentOfHalfInclination
            * std::sin( keplerianElements( longitudeOfAscendingNodeIndex ) );

    // Compute true longitude (modulo 2 PI to keep within interval -2PI to 2PI).
    modifiedEquinoctialState( trueLongitudeIndex )
            = basic_mathematics::computeModulo(
                argumentOfPeriapsisAndAscendingNode
                + keplerianElements( trueAnomalyIndex ), 2.0 * PI );

    // Give back result.
    return modifiedEquinoctialState;
}

//! Convert modified equinoctial to Keplerian orbital elements.
basic_mathematics::Vector6d convertModifiedEquinoctialToKeplerianElements(
        const basic_mathematics::Vector6d& modifiedEquinoctialElements,
        const bool avoidSingularityAtPiInclination )
// Using unknown source pdf, code archive E. Heeren and personal derivation based on Hintz 2008.
{
    using mathematical_constants::PI;

    // Declaration of output vector.
    basic_mathematics::Vector6d convertedKeplerianElements = basic_mathematics::
            Vector6d::Zero( 6 );

    // for ease of referencing, almost all modified equinoctial elements.
    double fElement = modifiedEquinoctialElements( fElementIndex );
    double gElement = modifiedEquinoctialElements( gElementIndex );
    double hElement = modifiedEquinoctialElements( hElementIndex );
    double kElement = modifiedEquinoctialElements( kElementIndex );

    // Tolerance for singularities.
    double singularityTolerance = 1.0e-15;

    // Compute eccentricity.
    double eccentricity = std::sqrt( fElement * fElement + gElement * gElement );
    convertedKeplerianElements( eccentricityIndex ) = eccentricity;

    // Compute semi-major axis.
    // If eccentricity is not near-parabolic.
    if ( std::fabs( eccentricity - 1.0 ) > singularityTolerance )
    {
        // Use semi-latus rectum and eccentricity to calculate semi-major axis with a=p/(1-e^2).
        convertedKeplerianElements( semiMajorAxisIndex ) =
                modifiedEquinoctialElements( semiLatusRectumIndex ) /
                ( 1.0 - eccentricity * eccentricity );
    }
    // This is (almost) a parabola and the semimajor axis will tend to infinity and be useless.
    else
    {
        // Give semi-latus rectum instead.
        convertedKeplerianElements( semiLatusRectumIndex ) =
                modifiedEquinoctialElements( semiLatusRectumIndex );
    }

    // Compute longitude of ascending node.

    // Compute solution in [-PI,PI] interval with atan2.
    double longitudeOfAscendingNode = std::atan2( kElement, hElement );

    // Store longitude of ascending node.
    convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = basic_mathematics::
            computeModulo( longitudeOfAscendingNode, 2.0 * PI );

    // Compute inclination.

    // If prograde factor must be positive, otherwise negative.
    double retrogradeFactor = avoidSingularityAtPiInclination ? -1.0 : 1.0;

    // Calculate magnitude of inclination with retrogradeFactor (derived based on Hintz, 2008).
    double hSquaredPlusKSquared = std::pow( hElement * hElement + kElement * kElement ,
                                            retrogradeFactor );

    convertedKeplerianElements( inclinationIndex ) =
            2.0 * std::atan( std::sqrt( hSquaredPlusKSquared ) );
    // Was: std::atan2(2.0 * std::sqrt(hSquaredPlusKSquared) , (1.0 - hSquaredPlusKSquared));

    // Compute argument of periapsis.

    // Helper quantity (omega + I * OMEGA ), for argument of periapsis and true anomaly.
    double argumentOfPeriapsisAndLongitude;

    // If eccentricity is (near) circular
    if ( eccentricity < singularityTolerance )
        // Then the composite argument of periapsis and longitude together cannot be determined
        // because both arguments of the atan2 functions will be zero.
    {
        // Set argument of periapsis to zero.
        convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0;

        // Set composite argument to longitude only, since argument of periapsis is now zero.
        argumentOfPeriapsisAndLongitude = retrogradeFactor * longitudeOfAscendingNode;
    }
    else
        // Argument of periapsis can be found from composite argument.
    {
        // Compute composite argument.
        argumentOfPeriapsisAndLongitude = std::atan2( gElement, fElement );

        // Compute argument of periapsis.
        convertedKeplerianElements( argumentOfPeriapsisIndex ) = basic_mathematics::
                computeModulo( argumentOfPeriapsisAndLongitude
                               - retrogradeFactor * longitudeOfAscendingNode, 2.0 * PI );
    }

    // Compute true anomaly.
    convertedKeplerianElements( trueAnomalyIndex ) = basic_mathematics::
            computeModulo( modifiedEquinoctialElements( trueLongitudeIndex )
                           - argumentOfPeriapsisAndLongitude, 2.0 * PI );

    // Return converted elements.
    return convertedKeplerianElements;
}

//! Convert Cartesian to modified equinoctial orbital elements using implicit MEE equation set.
basic_mathematics::Vector6d convertCartesianToModifiedEquinoctialElements(
        const basic_mathematics::Vector6d& cartesianElements,
        const double centralBodyGravitationalParameter )
{
    using mathematical_constants::PI;

    // Convert to keplerian elements.
    basic_mathematics::Vector6d keplerianElements = convertCartesianToKeplerianElements(
                cartesianElements, centralBodyGravitationalParameter );

    // Check whether orbit is retrograde.
    bool avoidSingularityAtPiInclination =
            mission_geometry::isOrbitRetrograde( keplerianElements );

    // Convert to modified equinoctial elements.
    return convertKeplerianToModifiedEquinoctialElements(
                keplerianElements, avoidSingularityAtPiInclination );
}

//! Convert Cartesian to modified equinoctial orbital elements using explicit MEE equation set.
basic_mathematics::Vector6d convertCartesianToModifiedEquinoctialElements(
        const basic_mathematics::Vector6d& cartesianElements,
        const double centralBodyGravitationalParameter,
        const bool avoidSingularityAtPiInclination )
{
    // Convert Cartesian to Keplerian to modified equinoctial elements.
    return convertKeplerianToModifiedEquinoctialElements(
                convertCartesianToKeplerianElements(
                    cartesianElements, centralBodyGravitationalParameter ),
                avoidSingularityAtPiInclination );
}

//! Convert Modified Equinoctial Elements to Cartesian Elements.
basic_mathematics::Vector6d convertModifiedEquinoctialToCartesianElements(
        const basic_mathematics::Vector6d& modifiedEquinoctialElements,
        const double centralBodyGravitationalParameter,
        const bool avoidSingularityAtPiInclination )
// Using unnamed pdf and code archive Bart Rˆmgens.
{
    // Creating output vector.
    basic_mathematics::Vector6d convertedCartesianElements = basic_mathematics::
            Vector6d::Zero( 6 );

    // If the prograde equations are to be used.
    if ( !avoidSingularityAtPiInclination )
    {
        // Local storage of elements for ease of access.
        double semiLatusRectum = modifiedEquinoctialElements( semiLatusRectumIndex );
        double fElement = modifiedEquinoctialElements( fElementIndex );
        double gElement = modifiedEquinoctialElements( gElementIndex );
        double hElement = modifiedEquinoctialElements( hElementIndex );
        double kElement = modifiedEquinoctialElements( kElementIndex );
        double trueLongitude = modifiedEquinoctialElements( trueLongitudeIndex );

        // Computing helper parameters.

        // Square-root
        double squareRootOfGravitationalParameterOverSemiLatusRectum =
                std::sqrt( centralBodyGravitationalParameter / semiLatusRectum );

        // Cosines and sine.
        double cosineTrueLongitude = std::cos( trueLongitude );
        double sineTrueLongitude = std::sin( trueLongitude );

        // s≤, a≤, w, r from references.
        double sSquaredParameter = 1.0 + hElement * hElement + kElement * kElement;
        double aSquaredParameter = hElement * hElement - kElement*kElement;
        double wParameter = 1.0 + fElement * cosineTrueLongitude
                + gElement * sineTrueLongitude;
        double radius = semiLatusRectum / wParameter;

        // Computing position and storing (using code archive Bart Rˆmgens and unnamed pdf).
        convertedCartesianElements( xCartesianPositionIndex )
                = radius / sSquaredParameter * ( cosineTrueLongitude + aSquaredParameter
                                                 * cosineTrueLongitude + 2.0 * hElement * kElement
                                                 * sineTrueLongitude );
        convertedCartesianElements( yCartesianPositionIndex )
                = radius / sSquaredParameter * ( sineTrueLongitude - aSquaredParameter
                                                 * sineTrueLongitude + 2.0 * hElement * kElement
                                                 * cosineTrueLongitude );
        convertedCartesianElements( zCartesianPositionIndex ) = 2.0 * radius / sSquaredParameter
                * ( hElement * sineTrueLongitude - kElement * cosineTrueLongitude );

        // Computing velocities (these can probably use more optimal computation).
        convertedCartesianElements( xCartesianVelocityIndex ) = -1.0 / sSquaredParameter
                * squareRootOfGravitationalParameterOverSemiLatusRectum
                * ( sineTrueLongitude + aSquaredParameter * sineTrueLongitude
                    - 2.0 * hElement * kElement * cosineTrueLongitude + gElement
                    - 2.0 * fElement * hElement * kElement + aSquaredParameter * gElement );
        convertedCartesianElements( yCartesianVelocityIndex ) = -1.0 / sSquaredParameter
                * squareRootOfGravitationalParameterOverSemiLatusRectum
                * ( -cosineTrueLongitude + aSquaredParameter * cosineTrueLongitude
                    + 2.0 * hElement * kElement * sineTrueLongitude - fElement
                    + 2.0 * gElement * hElement * kElement + aSquaredParameter * fElement );
        convertedCartesianElements( zCartesianVelocityIndex ) = 2.0 / sSquaredParameter
                * squareRootOfGravitationalParameterOverSemiLatusRectum
                * ( hElement * cosineTrueLongitude + kElement * sineTrueLongitude
                    + fElement * hElement + gElement * kElement );
    }
    else // use the indirect transformation via an intermediate Kepler state.
    {
        // Use the Keplerian state as an intermediary step, as there is no direct transformation
        // available in literature (personal derivation forgone for now due to time constraints).
        convertedCartesianElements =
                convertKeplerianToCartesianElements(
                    convertModifiedEquinoctialToKeplerianElements(
                        modifiedEquinoctialElements, avoidSingularityAtPiInclination ),
                    centralBodyGravitationalParameter );
    }

    // Return converted set of elements.
    return convertedCartesianElements;
}

} // namespace orbital_element_conversions

} // namespace tudat


#include "refill/system_models/linear_system_model.h"

#include <gtest/gtest.h>

#include <Eigen/Dense>

namespace refill {

TEST(LinearSystemModelTest, ConstructorTest) {
  // test default constructor
  LinearSystemModel* system_model_p = new LinearSystemModel;

  EXPECT_EQ(0, system_model_p->getStateDim());
  EXPECT_EQ(0, system_model_p->getInputDim());
  EXPECT_EQ(0, system_model_p->getNoiseDim());

  delete system_model_p;

  GaussianDistribution system_noise(Eigen::Vector2d::Zero(),
                                    Eigen::Matrix2d::Identity());

  // test constructor with system mapping and noise
  system_model_p = new LinearSystemModel(Eigen::Matrix2d::Identity(),
                                         system_noise);

  EXPECT_EQ(2, system_model_p->getStateDim());
  EXPECT_EQ(0, system_model_p->getInputDim());
  EXPECT_EQ(2, system_model_p->getNoiseDim());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), system_model_p->getSystemMapping());
  EXPECT_EQ(Eigen::MatrixXd::Zero(0, 0), system_model_p->getInputMapping());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), system_model_p->getNoiseMapping());

  delete system_model_p;

  // test constructor with system mapping, noise and input mapping
  system_model_p = new LinearSystemModel(Eigen::Matrix2d::Identity(),
                                         system_noise,
                                         Eigen::Matrix2d::Identity());

  EXPECT_EQ(2, system_model_p->getStateDim());
  EXPECT_EQ(2, system_model_p->getInputDim());
  EXPECT_EQ(2, system_model_p->getNoiseDim());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), system_model_p->getSystemMapping());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), system_model_p->getInputMapping());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), system_model_p->getNoiseMapping());

  delete system_model_p;

  // test constructor with system noise and system, input and noise mapping
  system_model_p = new LinearSystemModel(Eigen::Matrix2d::Identity(),
                                         system_noise,
                                         Eigen::Matrix2d::Identity(),
                                         Eigen::Matrix2d::Ones());

  EXPECT_EQ(2, system_model_p->getStateDim());
  EXPECT_EQ(2, system_model_p->getInputDim());
  EXPECT_EQ(2, system_model_p->getNoiseDim());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), system_model_p->getSystemMapping());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), system_model_p->getInputMapping());
  EXPECT_EQ(Eigen::Matrix2d::Ones(), system_model_p->getNoiseMapping());

  delete system_model_p;
}

TEST(LinearSystemModelTest, SetterTest) {
  GaussianDistribution system_noise(Eigen::Vector2d::Zero(),
                                    Eigen::Matrix2d::Identity());

  LinearSystemModel system_model;

  system_model.setModelParameters(Eigen::Matrix2d::Identity(), system_noise);

  EXPECT_EQ(2, system_model.getStateDim());
  EXPECT_EQ(0, system_model.getInputDim());
  EXPECT_EQ(2, system_model.getNoiseDim());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), system_model.getSystemMapping());
  EXPECT_EQ(Eigen::MatrixXd::Zero(0, 0), system_model.getInputMapping());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), system_model.getNoiseMapping());

  system_noise.setDistributionParameters(Eigen::Vector3d::Zero(),
                                         Eigen::Matrix3d::Identity());

  system_model.setModelParameters(Eigen::Matrix3d::Identity(), system_noise,
                                   Eigen::Matrix3d::Identity());

  EXPECT_EQ(3, system_model.getStateDim());
  EXPECT_EQ(3, system_model.getInputDim());
  EXPECT_EQ(3, system_model.getNoiseDim());
  EXPECT_EQ(Eigen::Matrix3d::Identity(), system_model.getSystemMapping());
  EXPECT_EQ(Eigen::Matrix3d::Identity(), system_model.getInputMapping());
  EXPECT_EQ(Eigen::Matrix3d::Identity(), system_model.getNoiseMapping());

  system_noise.setDistributionParameters(Eigen::Vector4d::Zero(),
                                         Eigen::Matrix4d::Identity());

  system_model.setModelParameters(Eigen::Matrix4d::Identity(), system_noise,
                                   Eigen::Matrix4d::Identity(),
                                   Eigen::Matrix4d::Ones());

  EXPECT_EQ(4, system_model.getStateDim());
  EXPECT_EQ(4, system_model.getInputDim());
  EXPECT_EQ(4, system_model.getNoiseDim());
  EXPECT_EQ(Eigen::Matrix4d::Identity(), system_model.getSystemMapping());
  EXPECT_EQ(Eigen::Matrix4d::Identity(), system_model.getInputMapping());
  EXPECT_EQ(Eigen::Matrix4d::Ones(), system_model.getNoiseMapping());
}

TEST(LinearSystemModelTest, GetterTest) {
  GaussianDistribution system_noise(Eigen::Vector2d::Zero(),
                                    Eigen::Matrix2d::Identity());

  LinearSystemModel system_model(Eigen::Matrix2d::Identity(), system_noise,
                                 Eigen::Matrix2d::Ones(),
                                 Eigen::Matrix2d::Constant(2.0));

  EXPECT_EQ(Eigen::Matrix2d::Identity(), system_model.getSystemMapping());
  EXPECT_EQ(Eigen::Matrix2d::Ones(), system_model.getInputMapping());
  EXPECT_EQ(Eigen::Matrix2d::Constant(2.0), system_model.getNoiseMapping());
  EXPECT_EQ(
      Eigen::Matrix2d::Identity(),
      system_model.getStateJacobian(Eigen::Vector2d::Zero(),
                                    Eigen::Vector2d::Zero()));
  EXPECT_EQ(
      Eigen::Matrix2d::Constant(2.0),
      system_model.getNoiseJacobian(Eigen::Vector2d::Zero(),
                                    Eigen::Vector2d::Zero()));
}

TEST(LinearSystemModelTest, PropagationTest) {
  GaussianDistribution system_noise(Eigen::Vector2d::Ones(),
                                    Eigen::Matrix2d::Identity());

  LinearSystemModel system_model(Eigen::Matrix2d::Identity(), system_noise,
                                 Eigen::Matrix2d::Identity(),
                                 Eigen::Matrix2d::Ones());

  EXPECT_EQ(Eigen::Vector2d::Constant(3.0),
            system_model.propagate(Eigen::Vector2d::Ones()));
  EXPECT_EQ(
      Eigen::Vector2d::Constant(4.0),
      system_model.propagate(Eigen::Vector2d::Ones(), Eigen::Vector2d::Ones(),
                             Eigen::Vector2d::Ones()));
}

}  // namespace refill


//######################################################################
//#   SDF_Fusion Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                or under supervision of Slobodan Ilic
//#######################################################################

#include <iostream>
#include <Eigen/Core>
#include "utils.hpp"
#include "io_utils.hpp"

#include "reconstructor_3d.hpp"
#include "marching_cubes.hpp"
#include "rgbd_types.hpp"

using namespace std;
using namespace cv;
using namespace Eigen;

sdf initialize_sdf(const Configuration &configuration, const vector<float> &volume)
{
	float voxel_size = configuration.get_voxel_size();

	int voxel_count_x = static_cast<int>(std::ceil((volume[1] - volume[0]) / voxel_size));
	int voxel_count_y = static_cast<int>(std::ceil((volume[3] - volume[2]) / voxel_size));
	int voxel_count_z = static_cast<int>(std::ceil((volume[5] - volume[4]) / voxel_size));

	sdf model(voxel_count_x, voxel_count_y, voxel_count_z, voxel_size, 0.f, 1.f);

	return model;
}

template <typename T>
Transform<T, 3, Isometry> to_isometry(const Matrix<T, 4, 4> &pose_matrix)
{
	Transform<T, 3, Isometry> pose;
	pose.setIdentity();

	pose.linear() = pose_matrix.block<3, 3>(0, 0);
	pose.translation() = pose_matrix.block<3, 1>(0, 3);

	return pose;
}

sdf generate_tsdf(const Mat& depth_map, const Mat& color_map, const Isometry3f &transformation_matrix, const Matrix3f &intrinsics, const vector<float>& dimensions,
	const Configuration& configuration) {

	sdf s = initialize_sdf(configuration, dimensions);
	const float voxel_size = configuration.get_voxel_size();

	float delta = voxel_size;
	float eta = 3 * voxel_size;

	Vector3f lower_left(dimensions[0], dimensions[2], dimensions[4]);

#pragma omp parallel for
	for (int z = 0; z < s.size_z; ++z)
		for (int y = 0; y < s.size_y; ++y)
			for (int x = 0; x < s.size_x; ++x) {
				Vector3f rp = lower_left + voxel_size * Vector3f(x + 0.5f, y + 0.5f, z + 0.5f);
				Vector3f transformed_point = transformation_matrix * rp;

				Vector2f point2d = project_point(transformed_point, intrinsics);

				int imgx = static_cast<int>(ceil(point2d(0)));
				int imgy = static_cast<int>(ceil(point2d(1)));

				if (imgx >= 0 && imgy >= 0 && imgx < depth_map.cols && imgy < depth_map.rows) {

					float depth = depth_map.at<float>(imgy, imgx);
					if (isfinite(depth) && depth > .0f) {
						const float signed_distance = depth - transformed_point(2);
						s.weights.at(x, y, z) = signed_distance > -eta;
						s.distance_field.at(x, y, z) = min(1.f, max(-1.f, signed_distance / delta));

						cv::Vec3b color = color_map.at<cv::Vec3b>(imgy, imgx);
						s.color_field.red.at(x, y, z) = color[2] / 255.f;
						s.color_field.green.at(x, y, z) = color[1] / 255.f;
						s.color_field.blue.at(x, y, z) = color[0] / 255.f;
					} // otherwise no need to change values (works only when they are initialized)
				}
			}

	return s;
}

void integrate_into_weighted_average(sdf& wa, const sdf& s) {
	color_cube<float> & wa_colors = wa.color_field;
	const color_cube<float> & s_colors = s.color_field;

#pragma omp parallel for
	for (int z = 0; z < wa.size_z; ++z)
		for (int y = 0; y < wa.size_y; ++y)
			for (int x = 0; x < wa.size_x; ++x) {
				float old_weight = wa.weights.at(x, y, z);
				float added_weight = s.weights.at(x, y, z);
				float new_weight = old_weight + added_weight;

				if (new_weight > 0.f) {
					//wa.distance_field.at(x, y, z) /= wa.weights.at(x, y, z);
					wa.distance_field.at(x, y, z) = (wa.distance_field.at(x, y, z) * old_weight + s.distance_field.at(x, y, z) * added_weight) / new_weight;

					//if (fabs(wa.distance_field.at(x, y, z)) < 1.f) { // assign color only at near-surface voxels
					wa_colors.red.at(x, y, z) = (wa_colors.red.at(x, y, z)   * old_weight + s_colors.red.at(x, y, z)   * added_weight) / new_weight;
					wa_colors.green.at(x, y, z) = (wa_colors.green.at(x, y, z) * old_weight + s_colors.green.at(x, y, z) * added_weight) / new_weight;
					wa_colors.blue.at(x, y, z) = (wa_colors.blue.at(x, y, z)  * old_weight + s_colors.blue.at(x, y, z)  * added_weight) / new_weight;
					//}
				}
				else {
					wa.distance_field.at(x, y, z) = -1.f;
					// and do not change color
				}
				wa.weights.at(x, y, z) = new_weight;
			}
}

sdf reconstruct_sdf(const Configuration& configuration, const vector<float>& volume, const vector<RgbdFile> &input_images, const vector<Matrix4f> &poses)
{

	sdf model = initialize_sdf(configuration, volume);
	const int number_of_frames = static_cast<int>(input_images.size());

	int64 elapsed_time = 0;

	for (int i = 0; i < number_of_frames; ++i)
	{
		RgbdFrame rgbd_frame = get_rgbd_frame(input_images[i]);
		Isometry3f pose = to_isometry(poses[i]);

		auto start_time = getTickCount();
		sdf current_sdf = generate_tsdf(rgbd_frame.second, rgbd_frame.first, pose.inverse(), configuration.get_intrinsics(),
		                                   volume, configuration);
		integrate_into_weighted_average(model, current_sdf);
		elapsed_time += getTickCount() - start_time;
		cout << "Integrated " << i + 1 << " out of " << number_of_frames << " frames into reconstruction" << endl;
	}

	std::cout << "sdf fusion elapsed time: " << elapsed_time / getTickFrequency() << " sec" << std::endl;

	return model;
}

bool run_reconstruction(const Configuration &configuration, const vector<float> &volume)
{
	vector<RgbdFile> input_images = get_input_images(configuration.get_out_rgb_images_dir(), configuration.get_out_depth_images_dir());
	vector<Matrix4f> poses = read_scene_poses<float>(configuration.get_poses_file());

	if (input_images.size() != poses.size())
	{
		cerr << "Number of frames (" << input_images.size() << ") and number of poses (" << poses.size() << ") don't match" << endl;
		return false;
	}

	sdf model = reconstruct_sdf(configuration, volume, input_images, poses);
	vector<marching_cubes::triangle> model_mesh = mesh_valid_only(model);

	save_mesh_ply(model_mesh, configuration.get_model_file(), true);
	return true;
}


#include "test_util.h"
#include <Eigen/Core>
#include "iris/iris.h"

int main () {

  Eigen::MatrixXd A(3,2);
  A << -1, 0,
       0, -1,
       1, 1;
  Eigen::VectorXd b(3);
  b << 0, 0, 1;
  iris::Polyhedron p(A, b);

  Eigen::MatrixXd A2(2,2);
  A2 << 2, 3,
        4, 5;
  Eigen::VectorXd b2(2);
  b2 << 6, 7;
  iris::Polyhedron other(A2, b2);

  p.appendConstraints(other);

  valuecheck(p.getNumberOfConstraints(), 5);
  Eigen::MatrixXd A_expected(5,2);
  A_expected << -1, 0,
                 0, -1,
                 1, 1,
                 2, 3,
                 4, 5;
  valuecheckMatrix(p.getA(), A_expected, 1e-12);

  return 0;
}


//
//  meanie3D-satconv
//

#include <string>
#include <iostream>
#include <boost/program_options.hpp>
#include <boost/program_options/parsers.hpp>

using namespace std;
using namespace boost;

#pragma mark -
#pragma mark Definitions

typedef enum {
    RadicanceToTemperature,
    TemperatureToRadiance
} ConversionType;

// Radiation constant #1
const double c1 = 1.19104e-05;
// Radiation constant #2
const double c2 = 1.43877;

// Names of the variables available for conversion
const string variables[8] = {
    "msevi_l15_ir_039",
    "msevi_l15_ir_087",
    "msevi_l15_ir_097",
    "msevi_l15_ir_108",
    "msevi_l15_ir_120",
    "msevi_l15_ir_134",
    "msevi_l15_wv_062",
    "msevi_l15_wv_073"
};

// Calibration constant alpha
const double alpha[8] = {
    0.9954,
    0.9996,
    0.9999,
    0.9983,
    0.9988,
    0.9981,
    0.9963,
    0.9991
};

// Calibration constant beta
const double beta[8] = {
    3.438,
    0.179,
    0.056,
    0.64,
    0.408,
    0.561,
    2.185,
    0.47
};

// Wavenumber
const double wavenum[8] = {
    2568.832,
    1148.62,
    1035.289,
    931.7,
    836.445,
    751.792,
    1600.548,
    1360.33
};

#pragma mark -
#pragma mark Command line parsing

void parse_commmandline(program_options::variables_map vm,
        int& variableIndex,
        double &value,
        ConversionType &conversionType)
{
    if (vm.count("temperature") != 0) {
        conversionType = RadicanceToTemperature;
    } else if (vm.count("radiance") != 0) {
        conversionType = TemperatureToRadiance;
    } else {
        cerr << "Specify -r or -t" << endl;
        exit(EXIT_FAILURE);
    }

    if (vm.count("value") == 0) {
        cerr << "Missing --value argument" << endl;
        exit(EXIT_FAILURE);
    }
    value = vm["value"].as<double>();

    if (vm.count("variable") == 0) {
        cerr << "Missing --variable argument" << endl;
        exit(EXIT_FAILURE);
    }
    string variable = vm["variable"].as<string>();

    variableIndex = -1;
    for (int i=0; i<8; i++) {
        if (variables[i] == variable) {
            variableIndex = i;
            break;
        }
    }
    if (variableIndex < 0) {
        cerr << "Illegal value for --variable" << endl;
        exit(EXIT_FAILURE);
    }

}

#pragma mark -
#pragma mark Helper Methods

/** Calculates the brightness temperature in degree centigrade
* from the given spectral radiance.
 *
* @param index of the variable to be converted
* @param radiance value for the given channel
* @return equivalent brightness temperature in [degree C]
*/
double brightness_temperature(const int var_index, const double &radiance) {
   double nu = wavenum[var_index];
   double Tbb = c2 * nu / log(1 + nu * nu * nu * c1 / radiance);
   double Tb = (Tbb - beta[var_index]) / alpha[var_index];
   return Tb - 273.15;
}

/** Inverse calculation. Spectral radiance from temperature
* in degree centigrade
 *
* @param index of the variable to be converted
* @param brightness temperature in [degree C]
* @return radiance value for the given channel
*/
double spectral_radiance(const size_t var_index, const double &temperature) {
   double nu = wavenum[var_index];
   double Tbb = (temperature + 273.15) * alpha[var_index] + beta[var_index];
   return nu * nu * nu * c1 / (exp(c2 * nu / Tbb) - 1);
}

#pragma mark -
#pragma mark MAIN

int main(int argc, char** argv)
{
    // Declare the supported options.
    namespace po = boost::program_options;

    po::positional_options_description p;
    p.add("value", -1);

    po::options_description desc("Converts spectral radiance to equivalent brightness temperature or vice versa.");
    desc.add_options()
            ("temperature,t", "radiance to temperature")
            ("radiance,r", "temperature to radiance")
            ("value", po::value<double>(), "value to convert")
            ("variable,v", po::value<string>(), "One of msevi_l15_ir_039, msevi_l15_ir_087, msevi_l15_ir_097, msevi_l15_ir_108, msevi_l15_ir_120, msevi_l15_ir_134, msevi_l15_wv_062,msevi_l15_wv_073")
            ;

    if (argc < 2) {
        cout << desc << "\n";
        exit(EXIT_SUCCESS);
    }

    po::variables_map vm;
    try
    {
        po::store(parse_command_line(argc, argv, desc, po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
        po::notify(vm);
    } catch (std::exception &e) {
        cerr << "FATAL:parsing command line caused exception: " << e.what()
                << ": check meanie3D-satconv --help for command line options"
                << endl;
        exit(EXIT_FAILURE);
    }

    // Evaluate user input
    double value;
    int variableIndex;
    ConversionType type;
    try {
        parse_commmandline(vm,variableIndex,value,type);
    } catch (const std::exception &e) {
        cerr << "ERROR:exception " << e.what() << endl;
        exit(EXIT_FAILURE);
    }

    switch (type) {
        case RadicanceToTemperature:
            cout << brightness_temperature(variableIndex,value) << endl;
            break;
        case TemperatureToRadiance:
            cout << spectral_radiance(variableIndex,value) << endl;
            break;
    }

    return EXIT_SUCCESS;
};


#include <cstdlib>
#include <iostream>
#include <complex>
#define BOOST_UBLAS_NO_ELEMENT_PROXIES
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/hermitian.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

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

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::hermitian_matrix<complex, ublas::lower, ublas::column_major> matrix_l;
    typedef ublas::hermitian_matrix<complex, ublas::upper, ublas::column_major> matrix_u;
    typedef typename vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    matrix_l A_l(n);
    matrix_u A_u(n);
    for (size_type j=0; j<n; ++j) {
      A_u(j, j)=rand_normal<complex>::get().real();
      A_l(j, j)=A_u(j, j);
      for (size_type i=0; i<j; ++i) {
	complex a();
    	A_u(i, j)=rand_normal<complex>::get();
	A_l(j, i)=std::conj(A_u(i, j));
       }
    }
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<complex>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<complex>::get();
    complex alpha(rand_normal<complex>::get());
    complex beta(rand_normal<complex>::get());
    vector y1(alpha*ublas::prod(A_l, x)+beta*y);
    vector y2(y);
    blas::hpmv(alpha, A_l, x, beta, y2);
    vector y3(y);
    blas::hpmv(alpha, A_u, x, beta, y3);
    std::cout << "testing boost::ublas containers\n"
    	      << "using ublas       : " << print_vec(y1) << '\n'
    	      << "using blas (lower): " << print_vec(y2) << '\n'
    	      << "using blas (upper): " << print_vec(y3) << '\n'
    	      << '\n';
  }
  return EXIT_SUCCESS;
}


#pragma once

#include <Eigen/Dense>
#include <polymesh/pm.hh>
#include <typed-geometry/tg.hh>
#include <LayoutEmbedding/Parametrization.hh>

namespace LayoutEmbedding
{

enum class LaplaceWeights
{
    Uniform,
    MeanValue,
};

/// Compute harmonic field using mean-value weights.
bool harmonic(
        const pm::vertex_attribute<tg::pos3>& _pos,
        const pm::vertex_attribute<bool>& _constrained,
        const Eigen::MatrixXd& _constraint_values,
        Eigen::MatrixXd& _res,
        const LaplaceWeights _weights,
        const bool _fallback_iterative = false);

/// Compute harmonic field using mean-value weights.
bool harmonic_parametrization(
        const pm::vertex_attribute<tg::pos3>& _pos,
        const pm::vertex_attribute<bool>& _constrained,
        const VertexParam& _constraint_values,
        VertexParam& _res,
        const LaplaceWeights _weights,
        const bool _fallback_iterative = false);

}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, tgamma) {
  using stan::math::tgamma;
  EXPECT_FLOAT_EQ(1.772453850905516, tgamma(0.5));
  EXPECT_FLOAT_EQ(1, tgamma(1));
  EXPECT_FLOAT_EQ(2.423965479935368, tgamma(3.2));
  EXPECT_FLOAT_EQ(6402373705728000, tgamma(19));
}

TEST(MathFunctions, tgammaStanMath) {
  EXPECT_THROW(stan::math::tgamma(0.0), std::domain_error);
}

TEST(MathFunctions, tgamma_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  EXPECT_PRED1(boost::math::isnan<double>, stan::math::tgamma(nan));
}