// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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 BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_REVERSE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_REVERSE_HPP

#include <boost/geometry/algorithms/reverse.hpp>

#include <boost/geometry/extensions/algebra/algorithms/detail.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// This is experimental implementation of reverse() which negates vectors
// and inverses rotations. It doesn't work for them as for Geometries.

template <typename Vector>
struct reverse<Vector, vector_tag>
{
    static inline void apply(Vector & v)
    {
        detail::algebra::neg<0, dimension<Vector>::value>(v);
    }
};

template <typename R>
struct reverse<R, rotation_quaternion_tag>
{
    static inline void apply(R & r)
    {
        detail::algebra::neg<1, 4>(r);
    }
};

template <typename R>
struct reverse<R, rotation_matrix_tag>
{
    static inline void apply(R & r)
    {
        detail::algebra::matrix_transpose<
            R, 0, 0, dimension<R>::value
        >::apply(r);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_CLEAR_HPP


#include <cstdio>
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <cfloat>
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>
#include <boost/filesystem.hpp>
#include <json/json.h>
#include <H5Cpp.h>

// OpenMP
#include <omp.h>
#define N_THREADS 16

#include "box_ray/vector3.h"
#include "box_ray/ray.h"
#include "box_ray/box.h"

/** \brief Class representing a point cloud in 3D. */
class PointCloud {
public:
  /** \brief Constructor. */
  PointCloud() {

  }

  /** \brief Constructor.
   * \param[in] points points
   */
  PointCloud(const std::vector<Eigen::Vector3f> &points) {
    this->points = points;
    this->colors = std::vector<Eigen::Vector3i>(this->points.size(), Eigen::Vector3i(0, 0, 0));
  }

  /** \brief Copy constructor.
   * \param[in] point_cloud point cloud to copy
   */
  PointCloud(const PointCloud &point_cloud) {
    this->points.clear();
    this->colors.clear();

    for (unsigned int i = 0; i < point_cloud.points.size(); i++) {
      this->points.push_back(point_cloud.points[i]);
      this->colors.push_back(point_cloud.colors[i]);
    }
  }

  /** \brief Destructor. */
  ~PointCloud() {

  }

  /** \brief Read point cloud from binary file.
   * \param[in] filepath path to binary file
   * \param[out] point_cloud read point cloud
   */
  static bool from_bin(const std::string &filepath, PointCloud &point_cloud) {

    // start with clear point cloud
    point_cloud.points.clear();
    point_cloud.colors.clear();

    // allocate 4 MB buffer (only ~130*4*4 KB are needed)
    int buffer_size = 1000000;
    float* data = new float[buffer_size];

    // important to start with clean memory to avoid reading points
    // from previous files!
    for (int i = 0; i < buffer_size; i++) {
      data[i] = 0;
    }

    // pointers
    float* px = data + 0;
    float* py = data + 1;
    float* pz = data + 2;

    // load point cloud
    std::ifstream* in = new std::ifstream(filepath, std::ifstream::in | std::ifstream::binary);

    if (!static_cast<bool>(*in)) {
      return false;
    }

    // https://stackoverflow.com/questions/19614581/reading-floating-numbers-from-bin-file-continuosly-and-outputting-in-console-win
    in->seekg(0, std::ios::end);
    int n_points = in->tellg()/sizeof(float)/4; // 4 floats per point
    //std::cout << "[Data] " << n_points << " points" << std::endl;
    in->seekg(0, std::ios::beg);

    in->read(reinterpret_cast<char*>(data), n_points*sizeof(float));

    for (int i = 0; i < n_points; i++) {
      Eigen::Vector3f point;
      // bounding boxes are in camera coordinate system
      // x = right, y = down, z = forward
      // but velodyne point cloud
      // is in velodyne system
      // x = forward, y = left, z = up
      point(0) = -*py;
      point(1) = *pz; // note the flip!
      point(2) = *px;

      if (std::abs(point(0)) > 1e-8 || std::abs(point(1)) > 1e-8 || std::abs(point(2)) > 1e-8) {
        if (std::abs(point(0)) < 1e8 && std::abs(point(1)) < 1e8 && std::abs(point(2)) < 1e8) {
          point_cloud.add_point(point);
        }
      }

      px += 4;
      py += 4;
      pz += 4;
    }

    in->close();
    delete in;
    delete[] data;

    return true;
  }

  /** \brief Add a point to the point cloud.
   * \param[in] point point to add
   */
  void add_point(const Eigen::Vector3f &point) {
    this->points.push_back(point);
    this->colors.push_back(Eigen::Vector3i::Zero());
  }

  /** \brief Add a colored point to the point cloud.
   * \param[in] point point to add
   * \param[in] color color of the point
   */
  void add_point(const Eigen::Vector3f &point, const Eigen::Vector3i &color) {
    this->points.push_back(point);
    this->colors.push_back(color);
  }

  /** \brief Get number of points.
   * \return number of points
   */
  unsigned int num_points() const {
    return this->points.size();
  }

  /** \brief Write point cloud to txt file.
   * \param[in] filepath path to file
   * \return success
   */
  bool to_txt(const std::string &filepath) {
    std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
    if (!static_cast<bool>(*out)) {
      return false;
    }

    (*out) << this->points.size() << std::endl;
    for (unsigned int i = 0; i < this->points.size(); i++) {
     (*out) << this->points[i](0) << " " << this->points[i](1) << " " << this->points[i](2) << std::endl;
    }

    out->close();
    delete out;

    return true;
  }

  /** \brief Write point cloud to txt file.
   * \param[in] filepath path to file
   * \return success
   */
  bool to_ply(const std::string &filepath) {
    std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
    if (!static_cast<bool>(*out)) {
      return false;
    }

    if (this->points.size() != this->colors.size()) {
      return false;
    }

    (*out) << "ply" << std::endl;
    (*out) << "format ascii 1.0" << std::endl;
    (*out) << "element vertex " << this->points.size() << std::endl;
    (*out) << "property float32 x" << std::endl;
    (*out) << "property float32 y" << std::endl;
    (*out) << "property float32 z" << std::endl;
    (*out) << "property uchar red" << std::endl;
    (*out) << "property uchar green" << std::endl;
    (*out) << "property uchar blue" << std::endl;
    (*out) << "end_header" << std::endl;

    for (unsigned int i = 0; i < this->points.size(); i++) {
      //out << this->points[i](0) << " " << this->points[i](1) << " " << this->points[i](2) << std::endl;
      (*out) << this->points[i](0) << " " << this->points[i](1) << " " << this->points[i](2) << " "
        << this->colors[i](0) << " " << this->colors[i](1) << " " << this->colors[i](2) << std::endl;
    }

    out->close();
    delete out;

    return true;
  }

  /** \brief Scale the point cloud.
   * \param[in] scale scales for each axis
   */
  void scale(const Eigen::Vector3f &scale) {
    for (unsigned int i = 0; i < this->points.size(); i++) {
      for (int d = 0; d < 3; d++) {
        this->points[i](d) *= scale(d);
      }
    }
  }

  /** \brief Scale the point cloud.
   * \param[in] scale overlal scale for all axes
   */
  void scale(float scale) {
    this->scale(Eigen::Vector3f(scale, scale, scale));
  }

  /** \brief Rotate the point cloud around the origin.
   * \param[in] rotation rotation matrix
   */
  void rotate(const Eigen::Matrix3f &rotation) {
    for (unsigned int i = 0; i < this->points.size(); i++) {
      this->points[i] = rotation*this->points[i];
    }
  }

  /** \brief Translate the point cloud.
   * \brief translation translation vector
   */
  void translate(const Eigen::Vector3f &translation) {
    for (unsigned int i = 0; i < this->points.size(); i++) {
      this->points[i] += translation;
    }
  }

private:
  /** \brief The points of the point cloud. */
  std::vector<Eigen::Vector3f> points;
  /** \brief Colors of the points. */
  std::vector<Eigen::Vector3i> colors;

};

/** \brief Read a Hdf5 file into an Eigen tensor.
 * \param[in] filepath path to file
 * \param[out] dense Eigen tensor
 * \return success
 */
template<int RANK>
bool read_hdf5(const std::string filepath, Eigen::Tensor<float, RANK, Eigen::RowMajor>& dense) {

  try {
    H5::H5File file(filepath, H5F_ACC_RDONLY);
    H5::DataSet dataset = file.openDataSet("tensor");

    /*
     * Get filespace for rank and dimension
     */
    H5::DataSpace filespace = dataset.getSpace();

    /*
     * Get number of dimensions in the file dataspace
     */
    size_t rank = filespace.getSimpleExtentNdims();

    if (rank != RANK) {
      std::cout << "[Error] invalid rank read: " << rank << std::endl;
      exit(1);
    }

    /*
     * Get and print the dimension sizes of the file dataspace
     */
    hsize_t dimsf[rank];
    filespace.getSimpleExtentDims(dimsf);

    std::cout << "[Data] HDF5 size: ";
    for (int i = 0; i < RANK; ++i) {
      std::cout << dimsf[i] << " ";
    }
    std::cout << std::endl;

    /*
     * Define the memory space to read dataset.
     */
    std::cout << "[Data] HDF5 allocating ..." << std::endl;
    H5::DataSpace mspace(rank, dimsf);

    //size_t buffer_size = 1;
    //for (int i = 0; i < RANK; ++i) {
    //  buffer_size *= dimsf[i];
    //}

    std::cout << "[Data] HDF5 casting ..." << std::endl;
    float* buffer = static_cast<float*>(dense.data());
    std::cout << "[Data] HDF5 reading ..." << std::endl;
    dataset.read(buffer, H5::PredType::NATIVE_FLOAT, mspace, filespace);

    //for (int i = 0; i < buffer_size; ++i) {
    //  std::cout << buffer[i] << std::endl;
    //}
  }

  // catch failure caused by the H5File operations
  catch(H5::FileIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSet operations
  catch(H5::DataSetIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSpace operations
  catch(H5::DataSpaceIException error) {
    error.printError();
    return false;
  }
}

/** \brief Write the given set of volumes to h5 file.
 * \param[in] filepath h5 file to write
 * \param[in] dense volume data
 */
template<int RANK>
bool write_hdf5(const std::string filepath, Eigen::Tensor<float, RANK, Eigen::RowMajor>& dense) {

  try {

    /*
     * Turn off the auto-printing when failure occurs so that we can
     * handle the errors appropriately
     */
    H5::Exception::dontPrint();

    /*
     * Create a new file using H5F_ACC_TRUNC access,
     * default file creation properties, and default file
     * access properties.
     */
    H5::H5File file(filepath, H5F_ACC_TRUNC);

    /*
     * Define the size of the array and create the data space for fixed
     * size dataset.
     */
    hsize_t dimsf[RANK];
    for (int i = 0; i < RANK; ++i) {
      dimsf[i] = dense.dimension(i);
    }

    H5::DataSpace dataspace(RANK, dimsf);

    /*
     * Define datatype for the data in the file.
     * We will store little endian INT numbers.
     */
    H5::IntType datatype(H5::PredType::NATIVE_FLOAT);
    datatype.setOrder(H5T_ORDER_LE);

    /*
     * Create a new dataset within the file using defined dataspace and
     * datatype and default dataset creation properties.
     */
    H5::DataSet dataset = file.createDataSet("tensor", datatype, dataspace);

    /*
     * Write the data to the dataset using default memory space, file
     * space, and transfer properties.
     */
    float* data = static_cast<float*>(dense.data());
    dataset.write(data, H5::PredType::NATIVE_FLOAT);
  }  // end of try block

  // catch failure caused by the H5File operations
  catch(H5::FileIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSet operations
  catch(H5::DataSetIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSpace operations
  catch(H5::DataSpaceIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSpace operations
  catch(H5::DataTypeIException error) {
    error.printError();
    return false;
  }

  return true;
}

/** \brief Small helper for safely retrieving values from Json root element. */
class JSON {
public:

  /** \brief Constructor.
   * \param[in] root JSON root element
   */
  JSON(Json::Value root) {
    this->root = root;
  }

  /** \brief Get the value of the given key as int.
   * \param[in] key key to retrieve
   * \return value
   */
  int get_int(std::string key) {
    if (!this->root.isMember(key)) {
      std::cout << "[Error] key " << key << " not found" << std::endl;
      exit(1);
    }

    return this->root[key].asInt();
  }

  /** \brief Get the value of the given key as float.
   * \param[in] key key to retrieve
   * \return value
   */
  float get_float(std::string key) {
    if (!this->root.isMember(key)) {
      std::cout << "[Error] key " << key << " not found" << std::endl;
      exit(1);
    }

    return this->root[key].asFloat();
  }

  /** \brief Get the value of the given key as string.
   * \param[in] key key to retrieve
   * \return value
   */
  std::string get_string(std::string key) {
    if (!this->root.isMember(key)) {
      std::cout << "[Error] key " << key << " not found" << std::endl;
      exit(1);
    }

    return this->root[key].asString();
  }

private:

  /** \brief Root element. */
  Json::Value root;
};

/** \brief Get the rotation matrix by reading the y-angle from angles.
 * \param[in] angles tensor holding the y-angles
 * \param[in] n which angle to take
 * \param[out] R rotation matrix
 */
void get_rotation_matrix(const Eigen::Tensor<float, 3, Eigen::RowMajor> &angles, const int k, const int n, Eigen::Matrix3f &R) {
  R = Eigen::Matrix3f::Zero();
  float radians_x = angles(n, k, 0);///180.f*M_PI;
  float radians_y = angles(n, k, 1);
  float radians_z = angles(n, k, 2);

  Eigen::Matrix3f R_x = Eigen::Matrix3f::Zero();
  R_x(0, 0) = 1;
  R_x(1, 1) = std::cos(radians_x); R_x(1, 2) = -std::sin(radians_x);
  R_x(2, 1) = std::sin(radians_x); R_x(2, 2) = std::cos(radians_x);

  Eigen::Matrix3f R_y = Eigen::Matrix3f::Zero();
  R_y(0, 0) = std::cos(radians_y); R_y(0, 2) = std::sin(radians_y);
  R_y(1, 1) = 1;
  R_y(2, 0) = - std::sin(radians_y); R_y(2, 2) = std::cos(radians_y);

  Eigen::Matrix3f R_z = Eigen::Matrix3f::Zero();
  R_z(0, 0) = std::cos(radians_z); R_z(0, 1) = -std::sin(radians_z);
  R_z(1, 0) = std::sin(radians_z); R_z(1, 1) = std::cos(radians_z);
  R_z(2, 2) = 1;

  R = R_z*(R_y*R_x);
}

/** \brief Simple struct representing arbitrary boxes. */
struct Box {
  /** \brief Minimum coordinates. */
  Eigen::Vector3f min;
  /** \brief Maximum coordinates. */
  Eigen::Vector3f max;

  /** \brief Constructor.
   */
  Box() {
    this->min = Eigen::Vector3f::Zero();
    this->max = Eigen::Vector3f::Zero();
  }

  /** \brief Constructor; construct a box from minimum and maximum coordinates.
   * \param[in] min minimum coordinates
   * \param[in] max maximum coordinates
   */
  Box(const Eigen::Vector3f &min, const Eigen::Vector3f &max) {
    this->min = min;
    this->max = max;
  }

  /** \brief Test box ray intersection.
   * \param[in] ray ray to intersect with
   * \return intersects
   */
  bool intersect(const Eigen::Vector3f origin, const Eigen::Vector3f &dest) {
    for (int d = 0; d < 3; d++) {
      assert(this->min(d) < this->max(d));
    }

    // convert to used data structure
    Eigen::Vector3f direction = dest - origin;
    box_ray_intersection::Vector3 _o(origin(0), origin(1), origin(2));
    box_ray_intersection::Vector3 _d(direction(0), direction(1), direction(2));
    box_ray_intersection::Ray _ray(_o, _d);
    box_ray_intersection::Vector3 _min(this->min(0), this->min(1), this->min(2));
    box_ray_intersection::Vector3 _max(this->max(0), this->max(1), this->max(2));
    box_ray_intersection::Box _box(_min, _max);

    // interval for valid its is [0,1] as the ray will
    // be the full-length vector to an observed point
    return _box.intersect(_ray, 0, 0.99);
  }

  /** \brief Check if a point is inside the box.
   * \param[in] point point to check
   * \return contained
   */
  bool contains(const Eigen::Vector3f &point) {
    bool contains = true;

    for (int d = 0; d < 3; d++) {
      if (point(d) < this->min(d) || point(d) > this->max(d)) {
        contains = false;
        break;
      }
    }

    return contains;
  }

  /** \brief Scale the box.
   * \param[in] scale scale per axis
   */
  void scale(const Eigen::Vector3f &scale) {
    for (int d = 0; d < 3; d++) {
      min(d) *= scale(d);
      max(d) *= scale(d);
    }
  }

  /** \brief Scale the box.
   * \param[in] scale scale over all axes
   */
  void scale(float scale) {
    this->scale(Eigen::Vector3f(scale, scale, scale));
  }

  /** \brief Scale the box.
   * \param[in] translation translation per axis
   */
  void translate(const Eigen::Vector3f &translation) {
    for (int d = 0; d < 3; d++) {
      min(d) += translation(d);
      max(d) += translation(d);
    }
  }
};

/** \brief Voxelization to emulate real KITTI setting.
 * \param[in] depth_maps depth maps to calculate point clouds
 * \param[in] angles angles used for rendering
 * \param[in] max_depth_value maximum depth value in depthmaps
 * \param[in] K camera parameters from rendering
 * \param[in] mesh_center center of mesh used for rendering
 * \param[out] input volumes containing voxelized point clouds
 * \param[out] space volumes containing voxelized space
 */
void voxelize_realistic_UNSCALED(int k, const Eigen::Tensor<float, 4, Eigen::RowMajor> &depth_maps,
    const Eigen::Tensor<float, 3, Eigen::RowMajor> &angles, float max_depth_value,
    const Eigen::Matrix3f& K, const Eigen::Vector3f& mesh_center,
    Eigen::Tensor<float, 5, Eigen::RowMajor>& input, Eigen::Tensor<float, 5, Eigen::RowMajor>& input_sdf,
    Eigen::Tensor<float, 5, Eigen::RowMajor>& space,
    const std::string txt_directory, float padding_factor, float lambda = 1, float ignore = 0) {

  const int N = input.dimension(0);
  const int height = input.dimension(2);
  const int width = input.dimension(3);
  const int depth = input.dimension(4);

  int image_height = depth_maps.dimension(2);
  int image_width = depth_maps.dimension(3);

  Eigen::Matrix3f Kinv = K.inverse();

#if defined(_OPENMP)
  omp_set_num_threads(N_THREADS);
#endif

  #pragma omp parallel
  {
    #pragma omp for
    for (int n = 0; n < N; n++) {
      std::cout << "[Data] realisticly voxelizing " << n << std::endl;

      Eigen::Matrix3f R;
      get_rotation_matrix(angles, k, n, R);
      Eigen::Matrix3f Rinv = R.transpose();

      int scale = std::max(height, std::max(width, depth));
      std::vector<Eigen::Vector3f> points;

      // get all points corresponding to the depth map
      for (int h = 0; h < image_height; h++) {
        for (int w = 0; w < image_width; w++) {
          if (depth_maps(n, k, h, w) < max_depth_value) {
            Eigen::Vector3f pixel;
            // Assume points in the middle of pixels.
            pixel(0) = w;// + 0.5f;
            pixel(1) = h;// + 0.5f;
            pixel(2) = 1;

            Eigen::Vector3f point = Kinv*pixel;
            point(2) = depth_maps(n, k, h, w);

            float r = std::rand() / (float) RAND_MAX;
            if (r < ignore) {
              point(2) = max_depth_value;
            }
            else if (lambda > 0) {
              r = std::rand() / (float) RAND_MAX;
              point(2) -= std::log(r)/lambda;
            }

            point(0) *= point(2); // !
            point(1) *= point(2); // !

            point -= mesh_center;

            // Now point cloud is centered at (0,0,0) and
            // can be rotated.
            point = Rinv*point;

            // Now scale the points.
            point(0) *= scale;
            point(1) *= scale;
            point(2) *= scale;

            // And translate the point.
            point += Eigen::Vector3f(width/2., height/2., depth/2.);

            points.push_back(point);
          }
        }
      }

      PointCloud point_cloud(points);

      std::string txt_file = txt_directory + "/" + std::to_string(n) + ".txt";
      std::cout << "[Data] writing " << txt_file << std::endl;
      bool success = point_cloud.to_txt(txt_file);

      if (!success) {
        std::cout << "[Error] could not write " << txt_file << std::endl;
        exit(1);
      }

      // This will be the camera origin/center in the new coordinate system.
      Eigen::Vector3f origin(0, 0, 0);
      origin -= mesh_center;
      origin = Rinv*origin;
      origin(0) *= scale;
      origin(1) *= scale;
      origin(2) *= scale;
      origin += Eigen::Vector3f(width/2., height/2., depth/2.);

      for (int h = 0; h < height; h++) {
        for (int w = 0; w < width; w++) {
          for (int d = 0; d < depth; d++) {
            input_sdf(n, k, h, w, d) = FLT_MAX;

            // the box corresponding to this voxel
            Eigen::Vector3f min(static_cast<float>(w), static_cast<float>(h), static_cast<float>(d));
            Eigen::Vector3f max(static_cast<float>(w + 1), static_cast<float>(h + 1), static_cast<float>(d + 1));
            Box voxel_box(min, max);

            Eigen::Vector3f center(w + 0.5f, h + 0.5f, d + 0.5f);

            // no rotation as the intersection does not work
            // and the point cloud is assumed to be rotated

            // go over all "rays"
            for (unsigned int i = 0; i < points.size(); i++) {
              float distance = (center - points[i]).norm();
              if (distance < input_sdf(n, k, h, w, d)) {
                input_sdf(n, k, h, w, d) = distance;
              }

              if (voxel_box.contains(points[i])) {
                input(n, k, h, w, d) = 1;
              }
              else if (voxel_box.intersect(origin, points[i])) {
                space(n, k, h, w, d) = 1;
              }
            }
          }
        }
      }
    }
  }
}

/** \brief Read all files in a directory matching the given extension.
 * \param[in] directory path to directory
 * \param[out] files read file paths
 * \param[in] extension extension to filter for
 */
void read_directory(const std::string directory, std::map<int, std::string>& files, const std::string extension = ".off") {

  boost::filesystem::path dir(directory);
  boost::filesystem::directory_iterator end;

  files.clear();
  for (boost::filesystem::directory_iterator it(dir); it != end; ++it) {
    if (it->path().extension().string() == extension) {
      int number = std::stoi(it->path().filename().string());
      files.insert(std::pair<int, std::string>(number, it->path().string()));
    }
  }
}

/** \brief Main entrance point of script.
 * Expects one argument, the path to the config file.
 */
int main(int argc, char** argv) {
  if (argc != 2) {
    std::cout << "[Error] Usage: voxelize_cuboids config.json" << std::endl;
    exit(1);
  }

  Json::Value root;
  Json::Reader reader;

  std::cout << "[Data] reading " << argv[1] << std::endl;
  std::ifstream json_file(argv[1], std::ifstream::in | std::ifstream::binary);
  reader.parse(json_file, root, false);
  JSON json(root);

  // Unsafe!
  int height = json.get_int("height");
  int width = json.get_int("width");
  int depth = json.get_int("depth");

  int image_height = json.get_int("image_height");
  int image_width = json.get_int("image_width");

  std::string suffix = json.get_string("suffix");
  int multiplier = json.get_int("multiplier");
  int n_observations = json.get_int("n_observations");

  std::string depth_file = json.get_string("depth_file") + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth) + suffix + ".h5";

  if (!boost::filesystem::exists(boost::filesystem::path(depth_file))) {
    std::cout << "[Error] depth file " << depth_file << " not found" << std::endl;
    exit(1);
  }

  std::string angles_file = json.get_string("render_orientation_file") + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth) + suffix + ".h5";

  if (!boost::filesystem::exists(boost::filesystem::path(angles_file))) {
    std::cout << "[Error] angles file " << angles_file << " not found" << std::endl;
    exit(1);
  }

  std::string off_directory = json.get_string("multiplied_directory");
  if (off_directory.empty()) {
    std::cout << "[Error] Read invalid off directory" << std::endl;
    exit(1);
  }

  boost::filesystem::path config_file(argv[1]);
  std::string set = config_file.stem().string();
  off_directory = off_directory + "/" + set + "/";

  std::string txt_directory = json.get_string("txt_gt_dir");
  if (txt_directory.empty()) {
    std::cout << "[Error] Read invalid ply gt directory" << std::endl;
    exit(1);
  }

  txt_directory = txt_directory + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth) + suffix;

  if (!boost::filesystem::is_directory(boost::filesystem::path(txt_directory))) {
    boost::filesystem::create_directories(boost::filesystem::path(txt_directory));
  }

  std::cout << "[Data] reading " << off_directory << std::endl;
  std::map<int, std::string> files;
  read_directory(off_directory, files);

  int N = files.size();
  std::cout << "[Data] setting up depth_maps: " << N << " " << n_observations << " " << image_height << " " << image_width << std::endl;
  Eigen::Tensor<float, 4, Eigen::RowMajor> depth_maps(N, n_observations, image_height, image_width);
  depth_maps.setZero();

  std::cout << "[Data] reading " << depth_file << std::endl;
  read_hdf5<4>(depth_file, depth_maps);

  std::cout << "[Data] setting up angles: " << N << " " << n_observations << " " << 3 << std::endl;
  Eigen::Tensor<float, 3, Eigen::RowMajor> angles(N, n_observations, 3);
  angles.setZero();

  std::cout << "[Data] reading " << angles_file << std::endl;
  read_hdf5<3>(angles_file, angles);

  std::cout << "[Data] setting up K " << angles_file << std::endl;
  float max_depth_value = json.get_float("max_depth_value");
  Eigen::Matrix3f K = Eigen::Matrix3f::Zero();
  K(0, 0) = json.get_float("focal_length_x");
  K(0, 2) = json.get_float("principal_point_x");
  K(1, 1) = json.get_float("focal_length_y");
  K(1, 2) = json.get_float("principal_point_y");
  K(2, 2) = 1;

  std::cout << "[Data] setting up mesh_center " << angles_file << std::endl;
  Eigen::Vector3f mesh_center;
  mesh_center(0) = json.get_float("mesh_center_x");
  mesh_center(1) = json.get_float("mesh_center_y");
  mesh_center(2) = json.get_float("mesh_center_z");

  std::cout << "[Data] setting up real_input " << angles_file << std::endl;
  Eigen::Tensor<float, 5, Eigen::RowMajor> real_input(N, n_observations, height, width, depth);
  real_input.setZero();
  std::cout << "[Data] setting up real_input_sdf " << angles_file << std::endl;
  Eigen::Tensor<float, 5, Eigen::RowMajor> real_input_sdf(N, n_observations, height, width, depth);
  real_input_sdf.setZero();
  std::cout << "[Data] setting up real_space " << angles_file << std::endl;
  Eigen::Tensor<float, 5, Eigen::RowMajor> real_space(N, n_observations, height, width, depth);
  real_space.setZero();

  float padding_factor = 1 + json.get_float("padding");
  float lambda = json.get_float("lambda");
  float ignore = json.get_float("ignore");

  for (int k = 0; k < n_observations; k++) {
    std::string k_txt_directory = txt_directory + "/" + std::to_string(k);
    if (!boost::filesystem::is_directory(boost::filesystem::path(k_txt_directory))) {
      boost::filesystem::create_directories(boost::filesystem::path(k_txt_directory));
    }
    voxelize_realistic_UNSCALED(k, depth_maps, angles, max_depth_value, K, mesh_center, real_input, real_input_sdf, real_space,
      k_txt_directory, padding_factor, lambda, ignore);
  }

  std::string real_input_file = json.get_string("input_file") + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth) + suffix + ".h5";
  bool success = write_hdf5(real_input_file, real_input);

  if (success) {
    std::cout <<"[Data] wrote " << real_input_file << std::endl;
  }
  else {
    std::cout << "[Error] error writing " << real_input_file << std::endl;
  }

  std::string real_input_sdf_file = json.get_string("input_sdf_file") + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth) + suffix + ".h5";
  success = write_hdf5(real_input_sdf_file, real_input_sdf);

  if (success) {
    std::cout <<"[Data] wrote " << real_input_sdf_file << std::endl;
  }
  else {
    std::cout << "[Error] error writing " << real_input_sdf_file << std::endl;
  }

  std::string real_space_file = json.get_string("space_file") + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth) + suffix + ".h5";
  success = write_hdf5(real_space_file, real_space);

  if (success) {
    std::cout <<"[Data] wrote " << real_space_file << std::endl;
  }
  else {
    std::cout << "[Error] error writing " << real_space_file << std::endl;
  }

  exit(0);
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_AUXILIARY_LATRS_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_AUXILIARY_LATRS_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/blas/detail/default_order.hpp>
#include <boost/numeric/bindings/diag_tag.hpp>
#include <boost/numeric/bindings/is_complex.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/is_real.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/trans_tag.hpp>
#include <boost/numeric/bindings/uplo_tag.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/utility/enable_if.hpp>

//
// The LAPACK-backend for latrs is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
template< typename Trans, typename Diag >
inline std::ptrdiff_t latrs( const char uplo, const Trans, const Diag,
        const char normin, const fortran_int_t n, const float* a,
        const fortran_int_t lda, float* x, float& scale, float* cnorm ) {
    fortran_int_t info(0);
    LAPACK_SLATRS( &uplo, &lapack_option< Trans >::value, &lapack_option<
            Diag >::value, &normin, &n, a, &lda, x, &scale, cnorm, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
template< typename Trans, typename Diag >
inline std::ptrdiff_t latrs( const char uplo, const Trans, const Diag,
        const char normin, const fortran_int_t n, const double* a,
        const fortran_int_t lda, double* x, double& scale, double* cnorm ) {
    fortran_int_t info(0);
    LAPACK_DLATRS( &uplo, &lapack_option< Trans >::value, &lapack_option<
            Diag >::value, &normin, &n, a, &lda, x, &scale, cnorm, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<float> value-type.
//
template< typename Trans, typename Diag >
inline std::ptrdiff_t latrs( const char uplo, const Trans, const Diag,
        const char normin, const fortran_int_t n,
        const std::complex<float>* a, const fortran_int_t lda,
        std::complex<float>* x, float& scale, float* cnorm ) {
    fortran_int_t info(0);
    LAPACK_CLATRS( &uplo, &lapack_option< Trans >::value, &lapack_option<
            Diag >::value, &normin, &n, a, &lda, x, &scale, cnorm, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<double> value-type.
//
template< typename Trans, typename Diag >
inline std::ptrdiff_t latrs( const char uplo, const Trans, const Diag,
        const char normin, const fortran_int_t n,
        const std::complex<double>* a, const fortran_int_t lda,
        std::complex<double>* x, double& scale, double* cnorm ) {
    fortran_int_t info(0);
    LAPACK_ZLATRS( &uplo, &lapack_option< Trans >::value, &lapack_option<
            Diag >::value, &normin, &n, a, &lda, x, &scale, cnorm, &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to latrs.
//
template< typename Value, typename Enable = void >
struct latrs_impl {};

//
// This implementation is enabled if Value is a real type.
//
template< typename Value >
struct latrs_impl< Value, typename boost::enable_if< is_real< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename VectorX, typename VectorCNORM >
    static std::ptrdiff_t invoke( const char uplo, const char normin,
            const MatrixA& a, VectorX& x, real_type& scale,
            VectorCNORM& cnorm ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef typename blas::detail::default_order< MatrixA >::type order;
        typedef typename result_of::trans_tag< MatrixA, order >::type trans;
        typedef typename result_of::diag_tag< MatrixA >::type diag;
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorX >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorCNORM >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorX >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorCNORM >::value) );
        BOOST_ASSERT( bindings::size(x) >= bindings::size_column_op(a,
                trans()) );
        BOOST_ASSERT( bindings::size_column_op(a, trans()) >= 0 );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(a, trans())) );
        BOOST_ASSERT( normin == 'Y' || normin == 'N' );
        return detail::latrs( uplo, trans(), diag(), normin,
                bindings::size_column_op(a, trans()),
                bindings::begin_value(a), bindings::stride_major(a),
                bindings::begin_value(x), scale,
                bindings::begin_value(cnorm) );
    }

};

//
// This implementation is enabled if Value is a complex type.
//
template< typename Value >
struct latrs_impl< Value, typename boost::enable_if< is_complex< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename VectorX, typename VectorCNORM >
    static std::ptrdiff_t invoke( const char uplo, const char normin,
            const MatrixA& a, VectorX& x, real_type& scale,
            VectorCNORM& cnorm ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef typename blas::detail::default_order< MatrixA >::type order;
        typedef typename result_of::trans_tag< MatrixA, order >::type trans;
        typedef typename result_of::diag_tag< MatrixA >::type diag;
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorX >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorX >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorCNORM >::value) );
        BOOST_ASSERT( bindings::size(x) >= bindings::size_column_op(a,
                trans()) );
        BOOST_ASSERT( bindings::size_column_op(a, trans()) >= 0 );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(a, trans())) );
        BOOST_ASSERT( normin == 'Y' || normin == 'N' );
        return detail::latrs( uplo, trans(), diag(), normin,
                bindings::size_column_op(a, trans()),
                bindings::begin_value(a), bindings::stride_major(a),
                bindings::begin_value(x), scale,
                bindings::begin_value(cnorm) );
    }

};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the latrs_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for latrs. Its overload differs for
//
template< typename MatrixA, typename VectorX, typename VectorCNORM >
inline std::ptrdiff_t latrs( const char uplo, const char normin,
        const MatrixA& a, VectorX& x, typename remove_imaginary<
        typename bindings::value_type< MatrixA >::type >::type& scale,
        VectorCNORM& cnorm ) {
    return latrs_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( uplo, normin, a, x, scale, cnorm );
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#ifndef __PAUTH_TYPES_HPP__
#define __PAUTH_TYPES_HPP__

#include "molecular.hpp"
#include <armadillo>
#include <tuple>

namespace pauth {

class metropolis;
class abstract_potential;

using callback = std::function<void(const metropolis &)>;
using stopping_criterion = std::function<bool(const metropolis &)>;
using data_accessor = std::function<const arma::mat &(const metropolis &)>;
using value_accessor = std::function<double(const metropolis &)>;

using metric = std::tuple<
                std::function<arma::vec(const arma::vec&, const arma::vec&,
                                     const arma::vec&)>,
                std::function<double(const arma::vec&, const arma::vec&,
                                     const arma::vec&)>
               >;
using bc_ret = std::tuple<bool, arma::vec>;
using bc = std::function<bc_ret(metropolis &, const size_t, arma::vec&)>;
using trial_move_generator = std::function<arma::vec(const arma::mat&, const size_t)>;
using acc = std::function<bool(const metropolis &, const double, const double)>;
using seed_gen = std::function<unsigned()>;

} // namespace pauth

/*! \brief Helper function for printing the integer value of an enum class
 *
 * \param   value   Enumeration value
 * \return          Enumeration value converted to integer
 */
template <typename Enumeration>
auto as_integer(Enumeration const value) ->
    typename std::underlying_type<Enumeration>::type {

  return static_cast<typename std::underlying_type<Enumeration>::type>(value);
}

#endif


/*******************************************************************************
* Copyright (c) 2018, RoboTICan, LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
*   list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
*   this list of conditions and the following disclaimer in the documentation
*   and/or other materials provided with the distribution.
*
* * Neither the name of RoboTICan nor the names of its
*   contributors may be used to endorse or promote products derived from
*   this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

/* Author: Elchay Rauper*/

#include <ros/ros.h>

#include <vector>
#include <boost/scoped_ptr.hpp>
#include <boost/thread/condition.hpp>

#include <actionlib/server/action_server.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Twist.h>

#include <kdl/chainfksolver.hpp>
#include <kdl/chain.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/frames.hpp>
#include "kdl/chainfksolverpos_recursive.hpp"
#include "kdl_parser/kdl_parser.hpp"

#include "tf_conversions/tf_kdl.h"
#include <message_filters/subscriber.h>
#include <tf/message_filter.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

#include <sensor_msgs/JointState.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Int32.h>

#include <urdf/model.h>

#include <trajectory_msgs/JointTrajectory.h>
#include <mobilican_msgs/PointHeadAction.h>
#include <mobilican_msgs/QueryTrajectoryState.h>
#include <mobilican_msgs/JointTrajectoryControllerState.h>

void printVector3(const char * label, tf::Vector3 v)
{
  printf("%s % 7.3f % 7.3f % 7.3f\n", label, v.x(), v.y(), v.z() );

}

class ControlHead
{
private:
  typedef actionlib::ActionServer<mobilican_msgs::PointHeadAction> PHAS;
  typedef PHAS::GoalHandle GoalHandle;

  std::string node_name_;
  std::string action_name_;
  std::string root_;
  std::string tip_;
  std::string pan_link_;
  std::string default_pointing_frame_;
  std::string pointing_frame_;
  tf::Vector3 pointing_axis_;
  std::vector< std::string> joint_names_;

  ros::NodeHandle nh_, pnh_;
  ros::Publisher pub_controller_command_;
  ros::Subscriber sub_controller_state_;
  ros::Subscriber command_sub_;
  ros::ServiceClient cli_query_traj_;
  ros::Timer watchdog_timer_;

  PHAS action_server_;
  bool has_active_goal_;
  GoalHandle active_goal_;
  tf::Stamped<tf::Point> target_in_pan_;
  std::string pan_parent_;
  double success_angle_threshold_;

  geometry_msgs::PointStamped target_;
  KDL::Tree tree_;
  KDL::Chain chain_;
  tf::Point target_in_root_;

  boost::scoped_ptr<KDL::ChainFkSolverPos> pose_solver_;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_;

  tf::TransformListener tfl_;
  urdf::Model urdf_model_;

public:

  ControlHead(const ros::NodeHandle &node) :
      node_name_(ros::this_node::getName())
      , action_name_("point_head_action")
      , nh_(node)
      , pnh_("~")
      , action_server_(nh_, action_name_.c_str(),
                         boost::bind(&ControlHead::goalCB, this, _1),
                         boost::bind(&ControlHead::cancelCB, this, _1), false)
      , has_active_goal_(false)
  {
    pnh_.param("pan_link", pan_link_, std::string("head_pan_link"));
    pnh_.param("default_pointing_frame", default_pointing_frame_, std::string("head_tilt_link"));
    pnh_.param("success_angle_threshold", success_angle_threshold_, 0.1);

    if(pan_link_[0] == '/') pan_link_.erase(0, 1);
    if(default_pointing_frame_[0] == '/') default_pointing_frame_.erase(0, 1);

    // Connects to the controller
    pub_controller_command_ =
      nh_.advertise<trajectory_msgs::JointTrajectory>("command", 2);
    sub_controller_state_ =
      nh_.subscribe("state", 1, &ControlHead::controllerStateCB, this);
    cli_query_traj_ =
        nh_.serviceClient<mobilican_msgs::QueryTrajectoryState>("/pan_tilt_trajectory_controller/query_state");

    // Should only ever happen on first call... move to constructor?
    if(tree_.getNrOfJoints() == 0)
    {
      std::string robot_desc_string;
      nh_.param("/robot_description", robot_desc_string, std::string());
      ROS_DEBUG("Reading tree from robot_description...");
      if (!kdl_parser::treeFromString(robot_desc_string, tree_)){
         ROS_ERROR("Failed to construct kdl tree");
         exit(-1);
      }
      if (!urdf_model_.initString(robot_desc_string)){
        ROS_ERROR("Failed to parse urdf string for urdf::Model.");
        exit(-2);
      }
    }

    ROS_DEBUG("Tree has %d joints and %d segments.", tree_.getNrOfJoints(), tree_.getNrOfSegments());

    action_server_.start();

    watchdog_timer_ = nh_.createTimer(ros::Duration(1.0), &ControlHead::watchdog, this);
  }

  void goalCB(GoalHandle gh)
  {
    // Before we do anything, we need to know that name of the pan_link's parent, which we will treat as the root.
    if (root_.empty())
    {
      for (int i = 0; i < 10; ++i)
      {
        try {
          tfl_.getParent(pan_link_, ros::Time(), root_);
          break;
        }
        catch (const tf::TransformException &ex) {}
        ros::Duration(0.5).sleep();
      }
      if (root_.empty())
      {
        ROS_ERROR("Could not get parent of %s in the TF tree", pan_link_.c_str());
        gh.setRejected();
        return;
      }
    }
    if(root_[0] == '/') root_.erase(0, 1);

    ROS_DEBUG("Got point head goal!");

    // Process pointing frame and axis
    const geometry_msgs::PointStamped &target = gh.getGoal()->target;
    pointing_frame_ = gh.getGoal()->pointing_frame;
    if(pointing_frame_.length() == 0)
    {
      ROS_WARN("Pointing frame not specified, using %s [1, 0, 0] by default.", default_pointing_frame_.c_str());
      pointing_frame_ = default_pointing_frame_;
      pointing_axis_ = tf::Vector3(1.0, 0.0, 0.0);
    }
    else
    {
      if(pointing_frame_[0] == '/') pointing_frame_.erase(0, 1);
      bool ret1 = false;
      try
      {
        std::string error_msg;
        ret1 = tfl_.waitForTransform(pan_link_, pointing_frame_, target.header.stamp,
                                   ros::Duration(5.0), ros::Duration(0.01), &error_msg);

        tf::vector3MsgToTF(gh.getGoal()->pointing_axis, pointing_axis_);
        if(pointing_axis_.length() < 0.1)
        {
          size_t found = pointing_frame_.find("optical_frame");
          if (found != std::string::npos)
          {
            ROS_WARN("Pointing axis appears to be zero-length. Using [0, 0, 1] as default for an optical frame.");
            pointing_axis_ = tf::Vector3(0, 0, 1);
          }
          else
          {
            ROS_WARN("Pointing axis appears to be zero-length. Using [1, 0, 0] as default for a non-optical frame.");
            pointing_axis_ = tf::Vector3(1, 0, 0);
          }
        }
        else
        {
          pointing_axis_.normalize();
        }
      }
      catch(const tf::TransformException &ex)
      {
        ROS_ERROR("Transform failure (%d): %s", ret1, ex.what());
        gh.setRejected();
        return;
      }
    }

    //Put the target point in the root frame (usually torso_lift_link).
    bool ret1 = false;
    try
    {
      std::string error_msg;
      ret1 = tfl_.waitForTransform(root_.c_str(), target.header.frame_id, target.header.stamp,
                                       ros::Duration(5.0), ros::Duration(0.01), &error_msg);

      geometry_msgs::PointStamped target_in_root_msg;
      tfl_.transformPoint(root_.c_str(), target, target_in_root_msg );
      tf::pointMsgToTF(target_in_root_msg.point, target_in_root_);
    }
    catch(const tf::TransformException &ex)
    {
      ROS_ERROR("Transform failure (%d): %s", ret1, ex.what());
      gh.setRejected();
      return;
    }

    if( tip_.compare(pointing_frame_) != 0 )
    {
      bool success = tree_.getChain(root_.c_str(), pointing_frame_.c_str(), chain_);
      if( !success )
      {
        ROS_ERROR("Couldn't create chain from %s to %s.", root_.c_str(), pointing_frame_.c_str());
        gh.setRejected();
        return;
      }
      tip_ = pointing_frame_;

      pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
      jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
      joint_names_.resize(chain_.getNrOfJoints());
    }

    unsigned int joints = chain_.getNrOfJoints();

//    int segments = chain_.getNrOfSegments();
//    ROS_INFO("Parsed urdf from %s to %s and found %d joints and %d segments.", root_.c_str(), pointing_frame_.c_str(), joints, segments);
//    for(int i = 0; i < segments; i++)
//    {
//      KDL::Segment segment = chain_.getSegment(i);
//      ROS_INFO("Segment %d, %s: joint %s type %s",
//               i, segment.getName().c_str(), segment.getJoint().getName().c_str(), segment.getJoint().getTypeName().c_str() );
//    }

    KDL::JntArray jnt_pos(joints), jnt_eff(joints);
    KDL::Jacobian jacobian(joints);

    mobilican_msgs::QueryTrajectoryState traj_state;
    traj_state.request.time = ros::Time::now() + ros::Duration(0.01);
    if (!cli_query_traj_.call(traj_state))
    {
      ROS_ERROR("Service call to query controller trajectory failed.");
      gh.setRejected();
      return;
    }
    if(traj_state.response.name.size() != joints)
    {
      ROS_ERROR("Number of joints mismatch: urdf chain vs. trajectory controller state.");
      gh.setRejected();
      return;
    }
    std::vector<urdf::JointLimits> limits_(joints);

    // Get initial joint positions and joint limits.
    for(unsigned int i = 0; i < joints; i++)
    {
      joint_names_[i] = traj_state.response.name[i];
      limits_[i] = *(urdf_model_.joints_[joint_names_[i].c_str()]->limits);
      ROS_DEBUG("Joint %d %s: %f, limits: %f %f", i, traj_state.response.name[i].c_str(), traj_state.response.position[i], limits_[i].lower, limits_[i].upper);
      //jnt_pos(i) = traj_state.response.position[i];
      jnt_pos(i) = 0;
    }

    int count = 0;
    int limit_flips = 0;
    float correction_angle = 2*M_PI;
    float correction_delta = 2*M_PI;
    while( ros::ok() && fabs(correction_delta) > 0.001)
    {
      //get the pose and jacobian for the current joint positions
      KDL::Frame pose;
      pose_solver_->JntToCart(jnt_pos, pose);
      jac_solver_->JntToJac(jnt_pos, jacobian);

      tf::Transform frame_in_root;
      tf::poseKDLToTF(pose, frame_in_root);

      tf::Vector3 axis_in_frame = pointing_axis_.normalized();
      tf::Vector3 target_from_frame = (target_in_root_ - frame_in_root.getOrigin()).normalized();
      tf::Vector3 current_in_frame = frame_in_root.getBasis().inverse()*target_from_frame;
      float prev_correction = correction_angle;
      correction_angle = current_in_frame.angle(axis_in_frame);
      correction_delta = correction_angle - prev_correction;
      ROS_DEBUG("At step %d, joint poses are %.4f and %.4f, angle error is %f", count, jnt_pos(0), jnt_pos(1), correction_angle);
      if(correction_angle < 0.5*success_angle_threshold_) break;
      tf::Vector3 correction_axis = frame_in_root.getBasis()*(axis_in_frame.cross(current_in_frame).normalized());
      //printVector3("correction_axis in root:", correction_axis);
      tf::Transform correction_tf(tf::Quaternion(correction_axis, 0.5*correction_angle), tf::Vector3(0,0,0));
      KDL::Frame correction_kdl;
      tf::transformTFToKDL(correction_tf, correction_kdl);

      // We apply a "wrench" proportional to the desired correction
      KDL::Frame identity_kdl;
      KDL::Twist twist = diff(correction_kdl, identity_kdl);
      KDL::Wrench wrench_desi;
      for (unsigned int i=0; i<6; i++)
        wrench_desi(i) = -1.0*twist(i);

      // Converts the "wrench" into "joint corrections" with a jacbobian-transpose
      for (unsigned int i = 0; i < joints; i++)
      {
        jnt_eff(i) = 0;
        for (unsigned int j=0; j<6; j++)
          jnt_eff(i) += (jacobian(j,i) * wrench_desi(j));
        jnt_pos(i) += jnt_eff(i);
      }

      // account for pan_link joint limit in back.
     // if(jnt_pos(0) < limits_[0].lower && limit_flips++ == 0){ jnt_pos(0) += 1.5*M_PI; }
     // if(jnt_pos(0) > limits_[0].upper && limit_flips++ == 0){ jnt_pos(0) -= 1.5*M_PI; }
      for (unsigned int i = 0; i < joints; i++)
      {
          jnt_pos(i) = std::max(limits_[i].lower, jnt_pos(i));
          jnt_pos(i) = std::min(limits_[i].upper, jnt_pos(i));
      }

     // jnt_pos(1) = std::max(limits_[1].lower, jnt_pos(1));
     // jnt_pos(1) = std::min(limits_[1].upper, jnt_pos(1));

      count++;

      if(limit_flips > 1){
        ROS_ERROR("Goal is out of joint limits, trying to point there anyway... \n");
        break;
      }
    }
    ROS_DEBUG("Iterative solver took %d steps", count);

    std::vector<double> q_goal(joints);

    for(unsigned int i = 0; i < joints; i++)
    {
      jnt_pos(i) = std::max(limits_[i].lower, jnt_pos(i));
      jnt_pos(i) = std::min(limits_[i].upper, jnt_pos(i));
      q_goal[i] = jnt_pos(i);
      ROS_DEBUG("Joint %d %s: %f", i, joint_names_[i].c_str(), jnt_pos(i));
    }

    if (has_active_goal_)
    {
      active_goal_.setCanceled();
      has_active_goal_ = false;
    }

    gh.setAccepted();
    active_goal_ = gh;
    has_active_goal_ = true;

    // Computes the duration of the movement.
    ros::Duration min_duration(0.1);

    if (gh.getGoal()->min_duration > min_duration)
        min_duration = gh.getGoal()->min_duration;

    // Determines if we need to increase the duration of the movement in order to enforce a maximum velocity.
    if (gh.getGoal()->max_velocity > 0)
    {
      // Very approximate velocity limiting.
      double dist = sqrt(pow(q_goal[0] - traj_state.response.position[0], 2) +
                         pow(q_goal[1] - traj_state.response.position[1], 2));
      ros::Duration limit_from_velocity(dist / gh.getGoal()->max_velocity);
      if (limit_from_velocity > min_duration)
        min_duration = limit_from_velocity;
    }

    // Computes the command to send to the trajectory controller.
    trajectory_msgs::JointTrajectory traj;
    traj.header.stamp = traj_state.request.time;

    traj.joint_names.push_back(traj_state.response.name[0]);
    traj.joint_names.push_back(traj_state.response.name[1]);

    traj.points.resize(1);
   // traj.points[0].positions = traj_state.response.position;
    //traj.points[0].velocities = traj_state.response.velocity;
    //traj.points[0].time_from_start = ros::Duration(0.0);
    traj.points[0].positions = q_goal;
    traj.points[0].velocities.push_back(0);
    traj.points[0].velocities.push_back(0);
    traj.points[0].time_from_start = ros::Duration(min_duration);

    pub_controller_command_.publish(traj);
  }

  void watchdog(const ros::TimerEvent &e)
  {
    ros::Time now = ros::Time::now();

    // Aborts the active goal if the controller does not appear to be active.
    if (has_active_goal_)
    {
      bool should_abort = false;
      if (!last_controller_state_)
      {
        should_abort = true;
        ROS_WARN("Aborting goal because we have never heard a controller state message.");
      }
      else if ((now - last_controller_state_->header.stamp) > ros::Duration(5.0))
      {
        should_abort = true;
        ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
                 (now - last_controller_state_->header.stamp).toSec());
      }

      if (should_abort)
      {
        // Stops the controller.
        trajectory_msgs::JointTrajectory empty;
        empty.joint_names = joint_names_;
        pub_controller_command_.publish(empty);

        // Marks the current goal as aborted.
        active_goal_.setAborted();
        has_active_goal_ = false;
      }
    }
  }

  void cancelCB(GoalHandle gh)
  {
    if (active_goal_ == gh)
    {
      // Stops the controller.
      trajectory_msgs::JointTrajectory empty;
      empty.joint_names = joint_names_;
      pub_controller_command_.publish(empty);

      // Marks the current goal as canceled.
      active_goal_.setCanceled();
      has_active_goal_ = false;
    }
  }

    mobilican_msgs::JointTrajectoryControllerStateConstPtr last_controller_state_;
  void controllerStateCB(const mobilican_msgs::JointTrajectoryControllerStateConstPtr &msg)
  {
    last_controller_state_ = msg;
    ros::Time now = ros::Time::now();

    if (!has_active_goal_)
      return;

    //! \todo Support frames that are not the pan link itself
    try
    {
      KDL::JntArray jnt_pos(msg->joint_names.size());
      for(unsigned int i = 0; i < msg->joint_names.size(); i++)
        jnt_pos(i) = msg->actual.positions[i];

      KDL::Frame pose;
      pose_solver_->JntToCart(jnt_pos, pose);

      tf::Transform frame_in_root;
      tf::poseKDLToTF(pose, frame_in_root);

      tf::Vector3 axis_in_frame = pointing_axis_.normalized();
      tf::Vector3 target_from_frame = target_in_root_ - frame_in_root.getOrigin();

      target_from_frame.normalize();
      tf::Vector3 current_in_frame = frame_in_root.getBasis().inverse()*target_from_frame;

      mobilican_msgs::PointHeadFeedback feedback;
      feedback.pointing_angle_error = current_in_frame.angle(axis_in_frame);
      active_goal_.publishFeedback(feedback);

      if (feedback.pointing_angle_error < success_angle_threshold_)
      {
        active_goal_.setSucceeded();
        has_active_goal_ = false;
      }
    }
    catch(const tf::TransformException &ex)
    {
      ROS_ERROR("Could not transform: %s", ex.what());
    }
  }

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "point_head_action");
  ros::NodeHandle node;
  ControlHead ch(node);
  ros::spin();
  return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DEMOTES_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEMOTES_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-swar
    Function object implementing demotes capabilities

    take two SIMD vectors of same type and elements of size n
    and return a vector collecting the two in a vector in which
    the elements have size n/2 properly saturated.

    Of course the applicability is conditioned by the existence of compatible
    SIMD vector types

    This is a convenient alias of @ref groups
  **/
  const boost::dispatch::functor<tag::demotes_> demotes = {};
} }
#endif

#include <boost/simd/function/scalar/groups.hpp>
#include <boost/simd/function/simd/demotes.hpp>

#endif


#include <fstream>
#include <vector>
#include <string>
#include <stdio.h>

#include <rw/math/Random.hpp>
#include <rw/math/Vector3D.hpp>
#include <rwlibs/task/GraspTask.hpp>

#include <boost/program_options/options_description.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/option.hpp>
#include <boost/program_options/parsers.hpp>

#include "util.hpp"

using namespace std;
using namespace rw::math;
using namespace rwlibs::algorithms;
using namespace rwlibs::task;
using namespace boost::program_options;

int calcPerturbedQuality(GraspTask::Ptr gtask, std::string outfile, int pertubations ){
    int count = 0, succCnt=0, failCnt=0;
    std::vector<std::pair<GraspSubTask*,GraspTarget*> > tasks = gtask->getAllTargets();
    GraspTask::Ptr ngtask = gtask->clone();
    for(size_t i = 0; i<tasks.size();i++){
        // save the target that the quality should be calculated for
        if(count==0){
            GraspSubTask nstask = tasks[i].first->clone();
            ngtask->addSubTask( nstask );
            ngtask->getSubTasks().back().addTarget( *tasks[i].second );
            count++;
            continue;
        }

        GraspResult::Ptr tres = tasks[i].second->getResult();
        if(tres->testStatus == GraspResult::Success || tres->testStatus == GraspResult::ObjectSlipped){
            succCnt++;
        } else {
            failCnt++;
        }

        if(count==pertubations){
            // set the quality of the target
            GraspResult::Ptr result = ngtask->getSubTasks().back().getTargets().back().getResult();

            double successProbability = ((double)succCnt)/((double)(succCnt+failCnt));
            result->qualityAfterLifting = Q(1, successProbability );
            succCnt = 0;
            failCnt = 0;
            count = 0;
        } else {
            count++;
        }
    }
    GraspTask::saveRWTask(ngtask, outfile);
    return 0;

}

std::vector<std::pair<Transform3D<>, RPY<> > > readPoses(std::string file){

    std::vector<std::pair<Transform3D<>, RPY<> > > data;
    char line[1000];
    std::ifstream in(file.c_str());
    in.getline(line,1000);
    in.getline(line,1000);
    in.getline(line,1000);

    //Unused: char tmpc;
    while(!in.eof() ){
        in.getline(line,1000);
        std::istringstream istr(line);
        Vector3D<float> pos_s, pos_e;
        EAA<float> rot_s, rot_e;
        RPY<float> rpy;

        sscanf(line,"%f %f %f %f %f %f %f  %f %f ",
               &pos_s[0], &pos_s[1], &pos_s[2], &rpy(0), &rpy(1), &rpy(2), &rot_s[0], &rot_s[1], &rot_s[2]);

        //std::cout << pos_e << std::endl;
        data.push_back( std::make_pair(Transform3D<>(cast<double>(pos_s),cast<double>(rot_s)), cast<double>(rpy) ) );

    }
    std::cout << "Size of intput: " << data.size() << std::endl;
    return data;
}

int main(int argc, char** argv)
{
	static const unsigned int SEED = static_cast<unsigned int>(time(NULL));
	Random::seed(SEED);
	srand(SEED);

    options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("output,o", value<string>()->default_value("out.xml"), "the output file.")
        ("stable", value<string>(), "Name of grasp task file.")
        ("misses", value<string>(), "Add small random pertubations to all targets.")
        ("angle", value<double>(), "angular standard deviation.")
        //("dist", value<double>(), "distance standard deviation.")
    ;
    positional_options_description optionDesc;
    optionDesc.add("input",-1);

    variables_map vm;
    //store(parse_command_line(argc, argv, desc), vm);
    store(command_line_parser(argc, argv).
              options(desc).positional(optionDesc).run(), vm);
    notify(vm);

	std::string stable_file = vm["stable"].as<std::string>();
	std::string misses_file = vm["misses"].as<std::string>();
    std::string output_file = vm["output"].as<std::string>();
    double angle_sd = vm["angle"].as<double>()*Deg2Rad;

    std::vector<std::pair<Transform3D<>,RPY<> > > stable = readPoses(stable_file);
    std::vector<std::pair<Transform3D<>,RPY<> > > misses = readPoses(misses_file);
    std::vector<double> qualityestimates( stable.size() );
    std::vector<double> qualityestimates_misses( misses.size() );
    // build the nodes
	typedef std::pair < std::size_t , bool > Value;
    typedef KDTreeQ<Value> NNSearch;
    std::vector<NNSearch::KDNode> nodes;
    for(std::size_t i=0;i<stable.size();i++){
        //Unused: Transform3D<> t3d = stable[i].first;
        //stable[i].second(1) = stable[i].second(1)+13*Deg2Rad;
        RPY<> rpy = stable[i].second;
        Q key(2, rpy(1), rpy(2));
        nodes.push_back( NNSearch::KDNode(key, Value(i, true)) );
    }
    for(std::size_t i=0;i<misses.size();i++){
    	//Unused: Transform3D<> t3d = misses[i].first;
        //misses[i].second(1) = misses[i].second(1)+13*Deg2Rad;
        RPY<> rpy = misses[i].second;
        Q key(2, rpy(1), rpy(2));
        nodes.push_back( NNSearch::KDNode(key, Value(i, false)) );
    }

    // create nodes for all successes
    //std::vector<KDTree<Pose6D<>, 6 >::KDNode> nodes;
    NNSearch *nntree = NNSearch::buildTree( nodes );

    // define the truncation
    Q diff(2, 2*angle_sd, 2*angle_sd);

    // now for each stable node predict its quality
    std::list<const NNSearch::KDNode*> result;
    result.clear();

    for(NNSearch::KDNode &nn : nodes) {
    //for(int i=0;i<stable.size();i++){
        result.clear();
        Transform3D<> t3d;
        RPY<> rpy ;
        Value nv = nn.value;
        if(nv.second){
            t3d = stable[nv.first].first;
            rpy = stable[nv.first].second;
        } else {
            t3d = misses[nv.first].first;
            rpy = misses[nv.first].second;
        }
        Q key(2, rpy(1), rpy(2));
        nntree->nnSearchRect(key-diff,key+diff, result);

        double sum = 0, sum1=0;
        double xo = rpy(1);
        double yo = rpy(2);

        //Unused: int N = result.size();
        for(const NNSearch::KDNode* n : result ) {
            Value v = n->value;
            RPY<> rpyo;
            if(v.second)
                rpyo = stable[v.first].second;
            else
                rpyo = misses[v.first].second;

            double x = rpyo(1);
            double y = rpyo(2);

            // calculate the
            double tmp_x = Math::sqr(x-xo)/(2*Math::sqr(angle_sd));
            double tmp_y = Math::sqr(y-yo)/(2*Math::sqr(angle_sd));

            double val = std::pow(2.718281828, -(tmp_x+tmp_y));
            //std::cout << val << " " <<  N << " y-yo" << y-yo << std::endl;
            //std::cout << val << " " <<  N << " x-xo" << x-xo << std::endl;
            if(v.second) // if o=1
                sum += val;
            sum1 += val;
        }

        double quality = sum/sum1;

        //std::cout << sum << " " << sum1 << " "<< quality <<  std::endl;
        if(nv.second){
            qualityestimates[nv.first] = quality;
        } else {
            qualityestimates_misses[nv.first] = quality;
        }
    }

    std::ofstream outf(output_file.c_str());

    for(std::size_t i=0;i<stable.size();i++){
        Transform3D<> t3d = stable[i].first;
        EAA<> eaa(t3d.R());
        RPY<> rpy = stable[i].second;
        outf << t3d.P()[0] << "\t" << t3d.P()[1] << "\t" << t3d.P()[2] << "\t";
        outf << rpy(0)*Rad2Deg << "\t" << rpy(1)*Rad2Deg << "\t" << rpy(2)*Rad2Deg << "\t";
        outf << qualityestimates[i] << "\n";
    }
    RW_WARN("1");
    for(std::size_t i=0;i<misses.size();i++){
        Transform3D<> t3d = misses[i].first;
        EAA<> eaa(t3d.R());
        RPY<> rpy = misses[i].second;
        outf << t3d.P()[0] << "\t" << t3d.P()[1] << "\t" << t3d.P()[2] << "\t";
        outf << rpy(0)*Rad2Deg << "\t" << rpy(1)*Rad2Deg << "\t" << rpy(2)*Rad2Deg << "\t";
        outf << qualityestimates_misses[i] << "\n";
    }

    outf.close();

	return 0;
}


#include "murk/crypto/block.hpp"

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

namespace murk::crypto {
  data ecb_crack_prepend_oracle(threaded_flow_t<data_const_ref, data> oracle, size_t block_size, uint8_t spam_byte, log_params log) {
    // Get the ctext

    boost::asio::thread_pool pool;

    data spam_buf(block_size, spam_byte);

    std::vector<data> offset_ctexts(block_size);

    // Fetch the offset blocks in parallel
    for (size_t byte_no = 0; byte_no < block_size; ++byte_no) {
      boost::asio::post(pool, [&spam_buf, byte_no, block_size, &oracle, &offset_ctexts]() mutable {
        // Adding a block of 0s to the first one makes the calculations a lot cleaner
        offset_ctexts[byte_no] = oracle(data_const_ref{spam_buf}.subspan(0, block_size - byte_no - 1));
      });
    }
    pool.join();

    size_t ctext_size = offset_ctexts.front().size() - block_size;

    data ptext;
    ptext.reserve(ctext_size);

    size_t ptext_size;
    {
      size_t ptext_size_change = 1;
      for (ptext_size_change = 1; ptext_size_change < offset_ctexts.size(); ++ptext_size_change)
        if (offset_ctexts[ptext_size_change].size() != offset_ctexts[ptext_size_change - 1].size())
          break;
      // If we overshot, then we keep the offset (for some reason)
      ssize_t offset = static_cast<ssize_t>(block_size) - static_cast<ssize_t>(ptext_size_change);
      ptext_size = static_cast<size_t>(static_cast<ssize_t>(ctext_size) - offset);
    }

    for (size_t block_begin = 0; block_begin < ctext_size; block_begin += block_size) {
      // work out the subblocks
      for (size_t byte_no = 0; byte_no < block_size; ++byte_no) {
        // Get the offset blocks with the current offset, and seek to where the current block begins
        //
        // We know that everything before this iter is valud
        auto target_iter = offset_ctexts[byte_no].begin() + static_cast<ssize_t>(block_begin);

        // TODO: parallelise the oracle invocation
        // TODO: we can generate a rainbow table for the first byte of the block
        for (size_t guess_byte = 0; guess_byte <= 255; ++guess_byte) {
          // Check if we have hit the end
          if (ptext.size() == ptext_size)
            return ptext;

          // Set up the buffer with the current candidate byte
          spam_buf.back() = static_cast<uint8_t>(guess_byte);
          // See what the actual result is
          auto guess_buf = oracle(spam_buf);
          // spam_buf will contain an encrypted offset block, in which all the values, except for the last are correct
          //
          // Initially this will be spam_bytes, which we used to offset the block initially
          //
          // As we gain more info, this will become the last 15 bytes of real plaintext, followed by a guessed byte
          //
          // We can compare this generated block with the offset one, thanks to ECB block reordering!
          if (std::equal(guess_buf.begin(), guess_buf.begin() + static_cast<ssize_t>(block_size), target_iter)) {
            // We found it
            ptext.push_back(static_cast<uint8_t>(guess_byte));
            // Move the data back a bit
            std::copy(spam_buf.begin() + 1, spam_buf.end(), spam_buf.begin());
            goto next_byte;
          }
        }
        // If we hit here, something has gone wrong
        throw std::invalid_argument("ECB prepend oracle giving inconsistent results");
        next_byte: {}
      }
    }

    // No padding?
    return ptext;
  }

  // Returns a tuple of the spam needed, and the resultant data offset
  std::pair<data, size_t> ecb_determine_spam(flow_t<data_const_ref, data> oracle, size_t block_size, uint8_t spam_byte, size_t repetitions) {
    std::pair<data, size_t> ret;
    data& spammed_ptext = ret.first = data(block_size * repetitions, spam_byte);
    size_t& data_offset = ret.second = 0;

    auto expected_block = ecb_guess_spammed_block(oracle, block_size, spam_byte);
    const auto spammed_ctext = repeat<uint8_t>(ecb_guess_spammed_block(oracle, block_size, spam_byte), repetitions);

    for(;;spammed_ptext.insert(spammed_ptext.begin(), spam_byte)) {
      auto res = oracle(spammed_ptext);
      auto iter = std::search(res.begin(), res.end(), spammed_ctext.begin(), spammed_ctext.end());
      if (iter == res.end())
        continue;

      // If we get here, then we have the requested state
      spammed_ptext.erase(spammed_ptext.begin(), spammed_ptext.begin() + static_cast<ssize_t>(block_size * 2));
      data_offset = static_cast<size_t>(iter - res.begin());
      break;
    }

    return ret;

//    // Spam bytes until we get a repeated block
//    //
//    // When we get a repeated block, we can subtract 2 block sizes from the spam to get the minimal required
//    //
//    // The offset will be equal to the position of the first repeated block

//    // We know that it is at least 2 block sizes to get a repetition
//    spam.resize(2 * block_size, spam_byte);

//    for(;;spam.emplace_back(spam_byte)) {
//      // Manually check for repetitions so we can get location data
//      auto res = insert_oracle(spam);
//      // This needs to be separate, as the vector would otherwise be deleted
//      auto chunks = chunk<const uint8_t>(res, block_size);

//      // The position of the repetition
//      size_t second_pos = 0;
//      // TODO: optimise
//      std::set<murk::data> s;
//      for (auto& i : chunks) {
//        if (!s.emplace(i.begin(), i.end()).second)
//          goto found;
//        second_pos += block_size;
//      }
//      // If we didn't find a repetition, skip this one
//      continue;
//      found:
//      // The position of the repeatee
//      data_offset = second_pos - block_size;
//      // Remove the fluff
//      spam.resize(spam.size() - (2 * block_size));
//      break;
//    }

//    return ret;
  }

  /// Encrypts arbitrary blocks
  flow_t<data_const_ref, data> ecb_make_block_enc_oracle(flow_t<data_const_ref, data> insert_oracle,
                                                         size_t block_size, std::pair<data, size_t> spam_res) {
    return [insert_oracle{std::move(insert_oracle)}, spam_res{std::move(spam_res)}, block_size](data_const_ref b) -> data {
      if (b.size() % block_size)
        throw std::invalid_argument("The input must be a multiple of block size. Maybe try pading it and adding it to the end?");
      auto buf = spam_res.first;
      buf.insert(buf.end(), b.begin(), b.end());
      auto res = insert_oracle(buf);
      // Less copying if we delete the end first
      res.erase(res.begin() + spam_res.second + b.size(), res.end());
      res.erase(res.begin(), res.begin() + spam_res.second);
      return res;
    };
  }
  data cbc_encrypt(data_const_ref b, data_const_ref iv, const Botan::BlockCipher& bc) {
    const auto block_size = bc.block_size();
    const auto n_full_blocks = b.size() / block_size;
    const auto remainder_len = b.size() % block_size;
    const auto final_offset = n_full_blocks * block_size;

    if (iv.size() != block_size)
      throw std::invalid_argument("IV length must be the same as the block size");

    data ret(final_offset + block_size);

    std::copy(iv.begin(), iv.end(), ret.begin());

    for (size_t i = 0; i < final_offset; i += block_size) {
      auto src = b.subspan(i, block_size);
      uint8_t* dest_begin = ret.data() + i;
      xor_bytes_inplace({dest_begin, block_size}, src);
      bc.encrypt(dest_begin);
      auto next = dest_begin + block_size;
      std::copy(dest_begin, next, next);
    }

    // Manual padding to stop copying or mallocs
    xor_bytes_inplace(data_ref{ret}.subspan(final_offset, b.size() % block_size), b.subspan(final_offset));
    auto final_padding = data_ref{ret}.subspan(final_offset + remainder_len);
    auto padding_val = static_cast<uint8_t>(final_padding.size());
    for (auto& i : final_padding)
      i ^= padding_val;
    bc.encrypt(ret.data() + final_offset);

    return ret;
  }

  data cbc_decrypt(data_const_ref b, data_const_ref iv, const Botan::BlockCipher& bc) {
    const auto block_size = bc.block_size();

    if (iv.size() != block_size)
      throw std::invalid_argument("IV length must be the same as the block size");
    if (b.size() % block_size)
      throw std::invalid_argument("Blocks must be a multiple of the block size");

    data ret(b.size());

    for (size_t i = 0; i < b.size(); i += block_size) {
      uint8_t* dest_begin = ret.data() + i;
      bc.decrypt(b.data() + i, dest_begin);

      data_const_ref old_ctext = i ? b.subspan(i - block_size, block_size) : iv;
      xor_bytes_inplace({dest_begin, block_size}, old_ctext);
    }

    pkcs7_remove_inplace(ret);

    return ret;
  }

  flow_t<data_const_ref, data> ecb_bypass_random_prefix_len(flow_t<data_const_ref, data> oracle, size_t block_size,
                                                            uint8_t spam_byte, uint8_t final_spam_byte, size_t repetitions) {
    data spammed_ptext(block_size * repetitions, spam_byte);
    auto spammed_ctext = repeat<uint8_t>(ecb_guess_spammed_block(oracle, block_size, spam_byte), repetitions);

    // Add a different final block, so that we can exclude shifts caused by the final byte of the random data
    // being the same as the spammed_ptext
    const auto final_spam = ecb_guess_spammed_block(oracle, block_size, final_spam_byte);
    spammed_ptext.insert(spammed_ptext.end(), block_size, final_spam_byte);
    spammed_ctext.insert(spammed_ctext.end(), final_spam.begin(), final_spam.end());

    return [oracle{std::move(oracle)}, spammed_ptext{std::move(spammed_ptext)}, spammed_ctext{std::move(spammed_ctext)}](data_const_ref b) -> data {
      auto ptext = spammed_ptext;
      ptext.insert(ptext.end(), b.begin(), b.end());

      while (true) {
        auto res = oracle(ptext);
        // If it is aligned, our double spam will come up
        auto iter = std::search(res.begin(), res.end(), spammed_ctext.begin(), spammed_ctext.end());

        if (iter == res.end())
          continue;

        // Now we have got a candidate, delete the prefix and the ctext spam
        res.erase(res.begin(), iter + static_cast<ssize_t>(spammed_ctext.size()));

        return res;
      }
    };
  }

  std::pair<data, size_t> cbc_determine_spam(flow_t<data_const_ref, data> oracle, size_t block_size, uint8_t spam_byte) {
    std::pair<data, size_t> ret;
    auto& buf = ret.first;
    auto& pos = ret.second = std::numeric_limits<size_t>::max();

    data last_res = oracle(buf);
    buf.push_back(spam_byte);
    data res = oracle(buf);
    while(true) {
      size_t this_pos = static_cast<size_t>(std::mismatch(last_res.begin(), last_res.end(), res.begin()).first  - last_res.begin());
      // Block align so that chance bytes aren't a problem
      this_pos = this_pos / block_size * block_size;

      std::swap(this_pos, pos);

      if (pos > this_pos)
        break;

      last_res = std::move(res);
      buf.push_back(spam_byte);
      res = oracle(buf);
    }
    // We went too far =(
    buf.pop_back();
    // We may have overshot by a block, so fix that if that is the case
    if (buf.size() >= block_size) {
      auto needed = buf.size() % block_size;
      auto extra = buf.size() / block_size * block_size;
      buf.resize(needed);
      pos -= extra;
    }

    return ret;
  }
/*

  data ecb_force_insert_oracle(data_const_ref b, flow_t<data_const_ref, data> block_oracle) {
//    if (input.size() % block_size)
//      throw std::invalid_argument("");*/
}


#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/arr/meta/is_vector.hpp>
#include <Eigen/Sparse>
#include <gtest/gtest.h>
#include <vector>

TEST(MathMeta, is_std_vector) {
  using stan::is_std_vector;
  EXPECT_FALSE((is_std_vector<bool>::value));
  EXPECT_FALSE((is_std_vector<double>::value));
  EXPECT_FALSE((is_std_vector<int>::value));

  EXPECT_TRUE((is_std_vector<std::vector<double>>::value));
  EXPECT_TRUE((is_std_vector<const std::vector<double>>::value));
  EXPECT_TRUE((is_std_vector<const std::vector<double>&>::value));

  EXPECT_FALSE((is_std_vector<Eigen::EigenBase<Eigen::MatrixXd>>::value));
  EXPECT_FALSE((is_std_vector<Eigen::Matrix<double, -1, -1>>::value));
  EXPECT_FALSE((is_std_vector<Eigen::SparseMatrix<double>>::value));
  EXPECT_FALSE((is_std_vector<Eigen::MatrixBase<Eigen::MatrixXd>>::value));

  EXPECT_FALSE((is_std_vector<const Eigen::Matrix<double, -1, -1>>::value));
  EXPECT_FALSE((is_std_vector<Eigen::SparseMatrix<double>&>::value));
  EXPECT_FALSE((is_std_vector<
                Eigen::MatrixBase<Eigen::Matrix<double, -1, -1>>&&>::value));
}


// TODO
// Make singleton.

#include <boost/function.hpp>
#include <boost/bind.hpp>
//#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <std_msgs/String.h>
#include <sensor_msgs/image_encodings.h>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>

#include <ctime>
#include <cstdio>

#include "/usr/local/include/raspicam/raspicam_cv.h"
#include <camera_info_manager/camera_info_manager.h>

#include "FindObject.h"

using namespace cv;
using namespace std;

extern FindObject* findObject;

#define TIME_STEPS 0

void FindObject::imageCb(Mat& image) {
	clock_t start;
	double durationCvtColor = 0;
	double durationInRange = 0;
	double durationErode1 = 0;
	double durationDilate1 = 0;
	double durationErode2 = 0;
	double durationDilate2 = 0;
	double durationCopyTo = 0;
	double durationFindLargest = 0;
	double durationShowWIndows = 0;
	double durationFindContours = 0;
	double durationContoursPoly = 0;

    //ROS_INFO("FindObject::imageCb] image.rows: %d, image.cols: %d", image.rows, image.cols);
    if (1 /*image.rows > 60 && image.cols > 60*/) {
    	Mat imgHSV;
    	if (TIME_STEPS) start = clock();
		cvtColor(image, imgHSV, CV_BGR2HSV); // Convert the captured frame from BGR to HSV
		if (TIME_STEPS) durationCvtColor = ( std::clock() - start ) / (double)CLOCKS_PER_SEC;

		Mat imgThresholded;
    	if (TIME_STEPS) start = clock();
		inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); //Threshold the image
		if (TIME_STEPS) durationInRange = ( std::clock() - start ) / (double)CLOCKS_PER_SEC;

		vector<vector<Point> > contours;
		vector<Vec4i> hierarchy;
		double contourSize;
		Mat tempImage;

    	if (TIME_STEPS) start = clock();
		imgThresholded.copyTo(tempImage);
		durationCopyTo = ( std::clock() - start ) / (double)CLOCKS_PER_SEC;

    	if (TIME_STEPS) start = clock();
		findContours(tempImage, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
		durationFindContours = ( std::clock() - start ) / (double)CLOCKS_PER_SEC;

    	if (TIME_STEPS) start = clock();
		vector<Rect> boundRect( contours.size() );
		vector<vector<Point> > contours_poly( contours.size() );
		durationContoursPoly = ( std::clock() - start ) / (double)CLOCKS_PER_SEC;

		//ROS_INFO("Thresholded iLowH: %d, iHighH: %d, iLowS: %d, iHighS: %d, iLowV: %d, iHighV: %d, countours: %d", iLowH, iHighH, iLowS, iHighS, iLowV, iHighV, contours.size());
		Point2f center;
		ROS_INFO("Found %d contours", contours.size());
		float radius;

    	if (TIME_STEPS) start = clock();
		if (!contours.empty()) {
			// Find largest blob.
			size_t maxBlobIndex = -1;
			int maxBlobSize = 0;
			for (size_t i = 0; i < contours.size(); i++) {
				contourSize = contourArea(contours[i]);
				if ((contourSize > maxBlobSize) && (contourSize >= contourSizeThreshold)) {
					maxBlobIndex = i;
					maxBlobSize = contourSize;
				}
			}

			if (maxBlobIndex != -1) {
				approxPolyDP( Mat(contours[maxBlobIndex]), contours_poly[maxBlobIndex], 3, true );
				boundRect[maxBlobIndex] = boundingRect( Mat(contours_poly[maxBlobIndex]) );
				minEnclosingCircle( (Mat) contours_poly[maxBlobIndex], center, radius );

				double x = center.x;
				double y = center.y;

				int cols = image.cols;
				int rows = image.rows;

				if (showWindows_) {
					Scalar color = Scalar(rand() % 255, rand() % 255, rand() % 255);
					circle(image, center, (int)radius, color, 2, 8, 0 );
				}

				stringstream msg;
				msg << "MidCamera:Found;X:" << x
					<< ";Y:" << y
					<< ";AREA:" << maxBlobSize
					<< ";I:" << maxBlobIndex
					<< ";ROWS:" << image.rows
					<< ";COLS:" << image.cols;
				std_msgs::String message;
				message.data = msg.str();
				midSampleFoundPub_.publish(message);
				ROS_INFO("[FindObject::imageCb] FOUND at x: %7.2f, y: %7.2f, area: %d", x, y, maxBlobSize);
			}
			durationFindLargest = ( std::clock() - start ) / (double)CLOCKS_PER_SEC;
		} else {
			stringstream msg;
			msg << "MidCamera:NotFound;X:0;Y:0;AREA:0;I:0;ROWS:"
				<< image.rows
				<< ";COLS:" << image.cols;
			std_msgs::String message;
			message.data = msg.str();
			midSampleFoundPub_.publish(message);
			ROS_INFO("[FindObject::imageCb] NOT FOUND");
			durationFindLargest = 0;
		}

		if (TIME_STEPS) start = clock();
		if (showWindows_) {
			imshow("[kaimi_mid_camera] Raw Image", image); //show the original image
			imshow("[kaimi_mid_camera] Thresholded Image", imgThresholded); //show the thresholded image
			// ROS_INFO("[FindObject::imageCb] showed images");
			cv::waitKey(25);
		}
		durationShowWIndows = ( std::clock() - start ) / (double)CLOCKS_PER_SEC;
		if (TIME_STEPS) ROS_INFO("durations cvtColor: %7.5f, inRange: %7.5f, erode1: %7.5f, dilate1: %7.5f, dilate2: %7.5f, erode2: %7.5f, findLargest: %7.5f, showWindows: %7.5f, copyTo: %7.5f, findContours: %7.5f, contoursPoly: %7.5f",
			durationCvtColor,
			durationInRange,
			durationErode1,
			durationDilate1,
			durationDilate2,
			durationErode2,
			durationFindLargest,
			durationShowWIndows,
			durationCopyTo,
			durationFindContours,durationContoursPoly);
    }
}

FindObject::FindObject() : it_(nh_),
	iLowH(101),
	iHighH(159),
	iLowS(124),
	iHighS(255),
	iLowV(13),
	iHighV(70),
	contourSizeThreshold(500),
	showWindows_(false) {
//	f = boost::bind(&FindObject::configurationCallback, _1, _2);
//	dynamicConfigurationServer.setCallback(f);

	ros::param::get("~image_topic_name", imageTopicName_);
	ros::param::get("~show_windows", showWindows_);
	ROS_INFO("PARAM image_topic_name: %s", imageTopicName_.c_str());
	ROS_INFO("PARAM show_windows: %d", showWindows_);
	ROS_INFO("[KaimiMidCamera iLowH: %d, iHighH: %d, iLowS: %d, iHighS: %d, iLowV: %d, iHighV: %d", iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);

	//showWindows_ = false;

	midSampleFoundPub_ = nh_.advertise<std_msgs::String>("midSampleFound", 2);
	if (showWindows_) {
		static const char* controlWindowName = "[kaimi_mid_camera] Control";

    	namedWindow("[kaimi_mid_camera] Raw Image", WINDOW_NORMAL);
    	namedWindow("[kaimi_mid_camera] Thresholded Image", WINDOW_NORMAL);

		// Create trackbars in "Control" window
		namedWindow(controlWindowName, CV_WINDOW_AUTOSIZE); //create a window called "Control"
		cvCreateTrackbar("LowH", controlWindowName, &iLowH, 179); //Hue (0 - 179)
		cvCreateTrackbar("HighH", controlWindowName, &iHighH, 179);

		cvCreateTrackbar("LowS", controlWindowName, &iLowS, 255); //Saturation (0 - 255)
		cvCreateTrackbar("HighS", controlWindowName, &iHighS, 255);

		cvCreateTrackbar("LowV", controlWindowName, &iLowV, 255); //Value (0 - 255)
		cvCreateTrackbar("HighV", controlWindowName, &iHighV, 255);

		cvCreateTrackbar("contourSizeThreshold", controlWindowName, &contourSizeThreshold, 2000);
	}

 	std::string camera_name = "midfield_camera";
	camera_info_manager::CameraInfoManager cinfo_(nh_, camera_name);

   	int fps;
	ros::param::get("~fps", fps);
    std::string color_mode = "BGR8";

    raspicam::RaspiCam_Cv cap;
    cap.open();
	ros::Rate rate(fps);
    while(ros::ok()) {
        Mat imgOriginal;

	 	clock_t start;
		double durationGrab;
		double durationRetrieve;

        if (TIME_STEPS) start = clock();
        cap.grab();
        durationGrab = ( std::clock() - start ) / (double)CLOCKS_PER_SEC;

        if (TIME_STEPS) start = clock();
        cap.retrieve(imgOriginal); // read a new frame from video
		durationRetrieve = ( std::clock() - start ) / (double)CLOCKS_PER_SEC;

		if (TIME_STEPS) ROS_INFO("Duration grab: %7.5f, retrieve: %7.5f", durationGrab, durationRetrieve);

        // std_msgs::Header header();
        // cv_bridge::CvImage imgmsg;
        // sensor_msgs::CameraInfo ci = cinfo_.getCameraInfo();
        // imgmsg.header.frame_id = camera_name + "_optical_frame";
        // ci.header.frame_id = imgmsg.header.frame_id;
        // imgmsg.encoding = sensor_msgs::image_encodings::BGR8;// color_mode;
        // imgmsg.image = imgOriginal;
        imageCb(imgOriginal);//, ci, ros::Time::now());
        rate.sleep();
        ros::spinOnce();
	};
}

FindObject& FindObject::Singleton() {
	static FindObject singleton_;
	return singleton_;
}