#include "tile/math/polynomial.h"

#include <boost/format.hpp>

#include "base/util/lookup.h"

namespace vertexai {
namespace tile {
namespace math {

template <typename T>
Polynomial<T>::Polynomial() {}

template <typename T>
Polynomial<T>::Polynomial(const T& c) : Polynomial<T>("", c) {}

template <typename T>
Polynomial<T>::Polynomial(const std::string& i, const T& c) {
  if (c) {
    map_[i] = c;
  }
}

template <typename T>
T Polynomial<T>::eval(const std::map<std::string, T>& values) const {
  T res = 0;
  for (const auto& kvp : map_) {
    if (kvp.first == "") {
      res += kvp.second;
    } else if (values.find(kvp.first) != values.end()) {
      res += kvp.second * safe_at(values, kvp.first);
    } else {
      throw std::runtime_error(
          str(boost::format("Failed to find value for %s, when evaluating %s") % kvp.first % toString()));
    }
  }
  return res;
}

template <typename T>
Polynomial<T> Polynomial<T>::partial_eval(const std::map<std::string, T>& values) const {
  Polynomial<T> r = *this;
  T off = 0;
  for (const auto& kvp : values) {
    off += get(kvp.first) * kvp.second;
    r.map_.erase(kvp.first);
  }
  r += off;
  return r;
}

template <typename T>
T Polynomial<T>::operator[](const std::string& var) const {
  auto it = map_.find(var);
  if (it == map_.end()) {
    return 0;
  }
  return it->second;
}

template <typename T>
const std::map<std::string, T>& Polynomial<T>::getMap() const {
  return map_;
}

template <typename T>
std::map<std::string, T>& Polynomial<T>::mutateMap() {
  return map_;
}

template <typename T>
Polynomial<T>& Polynomial<T>::operator+=(const Polynomial<T>& rhs) {
  for (const auto& kvp : rhs.map_) {
    T new_val = (map_[kvp.first] += kvp.second);
    if (new_val == 0) {
      map_.erase(kvp.first);
    }
  }
  return *this;
}

template <typename T>
bool Polynomial<T>::operator==(const Polynomial<T>& rhs) const {
  return map_ == rhs.map_;
}

template <typename T>
bool Polynomial<T>::operator<(const Polynomial<T>& rhs) const {
  return map_ < rhs.map_;
}

template <typename T>
Polynomial<T>& Polynomial<T>::operator-=(const Polynomial<T>& rhs) {
  return *this += -1 * rhs;
}

template <typename T>
Polynomial<T> Polynomial<T>::operator-() const {
  return -1 * (*this);
}

template <typename T>
Polynomial<T>& Polynomial<T>::operator*=(const T& rhs) {
  if (rhs == 0) {
    map_.clear();
  } else {
    for (auto& kvp : map_) {
      kvp.second *= rhs;
    }
  }
  return *this;
}

template <typename T>
Polynomial<T>& Polynomial<T>::operator/=(const T& rhs) {
  return *this *= (1 / rhs);
}

template <typename T>
T Polynomial<T>::constant() const {
  auto it = map_.find("");
  return (it == map_.end() ? 0 : it->second);
}

template <typename T>
void Polynomial<T>::setConstant(T value) {
  if (value == T(0)) {
    map_.erase("");
  } else {
    map_[""] = value;
  }
}

template <typename T>
T Polynomial<T>::tryDivide(const Polynomial<T>& p, bool ignoreConst) const {
  auto it = p.map_.begin();
  if (ignoreConst && it != p.map_.end() && it->first == "") {
    it++;
  }
  T val = 0;
  for (const auto& kvp : map_) {
    if (ignoreConst && kvp.first == "") {
      continue;
    }
    if (it == p.map_.end() || it->first != kvp.first) {
      return 0;  // Indexes don't exactly line up, fail
    }
    T div = kvp.second / it->second;
    if (val != 0 && div != val) {
      return 0;  // They don't all divide by the same number
    }
    val = div;
    it++;
  }
  if (it != p.map_.end()) {
    return 0;
  }
  return val;
}

template <typename T>
void Polynomial<T>::substitute(const std::string& var, const Polynomial<T>& replacement) {
  if (map_.count(var) == 0) {
    // If var isn't in this polynomial, nothing needs to be done
    return;
  }
  T coeff = safe_at(map_, var);
  map_.erase(var);
  (*this) += coeff * replacement;
}

template <typename T>
void Polynomial<T>::substitute(const std::map<std::string, Polynomial<T>>& replacements) {
  Polynomial result;
  for (const auto& name_value : map_) {
    auto replacement = replacements.find(name_value.first);
    if (replacement == replacements.end()) {
      result += Polynomial{name_value.first, name_value.second};
      continue;
    }
    result += replacement->second * name_value.second;
  }
  map_.swap(result.map_);
}

template <typename T>
void Polynomial<T>::substitute(const std::string& var, const T& replacement) {
  substitute(var, Polynomial<T>(replacement));
}

template <typename T>
Polynomial<T> Polynomial<T>::sym_eval(const std::map<std::string, Polynomial> values) const {
  Polynomial<T> out;
  for (const auto& kvp : map_) {
    if (kvp.first.empty()) {
      out += Polynomial<T>(kvp.second);
    } else {
      out += safe_at(values, kvp.first) * kvp.second;
    }
  }
  return out;
}

template <typename T>
std::string Polynomial<T>::GetNonzeroIndex() const {
  // Returns a nonconstant nonzero index, if one exists; otherwise returns empty string
  for (const auto& kvp : map_) {
    if (!(kvp.first.empty()) && kvp.second != 0) return kvp.first;
  }

  // No nonconstant index has a nonzero coefficient
  return std::string();
}

template <typename T>
T Polynomial<T>::get(const std::string& name) const {
  auto it = map_.find(name);
  if (it == map_.end()) {
    return T();
  }
  return it->second;
}

int64_t abs_value(int64_t value) { return std::llabs(value); }

Rational abs_value(Rational value) { return abs(value); }

template <typename T>
std::string Polynomial<T>::toString() const {
  std::stringstream ss;
  if (map_.size() == 0) {
    return "0";
  }
  bool first = true;
  for (const auto& kvp : map_) {
    if (first) {
      if (kvp.second < 0) {
        ss << "-";
      }
      first = false;
    } else {
      if (kvp.second > 0) {
        ss << " + ";
      } else {
        ss << " - ";
      }
    }
    auto value = abs_value(kvp.second);
    if (value != 1 || kvp.first == "") {
      ss << value;
      if (kvp.first != "") {
        ss << "*";
      }
    }
    ss << kvp.first;
  }
  return ss.str();
}

template class Polynomial<Rational>;
template class Polynomial<int64_t>;

SimpleConstraint::SimpleConstraint(const Polynomial<Rational>& _poly, int64_t _rhs) : poly(_poly), rhs(_rhs) {}

RangeConstraint::RangeConstraint(const Polynomial<Rational>& _poly, int64_t _range) : poly(_poly), range(_range) {}

bool RangeConstraint::IsParallel(const RangeConstraint& c) {
  if (this->poly.tryDivide(c.poly, true) != 0) return true;
  return false;
}

SimpleConstraint RangeConstraint::lowerBound() const { return SimpleConstraint(-poly, 0); }

SimpleConstraint RangeConstraint::upperBound() const { return SimpleConstraint(poly, range - 1); }

}  // namespace math
}  // namespace tile
}  // namespace vertexai


#include <boost/graph/property_maps/matrix_property_map.hpp>


/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

/**
 * This file contains an example of graphlab used for discrete loopy
 * belief propagation in a pairwise markov random field to denoise a
 * synthetic noisy image.
 *
 *  \author Joseph Gonzalez
 */

// INCLUDES ===================================================================>

// Including Standard Libraries
#include <cstdlib>
#include <cassert>
#include <cstring>
#include <fstream>
#include <iostream>
#include <vector>
#include <string>
#include <algorithm>
#include <limits>
#include <cmath>

#include <boost/program_options.hpp>

#include <graphlab.hpp>

#include "image.hpp"

// Include the macro for the for each operation
#include <graphlab/macros_def.hpp>

// STRUCTS (Edge and Vertex data) =============================================>

/**
 * The data associated with each directed edge in the pairwise markov
 * random field
 */
struct edge_data {
  graphlab::unary_factor message;
  graphlab::unary_factor old_message;
}; // End of edge data

/**
 * The data associated with each variable in the pairwise markov
 * random field
 */
struct vertex_data {
  graphlab::unary_factor potential;
  graphlab::unary_factor belief;
}; // End of vertex data

typedef graphlab::graph<vertex_data, edge_data> graph_type;
typedef graphlab::types<graph_type> gl_types;

gl_types::glshared<double> sh_bound;
gl_types::glshared<double> sh_damping;
gl_types::glshared<double> sh_ipfdamping;
gl_types::glshared<graphlab::binary_factor> sh_edgepot;
gl_types::glshared<graphlab::binary_factor> sh_truecounts;

// GraphLab Update Function ===================================================>

/** Construct denoising ising model based on the image */
void construct_graph(image& img,
                     size_t num_rings,
                     double sigma,
                     gl_types::graph& graph);

/** Get the counts of the true image */
void get_image_counts(image &trueimg,
                      graphlab::binary_factor &truebinarycount,
                      size_t arity);

/** Tests if two binary factors are equal */
bool binary_factor_equal(const graphlab::binary_factor &a,
                         const graphlab::binary_factor &b);
/**
 * The core belief propagation update function.  This update satisfies
 * the graphlab update_function interface.
 */
void bp_update(gl_types::iscope& scope,
               gl_types::icallback& scheduler);

/**
 * The edge potential sync simply counts the belief across the edges
 */
void edgepot_sync(gl_types::iscope &scope,  graphlab::any& acc);

/**
 * Performs an IPF update using the counts and the true counts
 */
void edgepot_apply(graphlab::any& result,  const graphlab::any& acc);

void edgepot_merge(graphlab::any& result,  const graphlab::any& acc);

// Command Line Parsing =======================================================>

struct options {
  size_t ncpus;
  double bound;
  double damping;
  size_t num_rings;
  size_t rows;
  size_t cols;
  double sigma;
  double lambda;
  size_t splash_size;
  std::string smoothing;
  std::string engine;
  std::string scope;
  std::string scheduler;
  std::string orig_fn;
  std::string noisy_fn;
  std::string pred_fn;
  std::string pred_type;
  std::string visualizer;
  std::string partmethod;
  size_t clustersize;
};

void get_image_counts(image &trueimg,
                      graphlab::binary_factor &truebinarycount,
                      size_t arity) {
  truebinarycount.resize(arity,arity);
  for (size_t i = 0; i < trueimg.rows(); ++i) {
    for (size_t j = 0; j < trueimg.cols(); ++j) {
      size_t color = trueimg.pixel(i,j);
      // check neighbors
      if (i > 1) {
        size_t color2 = trueimg.pixel(i-1,j);
        truebinarycount.logP(color,color2)++;
      }
      if (i < trueimg.rows() - 1) {
        size_t color2 = trueimg.pixel(i+1,j);
        truebinarycount.logP(color,color2)++;
      }
      if (j > 1) {
        size_t color2 = trueimg.pixel(i,j-1);
        truebinarycount.logP(color,color2)++;
      }
      if (j < trueimg.cols() - 1) {
        size_t color2 = trueimg.pixel(i,j+1);
        truebinarycount.logP(color,color2)++;
      }
    }
  }
}

bool binary_factor_equal(const graphlab::binary_factor &a,
                         const graphlab::binary_factor &b) {
  ASSERT_EQ(a.arity1(), b.arity1());
  ASSERT_EQ(a.arity2(), b.arity2());
  for (size_t i = 0;i < a.arity1(); ++i) {
     for (size_t j = 0;j < a.arity2(); ++j) {
      if ((a.logP(i,j) - b.logP(i,j)) > std::numeric_limits<double>::epsilon()) {
        return false;
      }
    }
  }
  return true;
}
// MAIN =======================================================================>
int main(int argc, char** argv) {
  std::cout << "This program creates and denoises a synthetic " << std::endl
            << "image using loopy belief propagation inside " << std::endl
            << "the graphlab framework." << std::endl;

  // set the global logger
  global_logger().set_log_level(LOG_WARNING);
  global_logger().set_log_to_console(true);

  double bound = 1E-4;
  double damping = 0.1;
  double ipfdamping = 0.1;
  size_t colors = 4;
  size_t rows = 100;
  size_t cols = 100;
  double sigma = 2;
  double lambda = 2;
  std::string smoothing = "laplace";
  std::string orig_fn = "source_img.pgm";
  std::string noisy_fn = "noisy_img.pgm";
  std::string pred_fn = "pred_img.pgm";
  std::string pred_type = "map";

  // Parse command line arguments --------------------------------------------->
  graphlab::command_line_options clopts("Loopy BP image denoising");
  clopts.attach_option("bound",
                       &bound, bound,
                       "Residual termination bound");
  clopts.attach_option("damping",
                       &damping, damping,
                       "The amount of message damping (higher = more damping)");
  clopts.attach_option("ipfdamping",
                       &ipfdamping, ipfdamping,
                       "The amount of IPF damping. (lower == more damping)");
  clopts.attach_option("colors",
                       &colors, colors,
                       "The number of colors in the noisy image");
  clopts.attach_option("rows",
                       &rows, rows,
                       "The number of rows in the noisy image");
  clopts.attach_option("cols",
                       &cols, cols,
                       "The number of columns in the noisy image");
  clopts.attach_option("sigma",
                       &sigma, sigma,
                       "Standard deviation of noise.");
  clopts.attach_option("lambda",
                       &lambda, lambda,
                       "Smoothness parameter (larger => smoother).");
  clopts.attach_option("smoothing",
                       &smoothing, smoothing,
                       "Options are {square, laplace}");
  clopts.attach_option("orig",
                       &orig_fn, orig_fn,
                       "Original image file name.");
  clopts.attach_option("noisy",
                       &noisy_fn, noisy_fn,
                       "Noisy image file name.");
  clopts.attach_option("pred",
                       &pred_fn, pred_fn,
                       "Predicted image file name.");
  clopts.attach_option("pred_type",
                       &pred_type, pred_type,
                       "Predicted image type {map, exp}");

  clopts.set_scheduler_type("splash(splash_size=100)");
  clopts.set_scope_type("edge");

  bool success = clopts.parse(argc, argv);
  if(!success) {
    return EXIT_FAILURE;
  }

  std::cout << "ncpus:          " << clopts.get_ncpus() << std::endl
            << "bound:          " << bound << std::endl
            << "damping:        " << damping << std::endl
            << "colors:         " << colors << std::endl
            << "rows:           " << rows << std::endl
            << "cols:           " << cols << std::endl
            << "sigma:          " << sigma << std::endl
            << "lambda:         " << lambda << std::endl
            << "smoothing:      " << smoothing << std::endl
            << "engine:         " << clopts.get_engine_type() << std::endl
            << "scope:          " << clopts.get_scope_type() << std::endl
            << "scheduler:      " << clopts.get_scheduler_type() << std::endl
            << "orig_fn:        " << orig_fn << std::endl
            << "noisy_fn:       " << noisy_fn << std::endl
            << "pred_fn:        " << pred_fn << std::endl
            << "pred_type:      " << pred_type << std::endl;

  // Create synthetic images -------------------------------------------------->
  // Creating image for denoising
  std::cout << "Creating a synthetic image. " << std::endl;
  image img(rows, cols);
  img.paint_sunset(colors);
  graphlab::binary_factor truecounts;
  get_image_counts(img, truecounts, colors);
  std::cout << "Saving image. " << std::endl;
  img.save(orig_fn.c_str());
  std::cout << "Corrupting Image. " << std::endl;
  img.corrupt(sigma);
  std::cout << "Saving corrupted image. " << std::endl;
  img.save(noisy_fn.c_str());

  std::cout << "True Counts: " << std::endl;
  std::cout << truecounts;

  // Create the graph --------------------------------------------------------->
  gl_types::core core;
  // Set the engine options
  core.set_engine_options(clopts);

  std::cout << "Constructing pairwise Markov Random Field. " << std::endl;
  construct_graph(img, colors, sigma, core.graph());

  // Setup global shared variables -------------------------------------------->
  // Initialize the edge agreement factor
  std::cout << "Initializing shared edge agreement factor. " << std::endl;

  // dummy variables 0 and 1 and num_rings by num_rings
  graphlab::binary_factor edge_potential(0, colors, 0, colors);
  // Set the smoothing type
  if(smoothing == "square") {
    edge_potential.set_as_agreement(lambda);
  } else if (smoothing == "laplace") {
    edge_potential.set_as_laplace(lambda);
  } else {
    std::cout << "Invalid smoothing stype!" << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << edge_potential << std::endl;

  // fill the shared variales
  sh_bound.set(bound);
  sh_damping.set(damping);
  sh_truecounts.set(truecounts);
  sh_edgepot.set(edge_potential);
  sh_ipfdamping.set(ipfdamping);
  // make the true counts

  // Running the engine ------------------------------------------------------->
  core.sched_options().add_option("update_function",bp_update);
  std::cout << "Running the engine. " << std::endl;

  graphlab::binary_factor zero;
  zero.resize(colors,colors);
  zero.set_as_agreement(0);
  core.set_sync(sh_edgepot,
                edgepot_sync,
                edgepot_apply,
                zero,
                rows*cols,
                edgepot_merge);
  graphlab::binary_factor oldedgepot = sh_edgepot.get_val();
  graphlab::timer ti;
  ti.start();
  // loop it a few times
  size_t update_count = 0;
  for (size_t i = 0;i < 10; ++i) {
    std::cout << "restart " << i << "\n";
    // Add the bp update to all vertices
    core.add_task_to_all(bp_update, 100.0);
    // Start the engine
    core.start();
    update_count += core.last_update_count();
    graphlab::binary_factor newedgepot = sh_edgepot.get_val();
    std::cout << newedgepot;
    if (binary_factor_equal(oldedgepot, newedgepot)) break;
    oldedgepot = newedgepot;
  }

  double runtime = ti.current_time();
  std::cout << "Finished Running engine in " << runtime
            << " seconds." << std::endl
            << "Total updates: " << update_count << std::endl
            << "Efficiency: " << (double(update_count) / runtime)
            << " updates per second "
            << std::endl;

  // Saving the output ----------------------------------------------------->
  std::cout << "Rendering the cleaned image. " << std::endl;
  if(pred_type == "map") {
    for(size_t v = 0; v < core.graph().num_vertices(); ++v) {
      const vertex_data& vdata = core.graph().vertex_data(v);
      img.pixel(v) = vdata.belief.max_asg();
    }
  } else if(pred_type == "exp") {
    for(size_t v = 0; v < core.graph().num_vertices(); ++v) {
      const vertex_data& vdata = core.graph().vertex_data(v);
      img.pixel(v) = vdata.belief.expectation();
    }
  } else {
    std::cout << "Invalid prediction type! : " << pred_type
              << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << "Saving cleaned image. " << std::endl;
  img.save(pred_fn.c_str());

  std::cout << "Done!" << std::endl;
  return EXIT_SUCCESS;
} // End of main

// Implementations
// ============================================================>
void bp_update(gl_types::iscope& scope,
               gl_types::icallback& scheduler) {
  //  std::cout << scope.vertex();;
  //  std::getchar();
  double bound = sh_bound.get_val();
  double damping = sh_damping.get_val();

  // Grab the state from the scope
  // ---------------------------------------------------------------->
  // Get the vertex data
  vertex_data& v_data = scope.vertex_data();

  // Get the in and out edges by reference
  gl_types::edge_list in_edges = scope.in_edge_ids();
  gl_types::edge_list out_edges = scope.out_edge_ids();
  assert(in_edges.size() == out_edges.size()); // Sanity check

  // Flip the old and new messages to improve safety when using the
  // unsynch scope
  foreach(graphlab::edge_id_t ineid, in_edges) {
    // Get the in and out edge data
    edge_data& in_edge = scope.edge_data(ineid);
    // Since we are about to receive the current message make it the
    // old message
    in_edge.old_message = in_edge.message;
  }

  // Compute the belief
  // ---------------------------------------------------------------->
  // Initialize the belief as the value of the factor
  v_data.belief = v_data.potential;
  foreach(graphlab::edge_id_t ineid, in_edges) {
    // Get the message
    const edge_data& e_data = scope.edge_data(ineid);
    // Notice we now use the old message since neighboring vertices
    // could be changing the new messages
    v_data.belief.times( e_data.old_message );
  }
  v_data.belief.normalize(); // finally normalize the belief

  // Compute outbound messages
  // ---------------------------------------------------------------->
  boost::shared_ptr<const graphlab::binary_factor> edge_factor_ptr =
    sh_edgepot.get_ptr();

  // Send outbound messages
  graphlab::unary_factor cavity, tmp_msg;
  for(size_t i = 0; i < in_edges.size(); ++i) {
    // Get the edge ids
    graphlab::edge_id_t outeid = out_edges[i];
    graphlab::edge_id_t ineid = in_edges[i];
    // CLEVER HACK: Here we are expoiting the sorting of the edge ids
    // to do fast O(1) time edge reversal
    assert(scope.target(outeid) == scope.source(ineid));
    // Get the in and out edge data
    const edge_data& in_edge = scope.edge_data(ineid);
    edge_data& out_edge = scope.edge_data(outeid);

    // Compute cavity
    cavity = v_data.belief;
    cavity.divide(in_edge.old_message); // Make the cavity a cavity
    cavity.normalize();

    // convolve cavity with the edge factor storing the result in the
    // temporary message
    tmp_msg.resize(out_edge.message.arity());
    tmp_msg.var() = out_edge.message.var();
    tmp_msg.convolve(*edge_factor_ptr, cavity);
    tmp_msg.normalize();

    // Damp the message
    tmp_msg.damp(out_edge.message, damping);

    // Compute message residual
    double residual = tmp_msg.residual(out_edge.old_message);

    // Assign the out message
    out_edge.message = tmp_msg;

    if(residual > bound) {
      gl_types::update_task task(scope.target(outeid), bp_update);
      scheduler.add_task(task, residual);
    }
  }
} // end of BP_update

void edgepot_sync(gl_types::iscope &scope,  graphlab::any& acc) {
  gl_types::edge_list in_edges = scope.in_edge_ids();
  gl_types::edge_list out_edges = scope.out_edge_ids();
  assert(in_edges.size() == out_edges.size()); // Sanity check

  graphlab::binary_factor& counts = acc.as<graphlab::binary_factor>();

  // Get the in and out edge data
  // the edge belief of u -- v
  // belief of u / msg_{v->u) * belief of v / msg_{u->v} * edgepot;

  const graphlab::unary_factor& blfu = scope.const_vertex_data().belief;

  foreach(graphlab::edge_id_t ineid, in_edges) {

    graphlab::vertex_id_t srcv = scope.source(ineid);
    // message from v->u
    const graphlab::unary_factor &msgvu =
      scope.const_edge_data(ineid).message;
    // belief at v
    const graphlab::unary_factor& blfv=
      scope.const_neighbor_vertex_data(srcv).belief;
    // get the message from u->v. requires the reverse edge
    graphlab::edge_id_t outeid = scope.reverse_edge(ineid);
    const graphlab::unary_factor &msguv =
      scope.const_edge_data(outeid).message;

    boost::shared_ptr<const graphlab::binary_factor> edge_factor =
      sh_edgepot.get_ptr();

    graphlab::binary_factor edge_belief;
    edge_belief.resize(blfu.arity(), blfv.arity());
    // loop through my assignments and my neighbor assignments

    // using logP to store actual counts
    for (size_t i = 0;i < blfu.arity(); ++i) {
      for (size_t j = 0;j < blfv.arity(); ++j) {
        edge_belief.logP(i,j) =
          blfu.logP(i) - msgvu.logP(i) + blfv.logP(j) -
          msguv.logP(j) + edge_factor->logP(i,j);
      }
    }
    edge_belief.normalize();
    for (size_t i = 0;i < edge_belief.arity1(); ++i) {
      for (size_t j = 0;j < edge_belief.arity2(); ++j) {
        counts.logP(i,j) += std::exp(edge_belief.logP(i,j));
      }
    }

  }
}

void edgepot_merge(graphlab::any& result,  const graphlab::any& acc) {
  graphlab::binary_factor& res = result.as<graphlab::binary_factor>();
  const graphlab::binary_factor& a = acc.as<graphlab::binary_factor>();

  for (size_t i = 0;i < res.arity1(); ++i) {
    for (size_t j = 0;j < res.arity2(); ++j) {
      res.logP(i,j) += a.logP(i,j);
    }
  }
}

void edgepot_apply(graphlab::any& result,  const graphlab::any& acc) {
  // IPF update
  graphlab::binary_factor& res = result.as<graphlab::binary_factor>();
  graphlab::binary_factor truecounts = sh_truecounts.get_val();
  const graphlab::binary_factor& curcounts =
    acc.as<graphlab::binary_factor>();
  double ipfdamping = sh_ipfdamping.get_val();
  // perform the IPF update
  // note that BP+IPF can be quite unstable.
  // (We do recommend the gradient update in practice)
  // so lets only update the parameter values if they change by > 1E-1
  for (size_t i = 0;i < res.arity1(); ++i) {
    for (size_t j = 0;j < res.arity2(); ++j) {
      // + 100 to avoid divide by 0 problems
      double newval =
        ipfdamping * log((truecounts.logP(i,j)+100) /
                         (curcounts.logP(i,j)+100)) +
        (1 - ipfdamping) * res.logP(i,j);
      if (std::fabs(res.logP(i,j) - newval) >= 1E-1) {
        res.logP(i,j) = newval;
      }
    }
  }
  std::cout << "sync of edge pot!\n";
  std::cout << res << std::endl;
}

void construct_graph(image& img,
                     size_t num_rings,
                     double sigma,
                     gl_types::graph& graph) {
  // Construct a single blob for the vertex data
  vertex_data vdata;
  vdata.potential.resize(num_rings);
  vdata.belief.resize(num_rings);
  vdata.belief.uniform();
  vdata.potential.uniform();
  vdata.belief.normalize();
  vdata.potential.normalize();
  // Add all the vertices
  double sigmaSq = sigma*sigma;
  for(size_t i = 0; i < img.rows(); ++i) {
    for(size_t j = 0; j < img.cols(); ++j) {
      // initialize the potential and belief
      uint32_t pixel_id = img.vertid(i, j);
      vdata.potential.var() = vdata.belief.var() = pixel_id;
      // Set the node potential
      double obs = img.pixel(i, j);
      for(size_t pred = 0; pred < num_rings; ++pred) {
        vdata.potential.logP(pred) =
          -(obs - pred)*(obs - pred) / (2.0 * sigmaSq);
      }
      vdata.potential.normalize();
      // Store the actual data in the graph
      size_t vertid = graph.add_vertex(vdata);
      // Ensure that we are using a consistent numbering
      assert(vertid == img.vertid(i, j));
    } // end of for j in cols
  } // end of for i in rows

  // Add the edges
  edge_data edata;
  edata.message.resize(num_rings);
  edata.message.uniform();
  edata.message.normalize();
  edata.old_message = edata.message;

  for(size_t i = 0; i < img.rows(); ++i) {
    for(size_t j = 0; j < img.cols(); ++j) {
      size_t vertid = img.vertid(i,j);
      if(i-1 < img.rows()) {
        edata.message.var() = img.vertid(i-1, j);
        edata.old_message.var() = edata.message.var();
        graph.add_edge(vertid, img.vertid(i-1, j), edata);
      }
      if(i+1 < img.rows()) {
        edata.message.var() = img.vertid(i+1, j);
        edata.old_message.var() = edata.message.var();
        graph.add_edge(vertid, img.vertid(i+1, j), edata);
      }
      if(j-1 < img.cols()) {
        edata.message.var() = img.vertid(i, j-1);
        edata.old_message.var() = edata.message.var();
        graph.add_edge(vertid, img.vertid(i, j-1), edata);
      } if(j+1 < img.cols()) {
        edata.message.var() = img.vertid(i, j+1);
        edata.old_message.var() = edata.message.var();
        graph.add_edge(vertid, img.vertid(i, j+1), edata);
      }
    } // end of for j in cols
  } // end of for i in rows
  graph.finalize();
} // End of construct graph


//
// Copyright 2010 Scott McMurray.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_HASH_STREAM_PREPROCESSOR_HPP
#define BOOST_HASH_STREAM_PREPROCESSOR_HPP

#include <boost/array.hpp>
#include <boost/hash/pack.hpp>
#include <boost/integer.hpp>
#include <boost/static_assert.hpp>
#include <boost/utility/enable_if.hpp>

#include <iterator>

namespace boost {
namespace hashes {

//
// This will do the usual Merkle-Damgård-style strengthening, padding with
// a 1 bit, then 0 bits as needed, then, if requested, the length.
//
template <typename endian,
          unsigned value_bits_,
          unsigned length_bits_,
          typename block_hash_T>
class stream_preprocessor {
  private:
    typedef block_hash_T block_hash_type;

    static unsigned const word_bits = block_hash_type::word_bits;
    typedef typename block_hash_type::word_type word_type;

    static unsigned const block_bits = block_hash_type::block_bits;
    static unsigned const block_words = block_hash_type::block_words;
    typedef typename block_hash_type::block_type block_type;
  public:
    typedef typename block_hash_type::digest_type digest_type;

    static unsigned const value_bits = value_bits_;
    typedef typename uint_t<value_bits>::least value_type;
    BOOST_STATIC_ASSERT(word_bits % value_bits == 0);
    static unsigned const block_values = block_bits/value_bits;
    typedef array<value_type, block_values> value_array_type;

  private:

    static unsigned const length_bits = length_bits_;
    // FIXME: do something more intelligent than capping at 64
    static unsigned const length_type_bits =
        length_bits < word_bits ?
            word_bits :
        length_bits > 64 ?
            64 :
            length_bits;
    typedef typename uint_t<length_type_bits>::least length_type;
    static unsigned const length_words = length_bits/word_bits;
    BOOST_STATIC_ASSERT(!length_bits || length_bits % word_bits == 0);
    BOOST_STATIC_ASSERT(block_bits % value_bits == 0);

    BOOST_STATIC_ASSERT(!length_bits || value_bits <= length_bits);

  private:
    void
    process_block() {
        // Convert the input into words
        block_type block;
        pack<endian, value_bits, word_bits>(value_array, block);

        // Process the block
        block_hash.update(block);

        // Reset seen if we don't need to track the length
        if (!length_bits) seen = 0;
    }
    template <typename Dummy>
    typename enable_if_c<length_bits && sizeof(Dummy)>::type
    append_length(length_type length) {
        // Convert the input into words
        block_type block;
        pack<endian, value_bits, word_bits>(value_array, block);

        // Append length
        array<length_type, 1> length_array = {{length}};
        array<word_type, length_words> length_words_array;
        pack<endian, length_bits, word_bits>(length_array, length_words_array);
        for (unsigned i = length_words; i; --i) {
            block[block_words - i] = length_words_array[length_words - i];
        }

        // Process the last block
        block_hash.update(block);
    }
    template <typename Dummy>
    typename disable_if_c<length_bits && sizeof(Dummy)>::type
    append_length(length_type) {
        // No appending requested, so nothing to do
    }
  public:
    stream_preprocessor &
    update_one(value_type value) {
        unsigned i = seen % block_bits;
        unsigned j = i / value_bits;
        value_array[j] = value;
        seen += value_bits;
        if (i == block_bits - value_bits) {
            // Process the completed block
            process_block();
        }
        return *this;
    }
    template <typename IterT>
    stream_preprocessor &
    update_n(IterT p, size_t n) {
#ifndef BOOST_HASH_NO_OPTIMIZATION
        for ( ; n && (seen % block_bits); --n, ++p) {
            update_one(*p);
        }
        for ( ; n >= block_values; n -= block_values, p += block_values) {
            // Convert the input into words
            block_type block;
            pack_n<endian, value_bits, word_bits>(p, block_values,
                                                  &block[0], block_words);

            // Process the block
            block_hash.update(block);
            seen += block_bits;

            // Reset seen if we don't need to track the length
            if (!length_bits) seen = 0;
        }
#endif
        for ( ; n; --n, ++p) {
            update_one(*p);
        }
        return *this;
    }
    template <typename IterT>
    stream_preprocessor &
    update(IterT b, IterT e, std::random_access_iterator_tag) {
        return update_n(b, e-b);
    }
    template <typename IterT, typename Category>
    stream_preprocessor &
    update(IterT b, IterT e, Category) {
        while (b != e) update_one(*b++);
        return *this;
    }
    template <typename IterT>
    stream_preprocessor &
    update(IterT b, IterT e) {
        typedef typename std::iterator_traits<IterT>::iterator_category cat;
        return update(b, e, cat());
    }
    template <typename ContainerT>
    stream_preprocessor &
    update(ContainerT const &c) {
        return update_n(c.data(), c.size());
    }
    digest_type
    end_message() {
        length_type length = seen;

        // Add a 1 bit
#ifdef BOOST_HASH_NO_OPTIMIZATION
        array<bool, value_bits> padding_bits = {{1}};
        array<value_type, 1> padding_values;
        pack<endian, 1, value_bits>(padding_bits, padding_values);
        update_one(padding_values[0]);
#else
        value_type pad = 0;
        detail::imploder_step<endian, 1, value_bits, 0>::step(1, pad);
        update_one(pad);
#endif

        // Pad with 0 bits
        while ((seen + length_bits) % block_bits != 0) {
            update_one(value_type());
        }

        // Append length
        append_length<int>(length);

        // Reset for next message
        seen = 0;

        // Calculate digest and reset block_hash
        return block_hash.end_message();
    }
    digest_type digest() const {
        return stream_preprocessor(*this).end_message();
    }

  public:
    stream_preprocessor() : value_array(), block_hash(), seen() {}
    void
    reset() {
        seen = 0;
        block_hash.reset();
    }

  private:
    value_array_type value_array;
    block_hash_type block_hash;
    length_type seen;
};

} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_STREAM_PREPROCESSOR_HPP


#include <gtest/gtest.h>

#include "scheme/nest/NEST.hh"
#include "HecatonicosachoronMap.hh"
#include "scheme/util/str.hh"
#include "scheme/io/dump_pdb_atom.hh"

#include <Eigen/Geometry>
// #include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <vector>
#include <boost/foreach.hpp>
#include <random>
#include <boost/lexical_cast.hpp>

namespace scheme { namespace nest { namespace pmap { namespace hecat_test {

using namespace Eigen;
using std::cout;
using std::endl;
using std::setprecision;
using std::vector;
using std::ofstream;
using std::string;
// using std::bitset;
using std::ostream;

double const r = (1.0+sqrt(5.0))/2.0;

void softassert( bool b, string s ){
	if(!b) cout << "WARNING: " << s << endl;
}

void
dumpatom(
	ostream & out,
	bool nothet,
	size_t iatom,
	string atom,
	string res,
	char chain,
	size_t ires,
	double x, double y, double z,
	double o, double b,
	string elem
){
	assert( atom.size()<5);
	assert( res.size()<4);
	softassert( x<10000 && x > -1000, "x out of bounds" );
	softassert( y<10000 && y > -1000, "y out of bounds" );
	softassert( z<10000 && z > -1000, "z out of bounds" );
	// cout << "ATOM   1604  C   GLU A 220       5.010  12.933   1.553  1.00 41.10           C" << endl;
	char buf[128];
	snprintf(buf,128,"%s%5lu %4s %3s %c%4lu    %8.3f%8.3f%8.3f%6.2f%6.2f %11s\n",
		nothet?"ATOM  ":"HETATM",
		iatom,
		atom.c_str(),
		res.c_str(),
		chain,
		ires,
		x,y,z
		,o,b,elem.c_str()
	);
	out << buf;
}

ostream & operator<<(ostream & out,Quaterniond const & q){
	char buf[128];
	snprintf(buf,128,"Quaterniond( %8.5f, %8.5f, %8.5f, %8.5f )",q.w(),q.x(),q.y(),q.z());
	out << buf;
	return out;
}

// Matrix4d projmat( Quaterniond q ){
// 	return Vector4d( q.w(), q.x(), q.y(), q.z() ) * RowVector4d( q.w(), q.x(), q.y(), q.z() );
// }

void dumpquats(string fname,vector<Quaterniond> &quats, Matrix4d proj, Quaterniond dir, double window=0.1){
	dir.normalize();
	ofstream out(fname.c_str());
	size_t ia=0, ir=0;
	int count = 0;
	BOOST_FOREACH(Quaterniond q,quats){
		// if( dir.dot(q) < mindot ) continue;
		// if( dir.dot(q) < 0.99 ) continue;
		Vector4d wxyz = proj*Vector4d(q.w(),q.x(),q.y(),q.z());
		if( fabs(wxyz[1]) > window || fabs(wxyz[2]) > window || fabs(wxyz[3]) > window ) continue;
		char chain = 'A';
		if( q.x() >= q.w() && q.x() >= q.x() && q.x() >= q.y() && q.x() >= q.z() ) chain = 'B';
		if( q.y() >= q.w() && q.y() >= q.x() && q.y() >= q.y() && q.y() >= q.z() ) chain = 'C';
		if( q.z() >= q.w() && q.z() >= q.x() && q.z() >= q.y() && q.z() >= q.z() ) chain = 'D';
		dumpatom(out,true,++ia,"QUAT","QUT",chain,++ir,3000*wxyz[1],3000*wxyz[2],3000*wxyz[3],1.0,1.0,"C");
		++count;
	}
	cout << dir << " dumped " << count << endl;
	out.close();
}

template<class M>
void dumppoints( string fname, M const & pts ){
	ofstream out(fname.c_str());
	size_t ia=0, ir=0;
	int count = 0;
	for(int i = 0; i < pts.cols(); ++i){
		// if( dir.dot(q) < mindot ) continue;
		dumpatom(out,true,++ia,"PNT ","PNT",'A',++ir,100.0*pts(0,i),100.0*pts(1,i),100.0*pts(2,i),1.0,1.0,"C");
		++count;
	}
	out.close();
}

// double sign(double d){
// 	if( d == 0) return 0;
// 	if( d > 0 ) return 1.0;
// 	else return -1.0;
// }

template<class T>
double min_cube_side(T const & coords, Matrix<typename T::Scalar,3,1> & cen){
	Matrix<typename T::Scalar,3,1> const mn = coords.rowwise().minCoeff();
	Matrix<typename T::Scalar,3,1> const mx = coords.rowwise().maxCoeff();
	Matrix<typename T::Scalar,3,1> const side = mx-mn;
	cen = (mn+mx)/2.0;
	double const cube = side.maxCoeff();
	return cube;
}
template<class T>
double min_cube_side(T const & coords){
	Matrix<typename T::Scalar,3,1> cen;
	return min_cube_side(coords,cen);
}

// TEST(hecatonicosachoron, sample_dodec_inscribe_cube){
// 	// dodecahedron
// 	// (±1, ±1, ±1)
// 	// (0, ±1/φ, ±φ)
// 	// (±1/φ, ±φ, 0)
// 	// (±φ, 0, ±1/φ)
// 	Matrix<double,3,20> dodec0;
// 	dodec0 << Vector3d(     +1,   +1,    +1    ).normalized(),
// 	          Vector3d(     +1,   +1,    -1    ).normalized(),
// 	          Vector3d(     +1,   -1,    +1    ).normalized(),
// 	          Vector3d(     +1,   -1,    -1    ).normalized(),
// 	          Vector3d(     -1,   +1,    +1    ).normalized(),
// 	          Vector3d(     -1,   +1,    -1    ).normalized(),
// 	          Vector3d(     -1,   -1,    +1    ).normalized(),
// 	          Vector3d(     -1,   -1,    -1    ).normalized(),
// 	          Vector3d(      0, +1.0/r,     +r ).normalized(),
// 	          Vector3d(      0, +1.0/r,     -r ).normalized(),
// 	          Vector3d(      0, -1.0/r,     +r ).normalized(),
// 	          Vector3d(      0, -1.0/r,     -r ).normalized(),
// 	          Vector3d( +1.0/r,     +r,    0   ).normalized(),
// 	          Vector3d( +1.0/r,     -r,    0   ).normalized(),
// 	          Vector3d( -1.0/r,     +r,    0   ).normalized(),
// 	          Vector3d( -1.0/r,     -r,    0   ).normalized(),
// 	          Vector3d(     +r,    0  , +1.0/r ).normalized(),
// 	          Vector3d(     +r,    0  , -1.0/r ).normalized(),
// 	          Vector3d(     -r,    0  , +1.0/r ).normalized(),
// 	          Vector3d(     -r,    0  , -1.0/r ).normalized();
// 	// dumppoints("dodec0.pdb",dodec0);

// 	std::mt19937 mt((unsigned int)time(0));
// 	std::normal_distribution<> rnorm;

// 	 // 0.90919
// 	Quaterniond Q0(
// 			-0.18683389537307968,
// 			-0.94619545649322123,
// 			0.1868233140277023,
// 			-0.18682693324842686
// 	);
// 	double const rad = 0.001;

// 	Vector3d cen;
// 	double const side0 = min_cube_side(dodec0,cen);
// 	double const area0 = side0*side0*side0;
// 	cout << "min cube side: " << side0 << endl;
// 	BOOST_VERIFY( cen.isApprox(Vector3d(0,0,0)));
// 	double minside = side0;
// 	Matrix<double,3,20> bestdodec;
// 	Quaterniond bestrot;
// 	for(size_t i = 0; i < 100000; ++i){
// 		// Quaterniond qrand = Quaterniond( rnorm(mt), rnorm(mt), rnorm(mt), rnorm(mt) );
// 		Quaterniond qrand( Q0.w()+rnorm(mt)*rad, Q0.x()+rnorm(mt)*rad, Q0.y()+rnorm(mt)*rad, Q0.z()+rnorm(mt)*rad );
// 		qrand.normalize();
// 		if(!i) qrand = Quaterniond(1,0,0,0);
// 		Matrix<double,3,20> rotdodec = qrand.matrix() * dodec0;
// 		double side = min_cube_side(rotdodec,cen);
// 		assert( cen.isApprox(Vector3d(0,0,0)));
// 		if( minside > side ){
// 			minside = side;
// 			bestrot = qrand;
// 			bestdodec = rotdodec;
// 			double const area = side*side*side;
// 			cout << "min cube side: " << side << " vol: " << area << " dodec vol frac: " << area / area0 << " " << i << endl;
// 			cout << bestrot << endl;
// 		}
// 	}
// 	cout << "BEST ROTATION OF DODEC: " << endl;
// 	cout << std::setprecision(17) << "\t\t\t" << bestrot.w() << ",\n\t\t\t" << bestrot.x() << ",\n\t\t\t" << bestrot.y() << ",\n\t\t\t" << bestrot.z() << endl;
// 	// dodec volume as constructed 2.78516386312, from this:
// 	// print Vec(1,1,1).normalized().distance( Vec(1/r, r, 0).normalized() )**3 * (15+7*sqrt(5)) /4.0

// 	dumppoints("0_dodec_0.pdb",dodec0/dodec0.block(0,0,3,1).norm());
// 	dumppoints("0_dodec_best.pdb",bestdodec/bestdodec.block(0,0,3,1).norm());
// 	// std::exit(0);

// }

vector<Quaterniond> make_half120cell(){
	// hexacosichoron (600-cell), 120 virts
	// (±½,±½,±½,±½),
	// and 8 vertices obtained from
	// (0,0,0,±1)
	// by permuting coordinates. The remaining 96 vertices are obtained by taking even permutations of
	// ½(±φ,±1,±1/φ,0).

	// hecatonicosachoron (120-cell), 600 virts
	// The 600 vertices of the 120-cell include all permutations of:[1]
	// (0, 0, ±2, ±2)
	// (±1, ±1, ±1, ±√5)
	// (±φ-2, ±φ, ±φ, ±φ)
	// (±φ-1, ±φ-1, ±φ-1, ±φ2)
	// and all even permutations of
	// (0, ±φ-2, ±1, ±φ2)
	// (0, ±φ-1, ±φ, ±√5)
	// (±φ-1, ±1, ±φ, ±2)
	// where φ (also called τ) is the golden ratio, (1+√5)/2.
	vector<Quaterniond> half120cell;
	{
		double const r = (1.0+sqrt(5.0))/2.0;
		vector<Quaterniond> c120;
		for(int s1 = 1; s1 > -2; s1-=2){
			// and 8 vertices obtained from (0,0,0,±1)
			c120.push_back( Quaterniond( s1, 0, 0, 0 ) );
			c120.push_back( Quaterniond(  0,s1, 0, 0 ) );
			c120.push_back( Quaterniond(  0, 0,s1, 0 ) );
			c120.push_back( Quaterniond(  0, 0, 0,s1 ) );
		for(int s2 = 1; s2 > -2; s2-=2){
		for(int s3 = 1; s3 > -2; s3-=2){
			// (±½,±½,±½,±½),
			c120.push_back( Quaterniond(  0.5, s1*0.5, s2*0.5, s3*0.5 ) );
			c120.push_back( Quaterniond( -0.5, s1*0.5, s2*0.5, s3*0.5 ) );
			// The remaining 96 vertices are obtained by taking even permutations of
			// ½(±φ,±1,±1/φ,0).
			double const a = s1*r/2.0, b=s2*1/2.0, c=s3/r/2.0, d=0;
			c120.push_back( Quaterniond( a,b,c,d ) ); // 0
			// c120.push_back( Quaterniond( a,b,d,c ) ); // 1
			// c120.push_back( Quaterniond( a,c,b,d ) ); // 1
			c120.push_back( Quaterniond( a,c,d,b ) ); // 2
			c120.push_back( Quaterniond( a,d,b,c ) ); // 2
			// c120.push_back( Quaterniond( a,d,c,b ) ); // 3
			// c120.push_back( Quaterniond( b,a,c,d ) ); // 1
			c120.push_back( Quaterniond( b,a,d,c ) ); // 2
			c120.push_back( Quaterniond( b,c,a,d ) ); // 2
			// c120.push_back( Quaterniond( b,c,d,a ) ); // 3
			// c120.push_back( Quaterniond( b,d,a,c ) ); // 3
			c120.push_back( Quaterniond( b,d,c,a ) ); // 4
			c120.push_back( Quaterniond( c,a,b,d ) ); // 2
			// c120.push_back( Quaterniond( c,a,d,b ) ); // 3
			// c120.push_back( Quaterniond( c,b,a,d ) ); // 3
			c120.push_back( Quaterniond( c,b,d,a ) ); // 4
			c120.push_back( Quaterniond( c,d,a,b ) ); // 4
			// c120.push_back( Quaterniond( c,d,b,a ) ); // 5
			// c120.push_back( Quaterniond( d,a,b,c ) ); // 3
			c120.push_back( Quaterniond( d,a,c,b ) ); // 4
			c120.push_back( Quaterniond( d,b,a,c ) ); // 4
			// c120.push_back( Quaterniond( d,b,c,a ) ); // 5
			// c120.push_back( Quaterniond( d,c,a,b ) ); // 5
			c120.push_back( Quaterniond( d,c,b,a ) ); // 6
		}}}

		/// provieds about 10% decrease to out-of-bounds samples
		Quaterniond alignq(
			-0.18683389537307968,
			-0.94619545649322123,
			 0.1868233140277023,
	 		-0.18682693324842686
		);
		Matrix4d alignment; alignment.fill(0);
		alignment.block(0,0,3,3) = alignq.matrix();
		alignment(3,3) = 1.0;
		BOOST_VERIFY( Vector4d(0,0,0,1).isApprox( alignment*Vector4d(0,0,0,1) ) );

		BOOST_FOREACH(Quaterniond q,c120){
			q.coeffs() = alignment * q.coeffs();
			if(  q.w()> 0 ||
				(q.w()==0 && q.x()>0) ||
				(q.w()==0 && q.x()==0 && q.y()>0) ||
				(q.w()==0 && q.x()==0 && q.y()==0 && q.z()>0)
			){
				half120cell.push_back( q );
			// } else {
				// BOOST_VERIFY( false );
			}
		}

		if( half120cell.size() != 60 ){
			cout << "problem making half-120-cell " << half120cell.size() << endl;
			// std::exit(-1);
		}
	}

	return half120cell;
}

Array<uint8_t,60,12>
make_half120cell_neighbors(
	vector<Quaterniond> const & half120cell
){
	Array<uint8_t,60,12> nbrs;
	for(size_t i = 0; i < 60; ++i){
		Quaterniond const & p = half120cell[i];
		size_t nnbr = 0;
		for(size_t j = 0; j < 60; ++j){
			if( i==j ) continue;
			Quaterniond const & q = half120cell[j];
			double ang = acos( 2.0*q.dot(p)*q.dot(p) - 1.0 )*180.0/M_PI;
			if( ang < 73.0 ){
				BOOST_VERIFY( nnbr < 12 );
				nbrs(i,nnbr++) = (uint8_t)j;
			}
		}
		BOOST_VERIFY( nnbr == 12 );
	}
	// for(size_t i = 0; i < 60; ++i){
	// 	cout << "nbrs " << i;
	// 	for(size_t j = 0; j < 12; ++j){
	// 		cout << " " << (int)nbrs(i,j);
	// 	}
	// 	cout << endl;
	// }
	return nbrs;
}
vector<Quaterniond> make_half120cell();

// TEST(hecatonicosachoron, dump_static_data){
// 	vector<Quaterniond> half120cell = make_half120cell();
// 	Array<uint8_t,60,12> nbrs = make_half120cell_neighbors(half120cell);

// 	cout << "\t\tstatic T const h120[240] = {" << endl;
// 	BOOST_FOREACH(Quaterniond q,half120cell){
// 		cout << "\t\t\t" << setprecision(17)  << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << "," << endl;
// 	}
// 	cout << "\t\t}" << endl;

// 	cout << "\t\tstatic T const h120inv[240] = {" << endl;
// 	BOOST_FOREACH(Quaterniond q,half120cell){
// 		q = q.inverse();
// 		cout << "\t\t\t" << setprecision(17)  << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << "," << endl;
// 	}
// 	cout << "\t\t}" << endl;

// 	cout << "\t\tstatic uint8_t const h120_nbrs[60,12] = {" << endl;
// 	for(int i = 0; i < 60; ++i){
// 		cout << "\t\t\t";
// 		for(int j = 0; j < 12; ++j){
// 			cout << static_cast<int>(nbrs(i,j)) << ",";
// 		}
// 		cout << endl;
// 	}
// 	cout << "\t\t}" << endl;

// 	cout << "\t\tstatic T const cellfaces[36] = {" << endl;
// 	for(int i = 0; i < 12; ++i){
// 		Vector3d nbr0 = half120cell[nbrs(0,i)].coeffs().block(0,0,3,1);
// 		nbr0 /= nbr0.norm();
// 		cout << "\t\t\t" << setprecision(17)  << nbr0[0] << "," << nbr0[1] << "," << nbr0[2] << "," << endl;
// 	}
// 	cout << "\t\t}" << endl;

// }

// TEST(hecatonicosachoron, cell_alignment){
// 	/////////////////////////////////////////////////////////////////////////////////////
// 	/////////////////////////////////////////////////////////////////////////////////////
// 	/////////////////////////////////////////////////////////////////////////////////////
// 	///////////// this doesn't make sense, must use vertices not centers! ///////////////
// 	/////////////////////////////////////////////////////////////////////////////////////
// 	/////////////////////////////////////////////////////////////////////////////////////
// 	vector<Quaterniond> half120cell = make_half120cell();
// 	Array<uint8_t,60,12> nbrs = make_half120cell_neighbors(half120cell);

// 	std::mt19937 mt((unsigned int)time(0));
// 	std::normal_distribution<> rnorm;

// 	Matrix<double,4,13> dodec0;
// 	for(size_t inbr = 0; inbr < 12; ++inbr){
// 		dodec0.col(inbr) = half120cell[ nbrs(0,inbr) ].coeffs();
// 	}
// 	dodec0.col(12) = Vector4d(0,0,0,1);
// 	// cout << dodec0.block(0,0,3,1).norm() << endl;
// 	// dumppoints("nbr_icos0.pdb",dodec0.block(0,0,3,13)/dodec0.block(0,0,3,1).norm());

// 	return;

// 	Vector3d cen;
// 	double const side0 = min_cube_side(dodec0.block(0,0,3,12),cen);
// 	BOOST_VERIFY( cen.isApprox(Vector3d(0,0,0)) );
// 	double const area0 = side0*side0*side0;
// 	cout << "min cube side: " << side0 << endl;

// 		// cout << dodec0.transpose() << endl;
// 	double mincube = side0;
// 	Matrix4d bestm4;
// 	for(size_t i = 0; i < 1; ++i){

// 		Matrix4d randm4; {
// 			randm4 <<   1, 5, 5, 0,
// 			            2, 3, 8, 0,
// 			            3, 4, 7, 0,
// 		                0, 0, 0, 1;
// 			HouseholderQR<Matrix4d> qr(randm4);
// 			randm4 = qr.householderQ();
// 			// cout << randm4 << endl << endl;
// 			// cout << ( randm4 * Vector4d(0,0,0,1) ).transpose() << endl;
// 			BOOST_VERIFY( Vector4d(0,0,0,1).isApprox(randm4*Vector4d(0,0,0,1)) );
// 		}

// 		Matrix<double,4,13>dodec = randm4 * dodec0;
// 		// cout << dodec.transpose() << endl;
// 		// dumppoints("test.pdb",dodec);

// 		double const side = min_cube_side(dodec.block(0,0,3,12),cen);
// 		BOOST_VERIFY( cen.isApprox(Vector3d(0,0,0)) );
// 		if( mincube > side ){
// 			mincube = side;
// 			bestm4 = randm4;
// 			double const area = side*side*side;
// 			cout << "min cube side: " << side << " vol: " << area << " dodec vol frac: " << area / area0 << endl;
// 		}
// 	}

// }

TEST(hecatonicosachoron,neighbor_identity_rots_init){

	vector<Quaterniond> half120cell = make_half120cell();
	Array<uint8_t,60,12> nbrs = make_half120cell_neighbors(half120cell);

	std::vector<Quaterniond> nbrs0;
	for(size_t inbr = 0; inbr < 12; ++inbr)
		nbrs0.push_back( half120cell[ nbrs(0,inbr) ] );

	// size_t ir = 0, ia = 0;
	size_t id = 0;
	BOOST_FOREACH( Quaterniond d, half120cell ){
		Quaterniond const dinv = d.inverse();
		// Quaterniond const dinvd = dinv*d;
		// std::cout << d << "  |  " << dinv << "  |  "  << dinvd << endl;
		// ofstream out(("120cell_"+str(id)+".pdb").c_str());
		for(size_t inbr = 0; inbr < 12; ++inbr){
			Quaterniond const q = half120cell[ nbrs(id,inbr) ];
			Quaterniond p = to_half_cell( dinv * q );
			// double norm = 100.0/p.coeffs().block(0,0,3,1).norm();
			// std::cout << d << "  |  " << q << "  |  "  << p << endl;
			if( fabs(p.norm()-1.0) > 0.0000001 ) std::exit(-1);
			// if( p.w() < 0.8  ) continue;
			// dumpatom(out,true,++ia,"QUAT","QUT",'A',++ir,norm*p.x(),norm*p.y(),norm*p.z(),1.0,1.0,"C");
			bool in_nbrs0 = false;
			for(size_t jnbr = 0; jnbr < 12; ++jnbr){
				if( p.isApprox(nbrs0[jnbr]) ){
					in_nbrs0 = true;
					// cout << id << " " << inbr << " " << jnbr << endl;
				}
			}
			ASSERT_TRUE(in_nbrs0);
		}
		// out.close();
		++id;
		// break;
	}
}

TEST(hecatonicosachoron,static_inv_check){
	typedef Map<Quaterniond const> QM;
	double const * h120raw = get_h120<double>();
	double const * h120invraw = get_h120inv<double>();

	Map< Matrix<double,4,60> const > h120( h120raw );
	Map< Matrix<double,4,60> const > h120inv( h120invraw );
	Map< Array<uint8_t,12,60> const > nbrs( get_h120_nbrs<uint8_t>() );

	for(int i = 0; i < 60; ++i){
		ASSERT_TRUE( Quaterniond(1,0,0,0).isApprox( h120_cellcen<double>(i)*h120_cellceninv<double>(i) ) );
	}
}

#ifdef CXX11
TEST(hecatonicosachoron,neighbor_identity_rots){
	typedef Map<Quaterniond const> QM;
	double const * h120raw = get_h120<double>();

	vector<Quaterniond> h120ref = make_half120cell();
	Array<uint8_t,60,12> nbrsref = make_half120cell_neighbors(h120ref);

	Map< Matrix<double,4,60> const > h120( h120raw );
	Map< Array<uint8_t,12,60> const > nbrs( get_h120_nbrs<uint8_t>() );

	for(int i = 0; i < 60; ++i){
		ASSERT_TRUE( h120ref[i].isApprox( QM( 4*i + h120raw ) ) );
		ASSERT_TRUE( h120ref[i].isApprox( h120_cellcen<double>(i) ) );
		for(int j = 0; j < 12; ++j){
			ASSERT_EQ( nbrsref(i,j), nbrs(j,i) );
		}
	}

	// cout << Quaterniond(h120.col(0)) << endl;
	// cout << Quaterniond(h120.col(1)) << endl;
	// cout << QM( h120raw+0 ) << endl;
	// cout << QM( h120raw+4 ) << endl;
	// cout << nbrs.col(0).transpose().cast<int>() << endl;
	// cout << nbrs.col(1).transpose().cast<int>() << endl;

	std::vector<QM> nbrs0;
	for(size_t inbr = 0; inbr < 12; ++inbr){
		nbrs0.push_back( QM( h120raw + 4*nbrs(inbr,0) ) );
	}
	// BOOST_FOREACH( Quaterniond q , nbrs0 ) cout << q << endl;

	// size_t ir = 0, ia = 0;
	for(int id = 0; id < 60; ++id){
		// QM d( 4*id + h120raw );
		// Quaterniond const dinv = d.inverse();
		QM dinv = h120_cellceninv<double>(id);
		// Quaterniond const dinvd = dinv*d;
		// std::cout << d << "  |  " << dinv << "  |  "  << dinvd << endl;
		// ofstream out(("120cell_"+str(id)+".pdb").c_str());
		for(size_t inbr = 0; inbr < 12; ++inbr){
			// QM q( h120raw + 4*nbrs(inbr,id) );
			Quaterniond p = to_half_cell( dinv * h120_cellnbr<double>(id,inbr) );
			// double norm = 100.0/p.coeffs().block(0,0,3,1).norm();
			// std::cout << d << "  |  " << q << "  |  "  << p << endl;
			if( fabs(p.norm()-1.0) > 0.0000001 ) std::exit(-1);
			// if( p.w() < 0.8  ) continue;
			// dumpatom(out,true,++ia,"QUAT","QUT",'A',++ir,norm*p.x(),norm*p.y(),norm*p.z(),1.0,1.0,"C");
			bool in_nbrs0 = false;
			for(size_t jnbr = 0; jnbr < 12; ++jnbr){
				if( p.isApprox(nbrs0[jnbr]) ){
					in_nbrs0 = true;
					// cout << id << " " << inbr << " " << jnbr << endl;
				}
			}
			ASSERT_TRUE(in_nbrs0);
		}
		// out.close();
		// break;
	}
}
#endif

TEST(hecatonicosachoron,cell_bounds){
	// cout << h120_cellcen<double>(0) << endl;
	// cout << cellnbr<double>(0,0) << endl;
	// cout << (h120_cellcen<double>(0).coeffs()+cellnbr<double>(0,0).coeffs()).normalized().transpose() << endl;
	// cout << std::setprecision(17) << (h120_cellcen<double>(0).coeffs()-cellnbr<double>(0,0).coeffs()).norm() << endl;
}

TEST(hecatonicosachoron,cell_lookup){
	// HecatonicosachoronMap<> map;
	// HecatonicosachoronMap<>::Params p;
	// for( size_t i = 0; i < 60; ++i){
	// 	size_t cell_index = 999;
	// 	map.value_to_params( h120_cellcen<double>(i).matrix(), 0, p, cell_index );
	// 	// cout << p << endl;
	// 	ASSERT_EQ( i, cell_index );
	// 	ASSERT_NEAR( p[0], 0.5, 0.0000001 );
	// 	ASSERT_NEAR( p[1], 0.5, 0.0000001 );
	// 	ASSERT_NEAR( p[2], 0.5, 0.0000001 );
	// }

	NEST<3,Matrix3d,HecatonicosachoronMap> nest;
	// ASSERT_EQ( 0, nest.get_index( nest.set_and_get(0,2), 2 ));

    int MAX_RESL = 3;
	#ifdef SCHEME_BENCHMARK
	MAX_RESL += 2;
	#endif

	for(int r = 0; r <= MAX_RESL; ++r){
		for(int i = 0; i < (int)nest.size(r); ++i){
			if( nest.set_state(i,r) ){
				size_t ilookup = nest.get_index( nest.value(), r );
				size_t ci0 = i >> (3*r);
				size_t ciL = ilookup >> (3*r);
				if( ci0 == ciL ) ASSERT_EQ( i , ilookup );
			}
		}
	}

}

TEST(hecatonicosachoron,covering){
	// cout << "QuaternionMap Covrad" << endl;
	int NRES = 8;
	int NITER = 10*1000;
	#ifdef SCHEME_BENCHMARK
		NITER = 1000*1000;
	#endif

	std::mt19937 rng((unsigned int)time(0));
	std::normal_distribution<> gauss;
	std::uniform_real_distribution<> uniform;

	NEST<3,Matrix3d,HecatonicosachoronMap> nest;
	for(int r = 0; r <= NRES; ++r){
		double maxdiff=0, avgdiff=0;
		for(int i = 0; i <= NITER; ++i){
			Eigen::Quaterniond q( fabs(gauss(rng)), gauss(rng), gauss(rng), gauss(rng) );
			q.normalize();
			// cout << q << endl;
			Eigen::Quaterniond qcen( nest.set_and_get( nest.get_index(q.matrix(),r) , r ) );
			maxdiff = std::max(maxdiff,q.angularDistance(qcen));
			avgdiff += q.angularDistance(qcen);
		}
		avgdiff /= NITER;
		size_t count = 0; for(size_t i = 0; i < nest.size(r)/60; ++i) if(nest.set_state(i,r)) ++count;
		count *= 60;
		// size_t count = nest.size(r);
		double volfrac = (double)count*(maxdiff*maxdiff*maxdiff)*4.0/3.0*M_PI / 8.0 / M_PI / M_PI;
		double avgfrac = (double)count*(avgdiff*avgdiff*avgdiff)*4.0/3.0*M_PI / 8.0 / M_PI / M_PI;
		printf("%2i %16lu %10.5f %10.5f %10.5f %10.5f %10.5f\n",
			r, count, maxdiff*180.0/M_PI, avgdiff*180.0/M_PI, maxdiff/avgdiff, volfrac, avgfrac );
	}

}

// TEST( hecatonicosachoron, visualize ){

// 	std::mt19937 rng((unsigned int)time(0));
// 	std::normal_distribution<> gauss;
// 	std::uniform_real_distribution<> uniform;

// 	// Quaterniond qrand( gauss(rng), gauss(rng), gauss(rng), gauss(rng) );
// 	Quaterniond qrand( 1,0,0,0 );
// 	qrand.normalize();
// 	Vector3d X = qrand*Vector3d(1,0  ,0  );
// 	Vector3d Y = qrand*Vector3d(0,1.2,0  );
// 	Vector3d Z = qrand*Vector3d(0,0  ,1.4);

// 	NEST<3,Matrix3d,HecatonicosachoronMap> nest;
// 	// size_t beg = 0;
// 	// while(!nest.set_state(beg,10)) beg = std::max<size_t>(uniform(rng)*(nest.size(10)-1000),0);
// 	for(size_t r = 0; r <= 4; ++r){
// 		int N = 2 * std::pow(8,r);
// 		int beg = std::max( 0, (int)nest.size(r)/12 - N/2 );
// 		std::ofstream out(("h120_"+boost::lexical_cast<std::string>(r)+".pdb").c_str());
// 		size_t count1 = 0, count2 = 0;
// 		// cout << r << " " << nest.size(r) << " " << (beg>>(4*(10-r))) << endl;
// 		// continue;
// 		// for(size_t i = beg>>(4*(10-r)); i < nest.size(r); ++i){
// 		for(size_t i = beg; i < nest.size(r); ++i){
// 			++count1;
// 			if( nest.set_state(i,r) ){
// 				++count2;
// 				if( count1 > N) break;
// 				Matrix3d m = nest.value();
// 				// cout << r << " " << i << " " << q.coeffs().transpose() << endl;
// 				 Vector3d ximg = m * X;
// 				 Vector3d yimg = m * Y;
// 				 Vector3d zimg = m * Z;;
// 				 // cout << r << " " << nest.cell_index(i,r) << endl;
// 				 io::dump_pdb_atom(out, nest.cell_index(i,r)<5?"H":"O" ,60*ximg);
// 				 io::dump_pdb_atom(out, nest.cell_index(i,r)<5?"H":"NI",60*yimg);
// 				 io::dump_pdb_atom(out, nest.cell_index(i,r)<5?"H":"N" ,60*zimg);
// 			}
// 		}
// 		out.close();
// 		cout << r << " " << count2 / (double)count1 << endl;
// 	}

// }

}}}}


/**
 * Copyright (c) 2013 Jonan Cruz-Martin
 *
 * Distributed under the terms of the MIT license, see the accompanying
 * file COPYING or http://opensource.org/licenses/MIT.
 *
 * @file
 * @brief Class definitions for domains of tetrahedra.
 */

#ifndef DOMAINS_TETRAHEDRAL_HPP
#define DOMAINS_TETRAHEDRAL_HPP

#include "types.hpp"
#include "../points/cartesian.hpp"
#include "../points/cylindrical.hpp"
#include "../points/polar.hpp"
#include "../points/spherical.hpp"

#include "../vertices/tetrahedral.hpp"
#include "../cells/tetrahedral.hpp"

#include <boost/python.hpp>
using namespace boost::python;

/**
 * Domain of lines in the cartesian 3D space.
 *
 * @since 0.1.0
 */
class TetrahedralCartesian3D_Domain {
	/**
	 * ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TetrahedralCartesian3D_Domain_t     domain;
public:
	TetrahedralCartesian3D_Domain() {};

	/**
	 * Return the number of vertices contained in the domain.
	 *
	 * @return unsigned integer
	 */
	unsigned int num_vertices();

	/**
	 * Create a new vertex in the domain based on the given point.
	 *
	 * @param point Point that represents the vertex. The point object will be copied.
	 */
	void make_vertex(PointCartesian3D point);

	/**
	 * Return the vertex identified by the given index.
	 *
	 * @param index Numerical vertex ID as assigned by ViennaGrid on vertex creation.
	 */
	TetrahedralCartesian3D_Vertex get_vertex(unsigned int index);

	/**
	 * Return a Python list containing all the vertices of the domain.
	 *
	 * @return Python list
	 */
	list get_vertices();

	/**
	 * Return the number of cells contained in the segment.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_cells();

	/**
	 * Create a new cell in the segment.
	 *
	 * @param vertex1
	 * @param vertex2
	 * @param vertex3
	 * @param vertex4
	 *
	 * @return The newly created cell.
	 *
	 * @since 0.1.0
	 */
	TetrahedralCartesian3D_Cell make_cell(TetrahedralCartesian3D_Vertex vertex1, TetrahedralCartesian3D_Vertex vertex2, TetrahedralCartesian3D_Vertex vertex3, TetrahedralCartesian3D_Vertex vertex4);

	/**
	 * Return a Python list containing all the cells contained in the segment.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_cells();

	/**
	 * Get a reference to the ViennaGrid domain object.
	 *
	 * @return Reference to the ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TetrahedralCartesian3D_Domain_t & get_domain();
};

/**
 * Domain of lines in the cylindrical space.
 *
 * @since 0.1.0
 */
class TetrahedralCylindrical3D_Domain {
	/**
	 * ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TetrahedralCylindrical3D_Domain_t     domain;
public:
	TetrahedralCylindrical3D_Domain() {};

	/**
	 * Return the number of vertices contained in the domain.
	 *
	 * @return unsigned integer
	 */
	unsigned int num_vertices();

	/**
	 * Create a new vertex in the domain based on the given point.
	 *
	 * @param point Point that represents the vertex. The point object will be copied.
	 */
	void make_vertex(PointCylindrical3D point);

	/**
	 * Return the vertex identified by the given index.
	 *
	 * @param index Numerical vertex ID as assigned by ViennaGrid on vertex creation.
	 */
	TetrahedralCylindrical3D_Vertex get_vertex(unsigned int index);

	/**
	 * Return a Python list containing all the vertices of the domain.
	 *
	 * @return Python list
	 */
	list get_vertices();

	/**
	 * Return the number of cells contained in the segment.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_cells();

	/**
	 * Create a new cell in the segment.
	 *
	 * @param vertex1
	 * @param vertex2
	 * @param vertex3
	 * @param vertex4
	 *
	 * @return The newly created cell.
	 *
	 * @since 0.1.0
	 */
	TetrahedralCylindrical3D_Cell make_cell(TetrahedralCylindrical3D_Vertex vertex1, TetrahedralCylindrical3D_Vertex vertex2, TetrahedralCylindrical3D_Vertex vertex3, TetrahedralCylindrical3D_Vertex vertex4);

	/**
	 * Return a Python list containing all the cells contained in the segment.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_cells();

	/**
	 * Get a reference to the ViennaGrid domain object.
	 *
	 * @return Reference to the ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TetrahedralCylindrical3D_Domain_t & get_domain();
};

/**
 * Domain of lines in the spherical space.
 *
 * @since 0.1.0
 */
class TetrahedralSpherical3D_Domain {
	/**
	 * ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TetrahedralSpherical3D_Domain_t     domain;
public:
	TetrahedralSpherical3D_Domain() {};

	/**
	 * Return the number of vertices contained in the domain.
	 *
	 * @return unsigned integer
	 */
	unsigned int num_vertices();

	/**
	 * Create a new vertex in the domain based on the given point.
	 *
	 * @param point Point that represents the vertex. The point object will be copied.
	 */
	void make_vertex(PointSpherical3D point);

	/**
	 * Return the vertex identified by the given index.
	 *
	 * @param index Numerical vertex ID as assigned by ViennaGrid on vertex creation.
	 */
	TetrahedralSpherical3D_Vertex get_vertex(unsigned int index);

	/**
	 * Return a Python list containing all the vertices of the domain.
	 *
	 * @return Python list
	 */
	list get_vertices();

	/**
	 * Return the number of cells contained in the segment.
	 *
	 * @return unsigned integer
	 *
	 * @since 0.1.0
	 */
	unsigned int num_cells();

	/**
	 * Create a new cell in the segment.
	 *
	 * @param vertex1
	 * @param vertex2
	 * @param vertex3
	 * @param vertex4
	 *
	 * @return The newly created cell.
	 *
	 * @since 0.1.0
	 */
	TetrahedralSpherical3D_Cell make_cell(TetrahedralSpherical3D_Vertex vertex1, TetrahedralSpherical3D_Vertex vertex2, TetrahedralSpherical3D_Vertex vertex3, TetrahedralSpherical3D_Vertex vertex4);

	/**
	 * Return a Python list containing all the cells contained in the segment.
	 *
	 * @return Python list
	 *
	 * @since 0.1.0
	 */
	list get_cells();

	/**
	 * Get a reference to the ViennaGrid domain object.
	 *
	 * @return Reference to the ViennaGrid domain object.
	 * @since 0.1.0
	 */
	TetrahedralSpherical3D_Domain_t & get_domain();
};

#endif /* end of include guard: DOMAINS_TETRAHEDRAL_HPP */


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/experimental/math/mersennetwister_multithreaded.hpp>
#include <boost/make_shared.hpp>

#define MT_MT_DISPATCH(method)                                                 \
    switch (threadId) {                                                        \
    case 0:                                                                    \
        return m0_->method();                                                  \
    case 1:                                                                    \
        return m1_->method();                                                  \
    case 2:                                                                    \
        return m2_->method();                                                  \
    case 3:                                                                    \
        return m3_->method();                                                  \
    case 4:                                                                    \
        return m4_->method();                                                  \
    case 5:                                                                    \
        return m5_->method();                                                  \
    case 6:                                                                    \
        return m6_->method();                                                  \
    case 7:                                                                    \
        return m7_->method();                                                  \
    default:                                                                   \
        QL_FAIL("thread " << threadId << " out of range [0...7]");             \
    }

namespace QuantLib {

MersenneTwisterMultiThreaded::MersenneTwisterMultiThreaded(
    const unsigned long seed) {
    m0_ = boost::make_shared<MersenneTwisterCustomRng<Mtdesc19937_0> >(seed);
    m1_ = boost::make_shared<MersenneTwisterCustomRng<Mtdesc19937_1> >(seed);
    m2_ = boost::make_shared<MersenneTwisterCustomRng<Mtdesc19937_2> >(seed);
    m3_ = boost::make_shared<MersenneTwisterCustomRng<Mtdesc19937_3> >(seed);
    m4_ = boost::make_shared<MersenneTwisterCustomRng<Mtdesc19937_4> >(seed);
    m5_ = boost::make_shared<MersenneTwisterCustomRng<Mtdesc19937_5> >(seed);
    m6_ = boost::make_shared<MersenneTwisterCustomRng<Mtdesc19937_6> >(seed);
    m7_ = boost::make_shared<MersenneTwisterCustomRng<Mtdesc19937_7> >(seed);
}

MersenneTwisterMultiThreaded::sample_type
MersenneTwisterMultiThreaded::next(unsigned int threadId) const {
    MT_MT_DISPATCH(next);
}

Real MersenneTwisterMultiThreaded::nextReal(unsigned int threadId) const {
    MT_MT_DISPATCH(nextReal);
}

unsigned long MersenneTwisterMultiThreaded::
operator()(unsigned int threadId) const {
    MT_MT_DISPATCH(operator());
}

inline unsigned long
MersenneTwisterMultiThreaded::nextInt32(unsigned int threadId) const {
    MT_MT_DISPATCH(nextInt32);
}
}


#include <cstdlib>
#include <iostream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <functional>
#include <iterator>
#include <boost/iterator/filter_iterator.hpp>

struct is_odd
{
    bool operator()(int a) { return 0 != a % 2; }
};

struct is_even
{
    bool operator()(int a) { return 0 == a % 2; }
};

struct sequence
{
    sequence() : index(0) {}
    int operator()() { return index++; }
    int index;
};

int main()
{
    std::vector<int> v(10);
    std::generate(v.begin(), v.end(), sequence());
    std::cout << "odd numbers: ";
    std::copy(
        boost::make_filter_iterator<is_odd>(v.begin(), v.end()),
        boost::make_filter_iterator<is_odd>(v.end(), v.end()),
        std::ostream_iterator<int>(std::cout, " "));
    std::cout << std::endl;
    std::cout << "even numbers: ";
    std::copy(
        boost::make_filter_iterator<is_even>(v.begin(), v.end()),
        boost::make_filter_iterator<is_even>(v.end(), v.end()),
        std::ostream_iterator<int>(std::cout, " "));
    std::cout << std::endl;

    return EXIT_SUCCESS;
}


//
//  hmm.h
//  mazeSolver
//
//  Created by Nikhil Joshi on 12/8/12.
//  Copyright (c) 2012 Nikhil Joshi. All rights reserved.
//

#ifndef __mazeSolver__hmm__
#define __mazeSolver__hmm__

#include <iostream>
#include <iomanip>
#include <sstream>
#include <vector>
#include <cmath>

#include <boost/dynamic_bitset.hpp>

#include <SFML/Graphics.hpp>

#include "table.hpp"

class hmm{
public:
    // id
    unsigned int m_id;
    // inputs
    std::vector<unsigned int> m_inputs;
    // outputs
    std::vector<unsigned int> m_outputs;
    // transition probabilities (unnormalized)
    std::vector<std::vector<unsigned int> > m_transTable;

    // fonts
    sf::Font m_font;
    // color (in which HMM is displayed)
    sf::Color m_color;

    // constructor
    hmm(unsigned int id,
        std::vector<unsigned int> inputs,
        std::vector<unsigned int> outputs,
        std::vector<std::vector<unsigned int> > transTable,
        std::vector<double> leftTop = std::vector<double>())
    :m_id(id), m_inputs(inputs), m_outputs(outputs), m_transTable(transTable){

        // set fonts
        m_font.loadFromFile("/Library/Fonts/Arial.ttf");

        // color
        m_color = sf::Color(230, 170, 0);
    }

    // copy constructor
    hmm(const hmm& o){
        m_id = o.m_id;
        m_inputs = o.m_inputs;
        m_outputs = o.m_outputs;
        m_transTable = o.m_transTable;
        m_font = o.m_font;
        m_color = o.m_color;
    }

    // assignment operator
    hmm& operator = (const hmm o){
        m_id = o.m_id;
        m_inputs = o.m_inputs;
        m_outputs = o.m_outputs;
        m_transTable = o.m_transTable;
        m_font = o.m_font;
        m_color = o.m_color;

        return *this;
    }

    // rank operator (for sorting)
    bool operator < (const hmm& o) const{
        return (m_id < o.m_id);
    }

    // member functions
//    // render the unit on a given window and return end corners
//    sf::FloatRect renderUnit(sf::RenderWindow& window,
//                             std::vector<double> leftTop = std::vector<double>(),
//                             bool withActivity = true,
//                             unsigned int inState = std::pow(2, 12.0),
//                             unsigned int outState = std::pow(2, 12.0));

    // render the unit on given window and return end corners
    sf::FloatRect renderUnit(sf::RenderTarget& tex,
                             std::vector<double> leftTop = std::vector<double>(),
                             bool withActivity = true,
                             unsigned int inState = std::pow(2, 12.0),
                             unsigned int outState = std::pow(2, 12.0));

};

#endif /* defined(__mazeSolver__hmm__) */


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* MIT License                                                                     */
/*                                                                                */
/* Copyright (c) 2022 Nariaki Tateiwa <n-tateiwa@kyudai.jp>                        */
/*                                                                                */
/* Permission is hereby granted, free of charge, to any person obtaining a copy    */
/* of this software and associated documentation files (the "Software"), to deal   */
/* in the Software without restriction, including without limitation the rights    */
/* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell       */
/* copies of the Software, and to permit persons to whom the Software is           */
/* furnished to do so, subject to the following conditions:                        */
/*                                                                                */
/* The above copyright notice and this permission notice shall be included in all  */
/* copies or substantial portions of the Software.                                 */
/*                                                                                */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR      */
/* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,        */
/* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE     */
/* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER          */
/* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,   */
/* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE   */
/* SOFTWARE.                                                                       */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/**@file    Reduction.cpp
 * @brief   Base class for Lattice basis reduction of NTL library.
 * @author  Nariaki Tateiwa
 *
 *
 *
 */

/*---+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----8----+----9----+----0----+----1----+----2*/

#include "Reduction.h"

#include <NTL/LLL.h>       // for BKZ_FP, BKZ_QP, LLL_FP, LLL_QP
#include <type_traits>     // for is_same
#include "NTL/mat_ZZ.h"    // for mat_ZZ
#include "Lattice.h"       // for Lattice

namespace LapTools
{

///
/// @brief run LLL algorithm in NTL
///
template<typename BasisFloat, typename GSFloat>
bool
Reduction<BasisFloat, GSFloat>::lll(
      )
{
   NTL::mat_ZZ Mbasis = L->toNLTMat();
   if( std::is_same<GSFloat, double>() )
      NTL::LLL_FP(Mbasis, delta, 0, 0, verbose);
   else
      NTL::LLL_QP(Mbasis, 0.99, 0, 0, verbose);
   L->fromNLTMat(Mbasis);
   return true;
}

///
/// @brief run bkz algorithm in NTL
/// @param[in] beta blocksize
///
template<typename BasisFloat, typename GSFloat>
bool
Reduction<BasisFloat, GSFloat>::bkz(
      int beta
      )
{
   NTL::mat_ZZ Mbasis = L->toNLTMat();
   if( std::is_same<GSFloat, double>() )
      NTL::BKZ_FP(Mbasis, delta, beta, 0, 0, verbose);
   else
      NTL::BKZ_QP(Mbasis, delta, beta, 0, 0, verbose);
   L->fromNLTMat(Mbasis);
   return true;
}

///
/// instantiation
///
template class Reduction<int, double>;
template class Reduction<int, long double>;
template class Reduction<long int, double>;
template class Reduction<long int, long double>;

}  // namespace LapTools


// Copyright (c) 2007  INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
// Author(s)     : Laurent Rineau

#include <iostream>
#include <string>
#include <vector>
#include <map>
#include <stack>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include <boost/format.hpp>

using std::cout;
using std::cin;
using std::endl;

int main()
{
  std::string header;
  cin >> header;

  if(header != "OFF")
  {
    std::cerr << "header is \"" << header << "\"\nshould be \"OFF\".\n";
    return 1;
  }

  cout << header << endl;
  unsigned int n_vertices;
  unsigned int n_facets;
  std::string dummy;

  cin >> n_vertices;
  cin >> n_facets;
  getline(cin, dummy);

  // Vector that maps from old vertex index to new vertex index,
  // as some (old) vertices may have identical point coordinates.
  std::vector<int> new_index(n_vertices);

  typedef boost::tuple<double, double, double> Point;
  // Vector that stores the points coordinates (in a Point).
  std::vector<Point> points(n_vertices);

  // Map that retains the mapping from the point coordinates (in a Point)
  // to the nex index.
  typedef std::map<Point, int> Renumber;
  Renumber renumber;

  unsigned int index = 0;
  for(unsigned int i = 0; i < n_vertices; ++i)
  {
    cin >> boost::get<0>(points[index])
        >> boost::get<1>(points[index]) >> boost::get<2>(points[index]);
    Renumber::const_iterator it = renumber.find(points[index]);
    if( it == renumber.end() )
    {
      renumber[points[index]] = index;
      new_index[i] = index;
      ++index;
    }
    else
    {
      new_index[i] = it->second;
    }
  }

  cout << index << " " << n_facets << dummy << endl;
  for(unsigned int i = 0; i < index; ++i)
  {
    cout << boost::get<0>(points[i]) << " "
         << boost::get<1>(points[i]) << " "
         << boost::get<2>(points[i]) << "\n";
  }

  // Vector that stores each facet.
  typedef std::vector<boost::tuple<int, int, int> > Facets;
  Facets facets;

  // For each (oriented) edge, that map stores a vector of adjacent facets,
  // with a boolean that tells if the edge is in the opposite orientation,
  // in the facet.
  // Edges are stores in the direction from the smallest index to the
  // greatest.
  typedef std::pair<int, int> Edge;
  typedef std::map<Edge, std::vector<std::pair<int, bool> > > Edges_map;
  Edges_map edges;

  // "nested function" opposite, that returns the edge, in the opposite
  // direction.
  struct {
    Edge operator()(Edge e) const {
      return std::make_pair(e.second, e.first);
    };
  } opposite;

  for(unsigned int i_facet = 0; i_facet < n_facets; ++i_facet)
  {
    // Read a facet, then reindex its vertices.
    int i, j, k;
    cin >> dummy >> i >> j >> k;
    if( dummy != "3" )
    {
      std::cerr << "In facet #" << i_facet << ", expected \"3\", found \""
                << dummy << "\"!\n";
      return 1;
    }
    i = new_index[i];
    j = new_index[j];
    k = new_index[k];
    facets.push_back(boost::make_tuple(i, j, k));

    // Create the three edges of the facet.
    Edge e[3];
    e[0] = std::make_pair(i, j);
    e[1] = std::make_pair(j, k);
    e[2] = std::make_pair(k, i);
    for(int i_edge = 0; i_edge < 3; ++i_edge)
    {
      if( e[i_edge].first < e[i_edge].second )
        edges[e[i_edge]].push_back(std::make_pair(i_facet, false));
      else
        edges[opposite(e[i_edge])].push_back(std::make_pair(i_facet, true));
    }
  }

  // Map that stores all already passed facet, and retains the orientation
  // of the facet. "true" means that the facet needs to be reoriented.
  std::map<int, bool> oriented_set;

  // Stack of facets indices to be handled.
  std::stack<int> stack;
  int seed_facet_candidate = 0;

  while (oriented_set.size() != n_facets) {
    // find a facet index that is not yet in 'oriented_set'.
    while( oriented_set.find(seed_facet_candidate) != oriented_set.end() )
      ++seed_facet_candidate;
    std::cerr << "Need seed facet: " << seed_facet_candidate << "\n";
    // push it in oriented set
    oriented_set[seed_facet_candidate] = false;
    stack.push(seed_facet_candidate);

    while(! stack.empty() ) {
      const int f = stack.top();
      stack.pop();
      const int i = boost::get<0>(facets[f]);
      const int j = boost::get<1>(facets[f]);
      const int k = boost::get<2>(facets[f]);
      Edge e[3];
      e[0] = std::make_pair(i, j);
      e[1] = std::make_pair(j, k);
      e[2] = std::make_pair(k, i);
      for(int ih = 0 ; ih < 3 ; ++ih)
      {
        bool f_orient = false;
        if(e[ih].first > e[ih].second) {
          f_orient = true;
          e[ih] = opposite(e[ih]);
        }

        Edges_map::iterator edge_it = edges.find(e[ih]);
        if(edge_it->second.size() == 2) { // regular edge
          int fn = edge_it->second[0].first;
          bool fn_orient = edge_it->second[0].second;
          if(fn == f)
          {
            fn = edge_it->second[1].first;
            fn_orient = edge_it->second[1].second;
          }
          if (oriented_set.find(fn) == oriented_set.end())
          {
            if(f_orient == fn_orient)
              oriented_set[fn] = ! oriented_set[f];
            else
              oriented_set[fn] = oriented_set[f];
            stack.push(fn);
          }
        } // end "if the edge is regular"
        else {
          std::cerr << boost::format("Irregular edge: (%1%,%2%)"
                                     ", %3% facets.\n")
            % e[ih].first % e[ih].second
            % edge_it->second.size();
        }
      } // end "for each neighbor of f"
    } // end "stack non empty"
  } // end "oriented_set not full"

  for(unsigned int i_facet = 0; i_facet < n_facets; ++i_facet)
  {
    const int i = boost::get<0>(facets[i_facet]);
    const int j = boost::get<1>(facets[i_facet]);
    const int k = boost::get<2>(facets[i_facet]);
    if(oriented_set[i_facet])
      cout << "3 " << j << " " << i << " " << k << "\n";
    else
      cout << "3 " << i << " " << j << " " << k << "\n";
  }
}


#ifndef ANIE_HEADER_MATH_MATRIX_HPP
#define ANIE_HEADER_MATH_MATRIX_HPP
#include <anie/config.hpp>

#include <anie/device.hpp>

#include <cstddef>
#include <vector>
#include <boost/compute.hpp>

namespace anie
{
	class ANIE_EXPORT matrix final
	{
	public:
		using iterator = boost::compute::vector<arithemtic_type>::iterator;
		using const_iterator = boost::compute::vector<arithemtic_type>::const_iterator;
		using reverse_iterator = boost::compute::vector<arithemtic_type>::reverse_iterator;
		using const_reverse_iterator = boost::compute::vector<arithemtic_type>::const_reverse_iterator;

	public:
		explicit matrix(const anie::device& device) noexcept;
		matrix(const anie::device& device, std::size_t size);
		matrix(const anie::device& device, std::size_t width, std::size_t height);
		matrix(const matrix& matrix);
		matrix(matrix&& matrix) noexcept;
		~matrix() = default;

	public:
		matrix& operator=(const matrix& matrix);
		matrix& operator=(matrix&& matrix) noexcept;
		bool operator==(const matrix& matrix) const;
		bool operator!=(const matrix& matrix) const;
		matrix operator+(const matrix& matrix) const;
		matrix operator-(const matrix& matrix) const;
		matrix operator*(const matrix& matrix) const;
		matrix& operator+=(const matrix& matrix);
		matrix& operator-=(const matrix& matrix);

	public:
		std::size_t width() const noexcept;

		void copy_to(std::vector<arithemtic_type>& vector) const;
		template<typename Iterator_>
		void copy_to(Iterator_ start) const
		{
			copy_to(start, data_.size());
		}
		template<typename Iterator_>
		void copy_to(Iterator_ start, std::size_t size) const
		{
			boost::compute::copy(data_.begin(), data_.begin() + size, start, device_.queue());
		}
		void copy_from(const std::vector<arithemtic_type>& vector);
		template<typename Iterator_>
		void copy_from(Iterator_ start, Iterator_ end)
		{
			boost::compute::copy(start, end, data_.begin(), device_.queue());
		}

		matrix transpose() const;

		iterator begin();
		const_iterator begin() const;
		const_iterator cbegin() const;
		iterator end();
		const_iterator end() const;
		const_iterator cend() const;
		reverse_iterator rbegin();
		const_reverse_iterator rbegin() const;
		const_reverse_iterator crbegin() const;
		reverse_iterator rend();
		const_reverse_iterator rend() const;
		const_reverse_iterator crend() const;

	public:
		anie::device device() const noexcept;
		std::size_t height() const noexcept;

	private:
		boost::compute::vector<arithemtic_type> data_;
		anie::device device_;
		std::size_t height_;
	};
}

#endif


/*
 * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * 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 the Autonomous Systems Lab, ETH Zurich 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 Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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.
 *
 */

#include <iostream>

#include <Eigen/Core>

#include <gtest/gtest.h>

#include "kindr/common/gtest_eigen.hpp"
#include "kindr/poses/Pose.hpp"
#include "kindr_ros/RosGeometryMsgPose.hpp"

// ROS
#include <geometry_msgs/Pose.h>

TEST(RosGeometryMsgPoseEigen, convertFromRosGeometryMsg) {
  const kindr::Position3D referenceTranslation(0.3, -1.5, 0.6);
  const kindr::RotationQuaternionPD referenceQuaternion(0.113, 0.071, -0.924, 0.35835);

  geometry_msgs::Pose geometryPoseMsg;
  geometryPoseMsg.orientation.x = referenceQuaternion.x();
  geometryPoseMsg.orientation.y = referenceQuaternion.y();
  geometryPoseMsg.orientation.z = referenceQuaternion.z();
  geometryPoseMsg.orientation.w = referenceQuaternion.w();
  geometryPoseMsg.position.x = referenceTranslation.x();
  geometryPoseMsg.position.y = referenceTranslation.y();
  geometryPoseMsg.position.z = referenceTranslation.z();

  kindr::HomogeneousTransformationPosition3RotationQuaternionD pose;
  kindr_ros::convertFromRosGeometryMsg(geometryPoseMsg, pose);

  kindr::expectNear(pose.getPosition().toImplementation(), referenceTranslation.toImplementation(), 1e-4, KINDR_SOURCE_FILE_POS);
  EXPECT_TRUE(pose.getRotation().isNear(referenceQuaternion, 1e-8));
}

TEST(RosGeometryMsgPoseEigen, convertToRosGeometryMsg) {
  const kindr::Position3D referenceTranslation(0.3, -1.1, -0.6);
  const kindr::RotationQuaternionPD referenceQuaternion(0.212, 0.0421, -0.958, 0.1885);

  kindr::HomogeneousTransformationPosition3RotationQuaternionD pose;
  pose.getPosition() = referenceTranslation;
  pose.getRotation() = referenceQuaternion;

  geometry_msgs::Pose geometryPoseMsg;
  kindr_ros::convertToRosGeometryMsg(pose, geometryPoseMsg);

  EXPECT_NEAR(geometryPoseMsg.orientation.x, referenceQuaternion.x(), 1e-8);
  EXPECT_NEAR(geometryPoseMsg.orientation.y, referenceQuaternion.y(), 1e-8);
  EXPECT_NEAR(geometryPoseMsg.orientation.z, referenceQuaternion.z(), 1e-8);
  EXPECT_NEAR(geometryPoseMsg.orientation.w, referenceQuaternion.w(), 1e-8);
  EXPECT_NEAR(geometryPoseMsg.position.x, referenceTranslation.x(), 1e-8);
  EXPECT_NEAR(geometryPoseMsg.position.y, referenceTranslation.y(), 1e-8);
  EXPECT_NEAR(geometryPoseMsg.position.z, referenceTranslation.z(), 1e-8);
}

TEST(RosGeometryMsgTransformationEigen, convertFromRosGeometryMsg) {
  const kindr::Position3D referenceTranslation(0.12, 1.5, 0.6);
  const kindr::RotationQuaternionPD referenceQuaternion(0.949, 0.133, 0.169, 0.230541);

  geometry_msgs::Transform geometryTransformMsg;
  geometryTransformMsg.rotation.x = referenceQuaternion.x();
  geometryTransformMsg.rotation.y = referenceQuaternion.y();
  geometryTransformMsg.rotation.z = referenceQuaternion.z();
  geometryTransformMsg.rotation.w = referenceQuaternion.w();
  geometryTransformMsg.translation.x = referenceTranslation.x();
  geometryTransformMsg.translation.y = referenceTranslation.y();
  geometryTransformMsg.translation.z = referenceTranslation.z();

  kindr::HomogeneousTransformationPosition3RotationQuaternionD transformation;
  kindr_ros::convertFromRosGeometryMsg(geometryTransformMsg, transformation);

  kindr::expectNear(transformation.getPosition().toImplementation(), referenceTranslation.toImplementation(), 1e-4, KINDR_SOURCE_FILE_POS);
  EXPECT_TRUE(transformation.getRotation().isNear(referenceQuaternion, 1e-8));
}

TEST(RosGeometryMsgTransformationEigen, convertToRosGeometryMsg) {
  const kindr::Position3D referenceTranslation(0.3, -1.1345, -0.6);
  const kindr::RotationQuaternionPD referenceQuaternion(0.212, 0.0421, -0.958, 0.1885);

  kindr::HomogeneousTransformationPosition3RotationQuaternionD transformation;
  transformation.getPosition() = referenceTranslation;
  transformation.getRotation() = referenceQuaternion;

  geometry_msgs::Transform geometryTransformMsg;
  kindr_ros::convertToRosGeometryMsg(transformation, geometryTransformMsg);

  EXPECT_NEAR(geometryTransformMsg.rotation.x, referenceQuaternion.x(), 1e-8);
  EXPECT_NEAR(geometryTransformMsg.rotation.y, referenceQuaternion.y(), 1e-8);
  EXPECT_NEAR(geometryTransformMsg.rotation.z, referenceQuaternion.z(), 1e-8);
  EXPECT_NEAR(geometryTransformMsg.rotation.w, referenceQuaternion.w(), 1e-8);
  EXPECT_NEAR(geometryTransformMsg.translation.x, referenceTranslation.x(), 1e-8);
  EXPECT_NEAR(geometryTransformMsg.translation.y, referenceTranslation.y(), 1e-8);
  EXPECT_NEAR(geometryTransformMsg.translation.z, referenceTranslation.z(), 1e-8);
}


#ifndef INC_FRAMETICKER_HPP__
#define INC_FRAMETICKER_HPP__

#include <opencv2/opencv.hpp>
#include <boost/circular_buffer.hpp>

class FrameTicker
{
	public :
		FrameTicker() :
			frameTicks_{5},
			start_(0),
			divider_(cv::getTickFrequency())
		{
		}

		// Save an elapsed time since last call
		// to mark, re-init start_ to right now
		void mark(void)
		{
			if (start_)
			{
				double end = cv::getTickCount();
				frameTicks_.push_back((end - start_) / divider_);
			}
			start_ = cv::getTickCount();
		}

		bool valid(void) const
		{
			return frameTicks_.full();
		}

		double getFPS(void) const
		{
			if (!valid())
				return -1;
			double sum = 0.0;
			for (auto it = frameTicks_.begin(); it != frameTicks_.end(); ++it)
				sum += *it;
			return frameTicks_.size() / sum;
		}
	private :
		boost::circular_buffer<double> frameTicks_;
		int64   start_;
		double  divider_;
};

#endif


#include <cstdlib>
#include <iostream>
#include <cassert>
#include <set>
#include <functional>
#include <boost/functional/hash.hpp>
#include <unordered_map>
#include "nested_list.h"
#include "consts.h"

using namespace std;

typedef mcfg::U U;
typedef mcfg::R R;
typedef mcfg::rvec_t rvec_t;

typedef std::initializer_list<U> rvec_lt;
typedef std::unordered_map<R,rvec_t> setR;
int debug=10;
bool verbose = true;
//#define get_MAX_INPUT_LEN 20 //uncomment to overwrite the one in consts.h

void verify(std::initializer_list<U> rv, setR& tracker, bool* flag=0)
{
  auto id = mcfg::index_bitvec<U,R,get_MAX_INPUT_LEN>(rv);
  rvec_t proper_rv;
  list2rvec(rv, proper_rv);
  if (verbose)
    cout << proper_rv << " => " << id << std::endl;
  if (tracker.find(id) != tracker.end()) {
    cout << proper_rv << " clashes with " << tracker[id];
    cout << ". Key=" << id << endl;
    if (flag)
      *flag=false;
    else
      assert(false);
  }
  else
    tracker.insert(make_pair(id, proper_rv));
}

//The structure of this method should mimick that of the actual inside algorithm in py-cky.h
//for this testing to be of use.
//Returns true if no clashes
bool verify(U n, int max_rangevector_dim) {
  setR track;
  bool flag=true;
  bool* fp = &flag;

  for (U i=0;i<n; ++i)
    verify({i,i+1}, track, fp);

  for (U gap = 2; gap <= n; ++gap) { // non-terminals. e.g. rule shape A -> B C
    for (U left = 0; left + gap <= n; ++left) {
      U right = left + gap;

      for (U c = left+1; c < right && max_rangevector_dim >= 2 ; ++c) { // c and d are indexing
        for (U d = c; d < right; ++d) {                                 // for parent_ranges_2

          for (U e = d+1; e < right && max_rangevector_dim >= 3; ++e) { // e and f are indexing
            for (U f = e; f < right; ++f) {                             // for parent_ranges_3

              for (U g = f+1; g < right && max_rangevector_dim >= 4; ++g) { // g and h are indexing
                for (U h = g; h < right; ++h) {                             // for parent_ranges_4

                  for (U i = h+1; i < right && max_rangevector_dim >= 5; ++i) { // i and j are indexing
                    for (U j = i; j < right; ++j) {                             // for parent_ranges_5
                      //Deal with cases where parent rangevec has 5 ranges
                      verify({left,c,d,e,f,g,h,i,j,right}, track, fp);
                    }
                  }
                  //Deal with cases where parent rangevec has 4 ranges
                  verify({left,c,d,e,f,g,h,right}, track, fp);
                }
              }
              //Deal with cases where parent rangevec has 3 ranges
              verify({left,c,d,e,f,right}, track, fp);
            }
          }

          //Deal with cases where parent rangevec has 2 ranges
          verify({left,c,d,right}, track, fp);
        }
      }
      //Deal with cases where parent rangevec has 1 range
      verify({left,right}, track, fp);
    }
  }
  return flag;
}

int main(int argc, const char *argv[])
{
  verbose= (argc>1);
  double bits = log(numeric_limits<R>::max())/log(2);
  cout << "Test for chart indexing and bit-space\n";
  cout << "=====================================\n";

  cout << "Chart is indexed with type R, which is " << bits << "-bit on this machine." << endl;
  cout << "At maximum input length ...., the implementation can handle up to .... ranges per rangevector.\n";

  for (int i=5; i<= get_MAX_INPUT_LEN; i+=5) {
    int blocks_avail =floor(bits / (ceil(log(i+1)/log(2))));
    cout << "\t\t" << i << "\t\t" << blocks_avail/2 << endl;
    //cout << "   at max input len " << i << " impl can handle up to " << blocks_avail/2 << " ranges in a rangevec\n";
  }
  cout << "\nTesting for key collisions...\n";
  for (int i=get_MAX_INPUT_LEN; i>0; i-=3)
  {
    cout << "n=" << i << "-----------\n";
    if (verify(i, get_MAX_RANGEVECTOR_DIM_IMPL))
      cout << "\tOK\n";
    else {
      cout << "\tBad.\n";
      //break;
    }
  }

  return 0;
}