/*!
  \file gpp_model_selection_test.cpp
  \rst
  Routines to test the functions in gpp_model_selection.cpp.

  These tests verify LogMarginalLikelihoodEvaluator and LeaveOneOutLogLikelihoodEvaluator and their optimizers:

  1. Ping testing (verifying analytic gradient computation against finite difference approximations)

     a. Following gpp_covariance_test.cpp, we define classes (PingLogLikelihood, PingHessianLogLikelihood) for evaluating
        log likelihood + gradient or log likelihood gradient + hessian (derivs wrt hyperparameters).
     b. Ping for derivative accuracy (PingLogLikelihoodTest, which is general enough for gradients and hessian); this is
        for derivatives wrt hyperparameters.  These are for unit testing analytic derivatives.

  2. Gradient Descent + Newton unit tests: using polynomials and other simple fucntions with analytically known optima
     to verify that the optimizers are performing correctly.
  3. Hyperparameter optimization: we run hyperparameter optimization on toy problems using LML and LOO-CV likelihood
     as objective functions.  Convergence to at least local maxima is verified for both gradient descent and newton optimizers.
     These function as integration tests.
\endrst*/

// #define OL_VERBOSE_PRINT

#include "gpp_model_selection_test.hpp"

#include <cmath>

#include <algorithm>
#include <iterator>
#include <stdexcept>
#include <string>
#include <vector>

#include <boost/random/uniform_real.hpp>  // NOLINT(build/include_order)

#include "gpp_common.hpp"
#include "gpp_covariance.hpp"
#include "gpp_domain.hpp"
#include "gpp_exception.hpp"
#include "gpp_logging.hpp"
#include "gpp_math.hpp"
#include "gpp_mock_optimization_objective_functions.hpp"
#include "gpp_model_selection.hpp"
#include "gpp_random.hpp"
#include "gpp_optimization.hpp"
#include "gpp_optimizer_parameters.hpp"
#include "gpp_test_utils.hpp"

namespace optimal_learning {

namespace {  // tests for pinging log likelihood measures wrt their hyperparameters

/*!\rst
  Supports evaluating log likelihood functions and their gradients wrt hyperparameters.

  The gradient is taken wrt ``hyperparameters[n_hyper]``, so this is the ``input_matrix``, ``X_{d,i}`` with ``i`` unused.
  The other inputs to log marginal are not differentiated against, so they are taken as input and stored by the constructor.
\endrst*/
template <typename LogLikelihoodEvaluator, typename CovarianceClass>
class PingLogLikelihood final : public PingableMatrixInputVectorOutputInterface {
 public:
  using CovarianceType = CovarianceClass;

  PingLogLikelihood(const CovarianceClass& covariance, double const * restrict points_sampled, double const * restrict points_sampled_value, double const * restrict noise_variance, int dim, int num_sampled) OL_NONNULL_POINTERS
      : num_hyperparameters_(covariance.GetNumberOfHyperparameters()),
        gradients_already_computed_(false),
        log_likelihood_eval_(points_sampled, points_sampled_value, noise_variance, dim, num_sampled),
        grad_log_marginal_likelihood_(num_hyperparameters_) {
  }

  virtual void GetInputSizes(int * num_rows, int * num_cols) const noexcept override OL_NONNULL_POINTERS {
    *num_rows = num_hyperparameters_;
    *num_cols = 1;
  }

  virtual int GetGradientsSize() const noexcept override OL_WARN_UNUSED_RESULT {
    return num_hyperparameters_*GetOutputSize();
  }

  virtual int GetOutputSize() const noexcept override OL_WARN_UNUSED_RESULT {
    return 1;
  }

  virtual void EvaluateAndStoreAnalyticGradient(double const * restrict hyperparameters, double * restrict gradients) noexcept override OL_NONNULL_POINTERS_LIST(2) {
    if (gradients_already_computed_ == true) {
      OL_WARNING_PRINTF("WARNING: grad_covariance data pointer NOT nullptr.  Attempting to free...\n");
    }
    gradients_already_computed_ = true;

    CovarianceClass covariance_local(log_likelihood_eval_.dim(), hyperparameters[0], hyperparameters + 1);
    typename LogLikelihoodEvaluator::StateType log_likelihood_state(log_likelihood_eval_, covariance_local);
    log_likelihood_eval_.ComputeGradLogLikelihood(&log_likelihood_state, grad_log_marginal_likelihood_.data());

    if (gradients != nullptr) {
      std::copy(grad_log_marginal_likelihood_.begin(), grad_log_marginal_likelihood_.end(), gradients);
    }
  }

  virtual double GetAnalyticGradient(int row_index, int OL_UNUSED(column_index), int OL_UNUSED(output_index)) const override OL_WARN_UNUSED_RESULT {
    if (gradients_already_computed_ == false) {
      OL_THROW_EXCEPTION(OptimalLearningException, "PingLogLikelihood::GetAnalyticGradient() called BEFORE EvaluateAndStoreAnalyticGradient. NO DATA!");
    }

    return grad_log_marginal_likelihood_[row_index];
  }

  virtual void EvaluateFunction(double const * restrict hyperparameters, double * restrict function_values) const noexcept override OL_NONNULL_POINTERS {
    CovarianceClass covariance_local(log_likelihood_eval_.dim(), hyperparameters[0], hyperparameters + 1);
    typename LogLikelihoodEvaluator::StateType log_likelihood_state(log_likelihood_eval_, covariance_local);

    *function_values = log_likelihood_eval_.ComputeLogLikelihood(log_likelihood_state);
  }

 private:
  //! number of hyperparameters of the underlying covariance function
  int num_hyperparameters_;
  //! whether gradients been computed and stored--whether this class is ready for use
  bool gradients_already_computed_;

  //! log likelihood evaluator that is being tested (e.g., LogMarginalLikelihood, LeaveOneOutLogLikelihood)
  LogLikelihoodEvaluator log_likelihood_eval_;

  //! the gradient of the log marginal measure wrt hyperparameters of covariance
  std::vector<double> grad_log_marginal_likelihood_;

  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(PingLogLikelihood);
};

/*!\rst
  Supports evaluating log likelihood functions and their hessians wrt hyperparameters.

  The hessian is taken wrt ``hyperparameters[n_hyper]``, so this is the ``input_matrix``, ``X_{d,i}`` with ``i`` unused.
  The other inputs to log marginal are not differentiated against, so they are taken as input and stored by the constructor.

  Hessians are tested against gradients, so ``GetOutputSize`` returns ``num_hyperparameters``.
\endrst*/
template <typename LogLikelihoodEvaluator, typename CovarianceClass>
class PingHessianLogLikelihood final : public PingableMatrixInputVectorOutputInterface {
 public:
  using CovarianceType = CovarianceClass;

  PingHessianLogLikelihood(const CovarianceClass& covariance, double const * restrict points_sampled, double const * restrict points_sampled_value, double const * restrict noise_variance, int dim, int num_sampled) OL_NONNULL_POINTERS
      : num_hyperparameters_(covariance.GetNumberOfHyperparameters()),
        gradients_already_computed_(false),
        log_likelihood_eval_(points_sampled, points_sampled_value, noise_variance, dim, num_sampled),
        hessian_log_marginal_likelihood_(Square(num_hyperparameters_)) {
  }

  virtual void GetInputSizes(int * num_rows, int * num_cols) const noexcept override OL_NONNULL_POINTERS {
    *num_rows = num_hyperparameters_;
    *num_cols = 1;
  }

  virtual int GetGradientsSize() const noexcept override OL_WARN_UNUSED_RESULT {
    return num_hyperparameters_*GetOutputSize();
  }

  virtual int GetOutputSize() const noexcept override OL_WARN_UNUSED_RESULT {
    return num_hyperparameters_;
  }

  virtual void EvaluateAndStoreAnalyticGradient(double const * restrict hyperparameters, double * restrict gradients) noexcept override OL_NONNULL_POINTERS_LIST(2) {
    if (gradients_already_computed_ == true) {
      OL_WARNING_PRINTF("WARNING: grad_covariance data pointer NOT nullptr.  Attempting to free...\n");
    }
    gradients_already_computed_ = true;

    CovarianceClass covariance_local(log_likelihood_eval_.dim(), hyperparameters[0], hyperparameters + 1);
    typename LogLikelihoodEvaluator::StateType log_likelihood_state(log_likelihood_eval_, covariance_local);
    log_likelihood_eval_.ComputeHessianLogLikelihood(&log_likelihood_state, hessian_log_marginal_likelihood_.data());

    if (gradients != nullptr) {
      std::copy(hessian_log_marginal_likelihood_.begin(), hessian_log_marginal_likelihood_.end(), gradients);
    }
  }

  virtual double GetAnalyticGradient(int row_index, int OL_UNUSED(column_index), int output_index) const override OL_WARN_UNUSED_RESULT {
    if (gradients_already_computed_ == false) {
      OL_THROW_EXCEPTION(OptimalLearningException, "PingHessianLogLikelihood::GetAnalyticGradient() called BEFORE EvaluateAndStoreAnalyticGradient. NO DATA!");
    }

    return hessian_log_marginal_likelihood_[row_index*num_hyperparameters_ + output_index];
  }

  virtual void EvaluateFunction(double const * restrict hyperparameters, double * restrict function_values) const noexcept override OL_NONNULL_POINTERS {
    CovarianceClass covariance_local(log_likelihood_eval_.dim(), hyperparameters[0], hyperparameters + 1);
    typename LogLikelihoodEvaluator::StateType log_likelihood_state(log_likelihood_eval_, covariance_local);

    log_likelihood_eval_.ComputeGradLogLikelihood(&log_likelihood_state, function_values);
  }

 private:
  //! number of hyperparameters of the underlying covariance function
  int num_hyperparameters_;
  //! whether gradients been computed and stored--whether this class is ready for use
  bool gradients_already_computed_;

  //! log likelihood evaluator that is being tested (e.g., LogMarginalLikelihood, LeaveOneOutLogLikelihood)
  LogLikelihoodEvaluator log_likelihood_eval_;

  //! the hessian of the log marginal measure wrt hyperparameters of covariance
  std::vector<double> hessian_log_marginal_likelihood_;

  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(PingHessianLogLikelihood);
};

/*!\rst
  Pings the gradients (hyperparameters) of the likelihood functions 50 times with randomly generated test cases
  Covariance fcn to check against is a template parameter of PingLogLikelihood.

  \param
    :class_name: name of the log likelihood being tested (for logging)
    :num_hyperparameters: number of hyperparameters of the underlying covariance function
    :epsilon: coarse, fine ``h`` sizes to use in finite difference computation
    :tolerance_fine: desired amount of deviation from the exact rate
    :tolerance_coarse: maximum allowable abmount of deviation from the exact rate
    :input_output_ratio: for ``||analytic_gradient||/||input|| < input_output_ratio``, ping testing is not performed, see PingDerivative()
  \return
    number of ping test failures
\endrst*/
template <typename PingLogLikelihood>
OL_WARN_UNUSED_RESULT int PingLogLikelihoodTest(char const * class_name, int num_hyperparameters, double epsilon[2], double tolerance_fine, double tolerance_coarse, double input_output_ratio) {
  int total_errors = 0;
  int errors_this_iteration;
  const int dim = 3;

  int num_being_sampled = 0;
  int num_to_sample = 1;
  int num_sampled = 7;

  std::vector<double> hyperparameters(num_hyperparameters);

  MockExpectedImprovementEnvironment EI_environment;
  std::vector<double> noise_variance(num_sampled, 0.0);

  UniformRandomGenerator uniform_generator(3141);
  boost::uniform_real<double> uniform_double(3.0, 5.0);

  for (int i = 0; i < 50; ++i) {
    EI_environment.Initialize(dim, num_to_sample, num_being_sampled, num_sampled);

    for (int j = 0; j < num_hyperparameters; ++j) {
      hyperparameters[j] = uniform_double(uniform_generator.engine);
    }

    typename PingLogLikelihood::CovarianceType sqexp(EI_environment.dim, 1.0, 1.0);
    PingLogLikelihood log_likelihood_evaluator(sqexp, EI_environment.points_sampled(), EI_environment.points_sampled_value(), noise_variance.data(), EI_environment.dim, EI_environment.num_sampled);
    log_likelihood_evaluator.EvaluateAndStoreAnalyticGradient(hyperparameters.data(), nullptr);
    errors_this_iteration = PingDerivative(log_likelihood_evaluator, hyperparameters.data(), epsilon, tolerance_fine, tolerance_coarse, input_output_ratio);

    if (errors_this_iteration != 0) {
      OL_PARTIAL_FAILURE_PRINTF("on iteration %d\n", i);
    }
    total_errors += errors_this_iteration;
  }

  if (total_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("%s hyperparameter gradient pings failed with %d errors\n", class_name, total_errors);
  } else {
    OL_PARTIAL_SUCCESS_PRINTF("%s hyperparameter gradient pings passed\n", class_name);
  }

  return total_errors;
}

}  // end unnamed namespace

int RunLogLikelihoodPingTests() {
  int total_errors = 0;
  int current_errors = 0;

  {
    double epsilon_log_marginal[2] = {1.0e-2, 2.0e-3};
    current_errors = PingLogLikelihoodTest<PingLogLikelihood<LogMarginalLikelihoodEvaluator, SquareExponentialSingleLength> >("Log Marginal Likelihood sqexp single", 2, epsilon_log_marginal, 5.0e-4, 1.0e-3, 1.0e-18);
    if (current_errors != 0) {
      OL_PARTIAL_FAILURE_PRINTF("pinging log likelihood failed with %d errors\n", current_errors);
    }
    total_errors += current_errors;
  }

  {
    double epsilon_log_marginal[2] = {1.0e-2, 2.0e-3};
    current_errors = PingLogLikelihoodTest<PingLogLikelihood<LogMarginalLikelihoodEvaluator, SquareExponential> >("Log Marginal Likelihood sqexp", 4, epsilon_log_marginal, 6.0e-3, 1.0e-3, 1.0e-18);
    if (current_errors != 0) {
      OL_PARTIAL_FAILURE_PRINTF("pinging log likelihood failed with %d errors\n", current_errors);
    }
    total_errors += current_errors;
  }

  {
    double epsilon_leave_one_out[2] = {1.0e-2, 2.0e-3};
    current_errors = PingLogLikelihoodTest<PingLogLikelihood<LeaveOneOutLogLikelihoodEvaluator, SquareExponentialSingleLength> >("Leave One Out Log Likelihood sqexp single", 2, epsilon_leave_one_out, 3.0e-4, 3.0e-4, 1.0e-18);
    if (current_errors != 0) {
      OL_PARTIAL_FAILURE_PRINTF("pinging leave one out failed with %d errors\n", current_errors);
    }
    total_errors += current_errors;
  }

  {
    double epsilon_leave_one_out[2] = {1.0e-2, 2.0e-3};
    current_errors = PingLogLikelihoodTest<PingLogLikelihood<LeaveOneOutLogLikelihoodEvaluator, SquareExponential> >("Leave One Out Log Likelihood sqexp", 4, epsilon_leave_one_out, 4.0e-3, 8.0e-4, 1.0e-18);
    if (current_errors != 0) {
      OL_PARTIAL_FAILURE_PRINTF("pinging leave one out failed with %d errors\n", current_errors);
    }
    total_errors += current_errors;
  }

  {
    double epsilon_log_marginal[2] = {1.0e-2, 2.0e-3};
    current_errors = PingLogLikelihoodTest<PingHessianLogLikelihood<LogMarginalLikelihoodEvaluator, SquareExponential> >("Log Marginal Likelihood Hessian sqexp", 4, epsilon_log_marginal, 3.0e-2, 3.0e-2, 1.0e-18);
    if (current_errors != 0) {
      OL_PARTIAL_FAILURE_PRINTF("pinging log marginal hessian failed with %d errors\n", current_errors);
    }
    total_errors += current_errors;
  }

  if (total_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("Pinging GP functions failed with %d errors\n\n", total_errors);
  } else {
    OL_PARTIAL_SUCCESS_PRINTF("Pinging GP functions passed\n");
  }

  return total_errors;
}

namespace {  // tests for gradient descent and newton optimization of log likelihood measures wrt hyperparameters

/*!\rst
  Tests hyperparameter optimization.  Basic code flow:

  **SETUP**

    0. Pick a domain and specify hyperparameters (random) + covariance type (hyperparameters, CovarianceClass)
    1. Generate N random points in the domain
    2. Build a GP on the specified hyperparameters, incrementally generating function values for each of the N points

  **OPTIMIZE**

    3. Specify new, random, and different (~1 order of mag larger) hyperparameters than those used to generate the data
       (hyperparameters_wrong, covariance_wrong)
    4. Starting with covariance_wrong, optimize hyperparameters.

  **CHECK**

    5. Rerun optimization once more starting from the optimized values: VERIFY that no change occurs (within tolerance)
    6. VERIFY that the log marginal likelihood after optimization is better than the likelihood before optimization
    7. (REMOVED, FLAKY) CHECK that optimized hyperparameters are close to the hyperparameters used to generate the data
\endrst*/
template <typename LogLikelihoodEvaluator, typename CovarianceClass>
OL_WARN_UNUSED_RESULT int HyperparameterLikelihoodOptimizationTestCore(LogLikelihoodTypes objective_mode) {
  using DomainType = TensorProductDomain;
  using HyperparameterDomainType = TensorProductDomain;
  const int dim = 2;
  const int num_sampled = 40;

  double initial_likelihood;
  double final_likelihood;

  // gradient descent parameters
  const double gamma = 0.5;
  const double pre_mult = 0.5;
  const double max_relative_change = 0.02;
  double tolerance = 1.0e-10;
  if (objective_mode == LogLikelihoodTypes::kLeaveOneOutLogLikelihood) {
    tolerance = 1.0e-7;  // less accurate b/c current implementation uses matrix inverse for speed
  }
  const int max_gradient_descent_steps = 600;
  const int max_num_restarts = 5;
  GradientDescentParameters gd_parameters(1, max_gradient_descent_steps, max_num_restarts, gamma, pre_mult, max_relative_change, tolerance);

  int total_errors = 0;
  int current_errors = 0;

  // covariance object that will be set with the wrong hyperparameters; used as an initial guess for optimization
  CovarianceClass covariance_wrong(dim, 1.0, 1.0);
  int num_hyperparameters = covariance_wrong.GetNumberOfHyperparameters();

  std::vector<double> hyperparameters_optimized(num_hyperparameters);  // optimized hyperparameters
  std::vector<double> hyperparameters_temp(num_hyperparameters);  // temp hyperparameters
  std::vector<double> hyperparameters_wrong(num_hyperparameters);  // wrong hyperparameters to start gradient descent

  // seed randoms
  UniformRandomGenerator uniform_generator(3141);
  boost::uniform_real<double> uniform_double_hyperparameter(1.0, 2.5);
  boost::uniform_real<double> uniform_double_lower_bound(-2.0, 0.5);
  boost::uniform_real<double> uniform_double_upper_bound(1.5, 2.7);

  ClosedInterval wrong_hyperparameter_range;
  if (objective_mode == LogLikelihoodTypes::kLogMarginalLikelihood) {
    wrong_hyperparameter_range = {2.5, 5.0};
  } else {
    wrong_hyperparameter_range = {0.3, 3.0};
  }
  boost::uniform_real<double> uniform_wrong_hyperparameters(wrong_hyperparameter_range.min, wrong_hyperparameter_range.max);
  FillRandomCovarianceHyperparameters(uniform_wrong_hyperparameters, &uniform_generator, &hyperparameters_wrong, &covariance_wrong);

  std::vector<ClosedInterval> hyperparameter_domain_bounds(num_hyperparameters, {1.0e-10, 1.0e10});
  HyperparameterDomainType hyperparameter_domain(hyperparameter_domain_bounds.data(), num_hyperparameters);

  std::vector<double> noise_variance(num_sampled, 0.1);
  MockGaussianProcessPriorData<DomainType> mock_gp_data(covariance_wrong, noise_variance, dim, num_sampled,
                                                        uniform_double_lower_bound, uniform_double_upper_bound,
                                                        uniform_double_hyperparameter, &uniform_generator);

  LogLikelihoodEvaluator log_likelihood_eval(mock_gp_data.gaussian_process_ptr->points_sampled().data(),
                                             mock_gp_data.gaussian_process_ptr->points_sampled_value().data(),
                                             mock_gp_data.gaussian_process_ptr->noise_variance().data(),
                                             dim, num_sampled);
  typename LogLikelihoodEvaluator::StateType log_likelihood_state(log_likelihood_eval, covariance_wrong);

  initial_likelihood = log_likelihood_eval.ComputeLogLikelihood(log_likelihood_state);
  OL_VERBOSE_PRINTF("initial likelihood: %.18E\n", initial_likelihood);

  RestartedGradientDescentHyperparameterOptimization(log_likelihood_eval, covariance_wrong, gd_parameters, hyperparameter_domain, hyperparameters_optimized.data());
  log_likelihood_state.SetHyperparameters(log_likelihood_eval, hyperparameters_optimized.data());
  final_likelihood = log_likelihood_eval.ComputeLogLikelihood(log_likelihood_state);

  // verify that convergence occurred
  covariance_wrong.SetHyperparameters(hyperparameters_optimized.data());
  gd_parameters.pre_mult *= 0.1;
  RestartedGradientDescentHyperparameterOptimization(log_likelihood_eval, covariance_wrong, gd_parameters, hyperparameter_domain, hyperparameters_temp.data());

  double norm_delta_hyperparameter;
  for (IdentifyType<decltype(hyperparameters_temp)>::type::size_type i = 0, size = hyperparameters_temp.size(); i < size; ++i) {
    hyperparameters_temp[i] -= hyperparameters_optimized[i];
  }
  norm_delta_hyperparameter = VectorNorm(hyperparameters_temp.data(), hyperparameters_temp.size());
  current_errors = 0;
  if (!CheckDoubleWithin(norm_delta_hyperparameter, 0.0, 1.0e-6)) {
    ++current_errors;
  }
  if (current_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("gradient descent did not full converge: hyperparameters still changed by (RMS): %.18E\n", norm_delta_hyperparameter);
  }
  total_errors += current_errors;

  current_errors = 0;
  if (final_likelihood <= initial_likelihood) {
    ++current_errors;  // expect improvement from optimization
  }
  if (final_likelihood >= 0.0) {
    ++current_errors;  // must be negative
  }
  if (current_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("final likelihood = %.18E is worse than initial likelihood = %.18E\n", final_likelihood, initial_likelihood);
  }
  total_errors += current_errors;

  // check that hyperparameter gradients are small
  std::vector<double> grad_log_marginal(num_hyperparameters);
  log_likelihood_eval.ComputeGradLogLikelihood(&log_likelihood_state, grad_log_marginal.data());

#ifdef OL_VERBOSE_PRINT
  OL_VERBOSE_PRINTF("grad log marginal: ");
  PrintMatrix(grad_log_marginal.data(), 1, grad_log_marginal.size());
#endif

  current_errors = 0;
  for (const auto& entry : grad_log_marginal) {
    if (objective_mode == LogLikelihoodTypes::kLogMarginalLikelihood) {
      if (!CheckDoubleWithinRelative(entry, 0.0, 5.0e-11)) {
        ++current_errors;
      }
    } else {
      if (!CheckDoubleWithinRelative(entry, 0.0, 5.0e-8)) {
        ++current_errors;
      }
    }
  }
  total_errors += current_errors;

  // would check that optimal hyperparameters are close to their initial values, but that does not seem to be a well grounded test.
  // may revisit getting better testing for this function in the future.
  // const double hyperparameter_tolerance = 0.5;
  // current_errors = 0;
  // for (IdentifyType<decltype(hyperparameters)>::type::size_type i = 0; i < hyperparameters.size(); ++i) {
  //   current_errors += CheckDoubleWithinRelative(hyperparameters_optimized[i], hyperparameters[i], hyperparameter_tolerance) == false;
  // }
  // if (current_errors != 0) {
  //   OL_PARTIAL_FAILURE_PRINTF("optimized hyperparameters are NOT close to true values!\n");
  // }
  // total_errors += current_errors;

  return total_errors;
}

/*!\rst
  Tests Newton hyperparameter optimization.  Basic code flow:

  **SETUP**

    0. Pick a domain and specify hyperparameters (random) + covariance type (hyperparameters, CovarianceClass)
    1. Generate N random points in the domain
    2. Build a GP on the specified hyperparameters, incrementally generating function values for each of the N points

  **OPTIMIZE**

    3. Specify new, random, and different (~1 order of mag larger) hyperparameters than those used to generate the data
       (hyperparameters_wrong, covariance_wrong)
    4. Starting with covariance_wrong, optimize hyperparameters.

  **CHECK**

    5. Rerun optimization once more starting from the optimized values: VERIFY that no change occurs (within tolerance).  Set very large time_factor for this check
    6. Verify that the log marginal likelihood after optimization is better than the likelihood before optimization
    7. Verify that the gradient is below newton tolerance.
    8. TODO(GH-121): Verify that the eigenvalues of the Hessian are all negative (=> maxima)
\endrst*/
template <typename LogLikelihoodEvaluator, typename CovarianceClass>
OL_WARN_UNUSED_RESULT int HyperparameterLikelihoodNewtonOptimizationTestCore(LogLikelihoodTypes OL_UNUSED(objective_mode)) {
  using DomainType = TensorProductDomain;
  using HyperparameterDomainType = TensorProductDomain;
  const int num_sampled = 45;
  const int dim = 2;

  double initial_likelihood;
  double final_likelihood;

  // gradient descent parameters
  const double gamma = 1.1;
  const double pre_mult = 1.0e-1;
  const double max_relative_change = 1.0;
  const double tolerance = 1.0e-13;
  const int max_newton_steps = 1000;
  NewtonParameters newton_parameters(1, max_newton_steps, gamma, pre_mult, max_relative_change, tolerance);

  int total_errors = 0;
  int current_errors = 0;

  // covariance object that will be set with the wrong hyperparameters; used as an initial guess for optimization
  CovarianceClass covariance_wrong(dim, 1.0, 1.0);
  int num_hyperparameters = covariance_wrong.GetNumberOfHyperparameters();

  std::vector<double> hyperparameters_optimized(num_hyperparameters);  // optimized hyperparameters
  std::vector<double> hyperparameters_temp(num_hyperparameters);  // temp hyperparameters
  std::vector<double> hyperparameters_wrong(num_hyperparameters);  // wrong hyperparameters to start gradient descent

  // seed randoms
  UniformRandomGenerator uniform_generator(5762);
  boost::uniform_real<double> uniform_double_hyperparameter(1.0, 2.5);
  boost::uniform_real<double> uniform_double_lower_bound(-2.0, 0.5);
  boost::uniform_real<double> uniform_double_upper_bound(2.0, 3.5);

  boost::uniform_real<double> uniform_double_for_wrong_hyperparameter(10.0, 30.0);
  FillRandomCovarianceHyperparameters(uniform_double_for_wrong_hyperparameter, &uniform_generator, &hyperparameters_wrong, &covariance_wrong);
  std::vector<ClosedInterval> hyperparameter_domain_bounds(num_hyperparameters, {1.0e-10, 1.0e10});
  HyperparameterDomainType hyperparameter_domain(hyperparameter_domain_bounds.data(), num_hyperparameters);

  std::vector<double> noise_variance(num_sampled, 0.1);
  MockGaussianProcessPriorData<DomainType> mock_gp_data(covariance_wrong, noise_variance, dim, num_sampled,
                                                        uniform_double_lower_bound, uniform_double_upper_bound,
                                                        uniform_double_hyperparameter, &uniform_generator);

  LogLikelihoodEvaluator log_likelihood_eval(mock_gp_data.gaussian_process_ptr->points_sampled().data(),
                                             mock_gp_data.gaussian_process_ptr->points_sampled_value().data(),
                                             mock_gp_data.gaussian_process_ptr->noise_variance().data(),
                                             dim, num_sampled);
  typename LogLikelihoodEvaluator::StateType log_likelihood_state(log_likelihood_eval, covariance_wrong);

  initial_likelihood = log_likelihood_eval.ComputeLogLikelihood(log_likelihood_state);
  OL_VERBOSE_PRINTF("initial likelihood: %.18E\n", initial_likelihood);

  total_errors += NewtonHyperparameterOptimization(log_likelihood_eval, covariance_wrong, newton_parameters, hyperparameter_domain, hyperparameters_optimized.data());
  covariance_wrong.SetHyperparameters(hyperparameters_optimized.data());
  log_likelihood_state.SetHyperparameters(log_likelihood_eval, hyperparameters_optimized.data());
  final_likelihood = log_likelihood_eval.ComputeLogLikelihood(log_likelihood_state);
#ifdef OL_VERBOSE_PRINT
  OL_VERBOSE_PRINTF("final likelihood: %.18E\n", final_likelihood);
  PrintMatrix(hyperparameters_optimized.data(), 1, num_hyperparameters);
#endif

  // check that hyperparameter gradients are small
  std::vector<double> grad_log_marginal(num_hyperparameters);
  log_likelihood_eval.ComputeGradLogLikelihood(&log_likelihood_state, grad_log_marginal.data());

#ifdef OL_VERBOSE_PRINT
  OL_VERBOSE_PRINTF("grad log marginal: ");
  PrintMatrix(grad_log_marginal.data(), 1, grad_log_marginal.size());
#endif

  current_errors = 0;
  for (const auto& entry : grad_log_marginal) {
    if (!CheckDoubleWithinRelative(entry, 0.0, 1.0e-13)) {
      ++current_errors;
    }
  }
  total_errors += current_errors;

  // verify that convergence occurred
  // set very aggressive time_factor
  newton_parameters.time_factor = 1.0e30;
  newton_parameters.gamma = 10.0;
  newton_parameters.max_num_steps = 10;
  total_errors += NewtonHyperparameterOptimization(log_likelihood_eval, covariance_wrong, newton_parameters, hyperparameter_domain, hyperparameters_temp.data());
#ifdef OL_VERBOSE_PRINT
  PrintMatrix(hyperparameters_optimized.data(), 1, num_hyperparameters);
#endif

  double norm_delta_hyperparameter;
  current_errors = 0;
  for (IdentifyType<decltype(hyperparameters_temp)>::type::size_type i = 0, size = hyperparameters_temp.size(); i < size; ++i) {
    hyperparameters_temp[i] -= hyperparameters_optimized[i];
  }
  norm_delta_hyperparameter = VectorNorm(hyperparameters_temp.data(), hyperparameters_temp.size());
  if (!CheckDoubleWithin(norm_delta_hyperparameter, 0.0, 1.0e-12)) {
    ++current_errors;
  }
  if (current_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("newton did not full converge: hyperparameters still changed by (RMS): %.18E\n", norm_delta_hyperparameter);
  }
  total_errors += current_errors;

  current_errors = 0;
  if (final_likelihood <= initial_likelihood) {
    ++current_errors;  // expect improvement from optimization
  }
  if (final_likelihood >= 0.0) {
    ++current_errors;  // must be negative
  }
  if (current_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("final likelihood = %.18E is worse than initial likelihood = %.18E\n", final_likelihood, initial_likelihood);
  }
  total_errors += current_errors;

  return total_errors;
}

/*!\rst
  Tests multistarted Newton optimization for hyperparameters.
  Compares result to newton optimization called from an initial guess very near the optimal solution.

  Tests Newton hyperparameter optimization.  Basic code flow:

  **SETUP**

    0. Pick a domain and specify hyperparameters (random) + covariance type (hyperparameters, CovarianceClass)
    1. Generate N random points in the domain
    2. Build a GP on the specified hyperparameters, incrementally generating function values for each of the N points

  **OPTIMIZE**

    3. Multistarted Newton optimization with multithreading enabled

  **CHECK**

    4. Rerun optimization once more starting from the values in step 0; this should be very near the real solution.
    5. Verify that these hyperparameters and the results of multistart Newton are the same
    6. Verify that the log marginal likelihood after multistart optimization is better than the likelihood at the
       hyperparameters from step 0 (since these are not optimal)
\endrst*/
template <typename LogLikelihoodEvaluator, typename CovarianceClass>
OL_WARN_UNUSED_RESULT int MultistartHyperparameterLikelihoodNewtonOptimizationTestCore(LogLikelihoodTypes OL_UNUSED(objective_mode)) {
  using DomainType = TensorProductDomain;
  using HyperparameterDomainType = TensorProductDomain;
  const int num_sampled = 42;
  const int dim = 2;

  double initial_likelihood;
  double final_likelihood;

  // gradient descent parameters
  const double gamma = 1.1;
  const double pre_mult = 1.0e-1;
  const double max_relative_change = 1.0;
  const double tolerance = 1.0e-14;
  const int max_newton_steps = 100;
  const int num_multistarts = 16;
  NewtonParameters newton_parameters(num_multistarts, max_newton_steps, gamma, pre_mult, max_relative_change, tolerance);

  const int max_num_threads = 4;
  ThreadSchedule thread_schedule(max_num_threads, omp_sched_dynamic);

  int total_errors = 0;
  int current_errors = 0;

  // seed randoms
  UniformRandomGenerator uniform_generator(5762);
  boost::uniform_real<double> uniform_double_hyperparameter(1.0, 2.5);
  boost::uniform_real<double> uniform_double_lower_bound(-2.0, 0.5);
  boost::uniform_real<double> uniform_double_upper_bound(2.0, 3.5);

  std::vector<double> noise_variance(num_sampled, 0.1);
  MockGaussianProcessPriorData<DomainType> mock_gp_data(CovarianceClass(dim, 1.0, 1.0), noise_variance, dim,
                                                        num_sampled, uniform_double_lower_bound,
                                                        uniform_double_upper_bound,
                                                        uniform_double_hyperparameter, &uniform_generator);
  int num_hyperparameters = mock_gp_data.covariance_ptr->GetNumberOfHyperparameters();

  std::vector<double> hyperparameters_truth(num_hyperparameters);  // truth hyperparameters
  std::vector<double> hyperparameters_optimized(num_hyperparameters);  // optimized hyperparameters
  std::vector<double> hyperparameters_temp(num_hyperparameters);  // temp hyperparameters

  // set up domain; allows initial guesses to range over [0.01, 10]
  std::vector<ClosedInterval> hyperparameter_log_domain_bounds(num_hyperparameters, {-2.0, 1.0});
  std::vector<ClosedInterval> hyperparameter_domain_bounds(hyperparameter_log_domain_bounds);
  for (auto& interval : hyperparameter_domain_bounds) {
    interval = {std::pow(10.0, interval.min), std::pow(10.0, interval.max)};
  }
  HyperparameterDomainType hyperparameter_domain(hyperparameter_domain_bounds.data(), num_hyperparameters);

  LogLikelihoodEvaluator log_likelihood_eval(mock_gp_data.gaussian_process_ptr->points_sampled().data(),
                                             mock_gp_data.gaussian_process_ptr->points_sampled_value().data(),
                                             mock_gp_data.gaussian_process_ptr->noise_variance().data(),
                                             dim, num_sampled);
  typename LogLikelihoodEvaluator::StateType log_likelihood_state(log_likelihood_eval, *mock_gp_data.covariance_ptr);

  initial_likelihood = log_likelihood_eval.ComputeLogLikelihood(log_likelihood_state);
  OL_VERBOSE_PRINTF("initial likelihood: %.18E\n", initial_likelihood);

  bool found_flag = false;
  MultistartNewtonHyperparameterOptimization(log_likelihood_eval, *mock_gp_data.covariance_ptr,
                                             newton_parameters, hyperparameter_log_domain_bounds.data(),
                                             thread_schedule, &found_flag, &uniform_generator,
                                             hyperparameters_optimized.data());
  if (!found_flag) {
    ++total_errors;
  }

  log_likelihood_state.SetHyperparameters(log_likelihood_eval, hyperparameters_optimized.data());
  final_likelihood = log_likelihood_eval.ComputeLogLikelihood(log_likelihood_state);
#ifdef OL_VERBOSE_PRINT
  OL_VERBOSE_PRINTF("final likelihood: %.18E\n", final_likelihood);
  PrintMatrix(hyperparameters_optimized.data(), 1, num_hyperparameters);
#endif

  // check that hyperparameter gradients are small
  std::vector<double> grad_log_marginal(num_hyperparameters);
  log_likelihood_eval.ComputeGradLogLikelihood(&log_likelihood_state, grad_log_marginal.data());

#ifdef OL_VERBOSE_PRINT
  OL_VERBOSE_PRINTF("grad log marginal: ");
  PrintMatrix(grad_log_marginal.data(), 1, grad_log_marginal.size());
#endif

  current_errors = 0;
  for (const auto& entry : grad_log_marginal) {
    if (!CheckDoubleWithinRelative(entry, 0.0, 1.0e-13)) {
      ++current_errors;
    }
  }
  total_errors += current_errors;

  // verify that convergence occurred, start from the hyperparameters used to generate data (real solution should be nearby)
  total_errors += NewtonHyperparameterOptimization(log_likelihood_eval, *mock_gp_data.covariance_ptr, newton_parameters, hyperparameter_domain, hyperparameters_truth.data());
#ifdef OL_VERBOSE_PRINT
  PrintMatrix(hyperparameters_truth.data(), 1, num_hyperparameters);
#endif

  double norm_delta_hyperparameter;
  current_errors = 0;
  for (IdentifyType<decltype(hyperparameters_temp)>::type::size_type i = 0, size = hyperparameters_truth.size(); i < size; ++i) {
    hyperparameters_temp[i] = hyperparameters_truth[i] - hyperparameters_optimized[i];
  }
  norm_delta_hyperparameter = VectorNorm(hyperparameters_temp.data(), hyperparameters_temp.size());
  if (!CheckDoubleWithin(norm_delta_hyperparameter, 0.0, 1.0e-12)) {
    ++current_errors;
  }
  if (current_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("multistart newton did not find the optimal solution: hyperparameters differ by (RMS): %.18E\n", norm_delta_hyperparameter);
  }
  total_errors += current_errors;

  current_errors = 0;
  if (final_likelihood <= initial_likelihood) {
    ++current_errors;  // expect improvement from optimization
  }
  if (final_likelihood >= 0.0) {
    ++current_errors;  // must be negative
  }
  if (current_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("final likelihood = %.18E is worse than initial likelihood = %.18E\n", final_likelihood, initial_likelihood);
  }
  total_errors += current_errors;

  // now check that if we call Newton again with the optimal solution, it will fail to find a new solution
  // and found_flag will be FALSE
  {
    newton_parameters.num_multistarts = 8;
    std::vector<double> initial_guesses(num_hyperparameters*newton_parameters.num_multistarts);
    hyperparameter_domain.GenerateUniformPointsInDomain(newton_parameters.num_multistarts,
                                                        &uniform_generator, initial_guesses.data());

    // insert optimal solution into initial_guesses
    std::copy(hyperparameters_optimized.begin(), hyperparameters_optimized.end(), initial_guesses.begin());

    // build state vector
    std::vector<typename LogLikelihoodEvaluator::StateType> log_likelihood_state_vector;
    SetupLogLikelihoodState(log_likelihood_eval, *mock_gp_data.covariance_ptr,
                            thread_schedule.max_num_threads, &log_likelihood_state_vector);

    OptimizationIOContainer io_container(log_likelihood_state_vector[0].GetProblemSize());
    InitializeBestKnownPoint(log_likelihood_eval, initial_guesses.data(), num_hyperparameters,
                             newton_parameters.num_multistarts,
                             log_likelihood_state_vector.data(), &io_container);

    io_container.found_flag = true;  // want to see that this flag is flipped to false

    NewtonOptimizer<LogLikelihoodEvaluator, TensorProductDomain> newton_opt;
    MultistartOptimizer<NewtonOptimizer<LogLikelihoodEvaluator, TensorProductDomain> > multistart_optimizer;
    multistart_optimizer.MultistartOptimize(newton_opt, log_likelihood_eval, newton_parameters,
                                            hyperparameter_domain, thread_schedule,
                                            initial_guesses.data(), newton_parameters.num_multistarts,
                                            log_likelihood_state_vector.data(),
                                            nullptr, &io_container);

    found_flag = io_container.found_flag;
    std::copy(io_container.best_point.begin(), io_container.best_point.end(), hyperparameters_temp.begin());

    // found_flag should be false b/c optimal solution is in the starting guesses
    if (found_flag) {
      ++total_errors;
    }

    // verify that the solution was not altered
    for (IdentifyType<decltype(hyperparameters_temp)>::type::size_type i = 0, size = hyperparameters_truth.size(); i < size; ++i) {
      if (!CheckDoubleWithin(hyperparameters_temp[i], hyperparameters_optimized[i], 0.0)) {
        ++total_errors;
      }
    }
  }

  return total_errors;
}

}  // end unnamed namespace

int HyperparameterLikelihoodOptimizationTest(OptimizerTypes optimizer_type, LogLikelihoodTypes objective_mode) {
  switch (optimizer_type) {
    case OptimizerTypes::kGradientDescent: {
      switch (objective_mode) {
        case LogLikelihoodTypes::kLogMarginalLikelihood: {
          return HyperparameterLikelihoodOptimizationTestCore<LogMarginalLikelihoodEvaluator, SquareExponential>(objective_mode);
        }
        case LogLikelihoodTypes::kLeaveOneOutLogLikelihood: {
          return HyperparameterLikelihoodOptimizationTestCore<LeaveOneOutLogLikelihoodEvaluator, SquareExponential>(objective_mode);
        }
        default: {
          OL_ERROR_PRINTF("%s: INVALID objective_mode choice: %d\n", OL_CURRENT_FUNCTION_NAME, objective_mode);
          return 1;
        }
      }  // end switch over objective_mode
    }  // end case kGradientDescent
    case OptimizerTypes::kNewton: {
      switch (objective_mode) {
        case LogLikelihoodTypes::kLogMarginalLikelihood: {
          // check base newton optimization
          int current_errors = 0;
          int total_errors = 0;
          current_errors = HyperparameterLikelihoodNewtonOptimizationTestCore<LogMarginalLikelihoodEvaluator, SquareExponential>(objective_mode);
          total_errors += current_errors;

          // check multistarted newton
          current_errors = MultistartHyperparameterLikelihoodNewtonOptimizationTestCore<LogMarginalLikelihoodEvaluator, SquareExponential>(objective_mode);
          total_errors += current_errors;
          return total_errors;
        }
        default: {
          OL_ERROR_PRINTF("%s: INVALID objective_mode choice: %d\n", OL_CURRENT_FUNCTION_NAME, objective_mode);
          return 1;
        }
      }  // end switch over objective_mode
    }  // end case kNewton
    default: {
      OL_ERROR_PRINTF("%s: INVALID optimizer_type choice: %d\n", OL_CURRENT_FUNCTION_NAME, optimizer_type);
      return 1;
    }
  }  // end switch over optimizer_type
}

int EvaluateLogLikelihoodAtPointListTest() {
  using DomainType = TensorProductDomain;
  using HyperparameterDomainType = TensorProductDomain;
  const int dim = 3;

  int total_errors = 0;

  // grid search parameters
  int num_grid_search_points = 100000;

  // random number generators
  UniformRandomGenerator uniform_generator(314);
  boost::uniform_real<double> uniform_double_hyperparameter(0.4, 1.3);
  boost::uniform_real<double> uniform_double_lower_bound(-2.0, 0.5);
  boost::uniform_real<double> uniform_double_upper_bound(2.0, 3.5);

  static const int kMaxNumThreads = 4;
  ThreadSchedule thread_schedule(kMaxNumThreads, omp_sched_guided);

  int num_sampled = 11;  // arbitrary
  std::vector<double> noise_variance(num_sampled, 0.002);
  MockGaussianProcessPriorData<DomainType> mock_gp_data(SquareExponential(dim, 1.0, 1.0), noise_variance,
                                                        dim, num_sampled, uniform_double_lower_bound,
                                                        uniform_double_upper_bound, uniform_double_hyperparameter,
                                                        &uniform_generator);

  using LogLikelihoodEvaluator = LogMarginalLikelihoodEvaluator;
  LogLikelihoodEvaluator log_marginal_eval(mock_gp_data.gaussian_process_ptr->points_sampled().data(),
                                           mock_gp_data.gaussian_process_ptr->points_sampled_value().data(),
                                           mock_gp_data.gaussian_process_ptr->noise_variance().data(),
                                           dim, num_sampled);
  int num_hyperparameters = mock_gp_data.covariance_ptr->GetNumberOfHyperparameters();
  std::vector<ClosedInterval> hyperparameter_log_domain_bounds(num_hyperparameters, {-2.0, 1.0});
  HyperparameterDomainType hyperparameter_log_domain(hyperparameter_log_domain_bounds.data(), num_hyperparameters);

  std::vector<double> grid_search_best_point(num_hyperparameters);
  std::vector<double> function_values(num_grid_search_points);
  std::vector<double> initial_guesses(num_hyperparameters*num_grid_search_points);
  num_grid_search_points = hyperparameter_log_domain.GenerateUniformPointsInDomain(num_grid_search_points,
                                                                                &uniform_generator,
                                                                                initial_guesses.data());
  for (auto& point : initial_guesses) {
    point = std::pow(10.0, point);
  }

  // domain in linear-space
  std::vector<ClosedInterval> hyperparameter_domain_linearspace_bounds(hyperparameter_log_domain_bounds);
  for (auto& interval : hyperparameter_domain_linearspace_bounds) {
    interval = {std::pow(10.0, interval.min), std::pow(10.0, interval.max)};
  }
  HyperparameterDomainType hyperparameter_domain_linearspace(hyperparameter_domain_linearspace_bounds.data(), num_hyperparameters);

  bool found_flag = false;
  EvaluateLogLikelihoodAtPointList(log_marginal_eval, *mock_gp_data.covariance_ptr,
                                   hyperparameter_domain_linearspace, thread_schedule,
                                   initial_guesses.data(), num_grid_search_points, &found_flag,
                                   function_values.data(), grid_search_best_point.data());

  if (!found_flag) {
    ++total_errors;
  }

  // find the max function_value and the index at which it occurs
  auto max_value_ptr = std::max_element(function_values.begin(), function_values.end());
  auto max_index = std::distance(function_values.begin(), max_value_ptr);

  // check that EvaluateLogLikelihoodAtPointList found the right point
  for (int i = 0; i < num_hyperparameters; ++i) {
    if (!CheckDoubleWithin(grid_search_best_point[i], initial_guesses[max_index*num_hyperparameters + i], 0.0)) {
      ++total_errors;
    }
  }

  // now check multi-threaded & single threaded give the same result
  {
    std::vector<double> grid_search_best_point_single_thread(num_hyperparameters);
    std::vector<double> function_values_single_thread(num_grid_search_points);
    ThreadSchedule single_thread_schedule(1, omp_sched_static);
    found_flag = false;
    EvaluateLogLikelihoodAtPointList(log_marginal_eval, *mock_gp_data.covariance_ptr,
                                     hyperparameter_domain_linearspace, single_thread_schedule,
                                     initial_guesses.data(), num_grid_search_points,
                                     &found_flag, function_values_single_thread.data(),
                                     grid_search_best_point_single_thread.data());

    if (!found_flag) {
      ++total_errors;
    }

    // check against multi-threaded result matches single
    for (int i = 0; i < num_hyperparameters; ++i) {
      if (!CheckDoubleWithin(grid_search_best_point[i], grid_search_best_point_single_thread[i], 0.0)) {
        ++total_errors;
      }
    }

    // check all function values match too
    for (int i = 0; i < num_grid_search_points; ++i) {
      if (!CheckDoubleWithin(function_values[i], function_values_single_thread[i], 0.0)) {
        ++total_errors;
      }
    }
  }

  return total_errors;
}

}  // end namespace optimal_learning


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/euler.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check euler behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::euler;
  using boost::simd::Euler;

  STF_TYPE_IS(decltype(Euler<T>()), T);
  STF_EQUAL(Euler<T>(), T(1));
  STF_EQUAL(euler( as(T{}) ),T(1));
}

STF_CASE_TPL( "Check euler behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::euler;
  using boost::simd::Euler;
  T e = static_cast<T>(0.5772156649015328606065120900821);

  STF_TYPE_IS(decltype(Euler<T>()), T);
  STF_IEEE_EQUAL(Euler<T>(), e);
  STF_IEEE_EQUAL(euler( as(T{}) ), e);
}


#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <iostream>

using namespace boost::accumulators;

int main()
{
  accumulator_set<int, features<tag::count>> acc;
  acc(4);
  acc(-6);
  acc(9);
  std::cout << count(acc) << '\n';
}


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

#include <iostream>
using namespace std;
#include <unistd.h>

#include <QList>

#include <Eigen/Dense>

#include <opencv2/opencv.hpp>
using namespace cv;

#include <openbr/plugins/openbr_internal.h>
#include <openbr/core/common.h>
#include <openbr/core/eigenutils.h>
#include <openbr/core/opencvutils.h>

#include <cuda_runtime.h>
#include <cublas_v2.h>
#include <cusolverDn.h>
#include "cudadefines.hpp"

namespace br { namespace cuda { namespace pca {
  void castFloatToDouble(float* a, int inca, double* b, int incb, int numElems);
  void castDoubleToFloat(double* a, int inca, float* b, int incb, int numElems);
}}}

namespace br
{
/*!
 * \ingroup transforms
 * \brief Projects input into learned Principal Component Analysis subspace using CUDA. Modified from original PCA plugin.
 * \author Colin Heinzmann \cite DepthDeluxe
 *
 * \br_property float keep Options are: [keep < 0 - All eigenvalues are retained, keep == 0 - No PCA is performed and the eigenvectors form an identity matrix, 0 < keep < 1 - Keep is the fraction of the variance to retain, keep >= 1 - keep is the number of leading eigenvectors to retain] Default is 0.95.
 * \br_property int drop The number of leading eigen-dimensions to drop.
 * \br_property bool whiten Whether or not to perform PCA whitening (i.e., normalize variance of each dimension to unit norm)
 */
class CUDAPCATransform : public Transform
{
    Q_OBJECT

protected:
    Q_PROPERTY(float keep READ get_keep WRITE set_keep RESET reset_keep STORED false)
    Q_PROPERTY(int drop READ get_drop WRITE set_drop RESET reset_drop STORED false)
    Q_PROPERTY(bool whiten READ get_whiten WRITE set_whiten RESET reset_whiten STORED false)

    BR_PROPERTY(float, keep, 0.95)
    BR_PROPERTY(int, drop, 0)
    BR_PROPERTY(bool, whiten, false)

    Eigen::VectorXf mean;
    Eigen::VectorXf eVals;
    Eigen::MatrixXf eVecs;

    cublasHandle_t cublasHandle;
    float* cudaMeanPtr;     // holds the "keep" long vector
    float* cudaEvPtr;       // holds all the eigenvectors

public:
    CUDAPCATransform() : keep(0.95), drop(0), whiten(false) {
      // try to initialize CUBLAS
      cublasStatus_t status;
      status = cublasCreate(&cublasHandle);
      CUBLAS_ERROR_CHECK(status);
    }

    ~CUDAPCATransform() {
      // tear down CUBLAS
      cublasDestroy(cublasHandle);
    }

private:
    double residualReconstructionError(const Template &src) const
    {
        Template proj;
        project(src, proj);

        Eigen::Map<const Eigen::VectorXf> srcMap(src.m().ptr<float>(), src.m().rows*src.m().cols);
        Eigen::Map<Eigen::VectorXf> projMap(proj.m().ptr<float>(), keep);

        return (srcMap - mean).squaredNorm() - projMap.squaredNorm();
    }

    void train(const TemplateList &cudaTrainingSet)
    {
      cublasStatus_t cublasStatus;
      cudaError_t cudaError;

      // put all the data into a single matrix to perform PCA
      const int instances = cudaTrainingSet.size();
      const int dimsIn = *(int*)cudaTrainingSet.first().m().ptr<void*>()[1]
                               * *(int*)cudaTrainingSet.first().m().ptr<void*>()[2];

      // copy the data over
      double* cudaDataPtr;
      CUDA_SAFE_MALLOC(&cudaDataPtr, instances*dimsIn*sizeof(cudaDataPtr[0]), &cudaError);
      for (int i=0; i < instances; i++) {
        br::cuda::pca::castFloatToDouble(
          (float*)(cudaTrainingSet[i].m().ptr<void*>()[0]),
          1,
          cudaDataPtr+i*dimsIn,
          1,
          dimsIn
        );
      }

      trainCore(cudaDataPtr, dimsIn, instances);

      CUDA_SAFE_FREE(cudaDataPtr, &cudaError);
    }

    void project(const Template &src, Template &dst) const
    {
      cudaError_t cudaError;

      void* const* srcDataPtr = src.m().ptr<void*>();
      float* srcGpuMatPtr = (float*)srcDataPtr[0];
      int rows = *((int*)srcDataPtr[1]);
      int cols = *((int*)srcDataPtr[2]);
      int type = *((int*)srcDataPtr[3]);

      if (type != CV_32FC1) {
        cout << "ERR: Invalid image type" << endl;
        throw 0;
      }

      // save the destination rows
      int dstRows = (int)keep;

      Mat dstMat = Mat(src.m().rows, src.m().cols, src.m().type());
      void** dstDataPtr = dstMat.ptr<void*>();
      float** dstGpuMatPtrPtr = (float**)dstDataPtr;
      dstDataPtr[1] = srcDataPtr[1];  *((int*)dstDataPtr[1]) = 1;
      dstDataPtr[2] = srcDataPtr[2];  *((int*)dstDataPtr[2]) = dstRows;
      dstDataPtr[3] = srcDataPtr[3];

      // allocate the memory and set to zero
      //cout << "Allocating destination memory" << endl;
      cublasStatus_t status;
      cudaMalloc(dstGpuMatPtrPtr, dstRows*sizeof(float));
      cudaMemset(*dstGpuMatPtrPtr, 0, dstRows*sizeof(float));

      {
        float negativeOne = -1.0f;
        status = cublasSaxpy(
          cublasHandle,       // handle
          dstRows,            // vector length
          &negativeOne,       // alpha (1)
          cudaMeanPtr,        // mean
          1,                  // stride
          srcGpuMatPtr,       // y, the source
          1                   // stride
        );
        CUBLAS_ERROR_CHECK(status);
      }

      {
        float one = 1.0f;
        float zero = 0.0f;
        status = cublasSgemv(
          cublasHandle,       // handle
          CUBLAS_OP_T,        // normal vector multiplication
          eVecs.rows(),       // # rows
          eVecs.cols(),       // # cols
          &one,               // alpha (1)
          cudaEvPtr,          // pointer to the matrix
          eVecs.rows(),       // leading dimension of matrix
          srcGpuMatPtr,       // vector for multiplication
          1,                  // stride (1)
          &zero,              // beta (0)
          *dstGpuMatPtrPtr,   // vector to store the result
          1                   // stride (1)
        );
        CUBLAS_ERROR_CHECK(status);
      }

      //cout << "Saving result" << endl;
      dst = dstMat;
      CUDA_SAFE_FREE(srcGpuMatPtr, &cudaError);
    }

    void store(QDataStream &stream) const
    {
        stream << keep << drop << whiten <<  mean << eVecs;
    }

    void load(QDataStream &stream)
    {
        stream >> keep >> drop >> whiten >> mean >> eVecs;

        //cout << "Starting load process" << endl;

        cudaError_t cudaError;
        cublasStatus_t cublasStatus;
        CUDA_SAFE_MALLOC(&cudaMeanPtr, mean.rows()*mean.cols()*sizeof(float), &cudaError);
        CUDA_SAFE_MALLOC(&cudaEvPtr, eVecs.rows()*eVecs.cols()*sizeof(float), &cudaError);

        //cout << "Setting vector" << endl;
        // load the mean vector into GPU memory
        cublasStatus = cublasSetVector(
          mean.rows()*mean.cols(),
          sizeof(float),
          mean.data(),
          1,
          cudaMeanPtr,
          1
        );
        CUBLAS_ERROR_CHECK(cublasStatus);

        //cout << "Setting the matrix" << endl;
        // load the eigenvector matrix into GPU memory
        cublasStatus = cublasSetMatrix(
          eVecs.rows(),
          eVecs.cols(),
          sizeof(float),
          eVecs.data(),
          eVecs.rows(),
          cudaEvPtr,
          eVecs.rows()
        );
        CUBLAS_ERROR_CHECK(cublasStatus);
    }

protected:
    void trainCore(double* cudaDataPtr, int dimsIn, int instances) {
      cudaError_t cudaError;

      const bool dominantEigenEstimation = (dimsIn > instances);

      Eigen::MatrixXd allEVals, allEVecs;

      // allocate the eigenvectors
      if (dominantEigenEstimation) {
        allEVals = Eigen::MatrixXd(instances, 1);
        allEVecs = Eigen::MatrixXd(dimsIn, instances);
      } else {
        allEVals = Eigen::MatrixXd(dimsIn, 1);
        allEVecs = Eigen::MatrixXd(dimsIn, dimsIn);
      }

      if (keep != 0) {
        performCovarianceSVD(cudaDataPtr, dimsIn, instances, allEVals, allEVecs);
      } else {
        // null case
        mean = Eigen::VectorXf::Zero(dimsIn);
        allEVecs = Eigen::MatrixXd::Identity(dimsIn, dimsIn);
        allEVals = Eigen::VectorXd::Ones(dimsIn);
      }

      // *****************
      // We have now found the eigenvalues and eigenvectors
      // *****************

      if (keep <= 0) {
          keep = dimsIn - drop;
      } else if (keep < 1) {
          // Keep eigenvectors that retain a certain energy percentage.
          const double totalEnergy = allEVals.sum();
          if (totalEnergy == 0) {
              keep = 0;
          } else {
              double currentEnergy = 0;
              int i=0;
              while ((currentEnergy / totalEnergy < keep) && (i < allEVals.rows())) {
                  currentEnergy += allEVals(i);
                  i++;
              }
              keep = i - drop;
          }
      } else {
          if (keep + drop > allEVals.rows()) {
              qWarning("Insufficient samples, needed at least %d but only got %d.", (int)keep + drop, (int)allEVals.rows());
              keep = allEVals.rows() - drop;
          }
      }

      // Keep highest energy vectors
      eVals = Eigen::VectorXf((int)keep, 1);
      eVecs = Eigen::MatrixXf(allEVecs.rows(), (int)keep);
      for (int i=0; i<keep; i++) {
          int index = i+drop;
          eVals(i) = allEVals(index);
          eVecs.col(i) = allEVecs.col(index).cast<float>();
          if (whiten) eVecs.col(i) /= sqrt(eVals(i));
      }

      // Debug output
      if (Globals->verbose) qDebug() << "PCA Training:\n\tDimsIn =" << dimsIn << "\n\tKeep =" << keep;
    }

    // computes the covariance matrix and then pulls the eigenvalues+eigenvectors
    // out of it using SVD of a symmetric matrix
    void performCovarianceSVD(double* cudaDataPtr, int dimsIn, int instances, Eigen::MatrixXd& allEVals, Eigen::MatrixXd& allEVecs) {
      cudaError_t cudaError;

      const bool dominantEigenEstimation = (dimsIn > instances);

      // used for temporary storage
      Eigen::VectorXd meanDouble(dimsIn);

      // compute the mean
      for (int i=0; i < dimsIn; i++) {
        cublasDasum(
          cublasHandle,
          instances,
          cudaDataPtr+i,
          dimsIn,
          meanDouble.data()+i
        );
      }

      // put data back on GPU for further processing
      double* cudaMeanDoublePtr;
      CUDA_SAFE_MALLOC(&cudaMeanDoublePtr, dimsIn*sizeof(cudaMeanDoublePtr[0]), &cudaError);
      cublasSetVector(
        dimsIn,
        sizeof(cudaMeanDoublePtr[0]),
        meanDouble.data(),
        1,
        cudaMeanDoublePtr,
        1
      );

      // scale to calculate average
      {
        double scaleFactor = 1.0/(double)instances;
        cublasDscal(
          cublasHandle,
          dimsIn,
          &scaleFactor,
          cudaMeanDoublePtr,
          1
        );
      }

      // subtract mean from data
      for (int i=0; i < instances; i++) {
        double negativeOne = -1.0;
        cublasDaxpy(
          cublasHandle,
          dimsIn,
          &negativeOne,
          cudaMeanDoublePtr,
          1,
          cudaDataPtr+i*dimsIn,
          1
        );
      }

      // convert to float form and copy the data back
      CUDA_SAFE_MALLOC(&cudaMeanPtr, dimsIn*sizeof(cudaMeanPtr[0]), &cudaError);
      br::cuda::pca::castDoubleToFloat(cudaMeanDoublePtr, 1, cudaMeanPtr, 1, dimsIn);

      // copy the data back
      mean = Eigen::VectorXf(dimsIn);
      cublasGetVector(
        dimsIn,
        sizeof(cudaMeanPtr[0]),
        cudaMeanPtr,
        1,
        mean.data(),
        1
      );

      // free up the memory
      CUDA_SAFE_FREE(cudaMeanDoublePtr, &cudaError);
      CUDA_SAFE_FREE(cudaMeanPtr, &cudaError);

      // allocate space for the covariance matrix
      double* cudaCovariancePtr;
      int covRows = allEVals.rows();
      CUDA_SAFE_MALLOC(&cudaCovariancePtr, covRows*covRows*sizeof(cudaCovariancePtr[0]), &cudaError);

      // compute the covariance matrix
      if (dominantEigenEstimation) {
        // cov = data.transpose() * data / (instances-1.0);
        const double scaleFactor = 1.0/(instances-1.0);
        const double zero = 0.0;
        cublasDgemm(
          cublasHandle,
          CUBLAS_OP_T,
          CUBLAS_OP_N,
          instances,
          instances,
          dimsIn,
          &scaleFactor,
          cudaDataPtr,
          dimsIn,
          cudaDataPtr,
          dimsIn,
          &zero,
          cudaCovariancePtr,
          covRows
        );
      } else {
        // cov = data * data.transpose() / (instances-1.0);
        const double scaleFactor = 1.0/(instances-1.0);
        const double zero = 0.0;
        cublasDgemm(
          cublasHandle,
          CUBLAS_OP_N,
          CUBLAS_OP_T,
          dimsIn,
          dimsIn,
          instances,
          &scaleFactor,
          cudaDataPtr,
          dimsIn,
          cudaDataPtr,
          dimsIn,
          &zero,
          cudaCovariancePtr,
          covRows
        );
      }

      cusolverDnHandle_t cusolverHandle;
      cusolverStatus_t cusolverStatus;
      cusolverDnCreate(&cusolverHandle);

      // allocate appropriate working space
      int svdLWork;
      cusolverDnDgesvd_bufferSize(
        cusolverHandle,
        covRows,
        covRows,
        &svdLWork
      );
      double* cudaSvdWork;
      CUDA_SAFE_MALLOC(&cudaSvdWork, svdLWork*sizeof(cudaSvdWork[0]), &cudaError);

      double* cudaUPtr;
      CUDA_SAFE_MALLOC(&cudaUPtr, covRows*covRows*sizeof(cudaUPtr[0]), &cudaError);
      double* cudaSPtr;
      CUDA_SAFE_MALLOC(&cudaSPtr, covRows*sizeof(cudaSPtr[0]), &cudaError);
      double* cudaVTPtr;
      CUDA_SAFE_MALLOC(&cudaVTPtr, covRows*covRows*sizeof(cudaVTPtr[0]), &cudaError);

      int* cudaSvdDevInfoPtr;
      CUDA_SAFE_MALLOC(&cudaSvdDevInfoPtr, sizeof(*cudaSvdDevInfoPtr), &cudaError);
      int svdDevInfo;

      // perform SVD on an n x m matrix, in this case the matrix is the covariance
      // matrix and is symmetric, meaning the SVD will calculate the eigenvalues
      // and eigenvectors for us.
      cusolverStatus = cusolverDnDgesvd(
        cusolverHandle,
        'A',                // all columns of unitary matrix
        'A',                // all columns of array VT
        covRows,            // m
        covRows,            // n
        cudaCovariancePtr,  // decomposing the covariance matrix
        covRows,            // lda
        cudaSPtr,           // holds S
        cudaUPtr,           // holds U
        covRows,            // ldu
        cudaVTPtr,          // holds VT
        covRows,            // ldvt
        cudaSvdWork,        // work buffer ptr
        svdLWork,           // length of the work buffer
        NULL,               // rwork, not used for real data types
        cudaSvdDevInfoPtr   // devInfo pointer
      );
      CUSOLVER_ERROR_CHECK(cusolverStatus);

      // get the eigenvalues and free memory
      cublasGetVector(
        covRows,
        sizeof(cudaSPtr[0]),
        cudaSPtr,
        1,
        allEVals.data(),
        1
      );
      CUDA_SAFE_FREE(cudaSvdWork, &cudaError);
      CUDA_SAFE_FREE(cudaSPtr, &cudaError);
      CUDA_SAFE_FREE(cudaVTPtr, &cudaError);
      CUDA_SAFE_FREE(cudaSvdDevInfoPtr, &cudaError);

      // if this is a dominant eigen estimation, then perform matrix multiplication again
      // if (dominantEigenEstimation) allEVecs = data * allEVecs;
      if (dominantEigenEstimation) {
        double* cudaMultedAllEVecs;
        CUDA_SAFE_MALLOC(&cudaMultedAllEVecs, dimsIn*instances*sizeof(cudaMultedAllEVecs[0]), &cudaError);
        const double one = 1.0;
        const double zero = 0;

        cublasDgemm(
          cublasHandle,   // handle
          CUBLAS_OP_N,    // transa
          CUBLAS_OP_N,    // transb
          dimsIn,         // m
          instances,      // n
          instances,      // k
          &one,           // alpha
          cudaDataPtr,    // A
          dimsIn,         // lda
          cudaUPtr,       // B
          instances,      // ldb
          &zero,          // beta
          cudaMultedAllEVecs, // C
          dimsIn          // ldc
        );

        // normalize result then divide the column by the norm
        for (int i=0; i < instances; i++) {
          // compute the norm
          double norm;
          cublasDnrm2(
            cublasHandle,
            dimsIn,
            cudaMultedAllEVecs+i*dimsIn,
            1,
            &norm
          );

          // now divide by it
          norm = 1.0/norm;
          cublasDscal(
            cublasHandle,
            dimsIn,
            &norm,
            cudaMultedAllEVecs+i*dimsIn,
            1
          );
        }

        // get the eigenvectors from the multiplied value
        cublasGetMatrix(
          dimsIn,
          instances,
          sizeof(cudaMultedAllEVecs[0]),
          cudaMultedAllEVecs,
          dimsIn,
          allEVecs.data(),
          dimsIn
        );

        // free the memory used for multiplication
        CUDA_SAFE_FREE(cudaMultedAllEVecs, &cudaError);
      } else {
        // normalize result then divide the column by the norm
        for (int i=0; i < instances; i++) {
          // compute the norm
          double norm;
          cublasDnrm2(
            cublasHandle,
            covRows,
            cudaUPtr+i*covRows,
            1,
            &norm
          );

          // now divide by it
          norm = 1.0/norm;
          cublasDscal(
            cublasHandle,
            covRows,
            &norm,
            cudaUPtr+i*covRows,
            1
          );
        }

        // get the eigenvectors straight from the SVD
        cublasGetMatrix(
          covRows,
          covRows,
          sizeof(cudaUPtr[0]),
          cudaUPtr,
          covRows,
          allEVecs.data(),
          covRows
        );
      }

      // free all the memory
      CUDA_SAFE_FREE(cudaCovariancePtr, &cudaError);
      CUDA_SAFE_FREE(cudaUPtr, &cudaError);
      cusolverDnDestroy(cusolverHandle);
    }
};

BR_REGISTER(Transform, CUDAPCATransform)
} // namespace br

#include "cuda/cudapca.moc"


#include <webots/Emitter.hpp>
#include <webots/Receiver.hpp>

#include <string>
#include <vector>
#include <cstdlib>
#include <iostream>
#include <cmath>
#include<limits>

#include <Eigen/Dense>

#include "tracking.h"
#include "greyWolf.h"

#define _USE_MATH_DEFINES

double calculate_euclideanDistance(double x1, double z1, double x2, double z2){
  return pow(pow(x2 - x1,2)+pow(z2 - z1,2),0.5);
}

GreyWolf::GreyWolf(webots::Emitter *emitter_handle, webots::Receiver *receiver_handle, int timestep, robot_tracking::Position init_pos){
  emitter = emitter_handle;
  receiver = receiver_handle;
  timeStep = timestep;
  receiver->enable(timeStep);
  printDebuggingInfo = false;
  starting_position = init_pos;
  init_information.clear();
  event_message.clear();
  a <<1,1;
  r1<<1,1;
  r2<< 1,1;
  if(printDebuggingInfo){
    std::cout<<"_________ROBOT INDEX___________:"<<robot_index<<"\n";
    std::cout<<starting_position.x<<starting_position.z<<"\n";
  }
}

void GreyWolf::set_swarmSettings(int index, int num_wolves, int num_iterations){
  robot_index = index;
  numberOfWolves = num_wolves;
  num_iter = num_iterations;
}

void GreyWolf::set_boundaries(int x_upper,int x_lower,int z_upper,int z_lower){
  x_upper_boundary = x_upper;
  x_lower_boundary = x_lower;
  z_upper_boundary = z_upper;
  z_lower_boundary = z_lower;
}

void GreyWolf::set_individualSettings(double explore_factor, double multiplication_factor, int social_number){
  k_exploration = explore_factor;
  k_multiplication = multiplication_factor;
  social_learning_number = social_number;
}

void GreyWolf::emit_currentPosition(int current_iter){
  message.event_id = current_iter;
  message.index = robot_index;
  message.robot_position = my_position;
  message.event_message.assign("Position Update");
  emitter->send(&message, sizeof(Event));
  event_message.push_back(message);
}

void GreyWolf::init_StatusUpdate(robot_tracking::Position starting_position){
  message.event_id = 0;
  message.index = robot_index;
  message.robot_position = my_position;
  message.event_message.assign("Initialisation");
  emitter->send(&message, sizeof(Event));
  init_information.push_back(message);
}

void GreyWolf::update_position(robot_tracking::Position position){
  prev_position = my_position;
  my_position = position;
}

void GreyWolf::set_targetPosition(robot_tracking::Position target){
  target_position = target;
}

void GreyWolf::get_allEvents(int current_iter){
    event_message.clear();
    Event *received_event;
    while(receiver->getQueueLength() > 0){
      received_event = (Event *) receiver->getData();
      if(received_event->event_message.compare("Initialisation") == 0)
        init_information.push_back(*received_event);
      else if(received_event->event_message.compare("Position Update") == 0){
        // if(received_event->event_id == current_iter)
          event_message.push_back(*received_event);
      }
      receiver->nextPacket();
    }
    if(printDebuggingInfo){
      std::cout<<"Printing Events\n";
      print_AllEvents();
    }
}

void GreyWolf::print_AllEvents(){
  std::cout<<"_________ROBOT INDEX___________:"<<robot_index<<"\n";
  for(std::vector<Event>::const_iterator i = event_message.begin(); i != event_message.end(); ++i){
    std::cout<<i->event_id<<"\t"<<i->index<<"\t"<<i->event_message<<"\n";
    std::cout<<"Position"<<"\t"<<i->robot_position.x<<"\t"<<i->robot_position.z<<"\t"<<i->robot_position.heading<<"\n";
  }
}

void GreyWolf::clear_initInformation(){
  init_information.clear();
}

void GreyWolf::clear_eventMessage(){
  event_message.clear();
}

double GreyWolf::calculate_EucDist(robot_tracking::Position P1,robot_tracking::Position P2){
  return std::pow(std::pow(P1.x - P2.x,2) + std::pow(P1.z - P2.z,2),0.5);
}

double GreyWolf::generateRandomNumber(double lowerLimit, double upperLimit){
  double rand_num = ((double)rand())/((double)RAND_MAX +1);
  rand_num = lowerLimit + ((upperLimit - lowerLimit)*rand_num);
  return rand_num;
}

void GreyWolf::calculate_Constants(int current_iter){
  double buff = (double)k_exploration*((double)1 - ((double)current_iter/(double)num_iter));
  // std::cout<<"Buff"<<buff;
  a << buff,buff;
  // std::cout<<a<<"\n";
  r1 << generateRandomNumber(0,2),generateRandomNumber(0,2);
  r2 << generateRandomNumber(0,2),generateRandomNumber(0,2);
  if(printDebuggingInfo){
    std::cout<<"current iter"<<current_iter<<" and num iter"<<num_iter<<"\n";
    std::cout<<"_________ROBOT INDEX___________:"<<robot_index<<"\n";
    std::cout<<"R1: "<<r1[0]<<"\t"<<r1[1]<<"\n";
    std::cout<<"R2: "<<r2[0]<<"\t"<<r2[1]<<"\n";
    std::cout<<"A: "<<a[0]<<"\t"<<a[1]<<"\n";
  }
}
// bool GreyWolf::compareEvents(Event e1, Event e2){
  // return (e1.index<e2.index);
// }

// bool comparePairs(const std::pair<GreyWolf::Event,double> &a,
              // const std::pair<GreyWolf::Event,double> &b)
// {
    // return (a.second < b.second);
// }

double GreyWolf::calculate_fitnessOfWolf(int current_iter, robot_tracking::Position wolfStartingPosition, robot_tracking::Position wolfCurrentPosition){

    double k_intraSwarmDistance = 1 - (current_iter/num_iter);
    // double k_intraSwarmDistance = 0;
    double intraSwarmDistance = 0, 	wolf_communicationThreshold = 1;
    for(std::vector<Event>::const_iterator i = event_message.begin(); i!= event_message.end(); ++i){
      // if(i->event_id == current_iter && i->index != robot_index){
      if(i->index != robot_index){
        intraSwarmDistance += wolf_communicationThreshold/calculate_EucDist(wolfCurrentPosition,i->robot_position);
      }
    }
  //Calculate individual coverage
    double coverageIndividual = calculate_EucDist(wolfStartingPosition, wolfCurrentPosition);
    double k_coverageIndividual = 1 - (current_iter/num_iter);
  //Calculate distance from the goal
    double k_distanceFromGoal = 1 + (current_iter/num_iter);
    double distanceFromGoal = calculate_EucDist(target_position,wolfCurrentPosition);

    double fitness = (k_coverageIndividual*coverageIndividual) + (k_intraSwarmDistance*intraSwarmDistance) - (k_distanceFromGoal*distanceFromGoal);
    return fitness;
}

void GreyWolf::calculate_FitnessRanker(int current_iter){
  // fitnessRanker.clear();
  std::sort(init_information.begin(), init_information.end(),[](Event e1, Event e2){
    return (e1.index < e2.index);
  });// Fix this!
  for(std::vector<Event>::const_iterator i = event_message.begin(); i != event_message.end(); ++i){
    //Get the corresponding starting position for that event
    robot_tracking::Position startingPosition = init_information[i->index].robot_position;
    //Made change to the first parameter of the fitness function.
    //Earlier, the current iter was passed as first argument, now the event id is passed.
    double fitness = calculate_fitnessOfWolf(i->event_id, startingPosition, i->robot_position);
    fitnessRanker.push_back(std::make_pair(*i,fitness));
  }
  //Sort the fitnessRanker
  std::sort(fitnessRanker.begin(), fitnessRanker.end(), [](const std::pair<GreyWolf::Event,double> &a, const std::pair<GreyWolf::Event,double> &b){
    return (a.second < b.second);
  });
}

robot_tracking::Position GreyWolf::calculateNextPosition(int current_iter){
  bool isOmega = true;
  Eigen::Vector2d target(target_position.x, target_position.z);
  Eigen::Vector2d currentPosition(my_position.x, my_position.z);
  Eigen::Vector2d nextPosition(0,0);
  robot_tracking::Position next_pos;
  fitnessRanker.clear();
  calculate_FitnessRanker(current_iter);
  calculate_Constants(current_iter);
  Eigen::Vector2d A,C,D,buff;

  for (int i=0;i<social_learning_number;i++){
    if(fitnessRanker[i].first.index == robot_index)
      isOmega = false;
  }
  //Calculate A
  A << 2*a[0]*r1[0] , 2*a[1]*r1[1];
  A -= a;
  //Calculate C
  C = 2*r2;

  if(isOmega){
    //Calculate D
    D << C[0]*target[0] , C[1]*target[1];
    D -= currentPosition;
    nextPosition << A[0]*D[0] ,A[1]*D[1];
    nextPosition  = target - nextPosition;
  }
  else{
    for (int j=0;j<social_learning_number;j++){
      D << C[0]*fitnessRanker[j].first.robot_position.x, C[1]*fitnessRanker[j].first.robot_position.z;
      D -= currentPosition;
      buff <<fitnessRanker[j].first.robot_position.x - A[0]*D[0] , fitnessRanker[j].first.robot_position.z - A[1]*D[1];
      nextPosition+= buff;
    }
    nextPosition *= 1/social_learning_number;
  }

  nextPosition = currentPosition + k_multiplication*(nextPosition - currentPosition);
  next_pos.x = nextPosition[0];
  next_pos.z = nextPosition[1];
  next_pos.heading = M_PI/3;

  // std::cout<<"_________POSITION of ROBOT INDEX___________:"<<robot_index<<"\n";
  // std::cout<<"before boundary snap = ("<<next_pos.x<<","<<next_pos.z<<","<<next_pos.heading*(180/M_PI)<<")\n";

  if(!check_insideBoundary(next_pos)){
    // std::cout<<"Boundary snap called\n";
    next_pos = snapToBoundary(my_position, next_pos);
  }

  // std::cout<<"_________POSITION of ROBOT INDEX___________:"<<robot_index<<"\n";
  // std::cout<<"after boundary snap = ("<<next_pos.x<<","<<next_pos.z<<","<<next_pos.heading*(180/M_PI)<<")\n";

  return next_pos;
}

robot_tracking::Position GreyWolf::snapToBoundary(robot_tracking::Position currentPosition, robot_tracking::Position nextPosition){
  // Set boundary points
  double x1 = currentPosition.x , z1 = currentPosition.z;
  double x2 = nextPosition.x , z2 = nextPosition.z;
  double x3,z3,x4,z4;
  double ta,tb;
  double x_new, z_new;

  // std::cout<<"_________Before snapping Function of ROBOT INDEX___________:"<<robot_index<<"\n";
  // std::cout<<"X1,Z1= ("<<x1<<","<<z1<<")\n";
  // std::cout<<"X2,Z2= ("<<x2<<","<<z2<<")\n";

  if(x1>=x_upper_boundary || x1<=x_lower_boundary)
    x1 = round(x1);
  if(z1>=z_upper_boundary || z1<=z_lower_boundary)
    z1 = round(z1);

  //Setup boundaries
  x3 = x_upper_boundary;
  z3 = z_upper_boundary;
  x4 = x_upper_boundary;
  z4 = z_lower_boundary;
  //Calculate constants
  ta = (((z3 - z4)*(x1 - x3)) + ((x4 - x3)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));
  tb = (((z1 - z2)*(x1 - x3)) + ((x2 - x1)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));

  if(ta>=0 && ta<=1 && tb<=1 && tb>=0){
    x_new = x1 + ta*(x2 - x1);
    z_new = z1 + ta*(z2 - z1);
  }

  else{
    //Setup boundaries
    x3 = x_upper_boundary;
    z3 = z_upper_boundary;
    x4 = x_lower_boundary;
    z4 = z_upper_boundary;
    //Calculate constants
    ta = (((z3 - z4)*(x1 - x3)) + ((x4 - x3)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));
    tb = (((z1 - z2)*(x1 - x3)) + ((x2 - x1)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));

    if(ta>=0 && ta<=1 && tb<=1 && tb>=0){
      x_new = x1 + ta*(x2 - x1);
      z_new = z1 + ta*(z2 - z1);
    }
    else{
      //Setup boundaries
      x3 = x_lower_boundary;
      z3 = z_lower_boundary;
      x4 = x_lower_boundary;
      z4 = z_upper_boundary;
      //Calculate constants
      ta = (((z3 - z4)*(x1 - x3)) + ((x4 - x3)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));
      tb = (((z1 - z2)*(x1 - x3)) + ((x2 - x1)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));

      if(ta>=0 && ta<=1 && tb<=1 && tb>=0){
        x_new = x1 + ta*(x2 - x1);
        z_new = z1 + ta*(z2 - z1);
      }
      else{
        //Setup boundaries
        x3 = x_lower_boundary;
        z3 = z_lower_boundary;
        x4 = x_upper_boundary;
        z4 = z_lower_boundary;
        //Calculate constants
        ta = (((z3 - z4)*(x1 - x3)) + ((x4 - x3)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));
        tb = (((z1 - z2)*(x1 - x3)) + ((x2 - x1)*(z1 - z3)))/(((x4 - x3)*(z1 - z2)) - ((x1 - x2)*(z4 - z3)));

        if(ta>=0 && ta<=1 && tb<=1 && tb>=0){
          x_new = x1 + ta*(x2 - x1);
          z_new = z1 + ta*(z2 - z1);
        }
        else{
          x_new = nextPosition.x;
          z_new = nextPosition.z;
        }
      }
    }
  }

  // std::cout<<"_________Before scaling of ROBOT INDEX___________:"<<robot_index<<"\n";
  // std::cout<<"X_new,Z_new= ("<<x_new<<","<<z_new<<")\n";

  //Scale next Position so that it always remains inside the boundary
  //Done to prevent the robot from always flocking to the edges
  // std::cout<<"_________Difference in positions ROBOT INDEX___________:"<<robot_index<<"\n";
  // std::cout<<"X_diff,Z_diff= ("<<(x_new - currentPosition.x)<<","<<(z_new - currentPosition.z)<<")\n";

  robot_tracking::Position newPosition;
  newPosition.x = x_new;
  newPosition.z = z_new;
  newPosition.heading = nextPosition.heading;

  return newPosition;
}

robot_tracking::Position GreyWolf::generateRandomPosition(){
  robot_tracking::Position newPos;
  newPos.x = generateRandomNumber(x_lower_boundary,x_upper_boundary);
  newPos.z = generateRandomNumber(z_lower_boundary,z_upper_boundary);
  newPos.heading = 0;
  return newPos;
}

bool GreyWolf::check_insideBoundary(robot_tracking::Position nextPosition){
  bool x_insideBoundary = false, z_insideBoundary = false;
  if(nextPosition.x> x_lower_boundary && nextPosition.x< x_upper_boundary)
    x_insideBoundary = true;
  if(nextPosition.z> z_lower_boundary && nextPosition.z< z_upper_boundary)
    z_insideBoundary = true;

  return (x_insideBoundary && z_insideBoundary);
}

robot_tracking::Position GreyWolf::get_previousPosition(){
  return prev_position;
}


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Copyright (c) 2017-2019 The moonet developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    CAmount nSubsidy = 0;

    nSubsidy = GetBlockValue(0);
    BOOST_CHECK(nSubsidy == 0.1 * COIN);
    nSum += nSubsidy;

    /* premine in block 1 (3,000,000 moonet) */
    nSubsidy = GetBlockValue(1);
    BOOST_CHECK(nSubsidy == 3000000 * COIN);
    nSum += nSubsidy;

    nSubsidy = GetBlockValue(0);
    BOOST_CHECK(nSubsidy == 0.1 * COIN);
    nSum += nSubsidy;

    for (int nHeight = 3; nHeight <= 291; nHeight += 1) {
        /* PoW Phase */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 3 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 292; nHeight <= 1728; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 2 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1729; nHeight <= 4608; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 5 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 4609; nHeight <= 8928; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 10 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 8929; nHeight <= 11808; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 25 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 11809; nHeight <= 16128; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 70 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 16129; nHeight <= 19008; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 100 * COIN);
        nSum += nSubsidy;
    }
    //Until TIER_BLOCK_HEIGHT
    for (int nHeight = 19009; nHeight <= 524500; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 200 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 524501; nHeight <= 524500 + 100; nHeight += 1) {
        /* Masternode tiers system implemented */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 100 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1578902; nHeight <= 1578902 + 100; nHeight += 1) {
        /* Masternode tiers system implemented */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 25 * COIN);
        nSum += nSubsidy;
    }
}

BOOST_AUTO_TEST_CASE(halving_test)
{
    double standartReward = 25;

    BOOST_CHECK(GetHalvingReward(0) == standartReward);
    BOOST_CHECK(GetHalvingReward(1578902) == standartReward);
    BOOST_CHECK(GetHalvingReward(1578902 + 525600) == (standartReward / 2));
    BOOST_CHECK(GetHalvingReward(1578902 + 525600 + 525600 + 525600 + 525600) == (standartReward / 5));
}

BOOST_AUTO_TEST_SUITE_END()


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// Distributed under the Boost
// Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or
// copy at http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_BLOOM_FILTER_DYNAMIC_COUNTING_BLOOM_FILTER_HPP
#define BOOST_BLOOM_FILTER_DYNAMIC_COUNTING_BLOOM_FILTER_HPP 1

#include <cmath>
#include <vector>

#include <boost/config.hpp>

#include <boost/static_assert.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_unsigned.hpp>

#include <boost/bloom_filter/detail/twohash_counting_apply_hash.hpp>
#include <boost/bloom_filter/detail/extenders.hpp>
#include <boost/bloom_filter/hash/default.hpp>
#include <boost/bloom_filter/hash/murmurhash3.hpp>

namespace boost {
  namespace bloom_filters {
    template <typename T,
	      size_t BitsPerBin = 4,
	      size_t HashValues = 2,
	      size_t ExpectedInsertionCount = 0,
	      class HashFunction1 = boost_hash<T>,
	      class HashFunction2 = murmurhash3<T>,
	      class ExtensionFunction = detail::square,
	      typename Block = size_t,
	      typename Allocator = std::allocator<Block> >
    class twohash_dynamic_counting_bloom_filter {

      // Block needs to be an integral type
      BOOST_STATIC_ASSERT( boost::is_integral<Block>::value == true);

      // Block needs to be an unsigned type
      BOOST_STATIC_ASSERT( boost::is_unsigned<Block>::value == true);

      // BitsPerBin needs to be greater than 0
      BOOST_STATIC_ASSERT( BitsPerBin > 0);

      // it doesn't make sense to ever support using a BitsPerBin value larger
      // than the number of bits per Block. In that case, the user shouldn't
      // be using a Bloom filter to represent their data.
      BOOST_STATIC_ASSERT( (BitsPerBin < (sizeof(Block) * 8) ) );

      // because of the nature of this implementation, the Bloom filter
      // can have internal fragmentation if the calculation for
      // bins_per_slot has a remainder. The severity of the  internal
      // fragmentation is equal to the remainder * the number of slots.
      // This check prevents internal fragmentation.
      // This also necessarily limits to bin sizes to one of:
      // [1,2,4,8,16,32(64-bit system only)] bits
      BOOST_STATIC_ASSERT( ((sizeof(Block) * 8) % BitsPerBin) == 0);

    public:
      typedef T value_type;
      typedef T key_type;
      typedef HashFunction1 hash_function1_type;
      typedef HashFunction2 hash_function2_type;
      typedef ExtensionFunction extension_function_type;
      typedef Block block_type;
      typedef Allocator allocator_type;
      typedef twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
						    HashValues,
						    ExpectedInsertionCount,
						    HashFunction1,
						    HashFunction2,
						    ExtensionFunction,
						    Block, Allocator> this_type;

      typedef std::vector<Block, Allocator> bucket_type;
      typedef typename bucket_type::iterator bucket_iterator;
      typedef typename bucket_type::const_iterator bucket_const_iterator;

      static const size_t default_num_bins = 32;

    private:
      static const size_t slot_bits = sizeof(block_type) * 8;

      size_t bucket_size(const size_t requested_bins) const {
	const size_t bin_bits = requested_bins * BitsPerBin;
	return bin_bits / slot_bits + 1;
      }

      typedef detail::twohash_counting_apply_hash<HashValues,
						  this_type> apply_hash_type;

    public:
      //! constructors
      twohash_dynamic_counting_bloom_filter()
	: bits(bucket_size(default_num_bins)),
	  _num_bins(default_num_bins)
      {
      }

      explicit twohash_dynamic_counting_bloom_filter(const size_t requested_bins)
	: bits(bucket_size(requested_bins)),
	  _num_bins(requested_bins)
      {
      }

      template <typename InputIterator>
      twohash_dynamic_counting_bloom_filter(const InputIterator start,
					    const InputIterator end)
	: bits(bucket_size(std::distance(start, end) * 4)),
	  _num_bins(std::distance(start, end) * 4)
      {
	for (InputIterator i = start; i != end; ++i)
	  this->insert(*i);
      }

      //! meta functions
      size_t num_bins() const
      {
	return this->_num_bins;
      }

      static BOOST_CONSTEXPR size_t expected_insertion_count()
      {
	return ExpectedInsertionCount;
      }

      static BOOST_CONSTEXPR size_t bits_per_bin()
      {
	return BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t bins_per_slot()
      {
	return sizeof(block_type) * 8 / BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t mask()
      {
	return static_cast<Block>(0 - 1) >> (sizeof(Block) * 8 - BitsPerBin);
      }

      size_t bit_capacity() const
      {
        return this->num_bins() * BitsPerBin;
      }

      static BOOST_CONSTEXPR size_t num_hash_functions()
      {
        return HashValues;
      }

      double false_positive_rate() const
      {
        const double n = static_cast<double>(this->count());
        static const double k = static_cast<double>(num_hash_functions());
        static const double m = static_cast<double>(this->num_bins());
        static const double e =
	  2.718281828459045235360287471352662497757247093699959574966;
        return std::pow(1 - std::pow(e, -k * n / m), k);
      }

      //? returns the number of bins that have at least 1 bit set
      size_t count() const
      {
	size_t ret = 0;

	for (bucket_const_iterator i = this->bits.begin(),
	       end = this->bits.end();
	     i != end; ++i) {
	  for (size_t bin = 0; bin < this->bins_per_slot(); ++bin) {
	    const size_t offset_bits = bin * BitsPerBin;
	    const size_t target_bits = (*i >> offset_bits) & this->mask();

	    if (target_bits > 0)
	      ++ret;
	  }
	}

        return ret;
      }

      bool empty() const
      {
	return this->count() == 0;
      }

      const bucket_type&
      data() const
      {
	return this->bits;
      }

      //! core ops
      void insert(const T& t)
      {
	apply_hash_type::insert(t,
				this->bits,
				this->num_bins());
      }

      template <typename InputIterator>
      void insert(const InputIterator start, const InputIterator end)
      {
	for (InputIterator i = start; i != end; ++i) {
	  this->insert(*i);
	}
      }

      void remove(const T& t)
      {
	apply_hash_type::remove(t,
				this->bits,
				this->num_bins());
      }

      template <typename InputIterator>
      void remove(const InputIterator start, const InputIterator end)
      {
	for (InputIterator i = start; i != end; ++i) {
	  this->remove(*i);
	}
      }

      bool probably_contains(const T& t) const
      {
	return apply_hash_type::contains(t,
					 this->bits,
					 this->num_bins());
      }

      //! auxiliary ops
      void clear()
      {
	for (bucket_iterator i = bits.begin(), end = bits.end();
	     i != end; ++i)
	  *i = 0;
      }

      void swap(twohash_dynamic_counting_bloom_filter& other)
      {
	twohash_dynamic_counting_bloom_filter tmp = other;
	other = *this;
	*this = tmp;
      }

      // equality comparison operators
      template <typename _T, size_t _BitsPerBin,
		size_t _HashValues, size_t _ExpectedInsertionCount,
		class _HashFn1, class _HashFn2, class _Extender,
		typename _Block, class _Allocator>
      friend bool
      operator==(const twohash_dynamic_counting_bloom_filter<_T, _BitsPerBin,
							     _HashValues,
							     _ExpectedInsertionCount,
							     _HashFn1,
							     _HashFn2,
							     _Extender,
							     _Block,
							     _Allocator>& lhs,
		 const twohash_dynamic_counting_bloom_filter<_T, _BitsPerBin,
							     _HashValues,
							     _ExpectedInsertionCount,
							     _HashFn1,
							     _HashFn2,
							     _Extender,
							     _Block,
							     _Allocator>& rhs);

      template <typename _T, size_t _BitsPerBin,
		size_t _HashValues, size_t _ExpectedInsertionCount,
		class _HashFn1, class _HashFn2, class _Extender,
		typename _Block, class _Allocator>
      friend bool
      operator!=(const twohash_dynamic_counting_bloom_filter<_T, _BitsPerBin,
							     _HashValues,
							     _ExpectedInsertionCount,
							     _HashFn1,
							     _HashFn2,
							     _Extender,
							     _Block,
							     _Allocator>& lhs,
		 const twohash_dynamic_counting_bloom_filter<_T, _BitsPerBin,
							     _HashValues,
							     _ExpectedInsertionCount,
							     _HashFn1,
							     _HashFn2,
							     _Extender,
							     _Block,
							     _Allocator>& rhs);

    private:
      bucket_type bits;
      size_t _num_bins;
    };

    template<class T, size_t BitsPerBin, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1, class HashFunction2,
	     class ExtensionFunction, typename Block,
	     class Allocator>
    void
    swap(twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
					       HashValues,
					       ExpectedInsertionCount,
					       HashFunction1,
					       HashFunction2,
					       ExtensionFunction,
					       Block,
					       Allocator>& lhs,
	 twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
					       HashValues,
					       ExpectedInsertionCount,
					       HashFunction1,
					       HashFunction2,
					       ExtensionFunction,
					       Block,
					       Allocator>& rhs)

    {
      lhs.swap(rhs);
    }

    template<class T, size_t BitsPerBin, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1, class HashFunction2,
	     class ExtensionFunction, typename Block,
	     class Allocator>
    bool
    operator==(const twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
							   HashValues,
							   ExpectedInsertionCount,
							   HashFunction1,
							   HashFunction2,
							   ExtensionFunction,
							   Block,
							   Allocator>& lhs,
	       const twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
							   HashValues,
							   ExpectedInsertionCount,
							   HashFunction1,
							   HashFunction2,
							   ExtensionFunction,
							   Block,
							   Allocator>& rhs)
    {
      if (lhs.bit_capacity() != rhs.bit_capacity())
	throw detail::incompatible_size_exception();

      return (lhs.bits == rhs.bits);
    }

    template<class T, size_t BitsPerBin, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1, class HashFunction2,
	     class ExtensionFunction, typename Block,
	     class Allocator>
    bool
    operator!=(const twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
							   HashValues,
							   ExpectedInsertionCount,
							   HashFunction1,
							   HashFunction2,
							   ExtensionFunction,
							   Block,
							   Allocator>& lhs,
	       const twohash_dynamic_counting_bloom_filter<T, BitsPerBin,
							   HashValues,
							   ExpectedInsertionCount,
							   HashFunction1,
							   HashFunction2,
							   ExtensionFunction,
							   Block,
							   Allocator>& rhs)
    {
      if (lhs.bit_capacity() != rhs.bit_capacity())
	throw detail::incompatible_size_exception();

      return !(lhs == rhs);
    }

  } // namespace bloom_filter
} // namespace boost
#endif


#include <Eigen/Core>
#include <set>
#include <map>
#include <queue>
#include <boost/foreach.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_types.h>
#include "cloudproc.hpp"
#include <iostream>

using namespace Eigen;
using namespace std;
using namespace pcl;

//#define DEBUG_PRINT(...) printf(__VA_ARGS__)
#define DEBUG_PRINT(...)

typedef vector<int> IntVec;
typedef vector<float> FloatVec;
namespace {

template <typename T>
struct Frontier {
  set<T> m_excluded;
  queue<T> m_frontier;
  T pop() {
    T out = m_frontier.front();
    m_frontier.pop();
    return out;
  }
  void push(const T& x) {
    if (m_excluded.find(x) == m_excluded.end()) m_frontier.push(x);
  }
  void push(const vector<T>& x) {
    BOOST_FOREACH(const T& t, x) {
      push(t);
      exclude(t);
    }
  }
  void exclude(const T& x) {
    m_excluded.insert(x);
  }
  bool empty() {
    return m_frontier.empty();
  }
};

const int UNLABELED = -1;
const int NO_NEXT_LABEL = -1;
int getNextUnlabeled(const vector<int>& labels, int start) {
  for (int i=start; i < labels.size(); ++i) {
    if (labels[i] == UNLABELED) {
      return i;
    }
  }
  return NO_NEXT_LABEL;
}

struct AlmostConvex {
  vector<PointNormal> m_pns;
  vector<int> m_inds;

  float deficit (const PointNormal& pn1, const PointNormal& pn2) {
    Vector3f p2minusp1 = pn2.getVector3fMap() - pn1.getVector3fMap();
    return fmaxf(p2minusp1.dot(pn2.getNormalVector3fMap()), -p2minusp1.dot(pn1.getNormalVector3fMap()))/p2minusp1.norm();
  }

  float calcMaxDeficit(const PointNormal& pn) {
    float out = 0;
    BOOST_FOREACH(const PointNormal& pn1, m_pns) {
      out = fmaxf(out, deficit(pn, pn1));
    }
    return out;
  }
  void add(const PointNormal& pn, int ind) {
    m_pns.push_back(pn);
    m_inds.push_back(ind);
  }

};

vector<int> getNeighbors(const pcl::KdTreeFLANN<PointXYZ>& tree, int i_pt, int k_neighbs, float maxdist) {
  k_neighbs += 1;
  IntVec neighb_inds(k_neighbs, -666);
  FloatVec sqdists(k_neighbs, -666);
//  int n_neighbs = tree.nearestKSearch(i_pt, k_neighbs, neighb_inds, sqdists);
  int n_neighbs = tree.radiusSearch(i_pt, maxdist, neighb_inds, sqdists, k_neighbs);
  return vector<int>(neighb_inds.begin()+1, neighb_inds.begin() + n_neighbs);
}

vector<int> getUnlabeled(const vector<int>& nbs, const vector<int>& labels) {
  vector<int> out;
  BOOST_FOREACH(const int& i, nbs) if (labels[i] == UNLABELED) out.push_back(i);
  return out;
}

}

namespace cloudproc {

void ConvexDecomp(pcl::PointCloud<pcl::PointNormal>::ConstPtr cloud, float thresh,
    /*optional outputs: */ std::vector<IntVec>* indices, std::vector< IntVec >* hull_indices) {

  float nb_rad = 2*thresh;
  int k_neighbs = 5;
  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>(true));
  tree->setEpsilon(0);
  tree->setInputCloud (toXYZ<PointNormal>(cloud));
  int n_pts = cloud->size();

  vector<int> labels(n_pts, UNLABELED);
  int label_cur = 0;

  int i_seed = 0;
  while ( (i_seed = getNextUnlabeled(labels, i_seed)) != NO_NEXT_LABEL) {

    DEBUG_PRINT("growing cluster from seed %i\n", i_seed);
    Frontier<int> frontier;
    AlmostConvex ac;

    ac.add(cloud->points[i_seed], i_seed);
    frontier.exclude(i_seed);
    labels[i_seed] = label_cur;

    vector<int> nbs = getNeighbors(*tree, i_seed, k_neighbs, nb_rad);
    vector<int> unlabeled_nbs = getUnlabeled(nbs, labels);
    frontier.push(unlabeled_nbs);

    while (!frontier.empty()) {
      int i_cur = frontier.pop();
      float deficit = ac.calcMaxDeficit(cloud->points[i_cur]);
      if (deficit < thresh) {
        ac.add(cloud->points[i_cur], i_cur);
        labels[i_cur] = label_cur;
        vector<int> nbs = getNeighbors(*tree, i_cur, k_neighbs, nb_rad);
        vector<int> unlabeled_nbs = getUnlabeled(nbs, labels);
        frontier.push(unlabeled_nbs);
//        BOOST_FOREACH(int i, unlabeled_nbs) assert( (cloud->points[i].getVector3fMap() - cloud->points[i_cur].getVector3fMap()).norm() < nb_rad );
        DEBUG_PRINT("added %i\n", i_cur);
        if (label_cur==263)cout << cloud->points[i_cur] << endl;
      }
      else {
        DEBUG_PRINT("rejected %i\n", i_cur);
      }
    }

    if (indices != NULL) indices->push_back(ac.m_inds);
    DEBUG_PRINT("finished cluster %i of size %i\n", label_cur, ac.m_inds.size());
    ++label_cur;

  }

}

}