#include "skadi/sample.h"

#include <cmath>
#include <cstddef>
#include <fstream>
#include <limits>
#include <list>
#include <regex>
#include <stdexcept>
#include <string>
#include <sys/stat.h>

#include <boost/optional.hpp>

#include "baldr/compression_utils.h"
#include "filesystem.h"
#include "midgard/logging.h"
#include "midgard/pointll.h"

namespace {
// srtmgl1 holds 1x1 degree tiles but oversamples the egde of the tile
// by .5 seconds on all sides. that means that the center of pixel 0 is
// located at the tiles lat,lon (which is important for bilinear filtering)
// it also means that there are 3601 pixels per row and per column
constexpr size_t HGT_DIM = 3601;
constexpr size_t HGT_PIXELS = HGT_DIM * HGT_DIM;
constexpr size_t HGT_BYTES = sizeof(int16_t) * HGT_PIXELS;
constexpr int16_t NO_DATA_VALUE = -32768;
constexpr int16_t NO_DATA_HIGH = 16384;
constexpr int16_t NO_DATA_LOW = -16384;
constexpr size_t TILE_COUNT = 180 * 360;

// macro is faster than inline function for this..
#define out_of_range(v) v > NO_DATA_HIGH || v < NO_DATA_LOW

std::list<std::string> get_files(const std::string& root_dir) {
  std::list<std::string> files;
  if (filesystem::exists(root_dir) && filesystem::is_directory(root_dir)) {
    for (filesystem::recursive_directory_iterator i(root_dir), end; i != end; ++i) {
      if (i->is_regular_file() || i->is_symlink()) {
        files.push_back(i->path().string());
      }
    }
  }
  // couldn't get data
  if (files.empty()) {
    LOG_WARN(root_dir + " currently has no elevation tiles");
  }
  return files;
}

template <typename fmt_t> uint16_t is_hgt(const std::string& name, fmt_t& fmt) {
  std::smatch m;
  std::regex e(".*/([NS])([0-9]{2})([WE])([0-9]{3})\\.hgt(\\.gz)?$");
  if (std::regex_search(name, m, e)) {
    // enum class format_t{ UNKNOWN = 0, GZIP = 1, RAW = 3 };
    fmt = static_cast<fmt_t>(m[5].length() ? (m[5] == ".gz" ? 1 : 0) : 3);
    auto lon = std::stoi(m[4]) * (m[3] == "E" ? 1 : -1) + 180;
    auto lat = std::stoi(m[2]) * (m[1] == "N" ? 1 : -1) + 90;
    if (lon >= 0 && lon < 360 && lat >= 0 && lat < 180) {
      return lat * 360 + lon;
    }
  }
  return std::numeric_limits<uint16_t>::max();
}

int16_t flip(int16_t value) {
  return ((value & 0xFF) << 8) | ((value >> 8) & 0xFF);
}

uint64_t file_size(const std::string& file_name) {
  // TODO: detect gzip and actually validate the uncompressed size?
  struct stat s;
  int rc = stat(file_name.c_str(), &s);
  return rc == 0 ? s.st_size : -1;
}

} // namespace

namespace valhalla {
namespace skadi {

::valhalla::skadi::sample::sample(const std::string& data_source)
    : unzipped_cache(-1, std::vector<int16_t>()), data_source(data_source) {
  // messy but needed
  while (this->data_source.size() &&
         this->data_source.back() == filesystem::path::preferred_separator) {
    this->data_source.pop_back();
  }

  // If data_source is empty, do not allocate/resize mapped cache.
  if (data_source.empty()) {
    LOG_DEBUG("No elevation data_source was provided");
    return;
  }
  mapped_cache.resize(TILE_COUNT);

  // check the directory for files that look like what we need
  auto files = get_files(data_source);
  for (const auto& f : files) {
    // make sure its a valid index
    format_t format = format_t::UNKNOWN;
    auto index = is_hgt(f, format);
    if (index < mapped_cache.size() && format != format_t::UNKNOWN) {
      auto size = file_size(f);
      if (format == format_t::RAW && size != HGT_BYTES) {
        LOG_WARN("Corrupt elevation data: " + f);
        continue;
      }
      mapped_cache[index].first = format;
      mapped_cache[index].second.map(f, size, POSIX_MADV_SEQUENTIAL);
    }
  }
}

const int16_t* sample::source(uint16_t index) const {
  // bail if its out of bounds
  if (index >= TILE_COUNT) {
    return nullptr;
  }

  // if we dont have anything maybe its lazy loaded
  auto& mapped = mapped_cache[index];
  if (mapped.second.get() == nullptr) {
    auto f = data_source + get_hgt_file_name(index);
    auto size = file_size(f);
    if (size != HGT_BYTES) {
      return nullptr;
    }
    mapped.first = format_t::RAW;
    mapped.second.map(f, size, POSIX_MADV_SEQUENTIAL);
  }

  // we have it raw or we dont
  if (mapped.first == format_t::RAW) {
    return static_cast<const int16_t*>(static_cast<const void*>(mapped.second.get()));
  }

  // if we have it already unzipped
  if (unzipped_cache.first == index) {
    return unzipped_cache.second.data();
  }

  // for setting where to read compressed data from
  auto src_func = [&mapped](z_stream& s) -> void {
    s.next_in = static_cast<Byte*>(static_cast<void*>(mapped.second.get()));
    s.avail_in = static_cast<unsigned int>(mapped.second.size());
  };

  // for setting where to write the uncompressed data to
  auto dst_func = [this](z_stream& s) -> int {
    unzipped_cache.second.resize(HGT_PIXELS);
    s.next_out = static_cast<Byte*>(static_cast<void*>(unzipped_cache.second.data()));
    s.avail_out = HGT_BYTES;
    return Z_FINISH; // we know the output will hold all the input
  };

  // we have to unzip it
  if (!baldr::inflate(src_func, dst_func)) {
    LOG_WARN("Corrupt compressed elevation data");
    unzipped_cache.first = -1;
    return nullptr;
  }

  // update the simple cache
  // TODO: LRU
  unzipped_cache.first = index;
  return unzipped_cache.second.data();
}

template <class coord_t> double sample::get(const coord_t& coord) const {
  // check the cache and load
  auto lon = std::floor(coord.first);
  auto lat = std::floor(coord.second);
  auto index = static_cast<uint16_t>(lat + 90) * 360 + static_cast<uint16_t>(lon + 180);

  // get the proper source of the data
  const auto* t = source(index);
  if (t == nullptr) {
    return NO_DATA_VALUE;
  }

  // figure out what row and column we need from the array of data
  // NOTE: data is arranged from upper left to bottom right, so y is flipped

  // fractional pixel
  double u = (coord.first - lon) * (HGT_DIM - 1);
  double v = (1.0 - (coord.second - lat)) * (HGT_DIM - 1);

  // integer pixel
  size_t x = std::floor(u);
  size_t y = std::floor(v);

  // coefficients
  double u_ratio = u - x;
  double v_ratio = v - y;
  double u_inv = 1 - u_ratio;
  double v_inv = 1 - v_ratio;
  double a_coef = u_inv * v_inv;
  double b_coef = u_ratio * v_inv;
  double c_coef = u_inv * v_ratio;
  double d_coef = u_ratio * v_ratio;

  // values
  double adjust = 0;
  auto a = flip(t[y * HGT_DIM + x]);
  auto b = flip(t[y * HGT_DIM + x + 1]);
  if (out_of_range(a)) {
    a_coef = 0;
  }
  if (out_of_range(b)) {
    b_coef = 0;
  }

  // first part of the bilinear interpolation
  auto value = a * a_coef + b * b_coef;
  adjust += a_coef + b_coef;
  // LOG_INFO('{' + std::to_string(y * HGT_DIM + x) + ',' + std::to_string(a) + '}');
  // LOG_INFO('{' + std::to_string(y * HGT_DIM + x + 1) + ',' + std::to_string(b) + '}');
  // only need the second part if you aren't right on the row
  // this also protects from a corner case where you sample past the end of the image
  if (y < HGT_DIM - 1) {
    auto c = flip(t[(y + 1) * HGT_DIM + x]);
    auto d = flip(t[(y + 1) * HGT_DIM + x + 1]);
    if (out_of_range(c)) {
      c_coef = 0;
    }
    if (out_of_range(d)) {
      d_coef = 0;
    }
    // LOG_INFO('{' + std::to_string((y + 1) * HGT_DIM + x) + ',' + std::to_string(c) + '}');
    // LOG_INFO('{' + std::to_string((y + 1) * HGT_DIM + x + 1) + ',' + std::to_string(d) + '}');
    value += c * c_coef + d * d_coef;
    adjust += c_coef + d_coef;
  }
  // if we are missing everything then give up
  if (adjust == 0) {
    return NO_DATA_VALUE;
  }
  // if we were missing some we need to adjust by that
  return value / adjust;
}

template <class coords_t> std::vector<double> sample::get_all(const coords_t& coords) const {
  std::vector<double> values;
  values.reserve(coords.size());
  for (const auto& coord : coords) {
    values.emplace_back(get(coord));
  }
  return values;
}

double sample::get_no_data_value() {
  return NO_DATA_VALUE;
}

template <class coord_t> uint16_t sample::get_tile_index(const coord_t& coord) {
  auto lon = std::floor(coord.first);
  auto lat = std::floor(coord.second);
  return static_cast<uint16_t>(lat + 90) * 360 + static_cast<uint16_t>(lon + 180);
}

std::string sample::get_hgt_file_name(uint16_t index) {
  auto x = (index % 360) - 180;
  auto y = (index / 360) - 90;

  std::string name(y < 0 ? "/S" : "/N");
  y = std::abs(y);
  if (y < 10) {
    name.push_back('0');
  }
  name.append(std::to_string(y));
  name.append(name);

  name.append(x < 0 ? "W" : "E");
  x = std::abs(x);
  if (x < 100) {
    name.push_back('0');
  }
  if (x < 10) {
    name.push_back('0');
  }
  name.append(std::to_string(x));
  name.append(".hgt");

  return name;
}

// explicit instantiations for templated get
template double sample::get<std::pair<double, double>>(const std::pair<double, double>&) const;
template double sample::get<std::pair<float, float>>(const std::pair<float, float>&) const;
template double sample::get<midgard::PointLL>(const midgard::PointLL&) const;
template double sample::get<midgard::Point2>(const midgard::Point2&) const;
template std::vector<double> sample::get_all<std::list<std::pair<double, double>>>(
    const std::list<std::pair<double, double>>&) const;
template std::vector<double> sample::get_all<std::vector<std::pair<double, double>>>(
    const std::vector<std::pair<double, double>>&) const;
template std::vector<double>
sample::get_all<std::list<std::pair<float, float>>>(const std::list<std::pair<float, float>>&) const;
template std::vector<double> sample::get_all<std::vector<std::pair<float, float>>>(
    const std::vector<std::pair<float, float>>&) const;
template std::vector<double>
sample::get_all<std::list<midgard::PointLL>>(const std::list<midgard::PointLL>&) const;
template std::vector<double>
sample::get_all<std::vector<midgard::PointLL>>(const std::vector<midgard::PointLL>&) const;
template std::vector<double>
sample::get_all<std::list<midgard::Point2>>(const std::list<midgard::Point2>&) const;
template std::vector<double>
sample::get_all<std::vector<midgard::Point2>>(const std::vector<midgard::Point2>&) const;
template uint16_t
sample::get_tile_index<std::pair<double, double>>(const std::pair<double, double>& coord);
template uint16_t
sample::get_tile_index<std::pair<float, float>>(const std::pair<float, float>& coord);
template uint16_t sample::get_tile_index<midgard::PointLL>(const midgard::PointLL& coord);
template uint16_t sample::get_tile_index<midgard::Point2>(const midgard::Point2& coord);

} // namespace skadi
} // namespace valhalla


// -*- C++ -*-
//
// Jiao Lin <jiao.lin@gmail.com>
//

#include <sstream>
#include <boost/python.hpp>
#include "mccomponents/kernels/sample/AbstractSQE.h"
#include "mccomponents/kernels/sample/SQE_EnergyFocusing_Kernel.h"
#include "mccomponents/boostpython_binding/wrap_kernel.h"

namespace wrap_mccomponents {

  void wrap_SQE_EnergyFocusing_Kernel()
  {
    using namespace boost::python;
    using namespace mccomponents::boostpython_binding;
    using namespace mccomponents::sample;

    typedef mccomponents::kernels::SQE_EnergyFocusing_Kernel w_t;

    kernel_wrapper<w_t>::wrap
      ("SQE_EnergyFocusing_Kernel",
       init<double, double, double,
       AbstractSQE &,
       double, double, double, double, double, double> ()
       [with_custodian_and_ward<1,5> () ]
       )
      ;
  }
}

// End of file


#include "../include/omnifirewire.h"

#include <numeric>

#include <boost/bind.hpp>

OmniFirewire::OmniFirewire(const std::string &serial_number, const std::string &name, const std::string &path_urdf, const std::string &path_srdf)
    : OmniBase(name, path_urdf, path_srdf),
      gimbal_filter_1(GIMBAL_FILTER_SIZE),
      gimbal_filter_2(GIMBAL_FILTER_SIZE),
      gimbal_filter_3(GIMBAL_FILTER_SIZE),
      serial_number_(serial_number),
      port_(-1), node_(-1),
      handle_(NULL), tx_handle_(NULL), rx_handle_(NULL),
      msg_rx_iso_channel(0x1001, 1, -1),
      msg_tx_iso_channel(0x1000, 1, -1),
      msg_start(0x1087, 1, 0x08),
      msg_stop(0x1087, 1, 0x00),
      msg_unknown(0x20010, 4, 0xf80f0000),
     thread_status(THREAD_READY)
{
    this->resetTorque();
}

OmniFirewire::~OmniFirewire()
{
    this->disconnect();
}

enum raw1394_iso_disposition OmniFirewire::callbackRead(raw1394handle_t handle, unsigned char* data, unsigned int len, unsigned char channel, unsigned char tag, unsigned char sy, unsigned int cycle, unsigned int dropped)
{
    // Get the OmniFirewire object for this handle.
    OmniFirewire* omni = reinterpret_cast<OmniFirewire*>(raw1394_get_userdata(handle));
    if (omni == NULL)
    {
        return RAW1394_ISO_OK;
    }

    const OmniFirewire::RxIsoBuffer* newbuf = reinterpret_cast<OmniFirewire::RxIsoBuffer*>(data);

    OmniBase::LockUnique lock( omni->getStateMutex() );

    // Calibrate if docked.
    if (!newbuf->status.undocked && !omni->state.calibrated)
    {
        ROS_INFO("Calibrating robot...");

        // Calibrate.
        // All angles should be ~= 0 when docked.
        omni->state.angles_docked[0] = 2.0 * M_PI *  newbuf->encoder_x / 15000.0;
        omni->state.angles_docked[1] = 2.0 * M_PI * -newbuf->encoder_y / 15000.0;
        omni->state.angles_docked[2] = 2.0 * M_PI *  newbuf->encoder_z / 15000.0 + 0.37;
        // Calibrated: the dock LED should be always on.
        omni->tx_iso_buffer_.status.dock_led0 = 1;
        omni->tx_iso_buffer_.status.dock_led1 = 1;
        omni->state.calibrated = true;
    }

    // Getting time difference between two consecutive readings.
//    omni->state.time_last_angle_acquisition = omni->state.time_current_angle_acquisition;
    omni->state.time_current_angle_acquisition = boost::posix_time::microsec_clock::local_time();

    // Compute the encoder values.
    // All angles should be ~= 0 when docked.
    omni->state.angles[0] = 2.0 * M_PI
            *  newbuf->encoder_x / 15000.0 - omni->state.angles_docked[0];

    omni->state.angles[1] = 2.0 * M_PI
            * -newbuf->encoder_y / 15000.0 - omni->state.angles_docked[1];

    omni->state.angles[2] = 2.0 * M_PI
            *  newbuf->encoder_z / 15000.0 - omni->state.angles_docked[2]
            - omni->state.angles[1] + 0.37;

    // Adding values to the filter vector.
    std::rotate(omni->gimbal_filter_1.begin(),
                omni->gimbal_filter_1.begin() + 1,
                omni->gimbal_filter_1.end());
    omni->gimbal_filter_1[GIMBAL_FILTER_SIZE - 1] = (double)newbuf->gimbal_a_x
            / (newbuf->gimbal_a_x + newbuf->gimbal_b_x);
    //
    std::rotate(omni->gimbal_filter_2.begin(),
                omni->gimbal_filter_2.begin() + 1,
                omni->gimbal_filter_2.end());
    omni->gimbal_filter_2[GIMBAL_FILTER_SIZE - 1] = (double)newbuf->gimbal_a_y
            / (newbuf->gimbal_a_y + newbuf->gimbal_b_y);
    //
    std::rotate(omni->gimbal_filter_3.begin(),
                omni->gimbal_filter_3.begin() + 1,
                omni->gimbal_filter_3.end());
    omni->gimbal_filter_3[GIMBAL_FILTER_SIZE - 1] = (double)newbuf->gimbal_a_z
            / (newbuf->gimbal_a_z + newbuf->gimbal_b_z);

    if (omni->pot_filter_count_ <= 0)
    {
        // Compute the mean.
        omni->pot_filter_accum_[0] =
                std::accumulate(omni->gimbal_filter_1.begin(), omni->gimbal_filter_1.end(), 0.0)
                / GIMBAL_FILTER_SIZE;
        omni->pot_filter_accum_[1] =
                std::accumulate(omni->gimbal_filter_2.begin(), omni->gimbal_filter_2.end(), 0.0)
                / GIMBAL_FILTER_SIZE;
        omni->pot_filter_accum_[2] =
                std::accumulate(omni->gimbal_filter_3.begin(), omni->gimbal_filter_3.end(), 0.0)
                / GIMBAL_FILTER_SIZE;

        // Compute the gimbal values.
        // All angles should be ~= 0 when docked.
        omni->state.angles[3] = -(5.48 * omni->pot_filter_accum_[0] - 2.74) + 0.020 + 0.025367101673835286;
        omni->state.angles[4] = -(5.28 * 1.9 * omni->pot_filter_accum_[1] + 2.64) + 8.76231837158503;
        omni->state.angles[5] =  (4.76 * omni->pot_filter_accum_[2] - 2.3) - 0.085;

    }
    else
    {
        --omni->pot_filter_count_;
    }

    // Calculates forward kinematics and joint velocities.
    omni->updateRobotState();

    // Get the button state.
    omni->state.buttons[0] = !newbuf->status.button1;
    omni->state.buttons[1] = !newbuf->status.button2;

    return RAW1394_ISO_OK;
}

enum raw1394_iso_disposition OmniFirewire::callbackWrite(raw1394handle_t handle, unsigned char *data, unsigned int* len, unsigned char *tag, unsigned char *sy, int, unsigned int)
{
    *len = 0;
    *tag = 0;
    *sy = 0;
    // Get the OmniFirewire object for this handle.
    OmniFirewire *omni = reinterpret_cast<OmniFirewire*>(raw1394_get_userdata(handle));
    if (omni == NULL)
    {
        return RAW1394_ISO_OK;
    }

    // Edit force output
    {
        OmniBase::LockShared lock( omni->getStateMutex() );
        if (omni->state.control_on)
        {
            omni->tx_iso_buffer_.force_x = omni->state.control[0];
            omni->tx_iso_buffer_.force_y = omni->state.control[1];
            omni->tx_iso_buffer_.force_z = omni->state.control[2];
        }
        else
        {
            omni->tx_iso_buffer_.status.force_enabled = 0;
        }
    }

    if (omni->enable_force_flag)
    {
        if (omni->tx_iso_buffer_.status.force_enabled == 1)
            omni->tx_iso_buffer_.status.force_enabled = 0;
        else
        {
            omni->tx_iso_buffer_.status.force_enabled = 1;
            omni->enable_force_flag = false;
        }
    }

    // Copy data.
    std::memcpy(data, &omni->tx_iso_buffer_, sizeof(omni->tx_iso_buffer_));
    *len = sizeof(omni->tx_iso_buffer_);
    *tag = 0;
    *sy = 0;
    return RAW1394_ISO_OK;
}

bool OmniFirewire::connect()
{
    // Make sure the device is disconnected at this point.
    disconnect();

    // Find the Phantom Omni.
    const std::vector<OmniInfo>& omnis = enumerate_omnis();
    typedef std::vector<OmniInfo>::const_iterator CIT;
    for (CIT it = omnis.begin(); it != omnis.end(); ++it) {
        if (it->serial == serial_number_) {
            // Serial number matched: found the omni!
            port_ = it->port;
            node_ = it->node;

            break;
        }
    }
    if (node_ == -1) {
        // Failed to find the omni.
        return false;
    }

    // Create a 1394 handle.
    handle_ = raw1394_new_handle();
    raw1394_set_userdata(handle_, this);

    // Set the port.
    if (raw1394_set_port(handle_, port_) == -1) {
        return false;
    }

    // Initialize the Tx buffer.
    tx_iso_buffer_.force_x     = 0x07ff;
    tx_iso_buffer_.force_y     = 0x07ff;
    tx_iso_buffer_.force_z     = 0x07ff;
    tx_iso_buffer_.status.bits = 0x53c0;
    tx_iso_buffer_.padding1    = 0x0000;
    tx_iso_buffer_.padding2    = 0x0000;
    tx_iso_buffer_.status.dock_led0 = 0;
    tx_iso_buffer_.status.dock_led1 = 1;
    tx_iso_buffer_.status.force_enabled = 1;

    // Reset the pot filter counter.
    pot_filter_accum_[0] = 0.0;
    pot_filter_accum_[1] = 0.0;
    pot_filter_accum_[2] = 0.0;
    pot_filter_count_ = GIMBAL_FILTER_SIZE;

    // Start isochronous transmission.
    if (!startIsochronousTransmission()) {
        ROS_ERROR("Failed to start isochronous transmission!");
        return false;
    }
    state.connected = true;
    ROS_INFO("Isochronous transmission started successfully.");
    return true;
}

bool OmniFirewire::connected()
{
    return this->state.connected;
}

void OmniFirewire::wakeup()
{
    raw1394_wake_up(rx_handle_);
    raw1394_wake_up(tx_handle_);
    this->disconnect();
    this->connect();
}

void OmniFirewire::disconnect()
{
    // Turn the dock LED off.
    tx_iso_buffer_.status.dock_led0 = 0;
    tx_iso_buffer_.status.dock_led1 = 0;

    {
        OmniBase::LockUnique lock( getStateMutex() );
        this->state.calibrated = false;
    }

    // Stop isochronous data transmission.
    stopIsochronousTransmission();

    port_ = -1;
    node_ = -1;

    if ( connected() )
    {
        raw1394_destroy_handle(handle_);
        handle_ = NULL;
    }

}

std::vector<OmniFirewire::OmniInfo> OmniFirewire::enumerate_omnis()
{
    std::vector<OmniInfo> omnis;
    raw1394handle_t handle = raw1394_new_handle();

    // Get information about the available ports.
    const int maxports = 5;
    struct raw1394_portinfo portinfo[maxports];
    int n_ports = raw1394_get_port_info(handle, portinfo, maxports);

    // Set messages
    Raw1394Msg msg_dev_id = Raw1394Msg(0x1006000c, sizeof(quadlet_t));
    Raw1394Msg msg_serial = Raw1394Msg(0x10060010, sizeof(quadlet_t));

    // Iterate through the list of ports.
    for (int p = 0 ; p < n_ports; p++) {
        // Open the port if we can.
        if (portinfo[p].nodes == 0 || raw1394_set_port(handle, p) == -1) {
            // Not the port we're interested in.
            continue;
        }

        // Iterate through the list of nodes attached to the port.
        for (int n = 0; n < portinfo[p].nodes; n++) {
            // Compute the node ID. See IEEE 1394 spec for details.
            uint32_t node = (1023 << 6) | n;

            // Check the device ID.
            msg_dev_id.read(handle, node);
            if (msg_dev_id.getData() != 0x000b9900) {
                // Not Phantom Omni.
                continue;
            }

            // Now we know it's Phantom Omni. Store the info.
            msg_serial.read(handle, node);
            const std::string &unpacked_serial = OmniFirewire::unpackSerialNumber(msg_serial);
            omnis.push_back(OmniInfo(unpacked_serial, p, node));
        }
    }

    return omnis;
}

void * OmniFirewire::isoThreadCallback()
{
    this->rx_handle_status = 0;
    this->tx_handle_status = 0;
    while (this->thread_status != THREAD_TERMINATE
           && this->rx_handle_status != -1
           && this->tx_handle_status != -1)
    {
        this->rx_handle_status = raw1394_loop_iterate(this->rx_handle_);
        this->tx_handle_status = raw1394_loop_iterate(this->tx_handle_);
    }

    this->state.connected = false;

    return NULL;
}

uint32_t OmniFirewire::pack_serial_number(const std::string& unpacked)
{
    // Serial number must have 11 characters.
    if (unpacked.size() != 11) {
        // Invalid length.
        return 0;
    }

    // Extract digits into 5 integers.
    uint32_t a, b, c, d, e;
    int filled = std::sscanf(unpacked.c_str(),
                             "%1u%2u%2u%1u%5u", &a, &b, &c, &d, &e);
    if (filled != 5) {
        // Extraction failed.
        return 0;
    }

    uint32_t packed = 0x80000001;
    packed |= ((a & 0x007) << 23);  // a maps to Bit 25..23.
    packed |= ((b & 0x00f) << 19);  // b maps to Bit 22..19.
    packed |= ((c & 0x01f) << 14);  // c maps to Bit 18..14.
    packed |= ((d & 0x007) << 11);  // d maps to Bit 13..11.
    packed |= ((e & 0x3ff) <<  1);  // e maps to Bit 10..1.
    return packed;
}

bool OmniFirewire::startIsochronousTransmission()
{
    // Make sure raw1394 is ready.
    if (handle_ == NULL) {
        return false;
    }

    // Stop previous transmission.
    stopIsochronousTransmission();

    // Find a free isochronous channel number for Tx.
    for (int ch = 0; ch < 63; ch++) {
        if (raw1394_channel_modify(handle_, ch, RAW1394_MODIFY_ALLOC) == 0) {
            msg_tx_iso_channel = ch;
            break;
        }
    }
    if (msg_tx_iso_channel == (quadlet_t) -1) {
        // Channel not found. Abort.
        stopIsochronousTransmission();
        return false;
    }

    // Set the Tx isochronous channel.
    msg_tx_iso_channel.write(handle_, node_);

    // Set the Rx isochronous channel.
    msg_rx_iso_channel.write(handle_, node_);

    // Not sure what this does, but it is required.
    msg_unknown.write(handle_, node_);

    // Tell Omni to start isochronous data transmission.
    msg_start.write(handle_, node_);

    // Register the read (rx) callback that will be called by the driver
    rx_handle_ = raw1394_new_handle_on_port(port_);
    raw1394_set_userdata(rx_handle_, this);
    if (raw1394_iso_recv_init(rx_handle_,
                              OmniFirewire::callbackRead,
                              100,
                              0x40,
                              msg_rx_iso_channel,
                              RAW1394_DMA_PACKET_PER_BUFFER,
                             -1) == -1)
    {
        stopIsochronousTransmission();
        return false;
    }

    if (raw1394_iso_recv_start(rx_handle_, -1, -1, 0) == -1)
    {
        stopIsochronousTransmission();
        return false;
    }

    // Register the write (tx) callback that will be called by the driver
    tx_handle_ = raw1394_new_handle_on_port(port_);
    raw1394_set_userdata(tx_handle_, this);
    if (raw1394_iso_xmit_init(tx_handle_,
                              OmniFirewire::callbackWrite,
                              1,
                              sizeof(tx_iso_buffer_),
                              msg_tx_iso_channel,
                              RAW1394_ISO_SPEED_100,
                             -1) == -1)
    {
        stopIsochronousTransmission();
        return false;
    }

    if (raw1394_iso_xmit_start(tx_handle_, -1, -1) == -1)
    {
        stopIsochronousTransmission();
        return false;
    }

    // Create iso thread
    thread_driver = ThreadPtr( new boost::thread( boost::bind(&OmniFirewire::isoThreadCallback, this) ) );
    thread_status = THREAD_STARTED;

    return true;
}

void OmniFirewire::stopIsochronousTransmission()
{
    // Stop the thread.
    if (thread_status != THREAD_READY) {
        thread_status = THREAD_TERMINATE;

        // Make sure any pending calls are destroyed
        raw1394_wake_up(rx_handle_);
        raw1394_wake_up(tx_handle_);

        thread_driver->join();
    }
    thread_status = THREAD_READY;

    // Make sure raw1394 is ready.
    if (handle_ == NULL) {
        return;
    }

    // Tell Omni to stop isochronous transmission.
    msg_stop.write(handle_, node_);

    if (msg_tx_iso_channel != (quadlet_t) -1) {
        raw1394_channel_modify(handle_, msg_tx_iso_channel, RAW1394_MODIFY_FREE);
        msg_tx_iso_channel.clearData();
    }

    if (msg_rx_iso_channel != (quadlet_t) -1) {
        raw1394_channel_modify(handle_, msg_rx_iso_channel, RAW1394_MODIFY_FREE);
        msg_rx_iso_channel.clearData();
    }

    if (tx_handle_ != NULL) {
        raw1394_destroy_handle(tx_handle_);
        tx_handle_ = NULL;
    }

    if (rx_handle_ != NULL) {
        raw1394_destroy_handle(rx_handle_);
        rx_handle_ = NULL;
    }
}

void OmniFirewire::mapTorque()
{
    state.control[0] = (state.control[0] + 1) / 2 * 4095;
    state.control[1] = (state.control[1] + 1) / 2 * 4095;
    state.control[2] = (state.control[2] + 1) / 2 * 4095;
}

std::string OmniFirewire::unpackSerialNumber(uint32_t packed)
{
    // Unpack the variables based on the following rule:
    //     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
    //     |1|0|0|0|0|0|  a  |   b   |    c    |  d  |         e         |1|
    //     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
    //     |31           24|23           16|15            8|7             0|

    uint32_t a = (packed >> 23) & 0x007;
    uint32_t b = (packed >> 19) & 0x00f;
    uint32_t c = (packed >> 14) & 0x01f;
    uint32_t d = (packed >> 11) & 0x007;
    uint32_t e = (packed >>  1) & 0x3ff;

    std::stringstream unpacked;
    unpacked << std::setfill('0');
    unpacked << std::setw(1) << a;
    unpacked << std::setw(2) << b;
    unpacked << std::setw(2) << c;
    unpacked << std::setw(1) << d;
    unpacked << std::setw(5) << e;
    return unpacked.str();
}


#include "SDALORCalculateWalkCorrection.h"
#include <boost/concept_check.hpp>

ClassImp(SDALORCalculateWalkCorrection);

SDALORCalculateWalkCorrection::SDALORCalculateWalkCorrection(const char* name, const char* title, const char* in_file_suffix, const char* out_file_suffix, std::map<int, double> thresholds): JPetCommonAnalysisModule(name, title, in_file_suffix, out_file_suffix)
{
  fThresholds = thresholds;
  gStyle->SetOptFit(1);
}

SDALORCalculateWalkCorrection::~SDALORCalculateWalkCorrection()
{

}

void SDALORCalculateWalkCorrection::begin()
{
  //prepare structure to contain corrections and data from strips
  //find out how many strips are there and make as many structures for coefficients storage

  const JPetParamBank& bank = getParamBank();

  for( int i = 0; i < bank.getPMsSize(); i++)
  {
    walkCoefficients dummy;
    dummy.scintID = bank.getPM(i).getID();
    dummy.alpha = 0;
    dummy.beta = 0;
    coefficients.push_back( dummy );
    std::vector<std::pair<double, double> > dummyVec;
    points[ bank.getPM(i).getID() ] = dummyVec;
  }

  std::cout << "Coefficients to be filled " << coefficients.size() << " dummies \n";

}

void SDALORCalculateWalkCorrection::end()
{
    //doWalkCorrectionForEachPMTIndividually();
    doWalkForTimeDifference();
    doWalkForTOF();
}

void SDALORCalculateWalkCorrection::doWalkForTOF()
{
    TCanvas* c = new TCanvas();
    TH2F* histo = new TH2F("","", 100, -0.3, 0.3, 100, -1000, 1000);
    TH1F* deltaT = new TH1F("","", 500, -1000, 1000);
    std::vector<double> times, chargeSquareInverted;
    TString filePath = (std::string)fHeader->getBaseFileName();
    	filePath = filePath(filePath.Last('/')+1, filePath.Length()-1);
        filePath = filePath(0, filePath.Last('.'));
	std::stringstream buf;
	buf << fHeader->getSourcePosition();
	filePath+= buf.str();

    for(unsigned int j = 0; j < fTOFs.size(); j++)
    {

      double time = (fTOFs[j].first);
      double chargeSquareInvert =  fTOFs[j].second  ;

      histo->Fill( chargeSquareInvert, time );
      deltaT->Fill(time);
      times.push_back(time);
      chargeSquareInverted.push_back(chargeSquareInvert);
    }

    histo->Draw("colz");
    c->SaveAs( filePath+"walkBefore.png" ) ;

    TGraph* graph = new TGraph( fTOFs.size(), &chargeSquareInverted[0], &times[0]);
    graph->Draw("AP");
    TF1* line = new TF1("line","[0]+x*[1]", -0.3, 0.3);
    graph->Fit("line");
    graph->GetXaxis()->SetTitle("1/sqrt(TOT) [1/sqrt(ps)]");
    graph->GetYaxis()->SetTitle("Time at threshold [ps]");
    graph->Draw("AP");
    c->SaveAs( filePath+"walkGraphBefore.png" ) ;

    deltaT->Fit("gaus");
    deltaT->Draw();
    c->SaveAs( filePath+"TOFBefore.png" );

    histo->Reset();
    deltaT->Reset();
    times.clear();
    chargeSquareInverted.clear();

    for(unsigned int j = 0; j < fTOFs.size(); j++)
    {
      double time = fTOFs[j].first - fTOFs[j].second *(double)line->GetParameter(1);
      double chargeSquareInvert =  fTOFs[j].second  ;

      histo->Fill( chargeSquareInvert, time );
      deltaT->Fill(time);
      times.push_back(time);
      chargeSquareInverted.push_back(chargeSquareInvert);
    }

    histo->Draw("colz");
    c->SaveAs( filePath+"walkAfter.png" );

    graph = new TGraph( fTOFs.size() , &chargeSquareInverted[0], &times[0]);
    graph->Draw("AP");
    c->SaveAs( filePath+"walkGraphAfter.png" );

    deltaT->Fit("gaus");
    deltaT->Draw();
    c->SaveAs( filePath+"TOFAfter.png" );

    std::ofstream resultFile;
    resultFile.open("walkResults.txt", std::ios::app);
    resultFile << "file:\t for TOF" << filePath;
    resultFile << "\tconstant:\t" << line->GetParameter(0) <<"\t+/-\t" << line->GetParError(0);
    resultFile << "\talpha:\t" << line->GetParameter(1) <<"\t+/-\t" << line->GetParError(1) << std::endl;
    resultFile.close();

    delete graph;
    delete deltaT;
    delete histo;
    delete c;
}

void SDALORCalculateWalkCorrection::doWalkForTimeDifference()
{
  const JPetParamBank& bank = getParamBank();
  for( int i = 0; i < bank.getScintillatorsSize(); i++)
  {
    TCanvas* c = new TCanvas();
    TH2F* histo = new TH2F("","", 50, -0.1, 0.4, 400, -10000, 10000);
    TH1F* deltaT = new TH1F("","", 400, -10000, 10000);
    std::vector<double> times, chargeSquareInverted;

    for(unsigned int j = 0; j < points[ bank.getScintillator(i).getID() ].size(); j++)
    {

      double time = points[ bank.getScintillator(i).getID() ][j].first;
      double chargeSquareInvert =  points[ bank.getScintillator(i).getID() ][j].second  ;

      histo->Fill( chargeSquareInvert, time );
      deltaT->Fill(time);
      times.push_back(time);
      chargeSquareInverted.push_back(chargeSquareInvert);
    }
    std::stringstream buf;
    buf << bank.getScintillator(i).getID();
    histo->GetYaxis()->SetRangeUser( histo->GetMean(2) - 1000, histo->GetMean(2) + 1000);
    histo->Draw("colz");

    TGraph* graph = new TGraph(points[bank.getScintillator(i).getID()].size(), &chargeSquareInverted[0], &times[0]);
    graph->Draw("AP");
    TF1* line = new TF1("line","[0]+x*[1]", 0, 1);
    graph->Fit("line");
    graph->GetXaxis()->SetTitle("1/#sqrt{pC} [1/#sqrt{ps}]");
    graph->GetYaxis()->SetTitle("Time at threshold [ps]");
    graph->Draw("AP");
    c->SaveAs( (buf.str()+"graphBefore.png").c_str() );

    deltaT->GetXaxis()->SetRangeUser(deltaT->GetMean() - 2000, deltaT->GetMean() + 2000);
    deltaT->Fit("gaus");
    deltaT->Draw();
    c->SaveAs( (buf.str()+"deltaTBefore.png").c_str() );

    histo->Draw("colz");
    line->Draw("same");
    c->SaveAs( (buf.str()+"Before.png").c_str() );
    TString filePath = (std::string)fHeader->getBaseFileName();
    	filePath = filePath(filePath.Last('/')+1, filePath.Length()-1);
        filePath = filePath(0, filePath.Last('.'));
	std::stringstream buf1;
	buf1 << fHeader->getSourcePosition();
	filePath+= buf1.str();

    std::ofstream resultFile;
    resultFile.open("walkResults.txt", std::ios::app);
    resultFile << "file:\t for DT \t" << filePath << "\t for scint " << bank.getScintillator(i).getID();
    resultFile << "\tconstant:\t" << line->GetParameter(0) <<"\t+/-\t" << line->GetParError(0);
    resultFile << "\talpha:\t" << line->GetParameter(1) <<"\t+/-\t" << line->GetParError(1) << std::endl;
    resultFile.close();
    histo->Reset();
    deltaT->Reset();
    times.clear();
    chargeSquareInverted.clear();

    for(unsigned int j = 0; j < points[ bank.getScintillator(i).getID() ].size(); j++)
    {
      double time = points[ bank.getScintillator(i).getID() ][j].first - (points[ bank.getScintillator(i).getID() ][j].second) *(double)line->GetParameter(1);
      double chargeSquareInvert = points[ bank.getScintillator(i).getID() ][j].second;

      histo->Fill( chargeSquareInvert, time);
      deltaT->Fill(time);
      times.push_back(time);
      chargeSquareInverted.push_back(chargeSquareInvert);
    }

    histo->GetYaxis()->SetRangeUser( histo->GetMean(2) - 1000, histo->GetMean(2) + 1000);
    histo->Draw("colz");
    c->SaveAs( (buf.str()+"After.root").c_str() );

    graph = new TGraph(points[bank.getScintillator(i).getID()].size(), &chargeSquareInverted[0], &times[0]);
    graph->Draw("AP");
    c->SaveAs( (buf.str()+"graphAfter.png").c_str() );

    deltaT->GetXaxis()->SetRangeUser(deltaT->GetMean() - 2000 , deltaT->GetMean() + 2000 );
    deltaT->Fit("gaus");
    deltaT->Draw();
    c->SaveAs( (buf.str()+"deltaTAfter.png").c_str() );
    buf.str(std::string() );

    histo->Reset();
    deltaT->Reset();
    times.clear();
    chargeSquareInverted.clear();
       delete graph;
    delete deltaT;
    delete histo;
    delete c;
  }
}

void SDALORCalculateWalkCorrection::doWalkCorrectionForEachPMTIndividually()
{
  TH1F* timeDiffAfterCorrection = new TH1F("","", 100, -1000, 1000);
  TCanvas* g = new TCanvas();
  std::vector< std::vector<double> > timesCorrected;
  const JPetParamBank& bank = getParamBank();
  // do plot and find coeeficients, save them in container
  for( int i = 0; i < bank.getPMsSize(); i++)
  {
    TCanvas* c = new TCanvas();
    TH2F* histo = new TH2F("","", 50, -0.1, 0.4, 400, -10000, 10000);
    TH1F* deltaT = new TH1F("","", 400, -10000, 10000);

    std::vector<double> times, chargeSquareInverted;
    for(unsigned int j = 0; j < points[ bank.getPM(i).getID() ].size(); j++)
    {

      double time = points[ bank.getPM(i).getID() ][j].first;
      double chargeSquareInvert =  1.0/sqrt( points[ bank.getPM(i).getID() ][j].second ) ;

      histo->Fill( chargeSquareInvert, time );
      deltaT->Fill(time);
      times.push_back(time);
      chargeSquareInverted.push_back(chargeSquareInvert);
    }

    std::stringstream buf;
    buf << bank.getPM(i).getID();
    histo->GetYaxis()->SetRangeUser( histo->GetMean(2) - 1000, histo->GetMean(2) + 1000);
    histo->Draw("colz");

    TGraph* graph = new TGraph(points[bank.getPM(0).getID()].size(), &chargeSquareInverted[0], &times[0]);
    graph->Draw("AP");
    TF1* line = new TF1("line","[0]+x*[1]", 0, 1);
    graph->Fit("line");
    graph->GetXaxis()->SetTitle("1/sqrt(Charge) [1/sqrt(pC)]");
    graph->GetYaxis()->SetTitle("Time at threshold [ps]");
    graph->Draw("AP");
    c->SaveAs( (buf.str()+"graphBefore.png").c_str() );

    deltaT->GetXaxis()->SetRangeUser(deltaT->GetMean() - 2000, deltaT->GetMean() + 2000);
    deltaT->Fit("gaus");
    deltaT->Draw();
    c->SaveAs( (buf.str()+"deltaTBefore.png").c_str() );

    histo->Draw("colz");
    line->Draw("same");
    c->SaveAs( (buf.str()+"Before.png").c_str() );

    histo->Reset();
    deltaT->Reset();
    times.clear();
    chargeSquareInverted.clear();

    for(unsigned int j = 0; j < points[ bank.getPM(i).getID() ].size(); j++)
    {
      double time = points[ bank.getPM(i).getID() ][j].first - 1.0/sqrt( points[ bank.getPM(i).getID() ][j].second ) *(double)line->GetParameter(1);
      double chargeSquareInvert =  1.0/sqrt( points[ bank.getPM(i).getID() ][j].second );

      histo->Fill( chargeSquareInvert, time);
      deltaT->Fill(time);
      times.push_back(time);
      chargeSquareInverted.push_back(chargeSquareInvert);
    }

    histo->GetYaxis()->SetRangeUser( histo->GetMean(2) - 1000, histo->GetMean(2) + 1000);
    histo->Draw("colz");
    c->SaveAs( (buf.str()+"After.png").c_str() );

    graph = new TGraph(points[bank.getPM(0).getID()].size(), &chargeSquareInverted[0], &times[0]);
    graph->Draw("AP");
    c->SaveAs( (buf.str()+"graphAfter.png").c_str() );

    deltaT->GetXaxis()->SetRangeUser(deltaT->GetMean() - 2000 , deltaT->GetMean() + 2000 );
    deltaT->Fit("gaus");
    deltaT->Draw();
    c->SaveAs( (buf.str()+"deltaTAfter.png").c_str() );

    buf.str(std::string() );

    timesCorrected.push_back(times);

    delete deltaT;
    delete histo;
    delete c;
  }

  for(unsigned int i = 0; i < timesCorrected[0].size(); i++ )
  {
      timeDiffAfterCorrection->Fill(timesCorrected[0][i] - timesCorrected[1][i]);
  }

  timeDiffAfterCorrection->Draw();
  timeDiffAfterCorrection->Fit("gaus");
  g->SaveAs("test.png");
  delete g;
  delete timeDiffAfterCorrection;

}

void SDALORCalculateWalkCorrection::prepareTimesForPMTCorrection(JPetLOR& fLOR)
{
  double threshold = fThresholds[ fLOR.getFirstHit().getSignalA().getRecoSignal().getPM().getScin().getID() ];
  std::pair<double,double> topLeft= std::make_pair(fLOR.getFirstHit().getSignalA().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getFirstHit().getSignalA().getRecoSignal().getCharge());
  std::pair<double,double> topRight= std::make_pair(fLOR.getFirstHit().getSignalB().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getFirstHit().getSignalB().getRecoSignal().getCharge());
  threshold = fThresholds[ fLOR.getSecondHit().getSignalA().getRecoSignal().getPM().getScin().getID() ];
  std::pair<double,double> bottomLeft= std::make_pair(fLOR.getSecondHit().getSignalA().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getSecondHit().getSignalA().getRecoSignal().getCharge());
  std::pair<double,double> bottomRight= std::make_pair(fLOR.getSecondHit().getSignalB().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getSecondHit().getSignalB().getRecoSignal().getCharge());

  (points[ fLOR.getFirstHit().getSignalA().getRecoSignal().getPM().getID() ]).push_back( topLeft );
  (points[ fLOR.getFirstHit().getSignalB().getRecoSignal().getPM().getID() ]).push_back( topRight);
  (points[ fLOR.getSecondHit().getSignalA().getRecoSignal().getPM().getID() ]).push_back( bottomLeft );
  (points[ fLOR.getSecondHit().getSignalB().getRecoSignal().getPM().getID() ]).push_back( bottomRight );

}

void SDALORCalculateWalkCorrection::prepareTimesForScintCorrection(JPetLOR& fLOR)
{
  double threshold = fThresholds[ fLOR.getFirstHit().getSignalA().getRecoSignal().getPM().getScin().getID() ];
  std::pair<double,double> topLeft= std::make_pair(fLOR.getFirstHit().getSignalA().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getFirstHit().getSignalA().getRecoSignal().getCharge());
  std::pair<double,double> topRight= std::make_pair(fLOR.getFirstHit().getSignalB().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getFirstHit().getSignalB().getRecoSignal().getCharge());

  threshold = fThresholds[ fLOR.getSecondHit().getSignalA().getRecoSignal().getPM().getScin().getID() ];
  std::pair<double,double> bottomLeft= std::make_pair(fLOR.getSecondHit().getSignalA().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getSecondHit().getSignalA().getRecoSignal().getCharge());
  std::pair<double,double> bottomRight= std::make_pair(fLOR.getSecondHit().getSignalB().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getSecondHit().getSignalB().getRecoSignal().getCharge());

  std::pair<double, double> top = std::make_pair( topLeft.first - topRight.first, 1.0/sqrt(topLeft.second) - 1.0/sqrt(topRight.second) );
  std::pair<double, double> bottom = std::make_pair( bottomLeft.first - bottomRight.first, 1.0/sqrt(bottomLeft.second) - 1.0/sqrt(bottomRight.second) );

  (points[ fLOR.getFirstHit().getSignalA().getRecoSignal().getPM().getScin().getID() ]).push_back( top );

  (points[ fLOR.getSecondHit().getSignalA().getRecoSignal().getPM().getScin().getID() ]).push_back( bottom );

  fTOFs.push_back( std::make_pair( (topLeft.first+ topRight.first) / 2.0 - (bottomLeft.first + bottomRight.first) / 2.0  , 1.0/sqrt(topLeft.second) + 1.0/sqrt(topRight.second) - 1.0/sqrt(bottomLeft.second) - 1.0/sqrt(bottomRight.second) ) );

}

void SDALORCalculateWalkCorrection::prepareTimesUsingTOT(JPetLOR& fLOR)
{
  double threshold = fThresholds[ fLOR.getFirstHit().getSignalA().getRecoSignal().getPM().getScin().getID() ];
  std::pair<double,double> topLeft= std::make_pair(fLOR.getFirstHit().getSignalA().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getFirstHit().getSignalA().getPhe());
  std::pair<double,double> topRight= std::make_pair(fLOR.getFirstHit().getSignalB().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getFirstHit().getSignalB().getPhe());

  threshold = fThresholds[ fLOR.getSecondHit().getSignalA().getRecoSignal().getPM().getScin().getID() ];
  std::pair<double,double> bottomLeft= std::make_pair(fLOR.getSecondHit().getSignalA().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getSecondHit().getSignalA().getPhe());
  std::pair<double,double> bottomRight= std::make_pair(fLOR.getSecondHit().getSignalB().getRecoSignal().getRecoTimeAtThreshold(threshold), fLOR.getSecondHit().getSignalB().getPhe());

  std::pair<double, double> top = std::make_pair( topLeft.first - topRight.first, 1.0/sqrt(topLeft.second) - 1.0/sqrt(topRight.second) );
  std::pair<double, double> bottom = std::make_pair( bottomLeft.first - bottomRight.first, 1.0/sqrt(bottomLeft.second) - 1.0/sqrt(bottomRight.second) );

  (points[ fLOR.getFirstHit().getSignalA().getRecoSignal().getPM().getScin().getID() ]).push_back( top );

  (points[ fLOR.getSecondHit().getSignalA().getRecoSignal().getPM().getScin().getID() ]).push_back( bottom );

  fTOFs.push_back( std::make_pair( (topLeft.first+ topRight.first) / 2.0 - (bottomLeft.first + bottomRight.first) / 2.0  , 1.0/(topLeft.second) + 1.0/(topRight.second) - 1.0/(bottomLeft.second) - 1.0/(bottomRight.second) ) );

}

void SDALORCalculateWalkCorrection::exec()
{
  fReader->getEntry(fEvent);
  //exctract data for each strip seperately
  JPetLOR& fLOR = dynamic_cast< JPetLOR& > ( fReader->getData() );

  prepareTimesForScintCorrection(fLOR);
  //prepareTimesForPMTCorrection(fLOR);
  //prepareTimesUsingTOT(fLOR);

  fEvent++;
}


#include <boost/serialization/base_object.hpp>

#include "Parameters.hpp"
#include "UtilSerialization.hpp"

namespace RGM
{
// ------ ParamUtil ------

ParamUtil::ParamUtil() :
    learningStatus_(0), regularizationCost_(0)
{
}

ParamUtil::~ParamUtil()
{
}

Scalar ParamUtil::learningStatus() const
{
    return learningStatus_;
}

Scalar & ParamUtil::getLearningStatus()
{
    return learningStatus_;
}

Scalar ParamUtil::regularizationCost() const
{
    return regularizationCost_;
}

Scalar & ParamUtil::getRegularizationCost()
{
    return regularizationCost_;
}

template<class Archive>
void ParamUtil::serialize(Archive & ar, const unsigned int version)
{
    ar & BOOST_SERIALIZATION_NVP(learningStatus_);
    ar & BOOST_SERIALIZATION_NVP(regularizationCost_);
}

INSTANTIATE_BOOST_SERIALIZATION(ParamUtil);

// ------ Appearance ------

Appearance::Appearance()
{
}

Appearance::Appearance(const Appearance & app)
{
    getW()                  = app.w();
    getLowerBound()         = app.lowerBound();
    getLearningStatus()     = app.learningStatus();
    getRegularizationCost() = app.regularizationCost();
}

Appearance::~Appearance()
{
}

void Appearance::init(int wd, int ht)
{
    Scalar Inf              = std::numeric_limits<Scalar>::infinity();

    getW()                  = Param::Constant(ht, wd, Cell::Zero());
    getLowerBound()         = Param::Constant(ht, wd, Cell::Constant(-Inf));

    getRegularizationCost() = 1.0F;
    getLearningStatus()     = 1.0F;
}

const Appearance::Param & Appearance::w() const
{
    return w_;
}

Appearance::Param & Appearance::getW()
{
    return w_;
}

const Appearance::Param & Appearance::lowerBound() const
{
    return lowerBound_;
}

Appearance::Param & Appearance::getLowerBound()
{
    return lowerBound_;
}

const Appearance::dParam & Appearance::gradient() const
{
    return gradient_;
}

Appearance::dParam & Appearance::getGradient()
{
    return gradient_;
}

template<class Archive>
void Appearance::serialize(Archive & ar, const unsigned int version)
{
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(ParamUtil);
    ar & BOOST_SERIALIZATION_NVP(w_);
    ar & BOOST_SERIALIZATION_NVP(lowerBound_);
}

INSTANTIATE_BOOST_SERIALIZATION(Appearance);

// ------ Offset ------

Offset::Offset() :
    w_(0), lowerBound_(0), gradient_(0)
{
}

Offset::Offset(const Offset & off)
{
    getW()                  = off.w();
    getLowerBound()         = off.lowerBound();
    getLearningStatus()     = off.learningStatus();
    getRegularizationCost() = off.regularizationCost();
}

Offset::~Offset()
{
}

Scalar Offset::w() const
{
    return w_;
}

Scalar & Offset::getW()
{
    return w_;
}

Scalar Offset::lowerBound() const
{
    return lowerBound_;
}

Scalar & Offset::getLowerBound()
{
    return lowerBound_;
}

const double & Offset::gradient() const
{
    return gradient_;
}

double & Offset::getGradient()
{
    return gradient_;
}

template<class Archive>
void Offset::serialize(Archive & ar, const unsigned int version)
{
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(ParamUtil);
    ar & BOOST_SERIALIZATION_NVP(w_);
    ar & BOOST_SERIALIZATION_NVP(lowerBound_);
}

INSTANTIATE_BOOST_SERIALIZATION(Offset);

// ------ Scaleprior ------

Scaleprior::Scaleprior() :
    w_(Param::Zero()), lowerBound_(Param::Zero())
{
}

Scaleprior::Scaleprior(const Scaleprior & prior)
{
    getW()                  = prior.w();
    getLowerBound()         = prior.lowerBound();
    getLearningStatus()     = prior.learningStatus();
    getRegularizationCost() = prior.regularizationCost();
}

Scaleprior::~Scaleprior()
{
}

const Scaleprior::Param & Scaleprior::w() const
{
    return w_;
}

Scaleprior::Param & Scaleprior::getW()
{
    return w_;
}

const Scaleprior::Param & Scaleprior::lowerBound() const
{
    return lowerBound_;
}

Scaleprior::Param & Scaleprior::getLowerBound()
{
    return lowerBound_;
}

const Scaleprior::dParam & Scaleprior::gradient() const
{
    return gradient_;
}

Scaleprior::dParam & Scaleprior::getGradient()
{
    return gradient_;
}

template<class Archive>
void Scaleprior::serialize(Archive & ar, const unsigned int version)
{
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(ParamUtil);
    ar & BOOST_SERIALIZATION_NVP(w_);
    ar & BOOST_SERIALIZATION_NVP(lowerBound_);
}

INSTANTIATE_BOOST_SERIALIZATION(Scaleprior);

// ------ Deformation ------

Deformation::Deformation() :
    w_(Param::Zero()), lowerBound_(Param::Zero())
{
}

Deformation::Deformation(const Deformation & def)
{
    getW()                  = def.w();
    getLowerBound()         = def.lowerBound();
    getLearningStatus()     = def.learningStatus();
    getRegularizationCost() = def.regularizationCost();
}

Deformation::Deformation(Scalar dx, Scalar dy) :
    lowerBound_(Param::Zero())
{
    getW() << dx * dx, dx, dy * dy, dy;
}

Deformation::~Deformation()
{
}

const Deformation::Param & Deformation::w() const
{
    return w_;
}

Deformation::Param & Deformation::getW()
{
    return w_;
}

const Deformation::Param & Deformation::lowerBound() const
{
    return lowerBound_;
}

Deformation::Param & Deformation::getLowerBound()
{
    return lowerBound_;
}

const Deformation::dParam & Deformation::gradient() const
{
    return gradient_;
}

Deformation::dParam & Deformation::getGradient()
{
    return gradient_;
}

template<class Archive>
void Deformation::serialize(Archive & ar, const unsigned int version)
{
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(ParamUtil);
    ar & BOOST_SERIALIZATION_NVP(w_);
    ar & BOOST_SERIALIZATION_NVP(lowerBound_);
}

INSTANTIATE_BOOST_SERIALIZATION(Deformation);

// ------ ParseInfo ------

ParseInfo::ParseInfo() :
    c_(-1), l_(-1), x_(-1), y_(-1), ds_(-1), dx_(-1), dy_(-1), score_(-10.0F), loss_(0.0F)
{
}

ParseInfo::ParseInfo(int c, int l, int x, int y, int ds, int dx, int dy, Scalar score,
                     const Rectangle_<Scalar> & bbox, Scalar loss) :
    c_(c), l_(l), x_(x), y_(y), ds_(ds), dx_(dx), dy_(dy), score_(score), loss_(loss),
    Rectangle_<Scalar>(bbox)
{
}

ParseInfo::ParseInfo(const ParseInfo & info) :
    c_(info.c_), l_(info.l_), x_(info.x_), y_(info.y_),
    ds_(info.ds_), dx_(info.dx_), dy_(info.dy_), score_(info.score_), loss_(info.loss_),
    Rectangle_<Scalar>(info.x(), info.y(), info.width(), info.height())
{
}

bool ParseInfo::clipBbox(int wd, int ht)
{
    float x1                = std::max<float>(0, x());
    float y1                = std::max<float>(0, y());
    float x2                = std::min<float>(wd-1, right());
    float y2                = std::min<float>(ht-1, bottom());
    float width             = x2 - x1 + 1;
    float height            = y2 - y1 + 1;

    if (width <=0 || height <=0) {
        return false;
    }

    setX(x1);
    setY(y1);
    setWidth(width);
    setHeight(height);

    return true;
}

template<class Archive>
void ParseInfo::serialize(Archive & ar, const unsigned int version)
{
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Rectangle_<Scalar>);
    ar & BOOST_SERIALIZATION_NVP(c_);
    ar & BOOST_SERIALIZATION_NVP(l_);
    ar & BOOST_SERIALIZATION_NVP(x_);
    ar & BOOST_SERIALIZATION_NVP(y_);
    ar & BOOST_SERIALIZATION_NVP(ds_);
    ar & BOOST_SERIALIZATION_NVP(dx_);
    ar & BOOST_SERIALIZATION_NVP(dy_);
    ar & BOOST_SERIALIZATION_NVP(score_);
    ar & BOOST_SERIALIZATION_NVP(loss_);
}

INSTANTIATE_BOOST_SERIALIZATION(ParseInfo);

} // namespace RGM


#ifndef STAN_MATH_PRIM_MAT_FUN_REP_ROW_VECTOR_HPP
#define STAN_MATH_PRIM_MAT_FUN_REP_ROW_VECTOR_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>

namespace stan {
namespace math {

template <typename T>
inline Eigen::Matrix<typename boost::math::tools::promote_args<T>::type, 1,
                     Eigen::Dynamic>
rep_row_vector(const T& x, int m) {
  check_nonnegative("rep_row_vector", "m", m);
  return Eigen::Matrix<typename boost::math::tools::promote_args<T>::type, 1,
                       Eigen::Dynamic>::Constant(m, x);
}

}  // namespace math
}  // namespace stan

#endif


#include "it/Entropy.hpp"
#include "binomial.hpp"
#include <algorithm>
#include <map>
#include <set>

#if BOOST_PYTHON_EXTENSIONS
#include <boost/python/extract.hpp>
#endif

#include "algorithm/TupleSpace.hpp"
#include "it/EntropyCalculator.hpp"

using namespace mist;
using namespace mist::algorithm;

TupleSpace::TupleSpace()
  : tuple_size(0){};
TupleSpace::~TupleSpace(){};

// default space for N variables in tuples size d
TupleSpace::TupleSpace(int N, int d)
  : tuple_size(d)
{
  if (N == 0 || d == 0) {
    throw TupleSpaceException("TupleSpace",
                              "Number of variables and dimension cannot be zero.");
  }
  tuple_t vars(N);
  for (int ii = 0; ii < N; ii++) {
    vars[ii] = ii;
  }
  this->addVariableGroup("default", vars);
  tuple_t groupTuple(d, 0);
  this->addVariableGroupTuple(groupTuple);
};

std::vector<std::string>
TupleSpace::names() const
{
  return variableNames;
}

void
TupleSpace::set_names(std::vector<std::string> const& names)
{
  variableNames = names;
}

int
TupleSpace::tupleSize() const
{
  return tuple_size;
}

void
TupleSpace::addVariableGroupTuple(std::vector<std::string> const& groupNames)
{
  TupleSpace::tuple_t groupIndexes;
  for (auto& name : groupNames) {
    try {
      groupIndexes.push_back(variableGroupNames[name]);
    } catch (std::out_of_range& e) {
      throw TupleSpaceException("addVariableGroupTuple",
                                "group named " + name + " does not exist.");
    }
  }
  addVariableGroupTuple(groupIndexes);
}

void
TupleSpace::addVariableGroupTuple(TupleSpace::tuple_t const& groupIndexes)
{
  // validate tuple size
  if (!tuple_size) {
    tuple_size = groupIndexes.size();
  } else if (tuple_size != groupIndexes.size()) {
    throw TupleSpaceException(
      "addVariableGroupTuple",
      "Could not add group tuple, all tuples must be the same size");
  }
  // validate group indexes
  for (auto group : groupIndexes) {
    if (group >= variableGroups.size()) {
      throw TupleSpaceException("addVariableGroupTuple",
                                "variable group index " +
                                  std::to_string(group) + " out of range.");
    }
  }
  variableGroupTuples.push_back(groupIndexes);
}

int
TupleSpace::addVariableGroup(std::string const& name,
                             TupleSpace::tuple_t const& vars)
{
  std::set<int> unique_vars;
  tuple_t group;
  for (auto var : vars) {
    // ignore duplicates within variable group
    if (unique_vars.find(var) == unique_vars.end()) {
      unique_vars.insert(var);
      group.push_back(var);
      // check for overlap with other variable groups
      if (seen_vars.find(var) != seen_vars.end()) {
        throw TupleSpaceException(
          "addVariableGroup",
          "WARNING: variable " + std::to_string(var) +
            " listed twice in variable group definitions. This exception can "
            "be caught if overlapping variable groups are desired (unusual).");
      }
      seen_vars.emplace(var);
    }
  }
  std::sort(group.begin(), group.end());
  variableGroups.push_back(group);
  variableGroupSizes.push_back(group.size());
  int index = variableGroups.size() - 1;
  variableGroupNames.emplace(name, index);
  return index;
}

TupleSpace::tuple_t const&
TupleSpace::getVariableGroup(int index) const
{
  try {
    return variableGroups.at(index);
  } catch (std::out_of_range& e) {
    throw TupleSpaceException("getVariableGroup",
                              "group index " + std::to_string(index) +
                                " out of range.");
  }
}

TupleSpace::tuple_t const&
TupleSpace::getVariableGroup(std::string const& name) const
{
  try {
    return variableGroups.at(variableGroupNames.at(name));
  } catch (std::out_of_range& e) {
    throw TupleSpaceException("getVariableGroup",
                              "group named " + name + " does not exist.");
  }
}

std::vector<std::size_t> const&
TupleSpace::getVariableGroupSizes() const
{
    return variableGroupSizes;
}

std::vector<TupleSpace::tuple_t> const&
TupleSpace::getVariableGroups() const
{
  return variableGroups;
}

std::vector<TupleSpace::tuple_t> const&
TupleSpace::getVariableGroupTuples() const
{
  return variableGroupTuples;
}

#if BOOST_PYTHON_EXTENSIONS
int
TupleSpace::pyAddVariableGroup(std::string const& name, p::list const& list)
{
  int n = p::len(list);
  // copy list in
  algorithm::TupleSpace::tuple_t vars(n);
  for (int ii = 0; ii < n; ii++) {
    p::extract<int> var(list[ii]);
    if (var.check()) {
      vars[ii] = var;
    } else {
      throw TupleSpaceException("pyAddVariableGroup",
                                "Expected list with elements type int");
    }
  }
  return addVariableGroup(name, vars);
}

void
TupleSpace::pyAddVariableGroupTuple(p::list const& list)
{
  int n = p::len(list); // Doesn't give the right answer ...
  algorithm::TupleSpace::tuple_t groups(n);
  for (int ii = 0; ii < n; ii++) {
    p::extract<int> gint(list[ii]); //TODO segaults
    p::extract<std::string> gname(list[ii]);
    if (gint.check()) {
      groups[ii] = p::extract<int>(list[ii]);
    } else if (gname.check()) {
      std::string name(gname);
      try {
        groups[ii] = variableGroupNames.at(name);
      } catch (std::exception& e) {
        throw TupleSpaceException("pyAddVariableGroupTuple",
                                  "group named " + name + " does not exist.");
      }
    } else {
      throw TupleSpaceException("pyAddVariableGroupTuple",
                                "Expected list with elements type int or str");
    }
  }
  addVariableGroupTuple(groups);
}
#endif

static std::vector<std::size_t>
groupSizes(std::vector<TupleSpace::tuple_t> const& groups)
{
  std::vector<std::size_t> group_sizes;
  for (auto& group : groups) {
    group_sizes.push_back(group.size());
  }
  return group_sizes;
}

static TupleSpace::tuple_t
groupAppearances(int d, TupleSpace::tuple_t const& group_tuple, int pos)
{
  // count group appearances following the current index
  TupleSpace::tuple_t app(d, 0);
  for (int gg = pos; gg < group_tuple.size(); gg++) {
    app[group_tuple[gg]]++;
  }
  return app;
}

static TupleSpace::tuple_t
groupAppearances(int d, TupleSpace::tuple_t const& group_tuple)
{
  return groupAppearances(d, group_tuple, 0);
}

TupleSpace::count_t
TupleSpace::count_tuples_group_tuple(tuple_t const& group_tuple) const
{
  count_t total = 1;
  auto const& N = this->variableGroupSizes;
  auto d = N.size();
  auto a = groupAppearances(d, group_tuple);
  for (int ii = 0; ii < d; ii++) {
    total *= binomial(N[ii], a[ii]);
  }
  return total;
}

TupleSpace::count_t
TupleSpace::count_tuples() const
{
  count_t total = 0;
  for (auto const& group_tuple : this->variableGroupTuples) {
    total += count_tuples_group_tuple(group_tuple);
  }
  return total;
}

// Fast-forward to the group containig the tuple at the target position.
static int
find_group(TupleSpace::count_t* count, TupleSpace::count_t target, TupleSpace const& ts)
{
  auto const& group_tuples = ts.getVariableGroupTuples();
  int gg = 0;
  for (auto const& group_tuple : group_tuples) {
    TupleSpace::count_t skip = ts.count_tuples_group_tuple(group_tuple);
    if ((skip + *count) > target) {
      break;
    }
    *count += skip;
    gg++;
  }
  return gg;
}

// Fast-forward the index at position pos so that the target tuple is found by
// incrementing indexes in the following positions. E.g. if the target 3-tuple
// is (1,4,8), then
//
//      find_index(..., pos=0, ...) returns 1.
//      find_index(..., pos=1, ...) returns 4.
//      find_index(..., pos=2, ...) returns 8.
//
static int
find_index(TupleSpace::tuple_t const& group_tuple,
           int pos,
           TupleSpace::count_t* count,
           TupleSpace::count_t target,
           std::vector<std::size_t> const& N,
           TupleSpace::tuple_t& starts)
{
  unsigned ii = 0;                // index
  unsigned gi = group_tuple[pos]; // group corresponding to index
  auto app = groupAppearances(N.size(), group_tuple, pos + 1);

  for (ii = starts[gi]; ii < N[gi]; ii++) {
    starts[gi] = ii + 1;
    // count the number of tuples skipped over by incrementing index
    // for last index skip = 1 (could be optimized with a special case)
    TupleSpace::count_t skip = 1;
    for (unsigned gg = 0; gg < N.size(); gg++) {
      // binomial combination reduces to linear when appearances == 1
      TupleSpace::count_t f = 1;
      for (unsigned kk = 0; kk < app[gg]; kk++) {
        skip = skip * (N[gg] - starts[gg] - kk);
        f *= (kk + 1);
      }
      skip /= f;
    }
    if ((skip + *count) > target) {
      break;
    }
    *count += skip;
  }
  if (ii >= N[gi]) {
    throw TupleSpaceException("find_index", "Failed to FFW index, out of range");
  }
  return ii;
}

TupleSpace::tuple_t
TupleSpace::find_tuple(count_t target) const
{
  // The fast-forward algorithm maintains a skipped tuple count so that when
  // the count equals the target count we have found the target tuple.
  count_t count = 0;

  auto const& N = this->variableGroupSizes;
  auto const& group_tuples = this->variableGroupTuples;

  // scan ahead to the group tuple that generates the target tuple
  unsigned gg = find_group(&count, target, *this);
  auto tuple_size = group_tuples[gg].size();
  tuple_t ret(tuple_size + 1);
  ret[0] = gg;

  // scan to the target tuple
  tuple_t starts(N.size(), 0);
  for (unsigned ii = 0; ii < tuple_size; ii++) {
    ret[ii + 1] = find_index(group_tuples[gg], ii, &count, target, N, starts);
  }

  return ret;
}

static void
traverse_d1(TupleSpace const& ts, TupleSpace::count_t start, TupleSpace::count_t stop, TupleSpaceTraverser& traverser)
{
  auto const& groups = ts.getVariableGroups();
  auto const& group_tuples = ts.getVariableGroupTuples();
  auto const& N = ts.getVariableGroupSizes();
  unsigned ngroups = groups.size();
  unsigned ngtuples = group_tuples.size();

  // tuple generation state
  bool init = true;
  bool work = true;
  auto count = start;
  TupleSpace::tuple_t starts(ngroups);
  starts.assign(ngroups,0);

  // fast-forward to starting group and tuple
  auto ffw = ts.find_tuple(start);

  // sub tuples on the stack
  TupleSpace::tuple_t t0(1);

  for (unsigned gg = ffw[0]; gg < ngtuples && work; gg++) {
    unsigned g0 = group_tuples[gg][0];

    // loop through all tuples generated by this group_tuple
    for (unsigned i0 = (init) ? ffw[1] : starts[g0]; i0 < N[g0] && work; i0++) {
      starts[g0] = i0 + 1;
      auto v0 = groups[g0][i0];
      t0[0] = v0;
      traverser.process_tuple(count, t0);
      count++;
      init = false;
      work = count < stop;
    }
    starts[g0] = 0;
  }
}

static void
traverse_d2(TupleSpace const& ts, TupleSpace::count_t start, TupleSpace::count_t stop, TupleSpaceTraverser& traverser)
{
  auto const& groups = ts.getVariableGroups();
  auto const& group_tuples = ts.getVariableGroupTuples();
  auto const& N = ts.getVariableGroupSizes();
  unsigned ngroups = groups.size();
  unsigned ngtuples = group_tuples.size();

  // tuple generation state
  bool init = true;
  bool work = true;
  auto count = start;
  TupleSpace::tuple_t starts(ngroups);
  starts.assign(ngroups,0);

  // fast-forward to starting group and tuple
  auto ffw = ts.find_tuple(start);

  // sub tuples on the stack
  TupleSpace::tuple_t t01(2);

  for (unsigned gg = ffw[0]; gg < ngtuples && work; gg++) {
    unsigned g0 = group_tuples[gg][0];
    unsigned g1 = group_tuples[gg][1];

    // loop through all tuples generated by this group_tuple
    for (unsigned i0 = (init) ? ffw[1] : starts[g0]; i0 < N[g0] && work; i0++) {
      starts[g0] = i0 + 1;
      auto v0 = groups[g0][i0];
      t01[0] = v0;
      for (unsigned i1 = (init) ? ffw[2] : starts[g1]; i1 < N[g1] && work; i1++) {
        starts[g1] = i1 + 1;
        auto v1 = groups[g1][i1];
        t01[1] = v1;
        traverser.process_tuple(count, t01);
        count++;
        init = false;
        work = count < stop;
      }
      starts[g1] = 0;
    }
    starts[g0] = 0;
  }
}

static void
traverse_d2_entropy(TupleSpace const& ts, TupleSpace::count_t start, TupleSpace::count_t stop, TupleSpaceTraverser& traverser, it::EntropyCalculator & ecalc)
{
  auto const& groups = ts.getVariableGroups();
  auto const& group_tuples = ts.getVariableGroupTuples();
  auto const& N = ts.getVariableGroupSizes();
  unsigned ngroups = groups.size();
  unsigned ngtuples = group_tuples.size();

  // tuple generation state
  bool init = true;
  bool work = true;
  auto count = start;
  TupleSpace::tuple_t starts(ngroups);
  starts.assign(ngroups,0);
  it::Entropy entropy((unsigned)it::d2::size);

  // fast-forward to starting group and tuple
  auto ffw = ts.find_tuple(start);

  // sub tuples on the stack
  TupleSpace::tuple_t t0(1);
  TupleSpace::tuple_t t1(1);
  TupleSpace::tuple_t t01(2);

  for (unsigned gg = ffw[0]; gg < ngtuples && work; gg++) {
    unsigned g0 = group_tuples[gg][0];
    unsigned g1 = group_tuples[gg][1];

    // loop through all tuples generated by this group_tuple
    for (unsigned i0 = (init) ? ffw[1] : starts[g0]; i0 < N[g0] && work; i0++) {
      starts[g0] = i0 + 1;
      auto v0 = groups[g0][i0];
      t0[0] = v0;
      t01[0] = v0;
      entropy[(unsigned)it::d2::e0] = ecalc.entropy(t0);
      for (unsigned i1 = (init) ? ffw[2] : starts[g1]; i1 < N[g1] && work; i1++) {
        starts[g1] = i1 + 1;
        auto v1 = groups[g1][i1];
        t1[0] = v1;
        t01[1] = v1;
        entropy[(unsigned)it::d2::e1]  = ecalc.entropy(t1);
        entropy[(unsigned)it::d2::e01] = ecalc.entropy(t01);
        traverser.process_tuple_entropy(count, t01, entropy);
        count++;
        init = false;
        work = count < stop;
      }
      starts[g1] = 0;
    }
    starts[g0] = 0;
  }
}

static void
traverse_d3(TupleSpace const& ts, TupleSpace::count_t start, TupleSpace::count_t stop, TupleSpaceTraverser& traverser)
{
  auto const& groups = ts.getVariableGroups();
  auto const& group_tuples = ts.getVariableGroupTuples();
  auto const& N = ts.getVariableGroupSizes();
  unsigned ngroups = groups.size();
  unsigned ngtuples = group_tuples.size();

  // tuple generation state
  bool init = true;
  bool work = true;
  auto count = start;
  TupleSpace::tuple_t starts(ngroups);
  starts.assign(ngroups,0);

  // fast-forward to starting group and tuple
  auto ffw = ts.find_tuple(start);

  // sub tuples on the stack
  TupleSpace::tuple_t t012(3);

  for (unsigned gg = ffw[0]; gg < ngtuples && work; gg++) {
    unsigned g0 = group_tuples[gg][0];
    unsigned g1 = group_tuples[gg][1];
    unsigned g2 = group_tuples[gg][2];

    // loop through all tuples generated by this group_tuple
    for (unsigned i0 = (init) ? ffw[1] : starts[g0]; i0 < N[g0] && work; i0++) {
      starts[g0] = i0 + 1;
      auto v0 = groups[g0][i0];
      t012[0] = v0;
      for (unsigned i1 = (init) ? ffw[2] : starts[g1]; i1 < N[g1] && work; i1++) {
        starts[g1] = i1 + 1;
        auto v1 = groups[g1][i1];
        t012[1] = v1;
        for (unsigned i2 = (init) ? ffw[3] : starts[g2]; i2 < N[g2] && work; i2++) {
          starts[g2] = i2 + 1;
          auto v2 = groups[g2][i2];
          t012[2] = v2;
          traverser.process_tuple(count, t012);
          count++;
          init = false;
          work = count < stop;
        }
        starts[g2] = 0;
      }
      starts[g1] = 0;
    }
    starts[g0] = 0;
  }
}

static void
traverse_d3_entropy(TupleSpace const& ts, TupleSpace::count_t start, TupleSpace::count_t stop, TupleSpaceTraverser& traverser, it::EntropyCalculator & ecalc)
{
  auto const& groups = ts.getVariableGroups();
  auto const& group_tuples = ts.getVariableGroupTuples();
  auto const& N = ts.getVariableGroupSizes();
  unsigned ngroups = groups.size();
  unsigned ngtuples = group_tuples.size();

  // tuple generation state
  bool init = true;
  bool work = true;
  auto count = start;
  TupleSpace::tuple_t starts(ngroups);
  starts.assign(ngroups,0);
  it::Entropy entropy((unsigned)it::d3::size);

  // sub tuples on the stack
  TupleSpace::tuple_t t0(1);
  TupleSpace::tuple_t t1(1);
  TupleSpace::tuple_t t2(1);
  TupleSpace::tuple_t t01(2);
  TupleSpace::tuple_t t02(2);
  TupleSpace::tuple_t t12(2);
  TupleSpace::tuple_t t012(3);

  // fast-forward to starting group and tuple
  auto ffw = ts.find_tuple(start);

  for (unsigned gg = ffw[0]; gg < ngtuples && work; gg++) {
    unsigned g0 = group_tuples[gg][0];
    unsigned g1 = group_tuples[gg][1];
    unsigned g2 = group_tuples[gg][2];

    // loop through all tuples generated by this group_tuple
    for (unsigned i0 = (init) ? ffw[1] : starts[g0]; i0 < N[g0] && work; i0++) {
      starts[g0] = i0 + 1;
      auto v0 = groups[g0][i0];
      t0[0] = v0;
      t02[0] = v0;
      t01[0] = v0;
      t012[0] = v0;
      entropy[(unsigned)it::d3::e0] = ecalc.entropy(t0);
      for (unsigned i1 = (init) ? ffw[2] : starts[g1]; i1 < N[g1] && work; i1++) {
        starts[g1] = i1 + 1;
        auto v1 = groups[g1][i1];
        t1[0] = v1;
        t01[1] = v1;
        t12[0] = v1;
        t012[1] = v1;
        entropy[(unsigned)it::d3::e1]  = ecalc.entropy(t1);
        entropy[(unsigned)it::d3::e01] = ecalc.entropy(t01);
        for (unsigned i2 = (init) ? ffw[3] : starts[g2]; i2 < N[g2] && work; i2++) {
          starts[g2] = i2 + 1;
          auto v2 = groups[g2][i2];
          t2[0] = v2;
          t02[1] = v2;
          t12[1] = v2;
          t012[2] = v2;
          entropy[(unsigned)it::d3::e2]   = ecalc.entropy(t2);
          entropy[(unsigned)it::d3::e02]  = ecalc.entropy(t02);
          entropy[(unsigned)it::d3::e12]  = ecalc.entropy(t12);
          entropy[(unsigned)it::d3::e012] = ecalc.entropy(t012);
          traverser.process_tuple_entropy(count, t012, entropy);
          count++;
          init = false;
          work = count < stop;
        }
        starts[g2] = 0;
      }
      starts[g1] = 0;
    }
    starts[g0] = 0;
  }
}

static void
traverse_d4(TupleSpace const& ts, TupleSpace::count_t start, TupleSpace::count_t stop, TupleSpaceTraverser& traverser)
{
  auto const& groups = ts.getVariableGroups();
  auto const& group_tuples = ts.getVariableGroupTuples();
  auto const& N = ts.getVariableGroupSizes();
  unsigned ngroups = groups.size();
  unsigned ngtuples = group_tuples.size();

  // tuple generation state
  bool init = true;
  bool work = true;
  auto count = start;
  TupleSpace::tuple_t starts(ngroups);
  starts.assign(ngroups,0);

  // fast-forward to starting group and tuple
  auto ffw = ts.find_tuple(start);

  // sub tuples on the stack
  TupleSpace::tuple_t t0123(4);

  for (unsigned gg = ffw[0]; gg < ngtuples && work; gg++) {
    unsigned g0 = group_tuples[gg][0];
    unsigned g1 = group_tuples[gg][1];
    unsigned g2 = group_tuples[gg][2];
    unsigned g3 = group_tuples[gg][3];

    // loop through all tuples generated by this group_tuple
    for (unsigned i0 = (init) ? ffw[1] : starts[g0]; i0 < N[g0] && work; i0++) {
      starts[g0] = i0 + 1;
      auto v0 = groups[g0][i0];
      t0123[0] = v0;
      for (unsigned i1 = (init) ? ffw[2] : starts[g1]; i1 < N[g1] && work; i1++) {
        starts[g1] = i1 + 1;
        auto v1 = groups[g1][i1];
        t0123[1] = v1;
        for (unsigned i2 = (init) ? ffw[3] : starts[g2]; i2 < N[g2] && work; i2++) {
          starts[g2] = i2 + 1;
          auto v2 = groups[g2][i2];
          t0123[2] = v2;
          for (unsigned i3 = (init) ? ffw[4] : starts[g3]; i3 < N[g3] && work; i3++) {
            starts[g3] = i3 + 1;
            auto v3 = groups[g3][i3];
            t0123[3] = v3;
            traverser.process_tuple(count, t0123);
            count++;
            init = false;
            work = count < stop;
          }
          starts[g3] = 0;
        }
        starts[g2] = 0;
      }
      starts[g1] = 0;
    }
    starts[g0] = 0;
  }
}

static void
traverse_d4_entropy(TupleSpace const& ts, TupleSpace::count_t start, TupleSpace::count_t stop, TupleSpaceTraverser& traverser, it::EntropyCalculator & ecalc)
{
  auto const& groups = ts.getVariableGroups();
  auto const& group_tuples = ts.getVariableGroupTuples();
  auto const& N = ts.getVariableGroupSizes();
  unsigned ngroups = groups.size();
  unsigned ngtuples = group_tuples.size();

  // tuple generation state
  bool init = true;
  bool work = true;
  auto count = start;
  TupleSpace::tuple_t starts(ngroups);
  starts.assign(ngroups,0);
  it::Entropy entropy((unsigned)it::d4::size);

  // sub tuples on the stack
  TupleSpace::tuple_t t0(1);
  TupleSpace::tuple_t t1(1);
  TupleSpace::tuple_t t2(1);
  TupleSpace::tuple_t t3(1);
  TupleSpace::tuple_t t01(2);
  TupleSpace::tuple_t t02(2);
  TupleSpace::tuple_t t03(2);
  TupleSpace::tuple_t t12(2);
  TupleSpace::tuple_t t13(2);
  TupleSpace::tuple_t t23(2);
  TupleSpace::tuple_t t012(3);
  TupleSpace::tuple_t t013(3);
  TupleSpace::tuple_t t023(3);
  TupleSpace::tuple_t t123(3);
  TupleSpace::tuple_t t0123(4);

  // fast-forward to starting group and tuple
  auto ffw = ts.find_tuple(start);

  for (unsigned gg = ffw[0]; gg < ngtuples && work; gg++) {
    unsigned g0 = group_tuples[gg][0];
    unsigned g1 = group_tuples[gg][1];
    unsigned g2 = group_tuples[gg][2];
    unsigned g3 = group_tuples[gg][3];

    // loop through all tuples generated by this group_tuple
    for (unsigned i0 = (init) ? ffw[1] : starts[g0]; i0 < N[g0] && work; i0++) {
      starts[g0] = i0 + 1;
      auto v0 = groups[g0][i0];
      t0[0] = v0;
      t01[0] = v0;
      t02[0] = v0;
      t03[0] = v0;
      t012[0] = v0;
      t013[0] = v0;
      t023[0] = v0;
      t0123[0] = v0;
      entropy[(unsigned)it::d4::e0] = ecalc.entropy(t0);
      for (unsigned i1 = (init) ? ffw[2] : starts[g1]; i1 < N[g1] && work; i1++) {
        starts[g1] = i1 + 1;
        auto v1 = groups[g1][i1];
        t1[0] = v1;
        t01[1] = v1;
        t12[0] = v1;
        t13[0] = v1;
        t012[1] = v1;
        t013[1] = v1;
        t123[0] = v1;
        t0123[1] = v1;
        entropy[(unsigned)it::d4::e1]  = ecalc.entropy(t1);
        entropy[(unsigned)it::d4::e01] = ecalc.entropy(t01);
        for (unsigned i2 = (init) ? ffw[3] : starts[g2]; i2 < N[g2] && work; i2++) {
          starts[g2] = i2 + 1;
          auto v2 = groups[g2][i2];
          t2[0] = v2;
          t02[1] = v2;
          t12[1] = v2;
          t23[0] = v2;
          t012[2] = v2;
          t023[1] = v2;
          t123[1] = v2;
          t0123[2] = v2;
          entropy[(unsigned)it::d4::e2]   = ecalc.entropy(t2);
          entropy[(unsigned)it::d4::e02]  = ecalc.entropy(t02);
          entropy[(unsigned)it::d4::e12]  = ecalc.entropy(t12);
          entropy[(unsigned)it::d4::e012] = ecalc.entropy(t012);
          for (unsigned i3 = (init) ? ffw[4] : starts[g3]; i3 < N[g3] && work; i3++) {
            starts[g3] = i3 + 1;
            auto v3 = groups[g3][i3];
            t3[0] = v3;
            t03[1] = v3;
            t13[1] = v3;
            t23[1] = v3;
            t013[2] = v3;
            t023[2] = v3;
            t123[2] = v3;
            t0123[3] = v3;
            entropy[(unsigned)it::d4::e3]    = ecalc.entropy(t3);
            entropy[(unsigned)it::d4::e03]   = ecalc.entropy(t03);
            entropy[(unsigned)it::d4::e13]   = ecalc.entropy(t13);
            entropy[(unsigned)it::d4::e23]   = ecalc.entropy(t23);
            entropy[(unsigned)it::d4::e013]  = ecalc.entropy(t013);
            entropy[(unsigned)it::d4::e023]  = ecalc.entropy(t023);
            entropy[(unsigned)it::d4::e123]  = ecalc.entropy(t123);
            entropy[(unsigned)it::d4::e0123] = ecalc.entropy(t0123);
            traverser.process_tuple_entropy(count, t0123, entropy);
            count++;
            init = false;
            work = count < stop;
          }
          starts[g3] = 0;
        }
        starts[g2] = 0;
      }
      starts[g1] = 0;
    }
    starts[g0] = 0;
  }
}

void
TupleSpace::traverse(TupleSpaceTraverser& traverser) const
{
  traverse(0, -1, traverser);
}

void
TupleSpace::traverse_entropy(it::EntropyCalculator &ecalc, TupleSpaceTraverser& traverser) const
{
  traverse_entropy(0, -1, ecalc, traverser);
}

void
TupleSpace::traverse(count_t start, count_t stop, TupleSpaceTraverser& traverser) const
{
  switch(tuple_size) {
    case 1: traverse_d1(*this, start, stop, traverser); break;
    case 2: traverse_d2(*this, start, stop, traverser); break;
    case 3: traverse_d3(*this, start, stop, traverser); break;
    case 4: traverse_d4(*this, start, stop, traverser); break;
    default:
      throw TupleSpaceException("traverse", "Tuple size greater than 4 unsupported.");
  }
}

void
TupleSpace::traverse_entropy(count_t start, count_t stop, it::EntropyCalculator &ecalc, TupleSpaceTraverser& traverser) const
{
  switch(tuple_size) {
    case 1:
      throw TupleSpaceException("traverse_entropy", "Tuple size 1 unsupported.");
    case 2: traverse_d2_entropy(*this, start, stop, traverser, ecalc); break;
    case 3: traverse_d3_entropy(*this, start, stop, traverser, ecalc); break;
    case 4: traverse_d4_entropy(*this, start, stop, traverser, ecalc); break;
    default:
      throw TupleSpaceException("traverse", "Tuple size greater than 4 unsupported.");
  }
}


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

#include "partition/multi_level_partition.hpp"

#define CHECK_SIZE_RANGE(range, ref) BOOST_CHECK_EQUAL(range.second - range.first, ref)
#define CHECK_EQUAL_RANGE(range, ref)                                                              \
    do                                                                                \
    {                                                                                \
        const auto &lhs = range;                                                                   \
        const auto &rhs = ref;                                                                     \
        BOOST_CHECK_EQUAL_COLLECTIONS(lhs.first, lhs.second, rhs.begin(), rhs.end());              \
    } while (0)

using namespace osrm;
using namespace osrm::partition;

BOOST_AUTO_TEST_SUITE(multi_level_partition_tests)

BOOST_AUTO_TEST_CASE(mlp_one)
{
    // node:                0  1  2  3  4  5  6  7  8  9 10 11
    std::vector<CellID> l1{{4, 4, 2, 2, 1, 1, 3, 3, 2, 2, 5, 5}};
    MultiLevelPartition mlp{{l1}, {6}};

    BOOST_CHECK_EQUAL(mlp.GetCell(1, 0), mlp.GetCell(1, 1));
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 2), mlp.GetCell(1, 3));
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 4), mlp.GetCell(1, 5));
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 6), mlp.GetCell(1, 7));
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 8), mlp.GetCell(1, 9));
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 10), mlp.GetCell(1, 11));
}

BOOST_AUTO_TEST_CASE(mlp_shuffled)
{
    // node:                0  1  2  3  4  5  6  7  8  9 10 11
    std::vector<CellID> l1{{4, 4, 2, 2, 1, 1, 3, 3, 2, 2, 5, 5}};
    std::vector<CellID> l2{{3, 3, 3, 3, 1, 1, 1, 1, 2, 2, 0, 0}};
    std::vector<CellID> l3{{0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1}};
    std::vector<CellID> l4{{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
    MultiLevelPartition mlp{{l1, l2, l3, l4}, {6, 4, 2, 1}};

    BOOST_CHECK_EQUAL(mlp.GetNumberOfCells(1), 6);
    BOOST_CHECK_EQUAL(mlp.GetNumberOfCells(2), 4);
    BOOST_CHECK_EQUAL(mlp.GetNumberOfCells(3), 2);
    BOOST_CHECK_EQUAL(mlp.GetNumberOfCells(4), 1);

    BOOST_CHECK_EQUAL(mlp.GetCell(1, 0), mlp.GetCell(1, 1));
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 2), mlp.GetCell(1, 3));
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 4), mlp.GetCell(1, 5));
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 6), mlp.GetCell(1, 7));
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 8), mlp.GetCell(1, 9));
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 10), mlp.GetCell(1, 11));

    BOOST_CHECK_EQUAL(mlp.GetCell(2, 0), mlp.GetCell(2, 1));
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 0), mlp.GetCell(2, 2));
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 0), mlp.GetCell(2, 3));
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 4), mlp.GetCell(2, 5));
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 4), mlp.GetCell(2, 6));
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 4), mlp.GetCell(2, 7));
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 8), mlp.GetCell(2, 9));
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 10), mlp.GetCell(2, 11));

    BOOST_CHECK_EQUAL(mlp.GetCell(3, 0), mlp.GetCell(3, 1));
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 0), mlp.GetCell(3, 2));
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 0), mlp.GetCell(3, 3));

    BOOST_CHECK_EQUAL(mlp.GetCell(3, 4), mlp.GetCell(3, 5));
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 4), mlp.GetCell(3, 6));
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 4), mlp.GetCell(3, 7));
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 4), mlp.GetCell(3, 8));
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 4), mlp.GetCell(3, 9));
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 4), mlp.GetCell(3, 10));
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 4), mlp.GetCell(3, 11));

    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 1));
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 2));
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 3));
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 4));
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 5));
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 6));
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 7));
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 8));
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 9));
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 10));
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), mlp.GetCell(4, 11));
}

BOOST_AUTO_TEST_CASE(mlp_sorted)
{
    // node:                0  1  2  3  4  5  6  7  8  9 10 11
    std::vector<CellID> l1{{0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5}};
    std::vector<CellID> l2{{0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 3, 3}};
    std::vector<CellID> l3{{0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1}};
    std::vector<CellID> l4{{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
    MultiLevelPartition mlp{{l1, l2, l3, l4}, {6, 4, 2, 1}};

    BOOST_CHECK_EQUAL(mlp.GetNumberOfCells(1), 6);
    BOOST_CHECK_EQUAL(mlp.GetNumberOfCells(2), 4);
    BOOST_CHECK_EQUAL(mlp.GetNumberOfCells(3), 2);
    BOOST_CHECK_EQUAL(mlp.GetNumberOfCells(4), 1);

    BOOST_CHECK_EQUAL(mlp.GetCell(1, 0), l1[0]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 1), l1[1]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 2), l1[2]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 3), l1[3]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 4), l1[4]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 5), l1[5]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 6), l1[6]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 7), l1[7]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 8), l1[8]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 9), l1[9]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 10), l1[10]);
    BOOST_CHECK_EQUAL(mlp.GetCell(1, 11), l1[11]);

    BOOST_CHECK_EQUAL(mlp.GetCell(2, 0), l2[0]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 1), l2[1]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 2), l2[2]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 3), l2[3]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 4), l2[4]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 5), l2[5]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 6), l2[6]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 7), l2[7]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 8), l2[8]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 9), l2[9]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 10), l2[10]);
    BOOST_CHECK_EQUAL(mlp.GetCell(2, 11), l2[11]);

    BOOST_CHECK_EQUAL(mlp.GetCell(3, 0), l3[0]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 1), l3[1]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 2), l3[2]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 3), l3[3]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 4), l3[4]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 5), l3[5]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 6), l3[6]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 7), l3[7]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 8), l3[8]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 9), l3[9]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 10), l3[10]);
    BOOST_CHECK_EQUAL(mlp.GetCell(3, 11), l3[11]);

    BOOST_CHECK_EQUAL(mlp.GetCell(4, 0), l4[0]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 1), l4[1]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 2), l4[2]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 3), l4[3]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 4), l4[4]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 5), l4[5]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 6), l4[6]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 7), l4[7]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 8), l4[8]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 9), l4[9]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 10), l4[10]);
    BOOST_CHECK_EQUAL(mlp.GetCell(4, 11), l4[11]);

    BOOST_CHECK_EQUAL(mlp.GetHighestDifferentLevel(0, 1), 0);
    BOOST_CHECK_EQUAL(mlp.GetHighestDifferentLevel(0, 2), 1);
    BOOST_CHECK_EQUAL(mlp.GetHighestDifferentLevel(0, 4), 3);
    BOOST_CHECK_EQUAL(mlp.GetHighestDifferentLevel(7, 8), 2);

    BOOST_CHECK_EQUAL(mlp.BeginChildren(2, 0), 0);
    BOOST_CHECK_EQUAL(mlp.EndChildren(2, 0), 2);
    BOOST_CHECK_EQUAL(mlp.BeginChildren(2, 1), 2);
    BOOST_CHECK_EQUAL(mlp.EndChildren(2, 1), 4);
    BOOST_CHECK_EQUAL(mlp.BeginChildren(2, 2), 4);
    BOOST_CHECK_EQUAL(mlp.EndChildren(2, 2), 5);
    BOOST_CHECK_EQUAL(mlp.BeginChildren(2, 3), 5);
    BOOST_CHECK_EQUAL(mlp.EndChildren(2, 3), 6);

    BOOST_CHECK_EQUAL(mlp.BeginChildren(3, 0), 0);
    BOOST_CHECK_EQUAL(mlp.EndChildren(3, 0), 1);
    BOOST_CHECK_EQUAL(mlp.BeginChildren(3, 1), 1);
    BOOST_CHECK_EQUAL(mlp.EndChildren(3, 1), 4);

    BOOST_CHECK_EQUAL(mlp.BeginChildren(4, 0), 0);
    BOOST_CHECK_EQUAL(mlp.EndChildren(4, 0), 2);
}

BOOST_AUTO_TEST_SUITE_END()