/*!
  \file gpp_python_knowledge_gradient_mcmc.cpp
  \rst
  This file has the logic to invoke C++ functions pertaining to knowledge gradient from Python.
  The data flow follows the basic 4 step from gpp_python_common.hpp.

  .. NoteL: several internal functions of this source file are only called from ``Export*()`` functions,
  so their description, inputs, outputs, etc. comments have been moved. These comments exist in
  ``Export*()`` as Python docstrings, so we saw no need to repeat ourselves.
\endrst*/
// This include violates the Google Style Guide by placing an "other" system header ahead of C and C++ system headers.  However,
// it needs to be at the top, otherwise compilation fails on some systems with some versions of python: OS X, python 2.7.3.
// Putting this include first prevents pyport from doing something illegal in C++; reference: http://bugs.python.org/issue10910
#include "Python.h"  // NOLINT(build/include)

#include "gpp_python_knowledge_gradient_mcmc.hpp"

// NOLINT-ing the C, C++ header includes as well; otherwise cpplint gets confused
#include <string>  // NOLINT(build/include_order)
#include <vector>  // NOLINT(build/include_order)

#include <boost/python/bases.hpp>  // NOLINT(build/include_order)
#include <boost/python/class.hpp>  // NOLINT(build/include_order)
#include <boost/python/def.hpp>  // NOLINT(build/include_order)
#include <boost/python/dict.hpp>  // NOLINT(build/include_order)
#include <boost/python/extract.hpp>  // NOLINT(build/include_order)
#include <boost/python/list.hpp>  // NOLINT(build/include_order)
#include <boost/python/object.hpp>  // NOLINT(build/include_order)
#include <boost/python/make_constructor.hpp>  // NOLINT(build/include_order)

#include "gpp_common.hpp"
#include "gpp_covariance.hpp"
#include "gpp_domain.hpp"
#include "gpp_exception.hpp"
#include "gpp_knowledge_gradient_mcmc_optimization.hpp"
#include "gpp_geometry.hpp"
#include "gpp_math.hpp"
#include "gpp_optimization.hpp"
#include "gpp_optimizer_parameters.hpp"
#include "gpp_python_common.hpp"

namespace optimal_learning {

namespace {
/*!\rst
  Surrogate "constructor" for GaussianProcess intended only for use by boost::python.  This aliases the normal C++ constructor,
  replacing ``double const * restrict`` arguments with ``const boost::python::list&`` arguments.
\endrst*/
GaussianProcessMCMC * make_gaussian_process_mcmc(const boost::python::list& hyperparameters_list,
                                                 const boost::python::list& noise_variance_list,
                                                 const boost::python::list& points_sampled,
                                                 const boost::python::list& points_sampled_value,
                                                 const boost::python::list& derivatives,
                                                 int num_mcmc, int num_derivatives, int dim, int num_sampled) {
  std::vector<double> hyperparameters_list_vector(num_mcmc*(dim+1));
  CopyPylistToVector(hyperparameters_list, num_mcmc*(dim+1), hyperparameters_list_vector);

  std::vector<double> noise_variance_list_vector(num_mcmc*(1+num_derivatives));
  CopyPylistToVector(noise_variance_list, num_mcmc*(1+num_derivatives), noise_variance_list_vector);

  std::vector<double> points_sampled_vector(dim*num_sampled);
  CopyPylistToVector(points_sampled, dim*num_sampled, points_sampled_vector);

  std::vector<double> points_sampled_value_vector(num_sampled*(1+num_derivatives));
  CopyPylistToVector(points_sampled_value, num_sampled*(1+num_derivatives), points_sampled_value_vector);

  std::vector<int> derivatives_vector(num_derivatives);
  CopyPylistToIntVector(derivatives, num_derivatives, derivatives_vector);

  GaussianProcessMCMC * new_gp_mcmc = new GaussianProcessMCMC(hyperparameters_list_vector.data(), noise_variance_list_vector.data(),
                                                              num_mcmc, points_sampled_vector.data(), points_sampled_value_vector.data(),
                                                              derivatives_vector.data(), num_derivatives,
                                                              dim, num_sampled);
  for (int i=0;i<num_mcmc;i++){
      new_gp_mcmc->gaussian_process_lst[i].SetRandomizedSeed(0);
  }
  return new_gp_mcmc;
}

double ComputeKnowledgeGradientMCMCWrapper(GaussianProcessMCMC& gaussian_process_mcmc,
                                           const int num_fidelity,
                                           const boost::python::object& optimizer_parameters,
                                           const boost::python::list& domain_bounds,
                                           const boost::python::list& discrete_pts,
                                           const boost::python::list& points_to_sample,
                                           const boost::python::list& points_being_sampled,
                                           int num_pts, int num_to_sample, int num_being_sampled,
                                           int max_int_steps, const boost::python::list& best_so_far,
                                           RandomnessSourceContainer& randomness_source) {
  int num_derivatives_input = 0;
  const boost::python::list gradients;

  PythonInterfaceInputContainer input_container_discrete(discrete_pts, gradients, gaussian_process_mcmc.dim()-num_fidelity,
                                                         num_pts*gaussian_process_mcmc.num_mcmc(), num_derivatives_input);
  PythonInterfaceInputContainer input_container(points_to_sample, points_being_sampled, gradients, gaussian_process_mcmc.dim(),
                                                num_to_sample, num_being_sampled, num_derivatives_input);

  bool configure_for_gradients = false;

  std::vector<ClosedInterval> domain_bounds_C(input_container.dim-num_fidelity);
  CopyPylistToClosedIntervalVector(domain_bounds, input_container.dim-num_fidelity, domain_bounds_C);

  std::vector<double> best_so_far_list(gaussian_process_mcmc.num_mcmc());
  CopyPylistToVector(best_so_far, gaussian_process_mcmc.num_mcmc(), best_so_far_list);

  TensorProductDomain domain(domain_bounds_C.data(), input_container.dim-num_fidelity);
  const GradientDescentParameters& gradient_descent_parameters = boost::python::extract<GradientDescentParameters&>(optimizer_parameters.attr("optimizer_parameters"));

  std::vector<typename KnowledgeGradientState<TensorProductDomain>::EvaluatorType> evaluator_vector;
  KnowledgeGradientMCMCEvaluator<TensorProductDomain> kg_evaluator(gaussian_process_mcmc, num_fidelity, input_container_discrete.points_to_sample.data(),
                                                                   num_pts, max_int_steps, domain, gradient_descent_parameters,
                                                                   best_so_far_list.data(), &evaluator_vector);

  std::vector<typename KnowledgeGradientEvaluator<TensorProductDomain>::StateType> state_vector;
  KnowledgeGradientMCMCEvaluator<TensorProductDomain>::StateType kg_state(kg_evaluator, input_container.points_to_sample.data(),
                                                                          input_container.points_being_sampled.data(),
                                                                          input_container.num_to_sample,
                                                                          input_container.num_being_sampled,
                                                                          num_pts, gaussian_process_mcmc.derivatives().data(),
                                                                          gaussian_process_mcmc.num_derivatives(), configure_for_gradients,
                                                                          randomness_source.normal_rng_vec.data(), &state_vector);
  return kg_evaluator.ComputeKnowledgeGradient(&kg_state);
}

boost::python::list ComputeGradKnowledgeGradientMCMCWrapper(GaussianProcessMCMC& gaussian_process_mcmc,
                                                            const int num_fidelity,
                                                            const boost::python::object& optimizer_parameters,
                                                            const boost::python::list& domain_bounds,
                                                            const boost::python::list& discrete_pts,
                                                            const boost::python::list& points_to_sample,
                                                            const boost::python::list& points_being_sampled,
                                                            int num_pts, int num_to_sample, int num_being_sampled,
                                                            int max_int_steps, const boost::python::list& best_so_far,
                                                            RandomnessSourceContainer& randomness_source) {
  int num_derivatives_input = 0;
  const boost::python::list gradients;

  PythonInterfaceInputContainer input_container_discrete(discrete_pts, gradients, gaussian_process_mcmc.dim()-num_fidelity,
                                                         num_pts*gaussian_process_mcmc.num_mcmc(), num_derivatives_input);
  PythonInterfaceInputContainer input_container(points_to_sample, points_being_sampled, gradients, gaussian_process_mcmc.dim(),
                                                num_to_sample, num_being_sampled, num_derivatives_input);

  std::vector<double> grad_KG(num_to_sample*input_container.dim);
  bool configure_for_gradients = true;

  std::vector<ClosedInterval> domain_bounds_C(input_container.dim-num_fidelity);
  CopyPylistToClosedIntervalVector(domain_bounds, input_container.dim-num_fidelity, domain_bounds_C);

  std::vector<double> best_so_far_list(gaussian_process_mcmc.num_mcmc());
  CopyPylistToVector(best_so_far, gaussian_process_mcmc.num_mcmc(), best_so_far_list);

  TensorProductDomain domain(domain_bounds_C.data(), input_container.dim-num_fidelity);
  const GradientDescentParameters& gradient_descent_parameters = boost::python::extract<GradientDescentParameters&>(optimizer_parameters.attr("optimizer_parameters"));

  std::vector<typename KnowledgeGradientState<TensorProductDomain>::EvaluatorType> evaluator_vector;
  KnowledgeGradientMCMCEvaluator<TensorProductDomain> kg_evaluator(gaussian_process_mcmc, num_fidelity, input_container_discrete.points_to_sample.data(),
                                                                   num_pts, max_int_steps, domain, gradient_descent_parameters,
                                                                   best_so_far_list.data(), &evaluator_vector);

  std::vector<typename KnowledgeGradientEvaluator<TensorProductDomain>::StateType> state_vector;
  KnowledgeGradientMCMCEvaluator<TensorProductDomain>::StateType kg_state(kg_evaluator, input_container.points_to_sample.data(),
                                                                          input_container.points_being_sampled.data(),
                                                                          input_container.num_to_sample,
                                                                          input_container.num_being_sampled,
                                                                          num_pts, gaussian_process_mcmc.derivatives().data(),
                                                                          gaussian_process_mcmc.num_derivatives(), configure_for_gradients,
                                                                          randomness_source.normal_rng_vec.data(), &state_vector);
  kg_evaluator.ComputeGradKnowledgeGradient(&kg_state, grad_KG.data());

  return VectorToPylist(grad_KG);
}

/*!\rst
  Utility that dispatches KG optimization based on optimizer type and num_to_sample.
  This is just used to reduce copy-pasted code.

  \param
    :optimizer_parameters: python/cpp_wrappers/optimization._CppOptimizerParameters
      Python object containing the DomainTypes domain_type and OptimizerTypes optimzer_type to use as well as
      appropriate parameter structs e.g., NewtonParameters for type kNewton).
      See comments on the python interface for multistart_expected_improvement_optimization_wrapper
    :gaussian_process: GaussianProcess object (holds points_sampled, values, noise_variance, derived quantities) that describes the
      underlying GP
    :input_container: PythonInterfaceInputContainer object containing data about points_being_sampled
    :domain: object specifying the domain to optimize over (see gpp_domain.hpp)
    :optimizer_type: type of optimization to use (e.g., null, gradient descent)
    :num_to_sample: how many simultaneous experiments you would like to run (i.e., the q in q,p-KG)
    :best_so_far: value of the best sample so far (must be min(points_sampled_value))
    :max_int_steps: maximum number of MC iterations
    :max_num_threads: maximum number of threads for use by OpenMP (generally should be <= # cores)
    :randomness_source: object containing randomness sources (sufficient for multithreading) used in KG computation
    :status: pydict object; cannot be None
  \output
    :randomness_source: PRNG internal states modified
    :status: modified on exit to describe whether convergence occurred
    :best_points_to_sample[num_to_sample][dim]: next set of points to evaluate
\endrst*/

template <typename DomainType>
void DispatchKnowledgeGradientMCMCOptimization(const boost::python::object& optimizer_parameters,
                                               const boost::python::object& optimizer_parameters_inner,
                                               GaussianProcessMCMC& gaussian_process_mcmc, const int num_fidelity,
                                               const PythonInterfaceInputContainer& input_container_discrete,
                                               const PythonInterfaceInputContainer& input_container,
                                               const DomainType& domain, const DomainType& inner_domain, OptimizerTypes optimizer_type,
                                               int num_pts, int num_to_sample, std::vector<double> best_so_far_list,
                                               int max_int_steps, int max_num_threads,
                                               RandomnessSourceContainer& randomness_source,
                                               boost::python::dict& status,
                                               double * restrict best_points_to_sample) {
  bool found_flag = false;

  switch (optimizer_type) {
    case OptimizerTypes::kNull: {
      ThreadSchedule thread_schedule(max_num_threads, omp_sched_static);
      // optimizer_parameters must contain an int num_random_samples field, extract it
      const GradientDescentParameters& gradient_descent_parameters_inner = boost::python::extract<GradientDescentParameters&>(optimizer_parameters_inner.attr("optimizer_parameters"));
      int num_random_samples = boost::python::extract<int>(optimizer_parameters.attr("num_random_samples"));

      ComputeKGMCMCOptimalPointsToSampleViaLatinHypercubeSearch(gaussian_process_mcmc, num_fidelity, gradient_descent_parameters_inner, domain, inner_domain, thread_schedule,
                                                                input_container.points_being_sampled.data(),
                                                                input_container_discrete.points_to_sample.data(),
                                                                num_random_samples, num_to_sample,
                                                                input_container.num_being_sampled,
                                                                num_pts, best_so_far_list.data(), max_int_steps,
                                                                &found_flag, &randomness_source.uniform_generator,
                                                                randomness_source.normal_rng_vec.data(),
                                                                best_points_to_sample);
      status[std::string("lhc_") + domain.kName + "_domain_found_update"] = found_flag;
      break;
    }  // end case kNull optimizer_type
    case OptimizerTypes::kGradientDescent: {
      // optimizer_parameters must contain a optimizer_parameters field
      // of type GradientDescentParameters. extract it
      const GradientDescentParameters& gradient_descent_parameters = boost::python::extract<GradientDescentParameters&>(optimizer_parameters.attr("optimizer_parameters"));
      const GradientDescentParameters& gradient_descent_parameters_inner = boost::python::extract<GradientDescentParameters&>(optimizer_parameters_inner.attr("optimizer_parameters"));
      ThreadSchedule thread_schedule(max_num_threads, omp_sched_dynamic);
      int num_random_samples = boost::python::extract<int>(optimizer_parameters.attr("num_random_samples"));

      bool random_search_only = false;
      ComputeKGMCMCOptimalPointsToSample(gaussian_process_mcmc, num_fidelity, gradient_descent_parameters, gradient_descent_parameters_inner, domain, inner_domain, thread_schedule,
                                         input_container.points_being_sampled.data(), input_container_discrete.points_to_sample.data(),
                                         num_to_sample, input_container.num_being_sampled, num_pts,
                                         best_so_far_list.data(), max_int_steps, random_search_only, num_random_samples, &found_flag,
                                         &randomness_source.uniform_generator, randomness_source.normal_rng_vec.data(), best_points_to_sample);

      status[std::string("gradient_descent_") + domain.kName + "_domain_found_update"] = found_flag;
      break;
    }  // end case kGradientDescent optimizer_type
    default: {
      std::fill(best_points_to_sample, best_points_to_sample + input_container.dim*num_to_sample, 0.0);
      OL_THROW_EXCEPTION(OptimalLearningException, "ERROR: invalid optimizer choice. Setting all coordinates to 0.0.");
      break;
    }
  }  // end switch over optimizer_type
}

boost::python::list MultistartKnowledgeGradientMCMCOptimizationWrapper(const boost::python::object& optimizer_parameters,
                                                                       const boost::python::object& optimizer_parameters_inner,
                                                                       GaussianProcessMCMC& gaussian_process_mcmc, const int num_fidelity,
                                                                       const boost::python::list& domain_bounds,
                                                                       const boost::python::list& discrete_pts,
                                                                       const boost::python::list& points_being_sampled,
                                                                       int num_pts, int num_to_sample, int num_being_sampled,
                                                                       const boost::python::list& best_so_far, int max_int_steps, int max_num_threads,
                                                                       RandomnessSourceContainer& randomness_source,
                                                                       boost::python::dict& status) {
  // TODO(GH-131): make domain objects constructible from python; and pass them in through
  // the optimizer_parameters python object

  // abort if we do not have enough sources of randomness to run with max_num_threads
  if (unlikely(max_num_threads > static_cast<int>(randomness_source.normal_rng_vec.size()))) {
    OL_THROW_EXCEPTION(LowerBoundException<int>, "Fewer randomness_sources than max_num_threads.", randomness_source.normal_rng_vec.size(), max_num_threads);
  }

  int num_to_sample_input = 0;  // No points to sample; we are generating these via KG optimization
  const boost::python::list points_to_sample_dummy;

  int num_derivatives_input = 0;
  const boost::python::list gradients;

  PythonInterfaceInputContainer input_container_discrete(discrete_pts, gradients, gaussian_process_mcmc.dim()-num_fidelity,
                                                         num_pts*gaussian_process_mcmc.num_mcmc(), num_derivatives_input);
  PythonInterfaceInputContainer input_container(points_to_sample_dummy, points_being_sampled, gradients, gaussian_process_mcmc.dim(),
                                                num_to_sample_input, num_being_sampled, num_derivatives_input);

  std::vector<ClosedInterval> domain_bounds_C(input_container.dim);
  CopyPylistToClosedIntervalVector(domain_bounds, input_container.dim, domain_bounds_C);

  std::vector<double> best_so_far_list(gaussian_process_mcmc.num_mcmc());
  CopyPylistToVector(best_so_far, gaussian_process_mcmc.num_mcmc(), best_so_far_list);

  std::vector<double> best_points_to_sample_C(input_container.dim*num_to_sample);

  DomainTypes domain_type = boost::python::extract<DomainTypes>(optimizer_parameters.attr("domain_type"));
  OptimizerTypes optimizer_type = boost::python::extract<OptimizerTypes>(optimizer_parameters.attr("optimizer_type"));
  switch (domain_type) {
    case DomainTypes::kTensorProduct: {
      TensorProductDomain domain(domain_bounds_C.data(), input_container.dim);
      TensorProductDomain inner_domain(domain_bounds_C.data(), input_container.dim-num_fidelity);

      DispatchKnowledgeGradientMCMCOptimization(optimizer_parameters, optimizer_parameters_inner, gaussian_process_mcmc, num_fidelity, input_container_discrete,
                                                input_container, domain, inner_domain, optimizer_type, num_pts, num_to_sample, best_so_far_list,
                                                max_int_steps, max_num_threads, randomness_source, status, best_points_to_sample_C.data());
      break;
    }  // end case OptimizerTypes::kTensorProduct
    case DomainTypes::kSimplex: {
      SimplexIntersectTensorProductDomain domain(domain_bounds_C.data(), input_container.dim);
      SimplexIntersectTensorProductDomain inner_domain(domain_bounds_C.data(), input_container.dim-num_fidelity);

      DispatchKnowledgeGradientMCMCOptimization(optimizer_parameters, optimizer_parameters_inner, gaussian_process_mcmc, num_fidelity, input_container_discrete,
                                                input_container, domain, inner_domain, optimizer_type, num_pts, num_to_sample, best_so_far_list,
                                                max_int_steps, max_num_threads, randomness_source, status, best_points_to_sample_C.data());
      break;
    }  // end case OptimizerTypes::kSimplex
    default: {
      std::fill(best_points_to_sample_C.begin(), best_points_to_sample_C.end(), 0.0);
      OL_THROW_EXCEPTION(OptimalLearningException, "ERROR: invalid domain choice. Setting all coordinates to 0.0.");
      break;
    }
  }  // end switch over domain_type

  return VectorToPylist(best_points_to_sample_C);
}

boost::python::list EvaluateKGMCMCAtPointListWrapper(GaussianProcessMCMC& gaussian_process_mcmc,
                                                     const int num_fidelity,
                                                     const boost::python::object& optimizer_parameters,
                                                     const boost::python::list& domain_bounds,
                                                     const boost::python::list& initial_guesses,
                                                     const boost::python::list& discrete_being_sampled,
                                                     int num_multistarts, int num_pts, int num_to_sample,
                                                     int num_being_sampled, const boost::python::list& best_so_far,
                                                     int max_int_steps, int max_num_threads,
                                                     RandomnessSourceContainer& randomness_source,
                                                     boost::python::dict& status) {
  // abort if we do not have enough sources of randomness to run with max_num_threads
  if (unlikely(max_num_threads > static_cast<int>(randomness_source.normal_rng_vec.size()))) {
    OL_THROW_EXCEPTION(LowerBoundException<int>, "Fewer randomness_sources than max_num_threads.", randomness_source.normal_rng_vec.size(), max_num_threads);
  }

  int num_to_sample_input = 0;  // No points to sample; we are generating these via KG optimization
  const boost::python::list points_to_sample_dummy;

  int num_derivatives_input = 0;
  const boost::python::list gradients;

  PythonInterfaceInputContainer input_container(discrete_being_sampled, gradients, gaussian_process_mcmc.dim(),
                                                num_pts*gaussian_process_mcmc.num_mcmc()+num_being_sampled, num_derivatives_input);
  int num_discrete_pts_and_pts_being_sampled = num_pts*gaussian_process_mcmc.num_mcmc()*(gaussian_process_mcmc.dim()-num_fidelity)
                                               + num_being_sampled*gaussian_process_mcmc.dim();
  std::vector<double> discrete_pts_and_pts_being_sampled(num_discrete_pts_and_pts_being_sampled);
  CopyPylistToVector(discrete_being_sampled, num_discrete_pts_and_pts_being_sampled, discrete_pts_and_pts_being_sampled);

  std::vector<double> result_point_C(input_container.dim);  // not used
  std::vector<double> result_function_values_C(num_multistarts);
  std::vector<double> initial_guesses_C(input_container.dim * num_multistarts);

  CopyPylistToVector(initial_guesses, input_container.dim * num_multistarts, initial_guesses_C);

  ThreadSchedule thread_schedule(max_num_threads, omp_sched_static);
  bool found_flag = false;

  std::vector<ClosedInterval> domain_bounds_C(input_container.dim);
  CopyPylistToClosedIntervalVector(domain_bounds, input_container.dim, domain_bounds_C);

  std::vector<double> best_so_far_list(gaussian_process_mcmc.num_mcmc());
  CopyPylistToVector(best_so_far, gaussian_process_mcmc.num_mcmc(), best_so_far_list);

  TensorProductDomain domain(domain_bounds_C.data(), input_container.dim);
  TensorProductDomain inner_domain(domain_bounds_C.data(), input_container.dim- num_fidelity);
  const GradientDescentParameters& gradient_descent_parameters = boost::python::extract<GradientDescentParameters&>(optimizer_parameters.attr("optimizer_parameters"));

  EvaluateKGMCMCAtPointList(gaussian_process_mcmc, num_fidelity, gradient_descent_parameters, domain, inner_domain, thread_schedule, initial_guesses_C.data(),
                            discrete_pts_and_pts_being_sampled.data() + num_pts*gaussian_process_mcmc.num_mcmc()*(gaussian_process_mcmc.dim()-num_fidelity),
                            discrete_pts_and_pts_being_sampled.data(), num_multistarts, num_to_sample, input_container.num_being_sampled,
                            num_pts, best_so_far_list.data(), max_int_steps, &found_flag, randomness_source.normal_rng_vec.data(),
                            result_function_values_C.data(), result_point_C.data());

  status["evaluate_KG_at_point_list"] = found_flag;

  return VectorToPylist(result_function_values_C);
}
}  // end unnamed namespace

void ExportKnowldegeGradientMCMCFunctions() {
  boost::python::class_<GaussianProcessMCMC, boost::noncopyable>("GaussianProcessMCMC", boost::python::no_init)
      .def("__init__", boost::python::make_constructor(&make_gaussian_process_mcmc), R"%%(
    Constructor for a ``GPP.GaussianProcess`` object.

    Seeds internal NormalRNG randomly.

    :param hyperparameters: covariance hyperparameters; see "Details on ..." section at the top of ``BOOST_PYTHON_MODULE``
    :type hyperparameters: list of len 2; index 0 is a float64 ``\alpha`` (signal variance) and index 1 is the length scales (list of floa64 of length ``dim``)
    :param points_sampled: points that have already been sampled
    :type points_sampled: list of float64 with shape (num_sampled, dim)
    :param points_sampled_value: values of the already-sampled points
    :type points_sampled_value: list of float64 with shape (num_sampled, )
    :param noise_variance: the ``\sigma_n^2`` (noise variance) associated w/observation, points_sampled_value
    :type noise_variance: list of float64 with shape (num_sampled, )
    :param dim: the spatial dimension of a point (i.e., number of independent params in experiment)
    :type param: int > 0
    :param num_sampled: number of already-sampled points
    :type num_sampled: int > 0
          )%%");

  boost::python::def("compute_knowledge_gradient_mcmc", ComputeKnowledgeGradientMCMCWrapper, R"%%(
    Compute knowledge gradient.
    If ``num_to_sample == 1`` and ``num_being_sampled == 0`` AND ``force_monte_carlo is false``, this will
    use (fast/accurate) analytic evaluation.
    Otherwise monte carlo-based KG computation is used.

    :param gaussian_process: GaussianProcess object (holds points_sampled, values, noise_variance, derived quantities)
    :type gaussian_process: GPP.GaussianProcess (boost::python ctor wrapper around optimal_learning::GaussianProcess)
    :param points_to_sample: initial points to load into state (must be a valid point for the problem);
      i.e., points at which to evaluate EI and/or its gradient
    :type points_to_sample: list of float64 with shape (num_to_sample, dim)
    :param points_being_sampled: points that are being sampled in concurrently experiments
    :type points_being_sampled: list of float64 with shape (num_being_sampled, dim)
    :param num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-EI)
    :type num_to_sample: int > 0
    :param num_being_sampled: number of points being sampled concurrently (i.e., the p in q,p-EI)
    :type num_being_sampled: int >= 0
    :param max_int_steps: number of MC integration points in EI
    :type max_int_steps: int >= 0
    :param best_so_far: best known value of objective so far
    :type best_so_far: float64
    :param force_monte_carlo: true to force monte carlo evaluation of EI
    :type force_monte_carlo: bool
    :param randomness_source: object containing randomness sources; only thread 0's source is used
    :type randomness_source: GPP.RandomnessSourceContainer
    :return: computed EI
    :rtype: float64 >= 0.0
    )%%");

  boost::python::def("compute_grad_knowledge_gradient_mcmc", ComputeGradKnowledgeGradientMCMCWrapper, R"%%(
    Compute the gradient of knowledge gradient evaluated at points_to_sample.
    If num_to_sample = 1 and num_being_sampled = 0 AND force_monte_carlo is false, this will
    use (fast/accurate) analytic evaluation.
    Otherwise monte carlo-based KG computation is used.

    :param gaussian_process: GaussianProcess object (holds points_sampled, values, noise_variance, derived quantities)
    :type gaussian_process: GPP.GaussianProcess (boost::python ctor wrapper around optimal_learning::GaussianProcess)
    :param points_to_sample: initial points to load into state (must be a valid point for the problem);
      i.e., points at which to evaluate EI and/or its gradient
    :type points_to_sample: list of float64 with shape (num_to_sample, dim)
    :param points_being_sampled: points that are being sampled in concurrently experiments
    :type points_being_sampled: list of float64 with shape (num_being_sampled, dim)
    :param num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-EI)
    :type num_to_sample: int > 0
    :param num_being_sampled: number of points being sampled concurrently (i.e., the p in q,p-EI)
    :type num_being_sampled: int >= 0
    :param max_int_steps: number of MC integration points in EI
    :type max_int_steps: int >= 0
    :param best_so_far: best known value of objective so far
    :type best_so_far: float64
    :param force_monte_carlo: true to force monte carlo evaluation of EI
    :type force_monte_carlo: bool
    :param randomness_source: object containing randomness sources; only thread 0's source is used
    :type randomness_source: GPP.RandomnessSourceContainer
    :return: gradient of EI (computed at points_to_sample + points_being_sampled, wrt points_to_sample)
    :rtype: list of float64 with shape (num_to_sample, dim)
    )%%");

  boost::python::def("multistart_knowledge_gradient_mcmc_optimization", MultistartKnowledgeGradientMCMCOptimizationWrapper, R"%%(
    Optimize expected improvement (i.e., solve q,p-EI) over the specified domain using the specified optimization method.
    Can optimize for num_to_sample new points to sample (i.e., aka "q", experiments to run) simultaneously.
    Allows the user to specify num_being_sampled (aka "p") ongoing/concurrent experiments.

    The _CppOptimizerParameters object is a python class defined in:
    python/cpp_wrappers/optimization._CppOptimizerParameters
    See that class definition for more details.

    This function expects it to have the fields:

    * domain_type (DomainTypes enum from this file)
    * optimizer_type (OptimizerTypes enum from this file)
    * num_random_samples (int, number of samples to 'dumb' search over, if 'dumb' search is being used.
      e.g., if optimizer = kNull or if to_sample > 1)
    * optimizer_parameters (*Parameters struct (gpp_optimizer_parameters.hpp) where * matches optimizer_type
      unused if optimizer_type == kNull)

    This function also has the option of using GPU to compute general q,p-EI via MC simulation. To enable it,
    make sure you have installed GPU components of MOE, otherwise, it will throw Runtime excpetion.

    .. WARNING:: this function FAILS and returns an EMPTY LIST if the number of random sources < max_num_threads

    :param optimizer_parameters: python object containing the DomainTypes domain_type and
      OptimizerTypes optimzer_type to use as well as
      appropriate parameter structs e.g., NewtonParameters for type kNewton)
    :type optimizer_parameters: _CppOptimizerParameters
    :param gaussian_process: GaussianProcess object (holds points_sampled, values, noise_variance, derived quantities)
    :type gaussian_process: GPP.GaussianProcess (boost::python ctor wrapper around optimal_learning::GaussianProcess)
    :param domain: [lower, upper] bound pairs for each dimension
    :type domain: list of float64 with shape (dim, 2)
    :param points_being_sampled: points that are being sampled in concurrently experiments
    :type points_being_sampled: list of float64 with shape (num_being_sampled, dim)
    :param num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-EI)
    :type num_to_sample: int > 0
    :param num_being_sampled: number of points being sampled concurrently (i.e., the p in q,p-EI)
    :type num_being_sampled: int >= 0
    :param best_so_far: best known value of objective so far
    :type best_so_far: float64
    :param max_int_steps: number of MC integration points in EI
    :type max_int_steps: int >= 0
    :param max_num_threads: max number of threads to use during EI optimization
    :type max_num_threads: int >= 1
    :param use_gpu: set to 1 if user wants to use GPU for MC computation
    :type use_gpu: bool
    :param which_gpu: GPU device ID
    :type which_gpu: int >= 0
    :param randomness_source: object containing randomness sources; only thread 0's source is used
    :type randomness_source: GPP.RandomnessSourceContainer
    :param status: pydict object (cannot be None!); modified on exit to describe whether convergence occurred
    :type status: dict
    :return: next set of points to eval
    :rtype: list of float64 with shape (num_to_sample, dim)
    )%%");

  boost::python::def("evaluate_KG_mcmc_at_point_list", EvaluateKGMCMCAtPointListWrapper, R"%%(
    Evaluates the expected improvement at each point in initial_guesses; can handle q,p-EI.
    Useful for plotting.

    Equivalent to::

      result = []
      for point in initial_guesses:
          result.append(compute_expected_improvement(point, ...))

    But this method is substantially faster (loop in C++ and multithreaded).

    .. WARNING:: this function FAILS and returns an EMPTY LIST if the number of random sources < max_num_threads

    :param num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-EI)
    :type num_to_sample: int > 0

    :param gaussian_process: GaussianProcess object (holds points_sampled, values, noise_variance, derived quantities)
    :type gaussian_process: GPP.GaussianProcess (boost::python ctor wrapper around optimal_learning::GaussianProcess)
    :param initial_guesses: points at which to evaluate EI
    :type initial_guesses: list of flaot64 with shape (num_multistarts, num_to_sample, dim)
    :param points_being_sampled: points that are being sampled in concurrently experiments
    :type points_being_sampled: list of float64 with shape (num_being_sampled, dim)
    :param num_multistarts: number of points at which to evaluate EI
    :type num_multistarts: int > 0
    :param num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-EI)
    :type num_to_sample: int > 0
    :param num_being_sampled: number of points being sampled concurrently (i.e., the p in q,p-EI)
    :type num_being_sampled: int >= 0
    :param best_so_far: best known value of objective so far
    :type best_so_far: float64
    :param max_int_steps: number of MC integration points in EI
    :type max_int_steps: int >= 0
    :param max_num_threads: max number of threads to use during EI optimization
    :type max_num_threads: int >= 1
    :param randomness_source: object containing randomness sources; only thread 0's source is used
    :type randomness_source: GPP.RandomnessSourceContainer
    :param status: pydict object (cannot be None!); modified on exit to describe whether convergence occurred
    :type status: dict
    :return: EI values at each point of the initial_guesses list, in the same order
    :rtype: list of float64 with shape (num_multistarts, )
    )%%");
}
}


/************************************************************************
MIT License

Copyright (c) 2021 Deqi Tang

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
************************************************************************/

#include <boost/program_options.hpp>
//#include <experimental/filesystem>
#include <boost/filesystem.hpp>
#include <iostream>
#include <string>
#include <algorithm>
#include <regex>
#include <cstdlib>
#include <cmath>
#include <armadillo>

#include "atomsciflow/parser/cif.h"
#include "atomsciflow/parser/xyz.h"
#include "atomsciflow/base/crystal.h"
#include "atomsciflow/utils.h"

#include "atomsciflow/cube_handle/cube_handle.h"

#include "atomsciflow/cmd/cmd_utils.h"

// needs: libboost-dev, libboost-program-options-dev

namespace po = boost::program_options;

//namespace filesys = std::experimental::filesystem;  // --std=c++17 -lstdc++fs
namespace filesys = boost::filesystem;     // --std=c++11 -lboost_filesystem -lboost_system

int main(int argc, char const* argv[]) {
    //

    po::options_description global("Global options");
    global.add_options()
        ("subcommand", po::value<std::string>(), "command to execute")
        ("subargs", po::value<std::vector<std::string> >(), "Arguments for command")
        ("help, h", "print out help information");

    po::positional_options_description pos;
    pos.add("subcommand", 1).add("subargs", -1);

    po::variables_map vm;

    po::parsed_options parsed = po::command_line_parser(argc, argv).
        options(global).
        positional(pos).
        allow_unregistered().
        run();

    po::store(parsed, vm);

    log_cmd_start("atomsciflow-cube.x");

    if (0 == vm.count("command")) { // or by vm.empty()
        std::cout << "You didn't specify any subcommand!\n";
        std::exit(1);
    }
    std::string subcommand = vm["subcommand"].as<std::string>();

    if ("along" == subcommand) {

        log_sub_cmd_start("along");//

        // along command has the following options:
        po::options_description opt_along("along options");
        opt_along.add_options()
            ("input, i", po::value<std::string>(), "input cube file")
            ("output, o", po::value<std::string>(), "output structure file")
            ("abscissa", po::value<std::vector<std::string> >()->multitoken(), "choose the direction to do the dimension reduction");
        //opts.add_options()
        //    ("input, i", po::value<std::string>(&input), "input structure file")
        //    ("output, o", po::value<std::string>(&output), "output structure file");
        // collect all the unrecognized options from the first pass. this will include the
        // (positional) command name so we need to erase that
        std::vector<std::string> opts = po::collect_unrecognized(parsed.options, po::include_positional);
        opts.erase(opts.begin());
        //parse again...
        po::store(po::command_line_parser(opts).options(opt_along).style(po::command_line_style::unix_style | po::command_line_style::allow_long_disguise).extra_style_parser(&allow_negative_numbers).run(), vm);
        std::cout << "parse arguments again" << "\n";

        if (vm.count("help")) {
            std::cout << opt_along << std::endl;
            std::exit(1);
        }
        po::notify(vm);

        std::cout << "Preparing to deal with input and output\n";

        if (vm.count("input") && vm.count("output")) {

            std::string input_file = vm["input"].as<std::string>();
            std::string output_file = vm["output"].as<std::string>();

            filesys::path in_path(input_file);
            filesys::path out_path(output_file);

            std::vector<std::string> abscissa = vm["abscissa"].as<std::vector<std::string>>();

            std::cout << "input: " << input_file << std::endl;
            std::cout << "output: " << output_file << std::endl;

            // read structure file
            std::cout << "in_path(extension)->" << in_path.extension().string() << std::endl;

            atomsciflow::CubeElectronDensity cube;
            cube.read_cube(input_file);

            double bohr_to_angstrom = 0.529177249;
            // data dimension reduction
            // the unit of value is actually not physical now!
            // cell_volume are in unit of Angstrom^3
            arma::mat latcell(3, 3);
            latcell.row(0) = arma::conv_to<arma::rowvec>::from(cube.crystal.cell[0]);
            latcell.row(1) = arma::conv_to<arma::rowvec>::from(cube.crystal.cell[1]);
            latcell.row(2) = arma::conv_to<arma::rowvec>::from(cube.crystal.cell[2]);

            std::cout << "latcell:\n";
            std::cout << latcell << std::endl;

            // std::cout << "test value:\n";
            // std::cout << arma::dot(arma::cross(latcell.row(0), latcell.row(1)), latcell.row(2)) << std::endl;

            double cell_volume = arma::dot(arma::cross(latcell.row(0), latcell.row(1)), latcell.row(2));

            double cell_volume_per_unit = cell_volume / (cube.ngridx * cube.ngridy * cube.ngridz);

            //std::cout << "cell volume per unit: " << cell_volume_per_unit << std::endl;

            int ngridx = cube.ngridx;
            int ngridy = cube.ngridy;
            int ngridz = cube.ngridz;
            // value in cube file are \rho(r)_of_electrons in unit of e/Bohr^3
            // namely number of electrons each Borh^3
            // so we have to convert it to e/Angstrom^3, through divide it by borh_to_angstrom**3

            double total_electrons = arma::accu(cube.data)  * cell_volume_per_unit / pow(bohr_to_angstrom, 3);

            std::cout << "======================================================\n";
            std::cout << "           Information collected\n";
            std::cout << "------------------------------------------------------\n";
            std::cout << "cell volume: " << cell_volume << " (A^3)\n";
            std::cout << "total electrons: " << total_electrons << "\n";

            // cube.data is in unit of e/Bohr^3
            // we will build data_red_? to be in unit of e/Anstrom, namely number of electrons per Angstrom
            // to do this we have to time the volume density with bohr_to_angstrom^-3
            std::vector<double> data_red_a;
            std::vector<double> data_red_b;
            std::vector<double> data_red_c;
            //arma::vec data_red_a;
            //arma::vec data_red_b;
            //arma::vec data_red_c;

            double a = arma::norm(latcell.row(0), 2);
            double b = arma::norm(latcell.row(1), 2);
            double c = arma::norm(latcell.row(2), 2);

            if (std::find(abscissa.begin(), abscissa.end(), "c") != abscissa.end()) {
                double len_ci = c / ngridz;
                auto size = arma::size(cube.data);
                double tmp;
                double rho_line;
                for (int ci = 0; ci < size[2]; ci++) {
                    tmp  = 0;
                    for (int bi = 0; bi < size[1]; bi++) {
                        for (int ai = 0; ai < size[0]; ai++) {
                            tmp += cube.data.at(ai, bi, ci);
                        }
                    }
                    rho_line = tmp * cell_volume_per_unit / pow(bohr_to_angstrom, 3) / len_ci;
                    data_red_c.push_back(rho_line);
                }
            }

            if (std::find(abscissa.begin(), abscissa.end(), "b") != abscissa.end())  {
                double len_bi = b / ngridy;
                auto size = arma::size(cube.data);
                double tmp;
                double rho_line;
                for (int bi  = 0; bi < size[1]; bi++) {
                    tmp = 0;
                    for (int ai = 0; ai < size[0]; ai++) {
                        for (int ci = 0; ci < size[2]; ci++) {
                            tmp += cube.data.at(ai, bi, ci);
                        }
                    }
                    rho_line = tmp * cell_volume_per_unit / pow(bohr_to_angstrom, 3) / len_bi;
                    data_red_b.push_back(rho_line);
                }
            }

            if (std::find(abscissa.begin(), abscissa.end(), "a") != abscissa.end())  {
                double len_ai = a / ngridx;
                auto size = arma::size(cube.data);
                double tmp;
                double rho_line;
                for (int ai = 0; ai < size[0]; ai++) {
                    tmp = 0;
                    for (int ci = 0; ci < size[2]; ci++) {
                        for (int bi = 0; bi < size[1]; bi++) {
                            tmp += cube.data.at(ai, bi, ci);
                        }
                    }
                    rho_line = tmp * cell_volume_per_unit / pow(bohr_to_angstrom, 3) / len_ai;
                    data_red_a.push_back(rho_line);
                }
            }

            std::cout << "Preparing to output the data\n";
            // output the data and make the plot
            if (std::find(abscissa.begin(), abscissa.end(), "c") != abscissa.end()) {
                std::ofstream fout;
                fout.open(output_file+".1d.c.data");
                fout << "#c(angstrom) rho(e) (number of electron per Angstrom)\n";
                arma::vec c_coord = arma::linspace(0, c, data_red_c.size());
                for (int i = 0; i < data_red_c.size(); i++) {
                    fout << c_coord.at(i) << " " << data_red_c.at(i) << "\n";
                }
                fout.close();
            }

            if (std::find(abscissa.begin(), abscissa.end(), "b") != abscissa.end()) {
                std::ofstream fout;
                fout.open(output_file+".1d.b.data");
                fout << "#b(angstrom) rho(e) (number of electron per Angstrom)\n";
                arma::vec b_coord = arma::linspace(0, b, data_red_b.size());
                for (int i = 0; i < data_red_b.size(); i++) {
                    fout << b_coord.at(i) << " " << data_red_b.at(i) << "\n";
                }
                fout.close();
            }

            if (std::find(abscissa.begin(), abscissa.end(), "a") != abscissa.end())  {
                std::ofstream fout;
                fout.open(output_file+".1d.a.data");
                fout << "#a(angstrom) rho(e) (number of electron per Angstrom)\n";
                arma::vec a_coord = arma::linspace(0, a, data_red_a.size());
                for (int i = 0; i < data_red_a.size(); i++) {
                    fout << a_coord.at(i) << " " << data_red_a.at(i) << "\n";
                }
                fout.close();
            }

            //std::ofstream fxx;
            //fxx.open("xxx.data");
            //fxx << cube.data << std::endl;
            //fxx.close();

        }

        log_sub_cmd_end("along");

    } else if ("diff" == subcommand) {
        //
        log_sub_cmd_start("diff");

        // along command has the following options:
        po::options_description opt_diff("diff options");
        opt_diff.add_options()
            ("input, i", po::value<std::vector<std::string> >()->multitoken(), "input three cube file: TOTAL PART1 PART2")
            ("output, o", po::value<std::string>(), "output structure file")
            ("abscissa", po::value<std::vector<std::string> >()->multitoken(), "choose the direction to do the dimension reduction");
        //opts.add_options()
        //    ("input, i", po::value<std::string>(&input), "input structure file")
        //    ("output, o", po::value<std::string>(&output), "output structure file");
        // collect all the unrecognized options from the first pass. this will include the
        // (positional) command name so we need to erase that
        std::vector<std::string> opts = po::collect_unrecognized(parsed.options, po::include_positional);
        opts.erase(opts.begin());
        //parse again...
        po::store(po::command_line_parser(opts).options(opt_diff).style(po::command_line_style::unix_style | po::command_line_style::allow_long_disguise).extra_style_parser(&allow_negative_numbers).run(), vm);
        std::cout << "parse arguments again" << "\n";

        if (vm.count("help")) {
            std::cout << opt_diff << std::endl;
            std::exit(1);
        }
        po::notify(vm);

        std::cout << "Preparing to deal with input and output\n";

        if (vm.count("input") && vm.count("output")) {

            // std::string input_file = vm["input"].as<std::string>();
            std::vector<std::string> input_files = vm["input"].as<std::vector<std::string>>();
            std::string output_file = vm["output"].as<std::string>();

            // filesys::path in_path(input_file);
            filesys::path out_path(output_file);

            std::vector<std::string> abscissa = vm["abscissa"].as<std::vector<std::string>>();

            std::cout << "input: \n"
                << input_files[0] << "\n"
                << input_files[1] << "\n"
                << input_files[2] << "\n" << std::endl;

            std::cout << "output: " << output_file << std::endl;

            atomsciflow::cube_diff_1d(input_files, output_file, abscissa);

        }

        log_sub_cmd_end("diff");

    } else {
        std::cout << "The specified subcommand is not defined!\n";
    }

    //
    return 0;
}


/****************************************************************************
**
** Copyright (C) 2017 TU Wien, ACIN, Vision 4 Robotics (V4R) group
** Contact: v4r.acin.tuwien.ac.at
**
** This file is part of V4R
**
** V4R is distributed under dual licenses - GPLv3 or closed source.
**
** GNU General Public License Usage
** V4R is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published
** by the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** V4R is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** Please review the following information to ensure the GNU General Public
** License requirements will be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
**
** Commercial License Usage
** If GPL is not suitable for your project, you must purchase a commercial
** license to use V4R. Licensees holding valid commercial V4R licenses may
** use this file in accordance with the commercial license agreement
** provided with the Software or, alternatively, in accordance with the
** terms contained in a written agreement between you and TU Wien, ACIN, V4R.
** For licensing terms and conditions please contact office<at>acin.tuwien.ac.at.
**
**
** The copyright holder additionally grants the author(s) of the file the right
** to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of their contributions without any restrictions.
**
****************************************************************************/

#include "v4r/attention_segmentation/algo.h"
#include "v4r/attention_segmentation/AttentionModuleErrors.h"

#include <Eigen/Dense>

namespace v4r {

void filterGaussian(cv::Mat &input, cv::Mat &output, cv::Mat &mask) {
  float kernel[5] = {1.0, 4.0, 6.0, 4.0, 1.0};

  cv::Mat temp = cv::Mat_<float>::zeros(input.rows, input.cols);

  for (int i = 0; i < input.rows; ++i) {
    for (int j = 0; j < input.cols; j = j + 2) {
      if (mask.at<float>(i, j) > 0) {
        float value = 0;
        float kernel_sum = 0;

        for (int k = 0; k < 5; ++k) {
          int jj = j + (k - 2);
          if ((jj >= 0) && (jj < input.cols)) {
            if (mask.at<float>(i, jj) > 0) {
              value += kernel[k] * input.at<float>(i, jj);
              kernel_sum += kernel[k];
            }
          }
        }

        if (kernel_sum > 0) {
          temp.at<float>(i, j) = value / kernel_sum;
        }
      }
    }
  }

  cv::Mat temp2 = cv::Mat_<float>::zeros(temp.rows, temp.cols);

  for (int i = 0; i < temp.rows; i = i + 2) {
    for (int j = 0; j < temp.cols; j = j + 2) {
      if (mask.at<float>(i, j) > 0) {
        float value = 0;
        float kernel_sum = 0;

        for (int k = 0; k < 5; ++k) {
          int ii = i + (k - 2);
          if ((ii >= 0) && (ii < temp.rows)) {
            if (mask.at<float>(ii, j) > 0) {
              value += kernel[k] * temp.at<float>(ii, j);
              kernel_sum += kernel[k];
            }
          }
        }

        if (kernel_sum > 0) {
          temp2.at<float>(i, j) = value / kernel_sum;
        }
      }
    }
  }

  int new_width = input.cols / 2;
  int new_height = input.rows / 2;

  cv::Mat mask_output = cv::Mat_<float>::zeros(new_height, new_width);
  output = cv::Mat_<float>::zeros(new_height, new_width);

  for (int i = 0; i < new_height; ++i) {
    for (int j = 0; j < new_width; ++j) {
      if (mask.at<float>(2 * i, 2 * j) > 0) {
        mask_output.at<float>(i, j) = 1.0;
        output.at<float>(i, j) = temp2.at<float>(2 * i, 2 * j);
      }
    }
  }

  mask_output.copyTo(mask);
}

void buildDepthPyramid(cv::Mat &image, std::vector<cv::Mat> &pyramid, cv::Mat &mask, unsigned int levelNumber) {
  pyramid.resize(levelNumber + 1);
  image.copyTo(pyramid.at(0));

  cv::Mat oldMask;
  mask.copyTo(oldMask);

  for (unsigned int i = 1; i <= levelNumber; ++i) {
    cv::Mat tempImage;

    filterGaussian(pyramid.at(i - 1), tempImage, oldMask);
    tempImage.copyTo(pyramid.at(i));
  }
}

int createPointCloudPyramid(std::vector<cv::Mat> &pyramidX, std::vector<cv::Mat> &pyramidY,
                            std::vector<cv::Mat> &pyramidZ, std::vector<cv::Mat> &pyramidIndices,
                            std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> &pyramidCloud) {
  assert(pyramidX.size() == pyramidY.size());
  assert(pyramidX.size() == pyramidZ.size());
  assert(pyramidX.size() == pyramidIndices.size());

  if (pyramidX.size() != pyramidY.size() && pyramidX.size() != pyramidZ.size() &&
      pyramidX.size() != pyramidIndices.size())
    return AM_DIFFERENTSIZES;

  unsigned int num_levels = pyramidX.size();

  pyramidCloud.resize(num_levels);

  for (unsigned int idx = 0; idx < num_levels; ++idx) {
    assert(pyramidX.at(idx).rows == pyramidY.at(idx).rows);
    assert(pyramidX.at(idx).cols == pyramidY.at(idx).cols);
    assert(pyramidX.at(idx).rows == pyramidZ.at(idx).rows);
    assert(pyramidX.at(idx).cols == pyramidZ.at(idx).cols);
    assert(pyramidX.at(idx).rows == pyramidIndices.at(idx).rows);
    assert(pyramidX.at(idx).cols == pyramidIndices.at(idx).cols);

    pyramidCloud.at(idx) = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>());

    unsigned int cur_height = pyramidX.at(idx).rows;
    unsigned int cur_width = pyramidX.at(idx).cols;

    pyramidCloud.at(idx)->points.resize(cur_height * cur_width);
    pyramidCloud.at(idx)->width = cur_width;
    pyramidCloud.at(idx)->height = cur_height;

    for (unsigned int i = 0; i < cur_height; ++i) {
      for (unsigned int j = 0; j < cur_width; ++j) {
        pcl::PointXYZRGB p_cur;
        p_cur.x = pyramidX.at(idx).at<float>(i, j);
        p_cur.y = pyramidY.at(idx).at<float>(i, j);
        p_cur.z = pyramidZ.at(idx).at<float>(i, j);

        int p_idx = i * cur_width + j;
        pyramidCloud.at(idx)->points.at(p_idx) = p_cur;
      }
    }
  }

  return AM_OK;
}

void createNormalPyramid(std::vector<cv::Mat> &pyramidNx, std::vector<cv::Mat> &pyramidNy,
                         std::vector<cv::Mat> &pyramidNz, std::vector<cv::Mat> &pyramidIndices,
                         std::vector<pcl::PointCloud<pcl::Normal>::Ptr> &pyramidNormal) {
  assert(pyramidNx.size() == pyramidNy.size());
  assert(pyramidNx.size() == pyramidNz.size());
  assert(pyramidIndices.size() == pyramidNz.size());

  unsigned int num_levels = pyramidNx.size();

  pyramidNormal.resize(num_levels);

  for (unsigned int idx = 0; idx < num_levels; ++idx) {
    assert(pyramidNx.at(idx).rows == pyramidNy.at(idx).rows);
    assert(pyramidNx.at(idx).cols == pyramidNy.at(idx).cols);
    assert(pyramidNx.at(idx).rows == pyramidNz.at(idx).rows);
    assert(pyramidNx.at(idx).cols == pyramidNz.at(idx).cols);
    assert(pyramidNx.at(idx).rows == pyramidIndices.at(idx).rows);
    assert(pyramidNx.at(idx).cols == pyramidIndices.at(idx).cols);

    pyramidNormal.at(idx) = pcl::PointCloud<pcl::Normal>::Ptr(new pcl::PointCloud<pcl::Normal>());

    unsigned int cur_height = pyramidNx.at(idx).rows;
    unsigned int cur_width = pyramidNx.at(idx).cols;

    pyramidNormal.at(idx)->points.clear();

    for (unsigned int i = 0; i < cur_height; ++i) {
      for (unsigned int j = 0; j < cur_width; ++j) {
        if (pyramidIndices.at(idx).at<float>(i, j) > 0) {
          pcl::Normal p_cur;
          p_cur.normal[0] = pyramidNx.at(idx).at<float>(i, j);
          p_cur.normal[1] = pyramidNy.at(idx).at<float>(i, j);
          p_cur.normal[2] = pyramidNz.at(idx).at<float>(i, j);

          pyramidNormal.at(idx)->points.push_back(p_cur);
        }
      }
    }

    pyramidNormal.at(idx)->width = pyramidNormal.at(idx)->points.size();
    pyramidNormal.at(idx)->height = 1;
  }
}

void createIndicesPyramid(std::vector<cv::Mat> &pyramidIndices,
                          std::vector<pcl::PointIndices::Ptr> &pyramidIndiceSets) {
  unsigned int num_levels = pyramidIndices.size();

  pyramidIndiceSets.resize(num_levels);

  for (unsigned int idx = 0; idx < num_levels; ++idx) {
    pyramidIndiceSets.at(idx) = pcl::PointIndices::Ptr(new pcl::PointIndices());

    unsigned int cur_height = pyramidIndices.at(idx).rows;
    unsigned int cur_width = pyramidIndices.at(idx).cols;

    pyramidIndiceSets.at(idx)->indices.clear();

    for (unsigned int i = 0; i < cur_height; ++i) {
      for (unsigned int j = 0; j < cur_width; ++j) {
        if (pyramidIndices.at(idx).at<float>(i, j) > 0) {
          int p_idx = i * cur_width + j;
          pyramidIndiceSets.at(idx)->indices.push_back(p_idx);
        }
      }
    }
  }
}

void upscaleImage(cv::Mat &input, cv::Mat &output, unsigned int width, unsigned int height) {
  assert(width >= (unsigned int)(2 * input.cols));
  assert(height >= (unsigned int)(2 * input.rows));

  output = cv::Mat_<float>::zeros(height, width);

  for (unsigned int i = 0; i < (unsigned int)input.rows; ++i) {
    for (unsigned int j = 0; j < (unsigned int)input.cols; ++j) {
      output.at<float>(2 * i, 2 * j) = input.at<float>(i, j);
    }
  }

  for (unsigned int i = 0; i < (unsigned int)output.rows; i = i + 2) {
    for (unsigned int j = 1; j < (unsigned int)output.cols - 1; j = j + 2) {
      output.at<float>(i, j) = (output.at<float>(i, j - 1) + output.at<float>(i, j + 1)) / 2.0;
    }
  }
  for (unsigned int i = 1; i < (unsigned int)output.rows - 1; i = i + 2) {
    for (unsigned int j = 0; j < (unsigned int)output.cols; j = j + 2) {
      output.at<float>(i, j) = (output.at<float>(i - 1, j) + output.at<float>(i + 1, j)) / 2.0;
    }
  }
  for (unsigned int i = 1; i < (unsigned int)output.rows - 1; i = i + 2) {
    for (unsigned int j = 1; j < (unsigned int)output.cols - 1; j = j + 2) {
      output.at<float>(i, j) = (output.at<float>(i - 1, j - 1) + output.at<float>(i - 1, j + 1) +
                                output.at<float>(i + 1, j - 1) + output.at<float>(i + 1, j + 1)) /
                               4.0;
    }
  }
}

void downscaleImage(cv::Mat &input, cv::Mat &output, unsigned int width, unsigned int height) {
  assert(2 * width <= (unsigned int)(input.cols));
  assert(2 * height <= (unsigned int)(input.rows));

  output = cv::Mat_<float>::zeros(height, width);

  for (unsigned int i = 0; i < (unsigned int)output.rows; ++i) {
    for (unsigned int j = 0; j < (unsigned int)output.cols; ++j) {
      output.at<float>(i, j) = input.at<float>(2 * i, 2 * j);
    }
  }
}

void scaleImage(std::vector<cv::Mat> &inputPyramid, cv::Mat &input, cv::Mat &output, int inLevel, int outLevel) {
  assert(input.cols == inputPyramid.at(inLevel).cols);
  assert(input.rows == inputPyramid.at(inLevel).rows);

  if (inLevel < outLevel) {
    input.copyTo(output);
    for (int l = inLevel + 1; l <= outLevel; ++l) {
      cv::Mat temp;
      downscaleImage(output, temp, inputPyramid.at(l).cols, inputPyramid.at(l).rows);
      temp.copyTo(output);
    }
  } else if (inLevel > outLevel) {
    input.copyTo(output);
    for (int l = inLevel - 1; l >= outLevel; --l) {
      cv::Mat temp;
      upscaleImage(output, temp, inputPyramid.at(l).cols, inputPyramid.at(l).rows);
      temp.copyTo(output);
    }
  } else {
    input.copyTo(output);
  }
}

bool inPoly(std::vector<cv::Point> &poly, cv::Point p) {
  cv::Point newPoint;
  cv::Point oldPoint;
  cv::Point p1, p2;

  bool inside = false;

  if (poly.size() < 3) {
    return (false);
  }

  oldPoint = poly.at(poly.size() - 1);
  for (unsigned int i = 0; i < poly.size(); i++) {
    newPoint = poly.at(i);
    if (newPoint.y > oldPoint.y) {
      p1 = oldPoint;
      p2 = newPoint;
    } else {
      p1 = newPoint;
      p2 = oldPoint;
    }

    if ((newPoint.y < p.y) == (p.y <= oldPoint.y) /* edge "open" at one end */
        && ((long)p.x - (long)p1.x) * (long)(p2.y - p1.y) < ((long)p2.x - (long)p1.x) * (long)(p.y - p1.y)) {
      inside = !inside;
    }
    oldPoint = newPoint;
  }
  return (inside);
}

void buildPolygonMap(cv::Mat &polygonMap, std::vector<std::vector<cv::Point>> &polygons) {
  for (int i = 0; i < polygonMap.rows; ++i) {
    for (int j = 0; j < polygonMap.cols; ++j) {
      unsigned int k = 0;
      while ((polygonMap.at<uchar>(i, j) == 0) && (k < polygons.size())) {
        if (inPoly(polygons.at(k), cv::Point(j, i)))
          polygonMap.at<uchar>(i, j) = k + 1;
        k += 1;
      }
    }
  }
}

void buildCountourMap(cv::Mat &polygonMap, std::vector<std::vector<cv::Point>> &polygons, cv::Scalar color) {
  for (unsigned int i = 0; i < polygons.size(); ++i) {
    for (unsigned int j = 0; j < polygons.at(i).size(); ++j) {
      cv::line(polygonMap, polygons.at(i).at(j), polygons.at(i).at((j + 1) % polygons.at(i).size()), color, 2);
    }
  }
}

float normPDF(float x, float mean, float stddev) {
  float val = exp(-(x - mean) * (x - mean) / (2 * stddev * stddev));
  val /= sqrt(2 * 3.14) * stddev;
  return val;
}

float normPDF(std::vector<float> x, std::vector<float> mean, cv::Mat stddev) {
  int dim = mean.size();

  EIGEN_ALIGN16 Eigen::MatrixXf _stddev = Eigen::MatrixXf::Zero(dim, dim);
  EIGEN_ALIGN16 Eigen::VectorXf _x = Eigen::VectorXf::Zero(dim);

  for (int i = 0; i < dim; ++i) {
    _x[i] = x.at(i) - mean.at(i);
    for (int j = 0; j < dim; ++j) {
      _stddev(i, j) = stddev.at<float>(i, j);
    }
  }

  float value = (_x.transpose()) * _stddev * _x;
  value /= -2;
  value = exp(value);

  float det = _stddev.determinant();

  value /= sqrt(pow(2 * 3.14, dim) * det);

  return (value);
}

void addNoise(cv::Mat &image, cv::Mat &nImage, cv::RNG &rng, float min, float max) {
  nImage = cv::Mat_<float>::zeros(image.rows, image.cols);
  for (int i = 0; i < image.rows; ++i) {
    for (int j = 0; j < image.cols; ++j) {
      float val = image.at<float>(i, j) + rng.uniform(min, max);
      nImage.at<float>(i, j) = (val < 0 ? 0 : (val > 1 ? 1 : val));
    }
  }
}

void normPDF(cv::Mat &mat, float mean, float stddev, cv::Mat &res) {
  res = cv::Mat_<float>::zeros(mat.rows, mat.cols);
  res = mat - mean;
  res = res.mul(res);
  float b = -1.0f / (2 * stddev * stddev);
  res = res * b;

  cv::exp(res, res);
  float a = 1.0f / (stddev * sqrt(2 * M_PI));
  res = res * a;
}

void normalizeDist(std::vector<float> &dist) {
  float total_sum = 0;
  for (unsigned int i = 0; i < dist.size(); ++i) {
    total_sum += dist.at(i);
  }

  for (unsigned int i = 0; i < dist.size(); ++i) {
    dist.at(i) /= total_sum;
  }
}

float getMean(std::vector<float> dist, float total_num) {
  float mean = 0;
  for (unsigned int i = 0; i < dist.size(); ++i) {
    mean += (dist.at(i) / total_num) * i;
  }
  return mean;
}

float getStd(std::vector<float> dist, float mean, float total_num) {
  float stdDev = 0;

  for (unsigned int i = 0; i < dist.size(); ++i) {
    stdDev += (i - mean) * (i - mean) * (dist.at(i) / total_num);
  }

  stdDev = sqrt(stdDev);

  return stdDev;
}

long commulativeFunctionArgValue(float x, std::vector<float> &A) {
  long min = 0;
  long max = A.size() - 1;
  while (max != min + 1) {
    long mid = (min + max) / 2;
    if (x <= A.at(mid)) {
      max = mid;
    } else {
      min = mid;
    }
  }

  if (A.at(min) >= x)
    return (min);
  else
    return (max);
}

void createContoursFromMasks(std::vector<cv::Mat> &masks, std::vector<std::vector<cv::Point>> &contours) {
  contours.clear();
  contours.resize(masks.size());

  for (unsigned int i = 0; i < masks.size(); ++i) {
    std::vector<std::vector<cv::Point>> temp;
    cv::Mat temp_image;
    masks.at(i).copyTo(temp_image);
    cv::findContours(temp_image, temp, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
    if (temp.size()) {
      long totalPointsNum = 0;
      for (unsigned int j = 0; j < temp.size(); ++j) {
        totalPointsNum += temp.at(j).size();
      }
      contours.at(i).resize(totalPointsNum);
      totalPointsNum = 0;
      for (unsigned int j = 0; j < temp.size(); ++j) {
        for (unsigned int k = 0; k < temp.at(j).size(); ++k) {
          contours.at(i).at(totalPointsNum) = temp.at(j).at(k);
          totalPointsNum += 1;
        }
      }
    }
  }
}

uchar Num01Transitions(cv::Mat s, int j, int i) {
  uchar p2 = s.at<uchar>(j - 1, i);
  uchar p3 = s.at<uchar>(j - 1, i + 1);
  uchar p4 = s.at<uchar>(j, i + 1);
  uchar p5 = s.at<uchar>(j + 1, i + 1);
  uchar p6 = s.at<uchar>(j + 1, i);
  uchar p7 = s.at<uchar>(j + 1, i - 1);
  uchar p8 = s.at<uchar>(j, i - 1);
  uchar p9 = s.at<uchar>(j - 1, i - 1);

  uchar Nt = 0;

  if ((p3 - p2) == 1)
    Nt++;
  if ((p4 - p3) == 1)
    Nt++;
  if ((p5 - p4) == 1)
    Nt++;
  if ((p6 - p5) == 1)
    Nt++;
  if ((p7 - p6) == 1)
    Nt++;
  if ((p8 - p7) == 1)
    Nt++;
  if ((p9 - p8) == 1)
    Nt++;
  if ((p2 - p9) == 1)
    Nt++;

  return Nt;
}

void MConnectivity(cv::Mat &s, uchar *element) {
  for (int i = 1; i < s.rows - 1; ++i) {
    for (int j = 1; j < s.cols - 1; ++j) {
      if (s.at<uchar>(i, j) > 0) {
        bool remove = true;
        for (int p = 0; p < 8; ++p) {
          int new_x = j + dx8[p];
          int new_y = i + dy8[p];

          uchar value = s.at<uchar>(new_y, new_x);

          if (element[p] < 2) {
            if (value != element[p]) {
              remove = false;
            }
          }
        }

        if (remove) {
          s.at<uchar>(i, j) = 0;
        }
      }
    }
  }
}

V4R_EXPORTS void Skeleton(cv::Mat a, cv::Mat &s) {
  int width = a.cols;
  int height = a.rows;

  a.copyTo(s);

  cv::Scalar prevsum(0);

  while (true) {
    cv::Mat m = cv::Mat_<uchar>::ones(height, width);

    for (int j = 1; j < height - 1; ++j) {
      for (int i = 1; i < width - 1; ++i) {
        if (s.at<uchar>(j, i) == 1) {
          uchar p2 = s.at<uchar>(j - 1, i);
          uchar p3 = s.at<uchar>(j - 1, i + 1);
          uchar p4 = s.at<uchar>(j, i + 1);
          uchar p5 = s.at<uchar>(j + 1, i + 1);
          uchar p6 = s.at<uchar>(j + 1, i);
          uchar p7 = s.at<uchar>(j + 1, i - 1);
          uchar p8 = s.at<uchar>(j, i - 1);
          uchar p9 = s.at<uchar>(j - 1, i - 1);

          uchar condA = p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9;
          uchar condB = Num01Transitions(s, j, i);
          uchar condC = p2 * p4 * p6;
          uchar condD = p4 * p6 * p8;

          if ((condA >= 2) && (condA <= 6) && (condB == 1) && (condC == 0) && (condD == 0))
            m.at<uchar>(j, i) = 0;
        }
      }
    }

    for (int j = 1; j < height - 1; ++j) {
      for (int i = 1; i < width - 1; ++i) {
        s.at<uchar>(j, i) = s.at<uchar>(j, i) * m.at<uchar>(j, i);
      }
    }

    m = cv::Mat_<uchar>::ones(height, width);
    for (int j = 1; j < height - 1; ++j) {
      for (int i = 1; i < width - 1; ++i) {
        if (s.at<uchar>(j, i) == 1) {
          uchar p2 = s.at<uchar>(j - 1, i);
          uchar p3 = s.at<uchar>(j - 1, i + 1);
          uchar p4 = s.at<uchar>(j, i + 1);
          uchar p5 = s.at<uchar>(j + 1, i + 1);
          uchar p6 = s.at<uchar>(j + 1, i);
          uchar p7 = s.at<uchar>(j + 1, i - 1);
          uchar p8 = s.at<uchar>(j, i - 1);
          uchar p9 = s.at<uchar>(j - 1, i - 1);

          uchar condA = p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9;
          uchar condB = Num01Transitions(s, j, i);
          uchar condC = p2 * p4 * p8;
          uchar condD = p2 * p6 * p8;

          if ((condA >= 2) && (condA <= 6) && (condB == 1) && (condC == 0) && (condD == 0))
            m.at<uchar>(j, i) = 0;
        }
      }
    }

    for (int j = 1; j < height - 1; ++j) {
      for (int i = 1; i < width - 1; ++i) {
        s.at<uchar>(j, i) = s.at<uchar>(j, i) * m.at<uchar>(j, i);
      }
    }

    cv::Scalar newsum = cv::sum(s);

    if (newsum(0) == prevsum(0))
      break;

    prevsum = newsum;
  }

  prevsum = cv::sum(s);

  uchar e1[8] = {2, 1, 2, 1, 2, 0, 0, 0};
  uchar e2[8] = {2, 1, 2, 0, 0, 0, 2, 1};
  uchar e3[8] = {0, 0, 2, 1, 2, 1, 2, 0};
  uchar e4[8] = {2, 0, 0, 0, 2, 1, 2, 1};

  while (true) {
    MConnectivity(s, e1);
    MConnectivity(s, e2);
    MConnectivity(s, e3);
    MConnectivity(s, e4);

    cv::Scalar newsum = cv::sum(s);

    if (newsum(0) == prevsum(0))
      break;

    prevsum = newsum;
  }
}

float calculateDistance(cv::Point center, cv::Point point) {
  float distance = sqrt((center.x - point.x) * (center.x - point.x) + (center.y - point.y) * (center.y - point.y));
  return (distance);
}

float calculateDistance(cv::Point center, cv::Point point, float sigma) {
  float distance = (center.x - point.x) * (center.x - point.x) + (center.y - point.y) * (center.y - point.y);
  distance = exp(-distance / (2 * sigma * sigma));
  return (distance);
}

void calculateObjectCenter(cv::Mat mask, cv::Point &center) {
  assert(mask.type() == CV_8UC1);

  float x_cen = 0;
  float y_cen = 0;
  cv::Scalar area = cv::sum(mask);

  for (int i = 0; i < mask.rows; ++i) {
    for (int j = 0; j < mask.cols; ++j) {
      uchar value = mask.at<uchar>(i, j);
      if (value > 0) {
        x_cen = x_cen + ((float)j) / area(0);
        y_cen = y_cen + ((float)i) / area(0);
      }
    }
  }

  if (mask.at<uchar>(y_cen, x_cen) == 0) {
    // search for closes 4-neighbour point
    std::list<cv::Point> points;
    points.push_back(cv::Point(x_cen, y_cen));
    cv::Mat used = cv::Mat_<uchar>::zeros(mask.rows, mask.cols);
    used.at<uchar>(y_cen, x_cen) = 1;
    while (points.size()) {
      cv::Point p = points.front();
      points.pop_front();
      if (mask.at<uchar>(p.y, p.x) > 0) {
        x_cen = p.x;
        y_cen = p.y;
        break;
      }

      for (int i = 0; i < 8; ++i) {
        int new_x = p.x + dx8[i];
        int new_y = p.y + dy8[i];

        if ((new_x < 0) || (new_y < 0) || (new_x >= used.cols) || (new_y >= used.rows))
          continue;

        if (used.at<uchar>(new_y, new_x) <= 0) {
          points.push_back(cv::Point(new_x, new_y));
          used.at<uchar>(new_y, new_x) = 1;
        }
      }
    }
  }

  center.x = x_cen;
  center.y = y_cen;
}

void calculateObjectCenter(std::vector<cv::Point> contour, cv::Mat mask, cv::Point &center) {
  float x_cen = 0;
  float y_cen = 0;

  for (unsigned int i = 0; i < contour.size(); ++i) {
    x_cen = x_cen + ((float)contour.at(i).x) / contour.size();
    y_cen = y_cen + ((float)contour.at(i).y) / contour.size();
  }

  if (mask.at<uchar>(y_cen, x_cen) == 0) {
    // search for closes 4-neighbour point
    std::list<cv::Point> points;
    points.push_back(cv::Point(x_cen, y_cen));
    cv::Mat used = cv::Mat_<uchar>::zeros(mask.rows, mask.cols);
    used.at<uchar>(y_cen, x_cen) = 1;
    while (points.size()) {
      cv::Point p = points.front();
      points.pop_front();
      if (mask.at<uchar>(p.y, p.x) > 0) {
        x_cen = p.x;
        y_cen = p.y;
        break;
      }

      for (int i = 0; i < 8; ++i) {
        int new_x = p.x + dx8[i];
        int new_y = p.y + dy8[i];

        if ((new_x < 0) || (new_y < 0) || (new_x >= used.cols) || (new_y >= used.rows))
          continue;

        if (used.at<uchar>(new_y, new_x) <= 0) {
          points.push_back(cv::Point(new_x, new_y));
          used.at<uchar>(new_y, new_x) = 1;
        }
      }
    }
  }

  center.x = x_cen;
  center.y = y_cen;
}

void get2DNeighbors(const cv::Mat &patches, cv::Mat &neighbors, int patchesNumber) {
  neighbors = cv::Mat_<bool>(patchesNumber, patchesNumber);
  neighbors.setTo(false);

  int dr[4] = {-1, 0, -1};
  int dc[4] = {0, -1, -1};

  for (int r = 1; r < patches.rows - 1; r++) {
    for (int c = 1; c < patches.cols - 1; c++) {
      // if the patch exist
      if (patches.at<int>(r, c) != -1) {
        int patchIdx = patches.at<int>(r, c);

        //@ep: why we did not use 1,-1 shift???
        for (int i = 0; i < 3; ++i)  //@ep: TODO 3->4
        {
          int nr = r + dr[i];
          int nc = c + dc[i];

          int currentPatchIdx = patches.at<int>(nr, nc);
          if (currentPatchIdx == -1)
            continue;

          if (patchIdx != currentPatchIdx) {
            neighbors.at<bool>(currentPatchIdx, patchIdx) = true;
            neighbors.at<bool>(patchIdx, currentPatchIdx) = true;
          }
        }
      }
    }
  }
}

}  // namespace v4r


//          Copyright Gavin Band 2008 - 2012.
// 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)

#ifndef METRO_SHOTGUNSTOCHASTICSEARCH_HPP
#define METRO_SHOTGUNSTOCHASTICSEARCH_HPP

#include <vector>
#include <unordered_map>
#include <boost/function.hpp>
#include <boost/functional/hash.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/discrete_distribution.hpp>

namespace metro {
	/*
	* This class performs a FINEMAP-like 'shotgun stochastic search'.
	* Given an integer N, starting with the empty state, this class searches by generating
	* all possible changes to the current state by adding / changing / removing any of the
	* N possible indicators.  It then computes a likelihood using the user-supplied
	* likelihood function.  Finally, it updates the current state by moving to one of the new states
	* uniformly weighted by the likelihood.
	* See Benner et al, "FINEMAP: Efficient variable selection using summary data from genome-wide association studies",
	* Bioinformatics (2016) for algorithm details.
	* This class merely implements the 'search' aspect of this algorithm.  To make efficient use of it,
	* the user must use a likelihood function that e.g. prohibits too-large numbers of predictor variables.
	*/
	struct ShotgunStochasticSearch {
		typedef std::vector< std::size_t > SelectedStates ;
		typedef boost::function< double ( SelectedStates const& ) > ComputeLL ;
		struct StateHash {
		public:
			size_t operator()(const SelectedStates &p) const {
				return boost::hash_value(p);
			}
		} ;

		typedef std::unordered_map< SelectedStates, double, StateHash > Store ;

		ShotgunStochasticSearch(
			std::size_t n,
			ComputeLL compute_ll,
			std::uint32_t rng_seed
		) ;

		/* Compute the next search update with the given likelihood function */
		SelectedStates const& update() ;

		/* Return our store of states visited so far */
		Store const& visited_states() const ;
		void visited_states( boost::function< void( SelectedStates const&, double ) > callback ) const ;

	private:
		std::size_t const m_N ;
		ShotgunStochasticSearch::ComputeLL m_compute_ll ;
		SelectedStates m_current_state ;
		Store m_lls ;
		boost::random::mt19937 m_rng;

	private:
		/* Generate all states obtained by deleting a member
		of the current state, and append to the given vector */
		void generate_deletions( std::vector< SelectedStates >* result ) ;

		/* Generate all states obtained by adding a member or changing a member
		of the current state, and append to the given vector */
		void generate_additions_and_changes( std::vector< SelectedStates >* result ) ;
	} ;
}

#endif


#ifndef N_BODY_PHYSICAL_HPP
#define N_BODY_PHYSICAL_HPP

#include "communication.hpp"
#include "config.hpp"
#include "data.hpp"
#include "logging.hpp"
#include "space.hpp"
#include "tree.hpp"
#include <boost/archive/xml_oarchive.hpp>
#include <boost/mpi.hpp>
#include <cmath>
#include <cstddef>

namespace n_body::physical {

using namespace n_body::data;

template <typename T, std::size_t Dimension>
void step(const config::Configuration<T> &config,
          const boost::mpi::communicator &comm, int root,
          data::Bodies<T, Dimension> &bodies,
          const data::tree::BodyTree<T, Dimension> &tree);

template <typename T, std::size_t Dimension, typename Iter>
void step(const config::Configuration<T> &config, Iter first, Iter last,
          const data::tree::BodyTree<T, Dimension> &tree);

template <typename T, std::size_t Dimension, typename Iter>
void step(const config::Configuration<T> &config, Iter first, Iter last,
          const data::Bodies<T, Dimension> &bodies);

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_tree_to_position(
    const config::Configuration<T> &config,
    const data::tree::BodyTree<T, Dimension> &tree,
    const data::Vector<T, Dimension> &position);

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_subtree_to_position(
    const config::Configuration<T> &config,
    const data::tree::BodyTree<T, Dimension> &tree, std::size_t root,
    const data::Vector<T, Dimension> &position);

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_position_to_position(
    const config::Configuration<T> &config,
    const data::Vector<T, Dimension> &other_position,
    data::Scalar<T> other_mass, const data::Vector<T, Dimension> &position);

// update bodies one step
template <typename T, std::size_t Dimension>
void step(const config::Configuration<T> &config,
          const boost::mpi::communicator &comm,
          data::Bodies<T, Dimension> &bodies,
          const data::tree::BodyTree<T, Dimension> &tree) {
  communication::Division division(comm, bodies.size());
  data::Bodies<T, Dimension> local_bodies(&bodies[division.begin],
                                          &bodies[division.end]);
  // step(config, local_bodies.begin(), local_bodies.end(), tree);
  step(config, local_bodies.begin(), local_bodies.end(), bodies);
  boost::mpi::all_gather(comm, local_bodies.data(), division.count, bodies);
}

// update bodies one step by iterator
template <typename T, std::size_t Dimension, typename Iter>
void step(const config::Configuration<T> &config, Iter first, Iter last,
          const data::Bodies<T, Dimension> &bodies) {
  for (; first != last; ++first) {
    data::Vector<T, Dimension> acceleration{
        0,
        0,
        0,
    };
    for (const auto &body : bodies) {
      acceleration += gravity_per_unit_mass_position_to_position(
          config, body.position, body.mass, first->position);
    }
    first->velocity += config.time * acceleration;
    first->position += config.time * first->velocity;
  }
}

// update bodies one step by iterator
template <typename T, std::size_t Dimension, typename Iter>
void step(const config::Configuration<T> &config, Iter first, Iter last,
          const data::tree::BodyTree<T, Dimension> &tree) {
  for (; first != last; ++first) {
    auto acceleration =
        gravity_per_unit_mass_tree_to_position(config, tree, first->position);
    first->velocity += config.time * acceleration;
    first->position += config.time * first->velocity;
  }
}

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_tree_to_position(
    const config::Configuration<T> &config,
    const data::tree::BodyTree<T, Dimension> &tree,
    const data::Vector<T, Dimension> &position) {
  if (tree.tree.empty())
    return {0, 0, 0};
  return gravity_per_unit_mass_subtree_to_position(config, tree, 0, position);
}

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_subtree_to_position(
    const config::Configuration<T> &config,
    const data::tree::BodyTree<T, Dimension> &tree, std::size_t root,
    const data::Vector<T, Dimension> &position) {

  if (tree.node(root).node_type() == data::tree::NodeType::Inner) {
    auto position_in_space = space::contains(tree.node(root).space, position);

    auto space_size = space::size_of_space(tree.node(root).space);
    auto distance = data::module_of(position - tree.node(root).center_of_mass);
    if (!position_in_space && (space_size / distance) < config.theta) {
      return gravity_per_unit_mass_position_to_position(
          config, tree.node(root).center_of_mass, tree.node(root).mass,
          position);
    } else {
      data::Vector<T, Dimension> sum{
          0,
          0,
          0,
      };
      for (std::size_t i = 0;
           i < data::tree::BodyTreeInnerNode<T, Dimension>::CHILDREN_NUMBER;
           ++i) {
        if (auto subtree = tree.child_of_node(root, i)) {
          sum += gravity_per_unit_mass_subtree_to_position(config, tree,
                                                           *subtree, position);
        }
      }
      return sum;
    }
  } else {
    // node is a leaf
    return gravity_per_unit_mass_position_to_position(
        config, tree.node(root).center_of_mass, tree.node(root).mass, position);
  }
}

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_position_to_position(
    const config::Configuration<T> &config,
    const data::Vector<T, Dimension> &other_position,
    data::Scalar<T> other_mass, const data::Vector<T, Dimension> &position) {
  auto dp = other_position - position;
  auto distance = data::module_of(dp);
  if (distance == 0) {
    return {
        0,
        0,
        0,
    };
  } // singularity
  auto G = config.G;
  auto soften = config.soften_length;
  auto result = G * other_mass /
                std::pow((soften_length * soften_length + distance * distance),
                         static_cast<T>(3) / static_cast<T>(2)) *
                dp;
  return result;
}

} // namespace n_body::physical

#endif