#ifndef WAVE_FEATURE_EXTRACTOR_HPP
#define WAVE_FEATURE_EXTRACTOR_HPP

#define EIGEN_USE_THREADS

#include <vector>
#include <array>
#include <algorithm>
#include <exception>

#include <omp.h>

#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>

#include "wave/utils/math.hpp"
#include "wave/utils/data.hpp"
#include "wave/odometry/odometry_types.hpp"

namespace wave {

using unlong = unsigned long;

template <int size>
using Earr = Eigen::array<long int, size>;
using ar1 = Earr<1>;
using ar2 = Earr<2>;
using ar3 = Earr<3>;

struct FeatureExtractorParams {
    // Feature extraction parameters
    unlong variance_window = 11;
    float variance_limit_rng = 1;
    float variance_limit_int = 1;

    unlong angular_bins = 12;
    float min_intensity = 10.0;
    float max_intensity = 50.0;
    float occlusion_tol = 0.1;   // Radians
    float occlusion_tol_2 = 1;   // m. Distance between points to initiate occlusion check
    int occlusion_filter_length = 10;
    float parallel_tol = 0.002;  // ditto
    float edge_tol = 0.1;       // Edge features must have score higher than this
    float flat_tol = 0.1;       // Plane features must have score lower than this
    float int_edge_tol = 2;     // Intensity edge features must have score greater than this
    float int_flat_tol = 0.1;   // Intensity edges must have range score lower than this
    int n_edge = 40;             // How many edge features to pick out per ring
    int n_flat = 100;            // How many plane features to pick out per ring
    int n_int_edge = 0;          // How many intensity edges to pick out per ring
    unlong knn = 5;              // 1/2 nearest neighbours for computing curvature
    unlong key_radius = 5;       // minimum number of points between keypoints on the same laser ring

    uint32_t N_SCORES = 5;
    uint32_t N_FEATURES = 5;

    /// Building these definitions is the responsibility of the application
    std::vector<FeatureDefinition> feature_definitions;
};

class FeatureExtractor {
 public:
    // Aliases are handy
    template<typename datatype>
    using Vec = std::vector<datatype>;

    template<typename datatype, typename alloctype>
    using VecA = std::vector<datatype, alloctype>;

    using Tensor2f = Eigen::Tensor<float, 2>;
    // type for incoming signals. Vector is ringsize long, each element is channels x points tensor
    using Tensorf = VecA<Tensor2f, Eigen::aligned_allocator<Tensor2f>>;

    using SKcombo = std::pair<Kernel, Signal>;

    // type for std::map of a 1 dimensional tensor
    using KernelMap = std::map<Kernel, Eigen::Tensor<float, 1>, std::less<Kernel>, Eigen::aligned_allocator<Eigen::Tensor<float, 1>>>;

    using SignalVec = Vec<std::map<Signal, Eigen::Tensor<float, 1>, std::less<Signal>, Eigen::aligned_allocator<Eigen::Tensor<float, 1>>>>;

    using ScoreMap = std::map<SKcombo, Eigen::Tensor<float, 1>, std::less<SKcombo>, Eigen::aligned_allocator<Eigen::Tensor<float, 1>>>;
    // type for outgoing keypoint indices
    // Each ring and feature type combination has a variable number of feature points, so the container
    // must support both. Indexed by feature id, then ring in that order.
    using TensorIdx = Vec<VecA<Eigen::Tensor<int, 1>, Eigen::aligned_allocator<Eigen::Tensor<int, 1>>>>;

    FeatureExtractor() = default;
    FeatureExtractor(FeatureExtractorParams params, unlong n_rings);

    void setParams(FeatureExtractorParams params, unlong n_rings);

    // Designed as function for outside world to call
    void getFeatures(const Tensorf &scan, const SignalVec &signals,
                     const std::vector<int> &range, TensorIdx &indices);

    FeatureExtractorParams param;

    bool ready = false;
    void setup();

    void computeScores(const SignalVec &signals, const Vec<int> &range);
    void preFilter(const Tensorf &scan, const SignalVec &signals, const Vec<int> &true_size);
    void buildFilteredScore(const Vec<int> &range);
    void sortAndBin(const Tensorf &scan, const SignalVec &signals, TensorIdx &feature_indices);

    void flagNearbyPoints(const uint32_t p_idx, const float pt_range, Eigen::Tensor<bool, 1> &valid);

    unlong n_ring; // This is separate from params because it depends on hardware

    // The resulting scores for each signal, grouped by kernel and ring
    // scores for each kernel-signal combo, indexed by ring id
    VecE<ScoreMap> scores;

    // Whether points are still considered candidates. Required to avoid picking neighbouring points
    VecA<Eigen::Tensor<bool, 1>, Eigen::aligned_allocator<Eigen::Tensor<bool, 1>>> valid_pts;

    // Scoring kernels, indexed along 1st dimension
    KernelMap kernels;

    // Container to sort scores with. Each is built depending on feature specification
    Vec<Vec<Vec<std::pair<unlong, float>>>> filtered_scores;

    std::vector<std::pair<Kernel, Signal>> proc_vec;
};
}

#endif  // WAVE_FEATURE_EXTRACTOR_HPP


//
// Created by philipp on 1/16/20.
//

#ifndef FUNNELS_CPP_EX1_FIXED_HH
#define FUNNELS_CPP_EX1_FIXED_HH

#include <Eigen/Core>
#include <cmath>
#include <sstream>
#include <algorithm>
#include <array>

#include <funnels/utils.hh>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795d
#endif

// Attention, in example 1, velocity coincides with the control
// signal
// Attention, assumes constant velocity/input

const double max_r_step = 0.5; // different funnel sizes in a family
const double min_r = 0.001; // Minimal radius
const double max_r = 0.5+5.*min_r; // Minimal radius
const double max_alpha_step = max_r_step*max_r_step; // Minimal step for alpha
const double min_alpha = min_r*min_r; // Minimal alpha for ellipsoids
const double max_alpha = max_r*max_r; // Minimal alpha for ellipsoids
const double max_vel = 1.; // Norm of velocity
const double min_vel_diff = 0.1; // Norm difference in velocity
const double min_ang_diff = 20.*M_PI/180.;
const double gamma_conv = 10.; // exponential convergence rate
const double max_traj_length = 3; // Length of the new segment
const double dt_step = 0.05; //time step between two verification points
const size_t n_max = 100;
const double max_time = 100.;
const double min_englobe_fac = 1.1;

const double plane_dist = 0.3;
const double inter_dist = 0.3;

Eigen::Matrix2d getR(double alpha);

template <class FUN_PTR_T>
std::pair<FUN_PTR_T, FUN_PTR_T> get_this_funnel(const FUN_PTR_T &src, double radius,
                          const Eigen::Vector4d &x0_O, const Eigen::Vector2d u){

  using fun_t = typename FUN_PTR_T::element_type;

  // Get maximal time before the maximal length is attained
  // or the playground is left
  Eigen::Vector4d x0 = x0_O;
  double n_u, t_max;
  if (u.norm() > 1e-3){
    n_u = u.norm()+1e-200;
    t_max = std::min(max_time, max_traj_length/n_u);
    // distance to x/y-border
    for (size_t i=0; i<2; i++){
      if(u(i)>=0.){
        t_max = std::min(t_max, (1.2-x0(i))/(u(i)+1e-200));
      }else{
        t_max = std::min(t_max, (-1.2-x0(i))/(u(i)+1e-200));
      }
    }
    if (t_max < 3.*dt_step){
      // trajectory is too short
      // initial is on the boundary and we are pointing outwards
      return std::pair(nullptr, nullptr);
    }
  }else{
    t_max = 3.*dt_step;
  }

  // Convert to number of points
  size_t this_n_verif = std::max((size_t)3, std::min((size_t) (t_max/dt_step), n_max));

  // todo check if this funnel already exists in the
  // funnel system
  // get the new name
  // Update the initial pos
  x0 = x0_O;

  std::stringstream sstream;
  sstream.setf(std::ios::fixed);
  sstream.precision(2);

  sstream << "fun_";
  for(size_t i=0; i<4; i++){
    sstream << (double) x0(i) << "_";
  }
  sstream << radius;
  std::string name = sstream.str();
  std::replace( name.begin(), name.end(), '.', 'p');
  std::replace( name.begin(), name.end(), '-', 'm');

  // If exactly this funnel exists, the name will exist
  if (utils_ext::loc_map.find(name)){
    return std::pair(nullptr, nullptr);
  }

  FUN_PTR_T new_fun1 = src->make_copy(name, 2.*this_n_verif);
  FUN_PTR_T new_fun2 = src->make_copy(name+"_twin", 2.*this_n_verif);

  // set the new radius
  new_fun1->set_P(src->get_P()*(src->get_alpha()/(radius*radius)));
  new_fun2->set_P(src->get_P()*(src->get_alpha()/(radius*radius)));
  // and convergence
  new_fun1->set_gamma(gamma_conv);
  new_fun2->set_gamma(gamma_conv);

  // Update pos and compute traj
  x0.block(2,0,2,1) -= -t_max*u;
  new_fun1->compute(x0, 0., 2.*t_max, u);
  // Mirror
  x0 = -x0_O;
  x0.block(2,0,2,1) -= -t_max*u;
  new_fun2->compute(x0, 0., 2.*t_max, u);

  // He starts a family
  new_fun1->start_family(new_fun1);
  new_fun2->start_family(new_fun2);

  return std::pair(new_fun1, new_fun2);
}

template <class FUN_PTR_T>
FUN_PTR_T get_this_funnel_1(const FUN_PTR_T &src, double radius,
    const Eigen::Vector4d &x0_O, const Eigen::Vector2d u){

  using fun_t = typename FUN_PTR_T::element_type;

  // Get maximal time before the maximal length is attained
  // or the playground is left
  Eigen::Vector4d x0 = x0_O;
  double n_u, t_max;
  if (u.norm() > 1e-3){
    n_u = u.norm()+1e-200;
    t_max = std::min(max_time, max_traj_length/n_u);
    // distance to x/y-border
    for (size_t i=0; i<2; i++){
      if(u(i)>=0.){
        t_max = std::min(t_max, (1.2-x0(i))/(u(i)+1e-200));
      }else{
        t_max = std::min(t_max, (-1.2-x0(i))/(u(i)+1e-200));
      }
    }
    if (t_max < 3.*dt_step){
      // trajectory is too short
      // initial is on the boundary and we are pointing outwards
      return nullptr;
    }
  }else{
    t_max = 3.*dt_step;
  }

  // Convert to number of points
  size_t this_n_verif = std::max((size_t)3, std::min((size_t) (t_max/dt_step), n_max));

  // todo check if this funnel already exists in the
  // funnel system
  // get the new name
  // Update the initial pos
  x0 = x0_O;

  std::stringstream sstream;
  sstream.setf(std::ios::fixed);
  sstream.precision(2);

  sstream << "fun_";
  for(size_t i=0; i<4; i++){
    sstream << (double) x0(i) << "_";
  }
  sstream << radius;
  std::string name = sstream.str();
  std::replace( name.begin(), name.end(), '.', 'p');
  std::replace( name.begin(), name.end(), '-', 'm');

  // If exactly this funnel exists, the name will exist
  if (utils_ext::loc_map.find(name)){
    return nullptr;
  }

  FUN_PTR_T new_fun = src->make_copy(name, this_n_verif);

  // set the new radius
  new_fun->set_P(src->get_P() * (src->get_alpha() / (radius * radius)));
  // and convergence
  new_fun->set_gamma(gamma_conv);

  // Update pos and compute traj
  x0.block(2,0,2,1) -= -t_max*u;
  new_fun->compute(x0, 0., 2. * t_max, u);
  // Mirror
  x0 = -x0_O;
  x0.block(2,0,2,1) -= -t_max*u;

  // He starts a family
  new_fun->start_family(new_fun);

  return new_fun;
}

template <class FUN_PTR_T>
std::vector<FUN_PTR_T> get_children(FUN_PTR_T &parent){

  using fun_t = typename FUN_PTR_T::element_type;
  using matrix_t = typename fun_t::matrix_t;

  FUN_PTR_T this_child_fun;
  FUN_PTR_T this_parent_fun = parent;

  std::stringstream sstream;
  std::string name;

  // Original unscaled matrix
  matrix_t P_p = parent->get_P()*parent->get_alpha();

  sstream.setf(std::ios::fixed);
  sstream.precision(2);

  std::vector<FUN_PTR_T> fun_vec_child;

  const double alpha_step = max_alpha_step;
  const double r_step = max_r_step;

  // Radius of parent
  double alpha = parent->get_alpha();

  if(std::sqrt(alpha) < min_r + r_step){
    return fun_vec_child;
  }

  alpha = std::max(min_alpha, (std::sqrt(alpha)-r_step)*(std::sqrt(alpha)-r_step));

  while(alpha>=min_alpha){
    // Get the name
    sstream.str(std::string());
    sstream << "fun_";
    auto x00 = parent->x0();
    for(size_t i=0; i<4; i++){
      sstream << (double) x00(i) << "_";
    }
    sstream << std::sqrt(alpha);
    name = sstream.str();
    std::replace( name.begin(), name.end(), '.', 'p');
    std::replace( name.begin(), name.end(), '-', 'm');
    if (utils_ext::loc_map.find(name)){
      // Already exists
      return fun_vec_child;
    }

    this_child_fun = this_parent_fun->make_copy(name, this_parent_fun->size());
    // set the new radius
    this_child_fun->set_P(P_p/alpha);
    this_child_fun->set_cyclic(this_parent_fun->is_cyclic());
    // And convergence
    this_child_fun->set_gamma(gamma_conv);

    fun_t::set_parent(this_parent_fun, this_child_fun);
    fun_vec_child.emplace_back(this_child_fun);
    this_parent_fun = this_child_fun;
    this_child_fun = nullptr;
    // Update alpha
    alpha = (std::sqrt(alpha)-r_step); // New alpha as r
    if (alpha <= min_r){
      break; // Necessary as negative radii become positive in next step
    }
    alpha *= alpha; // Convert to alpha
  }
  return fun_vec_child;
}

template<class FUN_PTR_T, class FUN_SYS>
void add_new_funnels(const FUN_PTR_T& src,
                     double t_src, FUN_SYS &fun_sys){

  // Src is dummy, only to keep the same signature

  using fun_t = typename FUN_PTR_T::element_type;

  // Return val
  FUN_PTR_T fun_parent1, fun_parent2;
  std::vector<FUN_PTR_T> fun_vec_child;

  Eigen::Vector2d pos_0;

  Eigen::Vector2d orth_p, vel_p, vel_1 = Eigen::Vector2d::Zero();
  vel_1(0) = 1.;

  Eigen::Vector4d x0;

  // The problem is purely symmetric

  // Grid of static funnels
  std::array<int, 2> signs = {-1,1};
  x0.setZero();
  vel_p.setZero();
  for (double x_s = plane_dist/2.; x_s <= 1.; x_s+=plane_dist){
    for (double y_s = plane_dist/2.; y_s <= 1.; y_s+=plane_dist) {
      for (auto s_x : signs){
        for (auto s_y : signs){
          x0(0) = s_x*x_s;
          x0(1) = s_y*y_s;
          fun_parent1 = get_this_funnel_1(src, max_r, x0, vel_p);
          if (fun_parent1 != nullptr) {
            // add
            fun_sys.add_funnel(fun_parent1, true);
            // all_children
            fun_vec_child = get_children(fun_parent1);
            for (auto a_f : fun_vec_child) {
              fun_sys.add_funnel(a_f);
            }
          }
        }
      }
    }
  }

  // "Dynamic" funnels
  // Loop over all velocities <-> planes
  for (double n_vel=inter_dist; n_vel<=1.0; n_vel+=inter_dist) {
      // Loop over all angles
    for (double d_alpha = 0.; d_alpha <= 2 * M_PI - d_alpha / 2;
         d_alpha += min_ang_diff) {
      // New velocity
      vel_p = n_vel * (getR(d_alpha) * vel_1);
      // New orthogonal
      orth_p = getR(M_PI / 2.) * getR(d_alpha) * vel_1;
      // Loop over all funnel origins
      pos_0 = orth_p * (plane_dist / 2.);
      while (true) {
        if ((pos_0.array().abs() >= 1.).any()) {
          // Out of the box
          break;
        }

        // Now we have a new parent funnel candidate
        x0.block(0, 0, 2, 1) = pos_0;
        // todo refactor norm vs normalized
        x0.block(2, 0, 2, 1) = vel_p;
        // Parent
        std::tie(fun_parent1, fun_parent2) = get_this_funnel(src, max_r, x0,
                                                             vel_p);

        if (fun_parent1 != nullptr) {
          // add
          fun_sys.add_funnel(fun_parent1, true);
          // all_children
          fun_vec_child = get_children(fun_parent1);
          for (auto a_f : fun_vec_child) {
            fun_sys.add_funnel(a_f);
          }
        }
        if (fun_parent2 != nullptr) {
          // add
          fun_sys.add_funnel(fun_parent2, true);
          // all_children
          fun_vec_child = get_children(fun_parent2);
          for (auto a_f : fun_vec_child) {
            fun_sys.add_funnel(a_f);
          }
        }
        // Update
        pos_0 += orth_p * plane_dist;
      } // origin pos
    } // angle
  } // plane
  return;
}

#endif //FUNNELS_CPP_EX1_FIXED_HH


// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_LOCALFUNCTIONAL_CODIM1_HH
#define DUNE_GDT_LOCALFUNCTIONAL_CODIM1_HH

#include <utility>
#include <vector>
#include <type_traits>
#include <limits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/densevector.hh>

#include <dune/geometry/quadraturerules.hh>

#include <dune/stuff/functions/interfaces.hh>

#include "../localevaluation/interface.hh"
#include "interface.hh"

namespace Dune {
namespace GDT {
namespace LocalFunctional {

// forward
template< class UnaryEvaluationImp >
class Codim1Integral;

namespace internal {

template< class UnaryEvaluationImp >
class Codim1IntegralTraits
{
  static_assert(std::is_base_of< LocalEvaluation::Codim1Interface< typename UnaryEvaluationImp::Traits, 1 >,
                                 UnaryEvaluationImp >::value,
                "UnaryEvaluationImp has to be derived from LocalEvaluation::Codim1Interface< ..., 1 >!");
public:
  typedef Codim1Integral< UnaryEvaluationImp > derived_type;
};

} // namespace internal

template< class UnaryEvaluationImp >
class Codim1Integral
    : public LocalFunctional::Codim1Interface< internal::Codim1IntegralTraits< UnaryEvaluationImp > >
{
public:
  typedef internal::Codim1IntegralTraits< UnaryEvaluationImp > Traits;
  typedef UnaryEvaluationImp                                   UnaryEvaluationType;

private:
  static const size_t numTmpObjectsRequired_ = 1;

public:
  template< class... Args >
  explicit Codim1Integral(Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(0)
  {}

  template< class... Args >
  explicit Codim1Integral(const int over_integrate, Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(boost::numeric_cast< size_t >(over_integrate))
  {}

  template< class... Args >
  explicit Codim1Integral(const size_t over_integrate, Args&& ...args)
    : evaluation_(std::forward< Args >(args)...)
    , over_integrate_(over_integrate)
  {}

  size_t numTmpObjectsRequired() const
  {
    return numTmpObjectsRequired_;
  }

  template< class E, class IntersectionType, class D, size_t d, class R, size_t r, size_t rC >
  void apply(const Stuff::LocalfunctionSetInterface< E, D, d, R, r, rC >& testBase,
             const IntersectionType& intersection,
             Dune::DynamicVector< R >& ret,
             std::vector< Dune::DynamicVector< R > >& tmpLocalVectors) const
  {
    // local inducing function
    const auto& entity = testBase.entity();
    const auto localFunctions = evaluation_.localFunctions(entity);
    // quadrature
    const auto integrand_order = evaluation_.order(localFunctions, testBase) + over_integrate_;
    const auto& faceQuadrature = QuadratureRules< D, d - 1 >::rule(intersection.type(),
                                                                   boost::numeric_cast< int >(integrand_order));
    // check vector and tmp storage
    ret *= 0.0;
    const size_t size = testBase.size();
    assert(ret.size() >= size);
    assert(tmpLocalVectors.size() >= numTmpObjectsRequired_);
    auto& localVector = tmpLocalVectors[0];
    // loop over all quadrature points
    for (auto quadPoint = faceQuadrature.begin(); quadPoint != faceQuadrature.end(); ++quadPoint) {
      const Dune::FieldVector< D, d - 1 > localPoint = quadPoint->position();
      const auto integrationFactor = intersection.geometry().integrationElement(localPoint);
      const auto quadratureWeight = quadPoint->weight();
      // evaluate local
      evaluation_.evaluate(localFunctions, testBase, intersection, localPoint, localVector);
      // compute integral
      assert(localVector.size() >= size);
      // loop over all test basis functions
      for (size_t ii = 0; ii < size; ++ii)
        ret[ii] += localVector[ii] * integrationFactor * quadratureWeight;
    } // loop over all quadrature points
  } // void apply(...) const

private:
  const UnaryEvaluationType evaluation_;
  const size_t over_integrate_;
}; // class Codim1Integral

} // namespace LocalFunctional
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_LOCALFUNCTIONAL_CODIM1_HH


//
// Copyright (c) Microsoft. All rights reserved.
// Licensed under the MIT license. See LICENSE.md file in the project root for full license information.
//
#include "stdafx.h"
#include <algorithm>
#include <array>
#include <random>
#include <numeric>
#include <boost/random/normal_distribution.hpp>
#include "../../../Source/Math/Matrix.h"
#include "../../../Source/Math/CPUMatrix.h"
#include "../../../Source/Math/GPUMatrix.h"
#include "../../../Source/Math/BatchNormalizationEngine.h"
#include "../../../Source/Math/CuDnnFactories.h"
#include "common.h"

namespace Microsoft { namespace MSR { namespace CNTK { namespace Test {

using vec = std::vector<float>;
using BNEng = BatchNormEngine<float>;

std::vector<std::tuple<TensorShape, size_t, bool, double, double>> GenerateBNTestConfigs()
{
    std::vector<std::tuple<TensorShape, size_t, bool, double, double>> res;
    // REVIEW alexeyk: how to test batches > 512? cuDNN does not support that so there is no baseline.
    double expAvgFactor = 1;
    for (auto blendFactor : {0.0, 1.0})
    {
        // Per activation (non-spatial)
        for (size_t n : {6, 13, 62, 512})
            for (size_t c : {1})
                for (size_t h : {1})
                    for (size_t w : {6, 17, 126, 2048})
                        res.push_back(std::make_tuple(TensorShape(w, h, c), n, false, expAvgFactor, blendFactor));

        // Spatial
        for (size_t n : {2, 11, 64})
            for (size_t c : {2, 13, 32})
                for (size_t h : {2, 11, 16})
                    for (size_t w : {2, 11, 16})
                        res.push_back(std::make_tuple(TensorShape(w, h, c), n, true, expAvgFactor, blendFactor));

        // For perf testing (similar to first layers of ResNet).
        res.push_back(std::make_tuple(TensorShape(56, 56, 64), 64, true, expAvgFactor, blendFactor));
        // Next test will fail in cuDNN due to bug we discovered (and reported to NVIDIA). Fixed in v4 prod.
        //res.push_back(std::make_tuple(TensorShape(2, 2, 2048), 2, true, expAvgFactor, blendFactor));
        res.push_back(std::make_tuple(TensorShape(2, 2, 2048), 64, true, expAvgFactor, blendFactor));
    }

    // Test running mean/variance.
    expAvgFactor = 0.1;
    res.push_back(std::make_tuple(TensorShape(2, 2, 2), 8, false, expAvgFactor, 0));
    res.push_back(std::make_tuple(TensorShape(2, 2, 2), 8, true, expAvgFactor, 0));

    // Test 1D and 2D tensor expansion (cuDNN supports 3D and 4D tensors only).
    for (bool spatial : {false, true})
        for (size_t n : {2, 8})
            for (size_t w : {2, 8})
                res.push_back(std::make_tuple(TensorShape(w), n, spatial, expAvgFactor, 0));

    for (bool spatial : {false, true})
        for (size_t n : {2, 8})
            for (size_t h : {2, 11})
                for (size_t w : {2, 8})
                    res.push_back(std::make_tuple(TensorShape(w, h), n, spatial, expAvgFactor, 0));

    return res;
}

BOOST_AUTO_TEST_SUITE(BatchNormalizationSuite)

BOOST_AUTO_TEST_CASE(BatchNormalizationForward)
{
    // TODO tests for expAvgFactor 0?
    std::mt19937 rng(0);
    boost::random::normal_distribution<float> nd;

    auto initMat = [&](SingleMatrix& buf, size_t r, size_t c, vec& data) -> SingleMatrix
    {
        data.resize(r * 3 * c);
        std::fill(begin(data), end(data), std::numeric_limits<float>::quiet_NaN());
        std::generate(begin(data) + r * c, begin(data) + 2 * r * c, [&] { return nd(rng); });
        buf.SetValue(r, 3 * c, buf.GetDeviceId(), data.data());
        // Get center slice.
        return buf.ColumnSlice(c, c);
    };

    int baseDeviceId = 0;
    for (int deviceId : {0})
    {
        for (const auto& cfg : GenerateBNTestConfigs())
        {
            const auto& inOutT = std::get<0>(cfg);
            size_t batchSize = std::get<1>(cfg);
            bool spatial = std::get<2>(cfg);
            double expAvg = std::get<3>(cfg);
            double blendFactor = 0; // cuDNN supports blendFactor == 0 (train) or 1 (eval) only.
            double eps = 1e-5; // CUDNN_BN_MIN_EPSILON
            bool inferenceOnly = false;

            auto engCudnn = BNEng::Create(baseDeviceId, inOutT, spatial, ImageLayoutKind::CHW, BatchNormEngineKind::CuDnn);
            auto engCntk = BNEng::Create(deviceId, inOutT, spatial, ImageLayoutKind::CHW, BatchNormEngineKind::Cntk);

            size_t crow = inOutT.GetNumElements();
            size_t ccol = batchSize;

            vec buf(crow * ccol);
            std::generate(begin(buf), end(buf), [&] { return nd(rng); });
            SingleMatrix in(crow, ccol, buf.data(), deviceId, matrixFlagNormal);
            SingleMatrix inB(crow, ccol, buf.data(), baseDeviceId, matrixFlagNormal);

            size_t crowScaleBias = spatial ? inOutT[inOutT.GetRank() - 1] : inOutT.GetNumElements();
            buf.resize(crowScaleBias);

            std::generate(begin(buf), end(buf), [&] { return nd(rng); });
            SingleMatrix scale(crowScaleBias, 1, buf.data(), deviceId, matrixFlagNormal);
            SingleMatrix scaleB(crowScaleBias, 1, buf.data(), baseDeviceId, matrixFlagNormal);
            std::generate(begin(buf), end(buf), [&] { return nd(rng); });
            SingleMatrix bias(crowScaleBias, 1, buf.data(), deviceId, matrixFlagNormal);
            SingleMatrix biasB(crowScaleBias, 1, buf.data(), baseDeviceId, matrixFlagNormal);

            SingleMatrix runMeanBuf(deviceId);
            SingleMatrix runMean = initMat(runMeanBuf, crowScaleBias, 1, buf);
            SingleMatrix runMeanB(runMean.DeepClone(), baseDeviceId);
            SingleMatrix runInvStdDevBuf(deviceId);
            SingleMatrix runInvStdDev = initMat(runInvStdDevBuf, crowScaleBias, 1, buf);
            SingleMatrix runInvStdDevB(runInvStdDev.DeepClone(), baseDeviceId);

            SingleMatrix saveMeanBuf(deviceId);
            SingleMatrix saveMean = initMat(saveMeanBuf, crowScaleBias, 1, buf);
            SingleMatrix saveMeanB(saveMean.DeepClone(), baseDeviceId);
            SingleMatrix saveInvStdDevBuf(deviceId);
            SingleMatrix saveInvStdDev = initMat(saveInvStdDevBuf, crowScaleBias, 1, buf);
            SingleMatrix saveInvStdDevB(saveInvStdDev.DeepClone(), baseDeviceId);

            SingleMatrix outBuf(deviceId);
            SingleMatrix out = initMat(outBuf, crow, ccol, buf);
            SingleMatrix outB(out.DeepClone(), baseDeviceId);

            CudaTimer time1;
            time1.Start();
            engCntk->Forward(in, scale, bias, inferenceOnly, expAvg, blendFactor, runMean, runInvStdDev, out, eps, saveMean, saveInvStdDev);
            time1.Stop();

            CudaTimer time2;
            time2.Start();
            engCudnn->Forward(inB, scaleB, biasB, inferenceOnly, expAvg, blendFactor, runMeanB, runInvStdDevB, outB, eps, saveMeanB, saveInvStdDevB);
            time2.Stop();

            std::stringstream tmsg;
            tmsg << "inOut tensor: " << (std::string)inOutT
                 << ", spatial = " << (spatial ? "true" : "false")
                 << ", expAvg = " << expAvg << ")";
            std::string msg = " are not equal, " + tmsg.str();
            std::string msgNan = " has NaNs, " + tmsg.str();
            std::string msgNotNan = " has buffer overflow/underflow, " + tmsg.str();

            float relErr = Err<float>::Rel;
            float absErr = Err<float>::Abs;
            std::string emsg;

            BOOST_REQUIRE_MESSAGE(!out.HasNan("out"), "out" << msgNan);
            BOOST_REQUIRE_MESSAGE(CheckEqual(out, outB, emsg, relErr, absErr * 20), "out" << msg << ". " << emsg);
            BOOST_REQUIRE_MESSAGE(CountNans(outBuf) == crow * 2 * ccol, "out" << msgNotNan);
            // REVIEW alexeyk: add cases for testing numerical stability.

            BOOST_REQUIRE_MESSAGE(!runMean.HasNan("runMean"), "runMean" << msgNan);
            BOOST_REQUIRE_MESSAGE(CheckEqual(runMean, runMeanB, emsg, relErr, absErr), "runMean" << msg << ". " << emsg);
            BOOST_REQUIRE_MESSAGE(CountNans(runMeanBuf) == crowScaleBias * 2, "runMean" << msgNotNan);

            BOOST_REQUIRE_MESSAGE(!runInvStdDev.HasNan("runInvStdDev"), "runInvStdDev" << msgNan);
            BOOST_REQUIRE_MESSAGE(CheckEqual(runInvStdDev, runInvStdDevB, emsg, relErr, absErr), "runInvStdDev" << msg << ". " << emsg);
            BOOST_REQUIRE_MESSAGE(CountNans(runInvStdDevBuf) == crowScaleBias * 2, "runInvStdDev" << msgNotNan);

            BOOST_REQUIRE_MESSAGE(!saveMean.HasNan("saveMean"), "saveMean" << msgNan);
            BOOST_REQUIRE_MESSAGE(CheckEqual(saveMean, saveMeanB, emsg, relErr, absErr), "saveMean" << msg << ". " << emsg);
            BOOST_REQUIRE_MESSAGE(CountNans(saveMeanBuf) == crowScaleBias * 2, "saveMean" << msgNotNan);

            BOOST_REQUIRE_MESSAGE(!saveInvStdDev.HasNan("saveInvStdDev"), "saveInvStdDev" << msgNan);
            BOOST_REQUIRE_MESSAGE(CheckEqual(saveInvStdDev, saveInvStdDevB, emsg, relErr, absErr), "saveInvStdDev" << msg << ". " << emsg);
            BOOST_REQUIRE_MESSAGE(CountNans(saveInvStdDevBuf) == crowScaleBias * 2, "saveInvStdDev" << msgNotNan);

#if 0
#ifndef _DEBUG
            float elapsedCntk = time1.Elapsed();
            float elapsedCudnn = time2.Elapsed();
            // Check performance. Current version of cuDNN (v4 RC) is significantly slower than CNTK implementation.
            // For optimal cases (vectorSize % 32 == 0 and batchSize % 32 == 0), CNTK implementation can be >5x faster than cuDNN.
            // Production version is about the same.
            if (crow >= 32 && ccol >= 32)
            {
                // Use conservative estimates.
                int speedup = 1;
                BOOST_REQUIRE_MESSAGE(speedup * elapsedCntk < elapsedCudnn,
                                      "CNTK implementation (" << elapsedCntk << "ms) must be faster than cuDNN (" << elapsedCudnn << "ms) by at least " << speedup << "x, what's changed? " << tmsg.str());
            }
#endif
#endif
        }
    }
}

BOOST_AUTO_TEST_CASE(BatchNormalizationBackward)
{
    std::mt19937 rng(0);
    boost::random::normal_distribution<float> nd;

    auto initMat = [&](SingleMatrix& buf, size_t r, size_t c, vec& data) -> SingleMatrix
    {
        data.resize(r * 3 * c);
        std::fill(begin(data), end(data), std::numeric_limits<float>::quiet_NaN());
        std::generate(begin(data) + r * c, begin(data) + 2 * r * c, [&] { return nd(rng); });
        buf.SetValue(r, 3 * c, buf.GetDeviceId(), data.data());
        // Get center slice.
        return buf.ColumnSlice(c, c);
    };

    int baseDeviceId = 0;
    for (int deviceId : {0})
    {
        for (const auto& cfg : GenerateBNTestConfigs())
        {
            const auto& inOutT = std::get<0>(cfg);
            size_t batchSize = std::get<1>(cfg);
            bool spatial = std::get<2>(cfg);

            auto engCudnn = BNEng::Create(baseDeviceId, inOutT, spatial, ImageLayoutKind::CHW, BatchNormEngineKind::CuDnn);
            auto engCntk = BNEng::Create(deviceId, inOutT, spatial, ImageLayoutKind::CHW, BatchNormEngineKind::Cntk);

            size_t crow = inOutT.GetNumElements();
            size_t ccol = batchSize;

            vec buf(crow * ccol);
            std::generate(begin(buf), end(buf), [&] { return nd(rng); });
            SingleMatrix x(crow, ccol, buf.data(), deviceId, matrixFlagNormal);
            SingleMatrix xB(crow, ccol, buf.data(), baseDeviceId, matrixFlagNormal);

            std::generate(begin(buf), end(buf), [&] { return nd(rng); });
            SingleMatrix dy(crow, ccol, buf.data(), deviceId, matrixFlagNormal);
            SingleMatrix dyB(crow, ccol, buf.data(), baseDeviceId, matrixFlagNormal);

            size_t crowScaleBias = spatial ? inOutT[inOutT.GetRank() - 1] : inOutT.GetNumElements();
            buf.resize(crowScaleBias);

            std::generate(begin(buf), end(buf), [&] { return nd(rng); });
            SingleMatrix scale(crowScaleBias, 1, buf.data(), deviceId, matrixFlagNormal);
            SingleMatrix scaleB(crowScaleBias, 1, buf.data(), baseDeviceId, matrixFlagNormal);

            std::generate(begin(buf), end(buf), [&] { return nd(rng); });
            SingleMatrix saveMean(crowScaleBias, 1, buf.data(), deviceId, matrixFlagNormal);
            SingleMatrix saveMeanB(crowScaleBias, 1, buf.data(), baseDeviceId, matrixFlagNormal);

            std::generate(begin(buf), end(buf), [&] { return nd(rng); });
            SingleMatrix saveInvStdDev(crowScaleBias, 1, buf.data(), deviceId, matrixFlagNormal);
            SingleMatrix saveInvStdDevB(crowScaleBias, 1, buf.data(), baseDeviceId, matrixFlagNormal);

            SingleMatrix dScaleBuf(deviceId);
            SingleMatrix dScale = initMat(dScaleBuf, crowScaleBias, 1, buf);
            SingleMatrix dScaleB(dScale.DeepClone(), baseDeviceId);
            SingleMatrix dBiasBuf(deviceId);
            SingleMatrix dBias = initMat(dBiasBuf, crowScaleBias, 1, buf);
            SingleMatrix dBiasB(dBias.DeepClone(), baseDeviceId);

            SingleMatrix dxBuf(deviceId);
            SingleMatrix dx = initMat(dxBuf, crow, ccol, buf);
            SingleMatrix dxB(dx.DeepClone(), baseDeviceId);

            CudaTimer time1;
            time1.Start();
            engCntk->Backward(x, dy, dx, scale, 0, saveMean, saveInvStdDev, dScale, dBias, false);
            engCntk->Backward(x, dy, dx, scale, 0, saveMean, saveInvStdDev, dScale, dBias, true);
            time1.Stop();

            CudaTimer time2;
            time2.Start();
            engCudnn->Backward(xB, dyB, dxB, scaleB, 0, saveMeanB, saveInvStdDevB, dScaleB, dBiasB, false);
            engCudnn->Backward(xB, dyB, dxB, scaleB, 0, saveMeanB, saveInvStdDevB, dScaleB, dBiasB, true);
            time2.Stop();

            std::stringstream tmsg;
            tmsg << "inOut tensor: " << (std::string)inOutT
                 << ", spatial = " << (spatial ? "true" : "false");
            std::string msg = " are not equal, " + tmsg.str();
            std::string msgNan = " has NaNs, " + tmsg.str();
            std::string msgNotNan = " has buffer overflow/underflow, " + tmsg.str();

            float relErr = Err<float>::Rel;
            float absErr = Err<float>::Abs;
            std::string emsg;

            BOOST_REQUIRE_MESSAGE(!dx.HasNan("dx"), "dx" << msgNan);
            BOOST_REQUIRE_MESSAGE(CheckEqual(dx, dxB, emsg, relErr * 16, absErr * 64), "dx" << msg << ". " << emsg);
            // BUGBUG: Why does this pass for CNTK engine?
            BOOST_REQUIRE_MESSAGE(CountNans(dxBuf) == crow * 2 * ccol, "out" << msgNotNan);
            // REVIEW alexeyk: add cases for testing numerical stability.

            BOOST_REQUIRE_MESSAGE(!dScale.HasNan("dScale"), "dScale" << msgNan);
            // After using boost norm_distribution, we have to adapt the tolerance value. But we get the same result on Windows and Linux.
            // When using std norm_distribution, different tolerance values are needed for Windows than for Linux.
            BOOST_REQUIRE_MESSAGE(CheckEqual(dScale, dScaleB, emsg, relErr * 88, absErr * 16), "dScale" << msg << ". " << emsg);
            BOOST_REQUIRE_MESSAGE(CountNans(dScaleBuf) == crowScaleBias * 2, "dScale" << msgNotNan);

            BOOST_REQUIRE_MESSAGE(!dBias.HasNan("dBias"), "dBias" << msgNan);
            BOOST_REQUIRE_MESSAGE(CheckEqual(dBias, dBiasB, emsg, relErr * 88, absErr * 16), "dBias" << msg << ". " << emsg);
            BOOST_REQUIRE_MESSAGE(CountNans(dBiasBuf) == crowScaleBias * 2, "dBias" << msgNotNan);

#if 0
#ifndef _DEBUG
            float elapsedCntk = time1.Elapsed();
            float elapsedCudnn = time2.Elapsed();
            // Check performance. Current version of cuDNN (v4 RC) is significanlty slower than CNTK implementation.
            // For optimal cases (vectorSize % 32 == 0 and batchSize % 32 == 0), CNTK implementation can be >5x faster than cuDNN.
            // Production version is about the same.
            if (crow >= 32 && ccol >= 32)
            {
                // Use conservative estimates.
                float speedup = 1.0f;
                BOOST_REQUIRE_MESSAGE(speedup * elapsedCntk < elapsedCudnn,
                                      "CNTK implementation (" << elapsedCntk << "ms) must be faster than cuDNN (" << elapsedCudnn << "ms) by at least " << speedup << "x, what's changed? " << tmsg.str());
            }
#endif
#endif
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()

} } } }


/* --------------------------------------------------------------------------
*
* (C) Copyright …
*
* ---------------------------------------------------------------------------
*/

/*!
 * @file EigenTransformToTransform3DConverter.cpp
 * @date 22/01/2018
 * @author Alessandro Bianco
 */

/*!
 * @addtogroup Converters
 *
 * Implementation of EigenTransformToTransform3DConverter.
 *
 *
 * @{
 */

/* --------------------------------------------------------------------------
 *
 * Includes
 *
 * --------------------------------------------------------------------------
 */

#include "EigenTransformToTransform3DConverter.hpp"
#include <Errors/Assert.hpp>
#include <Eigen/Geometry>

namespace Converters {

using namespace PoseWrapper;

/* --------------------------------------------------------------------------
 *
 * Public Member Functions
 *
 * --------------------------------------------------------------------------
 */
const PoseWrapper::Transform3DConstPtr EigenTransformToTransform3DConverter::Convert(const Eigen::Matrix4f& transform)
	{
	ASSERT(transform(3,0) == 0 && transform(3,1) == 0 && transform(3,2) == 0 && transform(3,3) == 1, "EigenTransformToTransform3DConverter, An invalid transformation matrix was passed.")
	Eigen::Matrix3f eigenRotationMatrix = transform.block(0,0,3,3);
	ASSERT_CLOSE(std::abs(eigenRotationMatrix.determinant()), 1.00, 0.00001, "EigenTransformToTransform3DConverter, An invalid rotation was passed during conversion.")
	Eigen::Quaternionf eigenRotation (eigenRotationMatrix);

	Transform3DPtr conversion = new Transform3D();

	SetPosition(*conversion, transform(0, 3), transform(1, 3), transform(2, 3) );
	SetOrientation(*conversion, eigenRotation.x(), eigenRotation.y(), eigenRotation.z(), eigenRotation.w());

	return conversion;
	}

const PoseWrapper::Transform3DSharedConstPtr EigenTransformToTransform3DConverter::ConvertShared(const Eigen::Matrix4f& transform)
	{
	Transform3DConstPtr conversion = Convert(transform);
	Transform3DSharedConstPtr sharedConversion(conversion);
	return sharedConversion;
	}

}

/** @} */


/* Cam Mannett 2020
 *
 * See LICENSE file
 */

#include "malbolge/virtual_cpu.hpp"
#include "malbolge/cpu_instruction.hpp"
#include "malbolge/log.hpp"

#include <boost/asio/io_context.hpp>
#include <boost/asio/post.hpp>

#include <thread>
#include <deque>
#include <unordered_map>
#include <atomic>

using namespace malbolge;

class virtual_cpu::impl_t : public std::enable_shared_from_this<impl_t>
{
public:
    class breakpoint
    {
    public:
        breakpoint(math::ternary a, std::size_t i = 0) :
            address_{a},
            ignore_count_{i},
            pre_{false}
        {}

        [[nodiscard]]
        bool operator()() noexcept
        {
            if (pre_) {
                pre_ = false;
                return false;
            }

            if (ignore_count_ == 0) {
                pre_ = true;
                return true;
            }

            --ignore_count_;
            return false;
        }

    private:
        math::ternary address_;
        std::size_t ignore_count_;

        // The breakpoint is fired (and this variable set) on entry to an
        // address, so on the next run() call the breakpoint will be hit again -
        // this variable being set high will cause the breakpoint to be ignored.
        // This must be set low after, otherwise the breakpoint will never fire
        // again
        bool pre_;
    };

    class input
    {
    public:
        input(std::string p) noexcept :
            phrase_{std::move(p)},
            view_{phrase_.data()}
        {}

        [[nodiscard]]
        char get() noexcept
        {
            if (view_.empty()) {
                return 0;
            }

            auto c = view_.front();
            view_.remove_prefix(1);
            return c;
        }

    private:
        std::string phrase_;
        std::string_view view_;
    };

    explicit impl_t(virtual_memory vm) :
        worker_guard_{ctx.get_executor()},
        vmem(std::move(vm)),
        c{vmem.begin()},
        d{vmem.begin()},
        p_counter{0},
        state_{virtual_cpu::execution_state::READY}
    {}

    [[nodiscard]]
    virtual_cpu::execution_state state() const noexcept
    {
        return state_;
    }

    void set_state(virtual_cpu::execution_state new_state,
                   std::exception_ptr eptr = {})
    {
        if (state_ == new_state) {
            return;
        }

        state_ = new_state;
        state_sig(state_, eptr);
    }

    void stop()
    {
        worker_guard_.reset();
        ctx.stop();
        if (thread.joinable()) {
            thread.join();
        }
    }

    void stopped_check()
    {
        // Once in a STOPPED state, it cannot change to another
        if (state_ == virtual_cpu::execution_state::STOPPED) {
            throw execution_exception{"vCPU has been stopped", p_counter};
        }
    }

    bool bp_check(virtual_memory::iterator reg_it);

    void run(bool schedule_next = true);

    boost::asio::io_context ctx;
    boost::asio::executor_work_guard<boost::asio::io_context::executor_type> worker_guard_;
    std::thread thread;

    virtual_memory vmem;
    std::deque<input> input_queue_;
    std::unordered_map<math::ternary, breakpoint> bps;

    // vCPU Registers
    math::ternary a;
    virtual_memory::iterator c;
    virtual_memory::iterator d;
    std::size_t p_counter;

    state_signal_type state_sig;
    output_signal_type output_sig;
    breakpoint_hit_signal_type bp_hit_sig;

private:
    std::atomic<virtual_cpu::execution_state> state_;
};

virtual_cpu::virtual_cpu(virtual_memory vmem) :
    impl_{std::make_shared<impl_t>(std::move(vmem))}
{
    impl_->thread = std::thread{[impl = impl_]() {
        auto eptr = std::exception_ptr{};
        try {
            impl->ctx.run();
        } catch (std::exception&) {
            eptr = std::current_exception();
        }
        impl->set_state(execution_state::STOPPED, eptr);
    }};
}

virtual_cpu::~virtual_cpu()
{
    if (!impl_) [[unlikely]] {
        return;
    }
    impl_->stop();
}

void virtual_cpu::run()
{
    impl_check();
    impl_->stopped_check();
    boost::asio::post(impl_->ctx, [impl = impl_]() {
        if (impl->state() == execution_state::RUNNING ||
            impl->state() == execution_state::WAITING_FOR_INPUT) {
            return;
        }

        impl->set_state(execution_state::RUNNING);
        impl->run();
    });
}

void virtual_cpu::pause()
{
    impl_check();
    impl_->stopped_check();
    boost::asio::post(impl_->ctx, [impl = impl_]() {
        if (impl->state() == execution_state::PAUSED ||
            impl->state() == execution_state::WAITING_FOR_INPUT) {
            return;
        }

        impl->set_state(execution_state::PAUSED);
    });
}

void virtual_cpu::step()
{
    impl_check();
    impl_->stopped_check();
    boost::asio::post(impl_->ctx, [impl = impl_]() {
        if (impl->state() == execution_state::WAITING_FOR_INPUT) {
            return;
        }

        impl->set_state(execution_state::PAUSED);
        impl->run(false);
    });
}

void virtual_cpu::add_input(std::string data)
{
    impl_check();
    boost::asio::post(impl_->ctx, [impl = impl_, data = std::move(data)]() {
        impl->input_queue_.emplace_back(std::move(data));
        if (impl->state() == execution_state::WAITING_FOR_INPUT) {
            impl->set_state(execution_state::RUNNING);
            impl->run();
        }
    });
}

void virtual_cpu::add_breakpoint(math::ternary address, std::size_t ignore_count)
{
    impl_check();
    boost::asio::post(impl_->ctx, [impl = impl_, address, ignore_count]() {
        auto bp = impl_t::breakpoint{address, ignore_count};
        impl->bps.insert_or_assign(address, std::move(bp));
    });
}

void virtual_cpu::remove_breakpoint(math::ternary address)
{
    impl_check();
    boost::asio::post(impl_->ctx, [impl = impl_, address]() {
        impl->bps.erase(address);
    });
}

void virtual_cpu::address_value(math::ternary address,
                                address_value_callback_type cb) const
{
    impl_check();
    boost::asio::post(impl_->ctx, [impl = impl_, address, cb = std::move(cb)]() {
        const auto value = impl->vmem[address];
        cb(address, value);
    });
}

void virtual_cpu::register_value(vcpu_register reg,
                                 register_value_callback_type cb) const
{
    impl_check();
    boost::asio::post(impl_->ctx, [impl = impl_, reg, cb = std::move(cb)]() {
        switch (reg) {
        case vcpu_register::A:
            cb(reg, {}, impl->a);
            break;
        case vcpu_register::C:
        {
            const auto address = static_cast<math::ternary::underlying_type>(
                                    std::distance(impl->vmem.begin(), impl->c));
            cb(reg, address, impl->vmem[address]);
            break;
        }
        case vcpu_register::D:
        {
            const auto address = static_cast<math::ternary::underlying_type>(
                                    std::distance(impl->vmem.begin(), impl->d));
            cb(reg, address, impl->vmem[address]);
            break;
        }
        default:
            throw execution_exception{
                "Unhandled register query: " + std::to_string(static_cast<int>(reg)),
                impl->p_counter
            };
        }
    });
}

virtual_cpu::state_signal_type::connection
virtual_cpu::register_for_state_signal(state_signal_type::slot_type slot)
{
    impl_check();
    return impl_->state_sig.connect(std::move(slot));
}

virtual_cpu::output_signal_type::connection
virtual_cpu::register_for_output_signal(output_signal_type::slot_type slot)
{
    impl_check();
    return impl_->output_sig.connect(std::move(slot));
}

virtual_cpu::breakpoint_hit_signal_type::connection
virtual_cpu::register_for_breakpoint_hit_signal(breakpoint_hit_signal_type::slot_type slot)
{
    impl_check();
    return impl_->bp_hit_sig.connect(std::move(slot));
}

void virtual_cpu::impl_check() const
{
    if (!impl_) [[unlikely]] {
        throw execution_exception{"vCPU backend destroyed, use-after-move?", 0};
    }
}

bool virtual_cpu::impl_t::bp_check(virtual_memory::iterator reg_it)
{
    const auto address = static_cast<math::ternary::underlying_type>(
        std::distance(vmem.begin(), reg_it)
    );
    auto it = bps.find(address);
    if (it == bps.end()) {
        return false;
    } else if (it->second()) {
        set_state(virtual_cpu::execution_state::PAUSED);
        bp_hit_sig(address);
        return true;
    }
    return false;
}

void virtual_cpu::impl_t::run(bool schedule_next)
{
    // A pause() needs to break the run()-chain
    if (state_ == virtual_cpu::execution_state::PAUSED && schedule_next) {
        return;
    }

    if (bp_check(c)) {
        return;
    }

    // Pre-cipher the instruction
    auto instr = pre_cipher_instruction(*c, c - vmem.begin());
    if (!instr) {
        throw execution_exception{
            "Pre-cipher non-whitespace character must be graphical "
                "ASCII: " + std::to_string(static_cast<int>(*c)),
            p_counter
        };
    }

    log::print(log::VERBOSE_DEBUG,
               "Step: ", p_counter, ", pre-cipher instr: ",
               static_cast<int>(*instr));

    switch (*instr) {
    case cpu_instruction::set_data_ptr:
        d = vmem.begin() + static_cast<std::size_t>(*d);
        break;
    case cpu_instruction::set_code_ptr:
        c = vmem.begin() + static_cast<std::size_t>(*d);
        break;
    case cpu_instruction::rotate:
        a = d->rotate();
        break;
    case cpu_instruction::op:
        a = *d = a.op(*d);
        break;
    case cpu_instruction::read:
    {
        if (input_queue_.empty()) {
            set_state(virtual_cpu::execution_state::WAITING_FOR_INPUT);
            log::print(log::VERBOSE_DEBUG, "\tWaiting for input...");
            return;
        }

        auto c = input_queue_.front().get();
        if (!c) {
            a = math::ternary::max;
            input_queue_.pop_front();
        } else {
            a = c;
        }
        break;
    }
    case cpu_instruction::write:
        if (a != math::ternary::max) {
            output_sig(static_cast<char>(a));
        }
        break;
    case cpu_instruction::stop:
        set_state(virtual_cpu::execution_state::STOPPED);
        return;
    default:
        // Nop
        break;
    }

    // Post-cipher the instruction
    auto pc = post_cipher_instruction(*c);
    if (!pc) {
        throw execution_exception{
            "Post-cipher non-whitespace character must be graphical "
                "ASCII: " + std::to_string(static_cast<int>(*c)),
            p_counter
        };
    }
    *c = *pc;

    log::print(log::VERBOSE_DEBUG,
               "\tPost-op regs - a: ", a,
               ", c[", std::distance(vmem.begin(), c), "]: ", *c,
               ", d[", std::distance(vmem.begin(), d), "]: ", *d);

    ++c;
    ++d;
    ++p_counter;
    if (schedule_next) {
        // Schedule the next iteration
        boost::asio::post(ctx, [impl = shared_from_this()]() {
            impl->run();
        });
    }
}

std::ostream& malbolge::operator<<(std::ostream& stream,
                                   virtual_cpu::vcpu_register register_id)
{
    static_assert(static_cast<int>(virtual_cpu::vcpu_register::NUM_REGISTERS) == 3,
                  "Register IDs have changed, update this");

    switch(register_id) {
    case virtual_cpu::vcpu_register::A:
        return stream << "A";
    case virtual_cpu::vcpu_register::C:
        return stream << "C";
    case virtual_cpu::vcpu_register::D:
        return stream << "D";
    default:
        return stream << "Unknown register ID: "
                      << static_cast<int>(register_id);
    }
}

std::ostream& malbolge::operator<<(std::ostream& stream,
                                   virtual_cpu::execution_state state)
{
    static_assert(static_cast<int>(virtual_cpu::execution_state::NUM_STATES) == 5,
                  "Number of execution states have change, update operator<<");

    switch (state) {
    case virtual_cpu::execution_state::READY:
        return stream << "READY";
    case virtual_cpu::execution_state::RUNNING:
        return stream << "RUNNING";
    case virtual_cpu::execution_state::PAUSED:
        return stream << "PAUSED";
    case virtual_cpu::execution_state::WAITING_FOR_INPUT:
        return stream << "WAITING_FOR_INPUT";
    case virtual_cpu::execution_state::STOPPED:
        return stream << "STOPPED";
    default:
        return stream << "Unknown vCPU state: " << static_cast<int>(state);
    }
}


#define CGAL_COMPACT_CONTAINER_DEBUG_TIME_STAMP 1
// Copyright (c) 2009 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Stephane Tayeb
//
//******************************************************************************
// File Description :
//******************************************************************************

#include <CGAL/Mesh_3/io_signature.h>
#include "test_meshing_utilities.h"
#include <CGAL/Polyhedral_mesh_domain_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>

#include <boost/type_traits/is_same.hpp>

#include <CGAL/Mesh_3/Dump_c3t3.h>

#include <CGAL/disable_warnings.h>

template <typename K, typename Concurrency_tag = CGAL::Sequential_tag>
struct Polyhedron_tester : public Tester<K>
{
  void polyhedron() const
  {
    typedef K Gt;
    typedef CGAL::Polyhedron_3<Gt> Polyhedron;
    typedef CGAL::Polyhedral_mesh_domain_3<Polyhedron, Gt> Mesh_domain;

    CGAL_static_assertion((boost::is_same<
                            typename Mesh_domain::Surface_patch_index,
                            std::pair<int, int>
                           >::value));

    typedef typename CGAL::Mesh_triangulation_3<
      Mesh_domain,
      typename CGAL::Kernel_traits<Mesh_domain>::Kernel,
      Concurrency_tag>::type Tr;
    typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr> C3t3;

    typedef CGAL::Mesh_criteria_3<Tr> Mesh_criteria;
    typedef typename Mesh_criteria::Facet_criteria Facet_criteria;
    typedef typename Mesh_criteria::Cell_criteria Cell_criteria;

    typedef typename Mesh_domain::Surface_patch_index Surface_patch_index;

    //-------------------------------------------------------
    // Data generation
    //-------------------------------------------------------
    Polyhedron polyhedron;
    std::ifstream input("data/sphere.off");
    input >> polyhedron;
    input.close();

    std::cout << "\tSeed is\t"
      << CGAL::get_default_random().get_seed() << std::endl;
    Mesh_domain domain(polyhedron, &CGAL::get_default_random());

    // Set mesh criteria
    Facet_criteria facet_criteria(30, 0.2, 0.02);
    Cell_criteria cell_criteria(2, 0.2);
    Mesh_criteria criteria(facet_criteria, cell_criteria);

    // Mesh generation
    C3t3 c3t3;
    typename Polyhedron::Point_iterator end = polyhedron.points_begin();
    int i=0;
    while ( i++ < 4 ) { ++end; }

    c3t3.insert_surface_points(polyhedron.points_begin(),
                               end,
                               domain.index_from_surface_patch_index(Surface_patch_index(0,1)));

    CGAL::refine_mesh_3(c3t3, domain, criteria,
                        CGAL::parameters::no_exude(),
                        CGAL::parameters::no_perturb());

    CGAL::remove_far_points_in_mesh_3(c3t3);

    // Verify
    double vol = 0.479171765761454;
    this->verify_c3t3_volume(c3t3, vol*0.95, vol*1.05);
#ifdef CGAL_LINKED_WITH_TBB
    // Parallel
    if (boost::is_convertible<Concurrency_tag, CGAL::Parallel_tag>::value)
    {
      this->verify(c3t3, domain, criteria, Polyhedral_tag(),
                   110, 140, 190, 235, 300, 450);
    }
    else
#endif //CGAL_LINKED_WITH_TBB
    {
      this->verify(c3t3, domain, criteria, Polyhedral_tag(),
                   119, 121, 200, 204, 350, 360);
    }

    // test the dump function
    CGAL::dump_c3t3(c3t3, "test_meshing_polyhedron-out");
  }
};

int main()
{
  Polyhedron_tester<K_e_i> test_epic;
  std::cerr << "Mesh generation from a polyhedron:\n";
  test_epic.polyhedron();

#ifdef CGAL_LINKED_WITH_TBB
  Polyhedron_tester<K_e_i, CGAL::Parallel_tag> test_epic_parallel;
  std::cerr << "Mesh generation from a polyhedron using Parallel_tag:\n";
  test_epic_parallel.polyhedron();
#else
  std::cerr << "TBB is not installed, or not configured."
            << "The parallel version cannot be tested.\n";
#endif

  return EXIT_SUCCESS;
}


/* An estimator that uses measurements from the motion capture system, and generates
 * an estimate of the vehicle's position, velocity, orientation, and angular velocity.
 */

#pragma once

#include <mutex>
#include <deque>

#include <Eigen/Dense>

#include "Common/Math/Vec3.hpp"
#include "Common/Math/Rotation.hpp"
#include "Common/Time/Timer.hpp"
#include "Common/Time/ManualTimer.hpp"

//TODO FIXME: this needs to be thread-safe, mutex

namespace Offboard {

template<class T_msg>
class PredictionPipe {
  static constexpr double SMALL_TIME = 1e-6;  // [s]
 public:
  PredictionPipe(BaseTimer* const timer, double delayTime)
      : _timer(timer),
        _delayTime(delayTime) {
    //Do nothing
  }

  void AddMessage(T_msg msg) {
    TimedMessage tmsg;
    tmsg.timeActive = _timer.GetSeconds<double>() + _delayTime;  //message is sent now, will be active after _delayTime
    tmsg.msg = msg;
    _messages.push_back(tmsg);
  }

  bool GetActiveMessage(double t, T_msg &out, double &timeRemaining) const {
    if (!_messages.size()) {
      return false;
    }
    //beginning at the end, find the newest message that's valid now.
    // Returns false if no valid message exists.
    auto msg = _messages.cend();
    double tLastMsg = 1e10;
    for (;;) {
      msg--;
      if ((t + SMALL_TIME) >= msg->timeActive) {
        out = msg->msg;
        timeRemaining = tLastMsg - msg->timeActive;
        return true;
      }
      tLastMsg = msg->timeActive;
      if (msg == _messages.cbegin()) {
        break;
      }
    }
    return false;
  }

  void ClearExpiredMessages(double currentTime) {
    int N = _messages.size();
    //this is inelegant
    for (int i = 0; i < N; i++) {
      if (_messages.size() < 2) {
        //last message always stays active.
        return;
      }

      if (_messages[1].timeActive <= currentTime) {
        _messages.pop_front();
      }
    };
  }

 private:
  struct TimedMessage {
    double timeActive;  //[s]
    T_msg msg;
  };

  Timer _timer;
  double _delayTime;
  std::deque<TimedMessage> _messages;

};

/**! A state estimator that tracks a 6DOF object, using mocap measurements
 * The estimator totally decouples position from attitude, and treats each position axis
 * separately.
 *
 * In translation, we don't do anything funny. For attitude, we assumea  first order
 * system is tracking attitude commands.
 */
class MocapStateEstimator {
 public:
  struct MocapEstimatedState {
    Vec3d pos, vel, angVel;
    Rotationd att;
  };

  MocapStateEstimator(BaseTimer* const masterTimer, unsigned const id,
                      double standardCommunicationsDelay);

  void SetStatistics(double measNoiseStdDevPos, double measNoiseStdDevAtt,
                     double procNoiseStdDevPos, double procNoiseStdDevAtt) {
    _measNoiseStdDevPos = measNoiseStdDevPos;
    _measNoiseStdDevAtt = measNoiseStdDevAtt;
    _procNoiseStdDevPos = procNoiseStdDevPos;
    _procNoiseStdDevAtt = procNoiseStdDevAtt;
  }

  unsigned GetID() const {
    return _id;
  }
  // This just predicts the states by running the dynamic for the dt second latency between estimator & controller.
  MocapEstimatedState GetPrediction(double const dt) const;

  unsigned GetPredictedMeasurementRejectionCount() const {
    std::lock_guard<std::mutex> guard(_mutexEstimate);
    return _numMeasRejected;
  }

  void SetPosition(Vec3d in) {
    std::lock_guard<std::mutex> guard(_mutexEstimate);
    _pos = in;
  }

  void SetVelocity(Vec3d in) {
    std::lock_guard<std::mutex> guard(_mutexEstimate);
    _vel = in;
  }

  void SetAttitude(Rotationd in) {
    std::lock_guard<std::mutex> guard(_mutexEstimate);
    _att = in;
  }

  void SetAngularVelocity(Vec3d in) {
    std::lock_guard<std::mutex> guard(_mutexEstimate);
    _angVel = in;
  }

  void SetPredictedValues(Vec3d angVel, Vec3d acceleration) {
    PredictionType p;
    p.acc = acceleration;
    p.angVel = angVel;
    p.ballistic = false;
    _predictionPipe.AddMessage(p);
  }

  void SetBallistic() {
    PredictionType p;
    p.acc = Vec3d(0, 0, -9.81);
    p.angVel = Vec3d(0, 0, 0);
    p.ballistic = true;
    _predictionPipe.AddMessage(p);
  }

  // This is the whole process of state estimation including initialization, prediction, and measurement update.
  void UpdateWithMeasurement(Vec3d const position, Rotationd const attitude);

  double GetTimeSinceLastGoodMeasurement() const {
    return _lastGoodMeasUpdate.GetSeconds<double>();
  }

  bool GetIsInitialized() const {
    return _initialized;
  }

  void Reset();  //Hard reset of all internal states
  void ResetVariance();

  void SetAngularVelocityTimeConstant(double const tau) {
    _timeConstantTrackAngVel = tau;
  }

 private:
  unsigned _id;
  ManualTimer _estimateTimer;  //time at which estimate is valid
  Timer _timer;  //wall clock
  Timer _lastGoodMeasUpdate;  //time since we last used a measurement

  bool _initialized;

  //our state estimate, updated with measurements, true at time _estimateTimer
  Vec3d _pos, _vel, _angVel;
  Rotationd _att;  //<vector in world frame> = _att*<vector in body frame>

  //filter variance
  Eigen::Matrix<double, 2, 2> _variancePosition;
  Eigen::Matrix<double, 2, 2> _varianceAttitude;

  Rotationd _lastMeasAtt;  //for "measuring" of angular velocity

  unsigned _numMeasRejected, _numMeasRejectedConsecutively;

  //Constants:
  double _timeConstantTrackAngVel;  // the time constant with which the system tracks the predicted angular velocity.
  double _measRejectDist;  //[std deviations] how unlikely a measurement do we accept?
  double _systemLatency;  //[s]

  //Noise model:
  double _measNoiseStdDevPos;  // [m]
  double _measNoiseStdDevAtt;  // [rad]
  double _procNoiseStdDevPos;  // translational acceleration [m/s**2]
  double _procNoiseStdDevAtt;  // angular velocity [rad/s**2]

  //Our prediction info:
  struct PredictionType {
    Vec3d acc;
    Vec3d angVel;
    bool ballistic;  //set to true if we assume ballistic free-fall (ignore acc/angvel)
  };
  PredictionPipe<PredictionType> _predictionPipe;

  //Thread safety:
  std::mutex mutable _mutexEstimate;
};

}


//          Copyright Carl Philipp Reh 2006 - 2019.
#include <fcppt/text.hpp>
// 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)

#include <sge/opengl/texture/pp_dims.hpp>
#include <sge/opengl/texture/mipmap/generate_levels.hpp>
#include <sge/opengl/texture/mipmap/parameters.hpp>
#include <sge/renderer/basic_dim.hpp>
#include <sge/renderer/texture/mipmap/level.hpp>
#include <sge/renderer/texture/mipmap/level_count.hpp>
#include <fcppt/use.hpp>
#include <fcppt/math/size_type.hpp>
#include <fcppt/math/dim/at.hpp>
#include <fcppt/math/dim/contents.hpp>
#include <fcppt/math/dim/init.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/preprocessor/seq/for_each.hpp>
#include <algorithm>
#include <fcppt/config/external_end.hpp>

namespace
{

template <fcppt::math::size_type Size>
sge::renderer::basic_dim<Size> reduce_dim(sge::renderer::basic_dim<Size> const &);

}

template <fcppt::math::size_type Size>
void sge::opengl::texture::mipmap::generate_levels(
    sge::opengl::texture::mipmap::parameters<Size> const &_parameters,
    sge::renderer::texture::mipmap::level_count const _levels)
{
  using dim = typename sge::opengl::texture::mipmap::parameters<Size>::dim;

  sge::renderer::texture::mipmap::level level(1U);

  for (dim size(::reduce_dim<Size>(_parameters.size()));
       fcppt::math::dim::contents(size) != 1U && level.get() <= _levels.get();
       size = ::reduce_dim<Size>(size), ++level)
  {
    _parameters.init_function().get()(
        _parameters.binding(),
        _parameters.config(),
        _parameters.buffer_type(),
        _parameters.format_order(),
        _parameters.format_base_type(),
        _parameters.internal_format(),
        level,
        size,
        nullptr);
  }
}

namespace
{

template <fcppt::math::size_type Size>
sge::renderer::basic_dim<Size> reduce_dim(sge::renderer::basic_dim<Size> const &_size)
{
  using dim = sge::renderer::basic_dim<Size>;

  return fcppt::math::dim::init<dim>(
      [&_size](auto const _index)
      {
        FCPPT_USE(_index);

        return std::max(
            fcppt::math::dim::at<_index>(_size) / fcppt::literal<typename dim::value_type>(2),
            fcppt::literal<typename dim::value_type>(1U));
      });
}

}

#define SGE_OPENGL_TEXTURE_MIPMAP_INSTANTIATE_GENERATE_LEVELS(seq, _, dimension) \
  template void sge::opengl::texture::mipmap::generate_levels<dimension>( \
      sge::opengl::texture::mipmap::parameters<dimension> const &, \
      sge::renderer::texture::mipmap::level_count);

BOOST_PP_SEQ_FOR_EACH(
    SGE_OPENGL_TEXTURE_MIPMAP_INSTANTIATE_GENERATE_LEVELS, _, SGE_OPENGL_TEXTURE_PP_DIMS)


#include <Eigen/Geometry>
#include <vector>
#include <set>

#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CameraInfo.h>

// PCL specific includes
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/people/ground_based_people_detection_app.h>
#include <pcl/common/time.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>

using namespace std;
typedef pcl::PointXYZRGBA PointT;
typedef sensor_msgs::PointCloud2 PointCloudT;

// PCL viewer //
pcl::visualization::PCLVisualizer viewer ("PCL Viewer");

// Mutex: //
boost::mutex cloud_mutex;

// Global Parameters
Eigen::VectorXf ground_coeffs; // Ground plane init
pcl::PointCloud<PointT> callback_cloud;
pcl::PointCloud<PointT>::Ptr cloud;
pcl::people::PersonClassifier<pcl::RGB> person_classifier; // Create classifier for people detection:
pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector;    // people detection object
string g_detectionFrame, g_baseLinkFrame, g_imageSourceRGB, g_imageSourceDepth, g_modalityName;
Eigen::Matrix3d g_intrinsicsMatrix = Eigen::Matrix3d::Zero();
boost::mutex g_intrinsicsMutex;

// ROS Components
boost::shared_ptr<ros::NodeHandle> g_paramNodeHandle;
boost::shared_ptr<tf::TransformListener> g_transformListener;
boost::shared_ptr<tf::TransformBroadcaster> g_transformBroadcaster;
ros::Publisher pub;

// Funtion to transform the Kinect coordinate system.
Eigen::Affine3d getCameraTransform(string frameId) {
    // Get the current position of the camera with regard to base_link
    tf::StampedTransform opticalFrameToBaseLinkTransform;
    g_transformListener->lookupTransform(g_baseLinkFrame, frameId, ros::Time(0), opticalFrameToBaseLinkTransform);

    Eigen::Affine3d opticalFrameToBaseLinkTransformEigen;
    tf::transformTFToEigen(opticalFrameToBaseLinkTransform, opticalFrameToBaseLinkTransformEigen);

    return opticalFrameToBaseLinkTransformEigen;
}

Eigen::Hyperplane<float, 3> getGroundPlane()
{
    // Get ground plane estimate [normal_x normal_y normal_z d] from parameters
    std::vector<float> original_ground_coeffs;
    string original_ground_coeffs_string;
    g_paramNodeHandle->getParam("ground_coeffs", original_ground_coeffs_string); // FIXME: Replace by dynparam
    {
        std::istringstream is(original_ground_coeffs_string);
        double value;
        while( is >> value ) {
            original_ground_coeffs.push_back(value);
        }
    }

    if (original_ground_coeffs.size() < 4) {
        ROS_WARN("Ground plane parameters not set, using defaults!");
        original_ground_coeffs.clear();
        original_ground_coeffs.push_back(0);
        original_ground_coeffs.push_back(0);
        original_ground_coeffs.push_back(1);
        original_ground_coeffs.push_back(0);
    }

    return Eigen::Hyperplane<float, 3>(
            Eigen::Vector3f(original_ground_coeffs[0], original_ground_coeffs[1], original_ground_coeffs[2]),
            original_ground_coeffs[3]);
}

Eigen::Quaterniond getGroundPlaneRotation(Eigen::Hyperplane<float, 3> groundPlane) {
    Eigen::Quaternion<double> groundPlaneRotation;
    groundPlaneRotation.setFromTwoVectors(Eigen::Vector3d(0,1,0), groundPlane.normal().cast<double>());
    return groundPlaneRotation;
}

Eigen::Affine3d getGroundPlaneTransform(Eigen::Hyperplane<float, 3> groundPlane) {
    return Eigen::Translation3d(groundPlane.normal().cast<double>() * groundPlane.offset())
            * getGroundPlaneRotation(groundPlane);
}

void cameraInfoCallback(const sensor_msgs::CameraInfo::Ptr& msg) {
    boost::array<double, 9> intrinsics = msg->K;
    Eigen::Matrix3d intrinsicsMatrix;
    for(int row = 0; row < 3; row++) for(int col = 0; col < 3; col++) intrinsicsMatrix(row, col) = intrinsics[row * 3 + col];

    boost::lock_guard<boost::mutex> lock(g_intrinsicsMutex);
    if(intrinsicsMatrix !=  g_intrinsicsMatrix) {
        g_intrinsicsMatrix = intrinsicsMatrix;
        std::cout << "Generate rgb intrinsics matrix done" << '\n';
    }
}

// Parameters
float min_confidence = -1.6;
float voxel_size = 0.06;

void gp_estimation_floor (const pcl::PointCloud<PointT>::Ptr &cloud)
{
  // Floor segmentation
  ground_coeffs.resize(4);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::SACSegmentation<PointT> seg; // Create the segmentation object
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);
  seg.setInputCloud (cloud);
  seg.segment (*inliers, *coefficients);

  ground_coeffs[0] = coefficients->values[0];
  ground_coeffs[1] = coefficients->values[1];
  ground_coeffs[2] = coefficients->values[2];
  ground_coeffs[3] = coefficients->values[3];
}

void detection (const PointCloudT::Ptr& msg)
{
  // Get camera transform
  Eigen::Affine3d cameraTransform;

  string cameraFrame = msg->header.frame_id;
  try {
      cameraTransform = getCameraTransform(cameraFrame);
  }
  catch (tf::TransformException& ex) {
      ROS_WARN_THROTTLE(5.0, "Transform from %s to %s unavailable: %s",
      cameraFrame.c_str(), g_baseLinkFrame.c_str(), ex.what());
      return;
  }

  // Set up transform from base_link into our own coordinate frame
  Eigen::Affine3d baseLinkToDetectionFrame = Eigen::Affine3d(
          Eigen::AngleAxisd(0.5*M_PI, Eigen::Vector3d::UnitX()) *
          Eigen::AngleAxisd(0.5*M_PI, Eigen::Vector3d::UnitZ())
      );

  // Publish inverse transform on TF
  tf::Transform detectionFrameToBaseLinkTF;
  tf::transformEigenToTF(baseLinkToDetectionFrame.inverse(), detectionFrameToBaseLinkTF);
  g_transformBroadcaster->sendTransform(tf::StampedTransform(detectionFrameToBaseLinkTF,
    msg->header.stamp, g_baseLinkFrame, g_detectionFrame));

  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::fromROSMsg (*msg, *cloud);

  // cameraTransform is from optical frame to base_link
  // baseLinkToDetectionFrame is from base_link into an y-pointing-down reference frame
  Eigen::Affine3d cameraToDetectionFrame = baseLinkToDetectionFrame * cameraTransform;

  // Rotate pointcloud so it is upright
  pcl::transformPointCloud(*cloud, *cloud, cameraToDetectionFrame);
  cloud->header.frame_id = g_detectionFrame; // Update frame, since we are applying a transform (important for visualization)

  // Proofs generate ground plane whith floor segmentation
  gp_estimation_floor(cloud);
  std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;
  people_detector.setGround(ground_coeffs);                    // set floor coefficients

                              // Detections //
  people_detector.setVoxelSize(voxel_size);                        // set the voxel size
  // Set RGB camera intrinsics matrix from camera info topic
  {
    boost::lock_guard<boost::mutex> lock(g_intrinsicsMutex);
    if(g_intrinsicsMatrix == Eigen::Matrix3d::Zero()) {
      ROS_WARN_THROTTLE(5.0, "Intrinsics matrix not set, make sure that the camera_info topic is set correctly and being published!");
      return;
    }
    else {
      people_detector.setIntrinsics(g_intrinsicsMatrix.cast<float>());
    }
  }
  people_detector.setClassifier(person_classifier);                // set person classifier

  // Detections
  // Perform people detection on the new cloud:
  std::vector<pcl::people::PersonCluster<PointT> > clusters;   // vector containing persons clusters
  people_detector.setInputCloud(cloud);
  people_detector.compute(clusters);                           // perform people detection

  ground_coeffs = people_detector.getGround();                 // get updated floor coefficients

  // Draw cloud and people bounding boxes in the viewer:
  viewer.removeAllPointClouds();
  viewer.removeAllShapes();
  pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
  viewer.addPointCloud<PointT> (cloud, rgb);
  unsigned int k = 0;
  for(std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
    {
    if(it->getPersonConfidence() > min_confidence)             // draw only people with confidence above a threshold
    {
      // draw theoretical person bounding box in the PCL viewer:
      it->drawTBoundingBox(viewer, k);
      k++;

      pcl::PointIndices clusters_indices = it->getIndices();
      std::vector<int> indices_ = clusters_indices.indices;
      for (unsigned int i = 0; i < indices_.size(); i++)
      {
          std::cout << indices_[i] << " People Bounding Box" << std::endl; // Proofs, no correctly obtain coordinates
      }
    }
    }
  std::cout << k << " People found" << std::endl;
  viewer.spinOnce();
}

int main (int argc, char** argv)
{
  // Initialize Viewer
  viewer.initCameraParameters ();
  viewer.removeAllPointClouds();
  viewer.addCoordinateSystem(1.0);

  // Initialize ROS
  ros::init (argc, argv, "simulate_pcl_people_detection");
  ros::NodeHandle nh("");
  g_paramNodeHandle = boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle("~"));

  // Parameters
  string svm_filename = ros::package::getPath(ROS_PACKAGE_NAME) + "/utils/people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml";
  g_detectionFrame = "pcl_people_detector_frame";
  g_baseLinkFrame = "camera_link";
  string input_cloud_topic = "/camera/depth_registered/points";
  string camera_info_topic = "/camera/rgb/camera_info";

  g_paramNodeHandle->getParam("svm_file", svm_filename);
  g_paramNodeHandle->getParam("base_link_frame", g_baseLinkFrame);
  g_paramNodeHandle->getParam("detection_frame", g_detectionFrame);
  g_paramNodeHandle->getParam("input_topic", input_cloud_topic);
  g_paramNodeHandle->getParam("camera_info_topic", camera_info_topic);

  // Set up TF listener & broadcaster
  g_transformListener = boost::shared_ptr<tf::TransformListener>(new tf::TransformListener(nh, ros::Duration(1.0)));
  g_transformBroadcaster = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());

  person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

  cloud = pcl::PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>);
  // Create a ROS subscriber for the input point clou
  ros::Subscriber cis = nh.subscribe(camera_info_topic, 1, &cameraInfoCallback);
  ros::Subscriber pcs = nh.subscribe (input_cloud_topic, 1, &detection);

  // Spin
  ros::spin ();
  return 0;
}


#include <iostream>
#include <vector>

#include <boost/lexical_cast.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include "util.hpp"

#include "elias_fano.hpp"
#include "mapper.hpp"

#include "perftest_common.hpp"

struct monotone_generator
{
    monotone_generator(uint64_t m, uint8_t bits, unsigned int seed)
	: m_gen(seed)
        , m_bits(bits)
    {
	m_stack.push_back(state_t(0, m, 0));
    }

    uint64_t next()
    {
	uint64_t cur_word, cur_m;
	uint8_t cur_depth;

	assert(m_stack.size());
	boost::tie(cur_word, cur_m, cur_depth) = m_stack.back();
	m_stack.pop_back();

	while (cur_depth < m_bits) {
	    boost::random::uniform_int_distribution<uint64_t> dist(0, cur_m);
	    uint64_t left_m = dist(m_gen);
	    uint64_t right_m = cur_m - left_m;

	    // push left and right children, if present
	    if (right_m > 0) {
		m_stack.push_back(state_t(cur_word | (uint64_t(1) << (m_bits - cur_depth - 1)),
					  right_m, cur_depth + 1));
	    }
	    if (left_m > 0) {
		m_stack.push_back(state_t(cur_word, left_m, cur_depth + 1));

	    }

	    // pop next child in visit
	    boost::tie(cur_word, cur_m, cur_depth) = m_stack.back();
	    m_stack.pop_back();
	}

	if (cur_m > 1) {
	    // push back the current leaf, with cur_m decreased by one
	    m_stack.push_back(state_t(cur_word, cur_m - 1, cur_depth));
	}

	return cur_word;
    }

    bool done() const
    {
	return m_stack.empty();
    }

private:
    typedef boost::tuple<uint64_t /* cur_word */,
			 uint64_t /* cur_m */,
			 uint64_t /* cur_depth */> state_t;
    std::vector<state_t> m_stack;
    boost::random::mt19937 m_gen;
    uint8_t m_bits;
};

void ef_enumeration_benchmark(uint64_t m, uint8_t bits)
{
    succinct::elias_fano::elias_fano_builder bvb(uint64_t(1) << bits, m);
    monotone_generator mgen(m, bits, 37);
    for (size_t i = 0; i < m; ++i) {
	bvb.push_back(mgen.next());
    }
    assert(mgen.done());

    succinct::elias_fano ef(&bvb);
    succinct::mapper::size_tree_of(ef)->dump();

    double elapsed;
    uint64_t foo = 0;
    SUCCINCT_TIMEIT(elapsed) {
	succinct::elias_fano::select_enumerator it(ef, 0);
	for (size_t i = 0; i < m; ++i) {
	    foo ^= it.next();
	}
    }
    volatile uint64_t vfoo = foo;
    (void)vfoo; // silence warning

    std::cerr << "Elapsed: " << elapsed / 1000 << " msec\n"
	      << double(m) / elapsed << " Mcodes/s" << std::endl;
}

int main(int argc, char** argv)
{
    if (argc != 3) {
        std::cerr << "Invalid arguments" << std::endl;
        std::terminate();
    }
    size_t m = boost::lexical_cast<uint64_t>(argv[1]);
    uint8_t bits = uint8_t(boost::lexical_cast<int>(argv[2]));

    ef_enumeration_benchmark(m, bits);
}


#include "modules/neuralNetwork/layer/layer.hpp"
#include "modules/neuralNetwork/neuralNetwork.hpp"

#ifdef _KORALI_USE_CUDNN
  #include "auxiliar/cudaUtils.hpp"
#endif

#ifdef _KORALI_USE_ONEDNN
  #include "auxiliar/dnnUtils.hpp"
using namespace dnnl;
#endif

#include <Eigen/Dense>
using namespace Eigen;

namespace korali
{
namespace neuralNetwork
{
;

void Layer::createForwardPipeline()
{
  // Obtaining batch size
  ssize_t N = _batchSize;

  // Checking Layer sizes
  if (_outputChannels == 0) KORALI_LOG_ERROR("Node count for layer (%lu) should be larger than zero.\n", _index);
  ssize_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    _outputValues = (float *)malloc(N * OC * sizeof(float));
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Setting propagation kind
    _propKind = _nn->_mode == "Training" ? prop_kind::forward_training : prop_kind::forward_inference;

    // Creating layer's data memory storage
    const memory::dims layerDims = {N, OC};
    auto dataMemDesc = memory::desc(layerDims, memory::data_type::f32, memory::format_tag::nc);

    // Creating activation layer memory
    _outputMem.resize(_nn->_timestepCount);
    for (size_t t = 0; t < _nn->_timestepCount; t++)
      _outputMem[t] = memory(dataMemDesc, _nn->_dnnlEngine);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    // Setting propagation mode
    _forwardMode = _nn->_mode == "Training" ? CUDNN_FWD_MODE_TRAINING : CUDNN_FWD_MODE_INFERENCE;

    cudnnErrCheck(cudnnCreateTensorDescriptor(&_outputTensorDesc));
    cudnnErrCheck(cudnnSetTensor4dDescriptor(_outputTensorDesc, CUDNN_TENSOR_NCHW, CUDNN_DATA_FLOAT, N, OC, 1, 1));

    _outputTensor.resize(_nn->_timestepCount);
    for (size_t t = 0; t < _nn->_timestepCount; t++)
      cudaErrCheck(cudaMalloc((void **)&_outputTensor[t], N * OC * sizeof(float)));
  }
#endif
}

std::vector<std::vector<float>> Layer::getOutput()
{
  size_t N = _batchSize;
  size_t OC = _outputChannels;

  std::vector<float> outputVals(N * OC);

  // Copying previous layer's output to this layer's output
  if (_nn->_engine == "Korali")
  {
    memcpy(outputVals.data(), _outputValues, N * OC * sizeof(float));
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    int t = _nn->_timestepCount - 1;
    read_from_dnnl_memory(outputVals.data(), _outputMem[t]);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    int t = _nn->_timestepCount - 1;
    cudaErrCheck(cudaMemcpy(outputVals.data(), _outputTensor[t], N * OC * sizeof(float), cudaMemcpyDeviceToHost));
  }
#endif

  std::vector<std::vector<float>> outputVector(N);
  for (size_t i = 0; i < N; i++) outputVector[i].resize(OC);
  for (size_t i = 0; i < N; i++)
    for (size_t j = 0; j < OC; j++)
      outputVector[i][j] = outputVals[i * OC + j];

  return outputVector;
}

void Layer::createBackwardPipeline()
{
  if (_nn->_mode == "Inference")
    KORALI_LOG_ERROR("Requesting layer's backward pipeline creation but NN was configured for inference only.\n");

  /*********************************************************************************
   *  Initializing memory objects and primitives for BACKWARD propagation
   *********************************************************************************/

  size_t N = _batchSize;
  size_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    _outputGradient = (float *)malloc(N * OC * sizeof(float));
  }

// Creating backward propagation primitives for activation functions
#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Creating data-related gradient memory
    _outputGradientMem.resize(_nn->_timestepCount);
    for (size_t t = 0; t < _nn->_timestepCount; t++)
      _outputGradientMem[t] = memory(_outputMem[t].get_desc(), _nn->_dnnlEngine);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    _outputGradientTensor.resize(_nn->_timestepCount);
    for (size_t i = 0; i < _nn->_timestepCount; i++)
      cudaErrCheck(cudaMalloc((void **)&_outputGradientTensor[i], N * OC * sizeof(float)));
  }
#endif
}

std::vector<float> Layer::generateInitialHyperparameters()
{
  std::vector<float> hyperparameters;
  return hyperparameters;
}

void Layer::createHyperparameterMemory()
{
  _hyperparameterCount = 0;
}

void Layer::backwardHyperparameters(const size_t t)
{
  if (_nn->_mode == "Inference")
    KORALI_LOG_ERROR("Requesting Layer hyperparameter gradient propagation but NN was configured for inference only.\n");
};

void Layer::setConfiguration(knlohmann::json& js)
{
 if (isDefined(js, "Results"))  eraseValue(js, "Results");

 if (isDefined(js, "Output Channels"))
 {
 try { _outputChannels = js["Output Channels"].get<size_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ layer ] \n + Key:    ['Output Channels']\n%s", e.what()); }
   eraseValue(js, "Output Channels");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Output Channels'] required by layer.\n");

 if (isDefined(js, "Weight Scaling"))
 {
 try { _weightScaling = js["Weight Scaling"].get<float>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ layer ] \n + Key:    ['Weight Scaling']\n%s", e.what()); }
   eraseValue(js, "Weight Scaling");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Weight Scaling'] required by layer.\n");

 Module::setConfiguration(js);
 _type = "layer";
 if(isDefined(js, "Type")) eraseValue(js, "Type");
 if(isEmpty(js) == false) KORALI_LOG_ERROR(" + Unrecognized settings for Korali module: layer: \n%s\n", js.dump(2).c_str());
}

void Layer::getConfiguration(knlohmann::json& js)
{

 js["Type"] = _type;
   js["Output Channels"] = _outputChannels;
   js["Weight Scaling"] = _weightScaling;
 Module::getConfiguration(js);
}

void Layer::applyModuleDefaults(knlohmann::json& js)
{

 std::string defaultString = "{\"Output Channels\": 0, \"Weight Scaling\": 1.0}";
 knlohmann::json defaultJs = knlohmann::json::parse(defaultString);
 mergeJson(js, defaultJs);
 Module::applyModuleDefaults(js);
}

void Layer::applyVariableDefaults()
{

 Module::applyVariableDefaults();
}

;

} //neuralNetwork
} //korali
;


#include <tuple>
#include <Eigen/Core>
#include <iostream>

#include <npe.h>

npe_function(dense_like_1)
npe_arg(a, sparse_double, sparse_float)
npe_arg(b, npe_dense_like(a))
npe_begin_code()

npe_Matrix_b ret1 = a * b;

return npe::move(ret1);
npe_end_code()

npe_function(dense_like_2)
npe_arg(a, sparse_double, sparse_float)
npe_arg(b, npe_dense_like(a))
npe_arg(c, npe_dense_like(a))
npe_begin_code()

npe_Matrix_b ret1 = b;

return npe::move(ret1);
npe_end_code()

npe_function(dense_like_3)
npe_arg(a, sparse_double, sparse_float)
npe_arg(b, npe_dense_like(a))
npe_arg(c, npe_dense_like(b))
npe_begin_code()

npe_Matrix_b ret1 = b;

return npe::move(ret1);
npe_end_code()

npe_function(dense_like_4)
npe_arg(a, dense_double, dense_float)
npe_arg(b, npe_dense_like(a))
npe_arg(c, npe_dense_like(b))
npe_begin_code()

npe_Matrix_b ret1 = b;

return npe::move(ret1);
npe_end_code()

npe_function(dense_like_5)
npe_arg(a, sparse_double, sparse_float)
npe_arg(b, npe_dense_like(a))
npe_arg(c, npe_sparse_like(b))
npe_arg(d, npe_matches(c))
npe_arg(e, npe_matches(b))
npe_arg(f, npe_dense_like(b))
npe_arg(g, npe_matches(b))
npe_arg(h, npe_matches(a))
npe_begin_code()

npe_Matrix_b ret1 = a * b;

return npe::move(ret1);
npe_end_code()