/**
 * @file implementation/FovDistortion.hpp
 * @brief Header implementation file for the FovDistortion class.
 * @author Jianzhu Huai
 */

#include <Eigen/LU>
#include <iostream>

/// \brief okvis Main namespace of this package.
namespace okvis {
/// \brief cameras Namespace for camera-related functionality.
namespace cameras {

// The default constructor with all zero ki
FovDistortion::FovDistortion() : w_(0.0) { parameters_[0] = 0.0; }

// Constructor initialising ki
FovDistortion::FovDistortion(double w) {
  parameters_[0] = w;
  w_ = w;
}

bool FovDistortion::setParameters(const Eigen::VectorXd& parameters) {
  if (parameters.size() != NumDistortionIntrinsics) {
    return false;
  }
  parameters_ = parameters;
  w_ = parameters[0];
  return true;
}

bool FovDistortion::distort(const Eigen::Vector2d& pointUndistorted,
                            Eigen::Vector2d* pointDistorted) const {
  const double u = pointUndistorted(0);
  const double v = pointUndistorted(1);
  const double radius2 = u * u + v * v;
  const double radius = std::sqrt(radius2);
  const double tanwhalf = tan(w_ / 2.);
  const double atan_wrd = atan(2. * tanwhalf * radius);
  double omega2 = w_ * w_;
  double factor;
  const double eps = 1e-5;
  if (omega2 < eps) {
    // Limit w_ > 0.
    factor = omega2 / 12 - (omega2 * radius2) / 3 + 1;
  } else {
    if (radius2 < eps) {
      // Limit radius > 0.
      const double tanwhalfsq = tanwhalf * tanwhalf;
      factor = -(2 * tanwhalf * (4 * radius2 * tanwhalfsq - 3)) / (3 * w_);
    } else {
      factor = atan_wrd / (radius * w_);
    }
  }
  *pointDistorted = pointUndistorted * factor;
  return true;
}

bool FovDistortion::distort(const Eigen::Vector2d& pointUndistorted,
                            Eigen::Vector2d* pointDistorted,
                            Eigen::Matrix2d* pointJacobian,
                            Eigen::Matrix2Xd* parameterJacobian) const {
  const double u = pointUndistorted(0);
  const double v = pointUndistorted(1);

  const double radius2 = u * u + v * v;
  const double radius = std::sqrt(radius2);
  const double tanwhalf = tan(w_ / 2.);
  const double tanwhalfsq = tanwhalf * tanwhalf;
  const double atan_wrd = atan(2. * tanwhalf * radius);
  double omega2 = w_ * w_;
  double factor;
  const double eps = 1e-5;
  if (omega2 < eps) {
    // Limit w_ > 0.
    factor = omega2 / 12 - (omega2 * radius2) / 3 + 1;
  } else {
    if (radius2 < eps) {
      // Limit radius > 0.
      factor = -(2 * tanwhalf * (4 * radius2 * tanwhalfsq - 3)) / (3 * w_);
      // factor = 2. * tanwhalf / w_;
    } else {
      factor = atan_wrd / (radius * w_);
    }
  }
  *pointDistorted = pointUndistorted * factor;

  Eigen::Matrix2d& J = *pointJacobian;
  double dfactor_dradius;
  double dradius_du;
  double dradius_dv;
  double dfactor_du;
  double dfactor_dv;

  if (omega2 < eps) {
    dfactor_du = -(2 * omega2 * u) / 3;
    dfactor_dv = -(2 * omega2 * v) / 3;
    J << factor + u * dfactor_du, u * dfactor_dv, v * dfactor_du,
        factor + v * dfactor_dv;

  } else if (radius2 < eps) {
    dfactor_du = -(16 * u * tanwhalfsq * tanwhalf) / (3 * w_);
    dfactor_dv = -(16 * v * tanwhalfsq * tanwhalf) / (3 * w_);
    J << factor + u * dfactor_du, u * dfactor_dv, v * dfactor_du,
        factor + v * dfactor_dv;
  } else {
    dradius_du = u / radius;
    dradius_dv = v / radius;
    dfactor_dradius =
        (2 * tanwhalf) / (w_ * radius * (4 * radius2 * tanwhalfsq + 1)) -
        atan(2 * radius * tanwhalf) / (w_ * radius2);
    J << factor + u * dfactor_dradius * dradius_du,
        u * dfactor_dradius * dradius_dv, v * dfactor_dradius * dradius_du,
        factor + v * dfactor_dradius * dradius_dv;
  }

  if (parameterJacobian) {
    Eigen::Matrix2Xd& Ji = *parameterJacobian;
    Ji.resize(2, NumDistortionIntrinsics);
    double dfactor_domega;
    if (omega2 < eps) {
      dfactor_domega = -(w_ * (4 * radius2 - 1)) / 6;
    } else if (radius2 < eps) {
      dfactor_domega = (tanwhalfsq + 1) / w_ +
                       radius2 * ((8 * tanwhalfsq * tanwhalf) / (3 * omega2) -
                                  (4 * tanwhalfsq * (tanwhalfsq + 1)) / w_) -
                       (2 * tanwhalf) / omega2;
    } else {
      dfactor_domega =
          (tanwhalfsq + 1) / (w_ * (4 * radius2 * tanwhalfsq + 1)) -
          atan(2 * radius * tanwhalf) / (omega2 * radius);
    }
    Ji << u * dfactor_domega, v * dfactor_domega;
  }
  return true;
}

bool FovDistortion::distortWithExternalParameters(
    const Eigen::Vector2d& /*pointUndistorted*/, const Eigen::VectorXd& /*parameters*/,
    Eigen::Vector2d* /*pointDistorted*/, Eigen::Matrix2d* /*pointJacobian*/,
    Eigen::Matrix2Xd* /*parameterJacobian*/) const {
  throw std::runtime_error("FOV distortWithExternalParameters not implemented!");
  return false;
}
bool FovDistortion::undistort(const Eigen::Vector2d& pointDistorted,
                              Eigen::Vector2d* pointUndistorted) const {
  const Eigen::Vector2d& y = pointDistorted;

  double mul2tanwby2 = tan(w_ / 2.0) * 2.0;

  // Calculate distance from point to center.
  double r_d = y.norm();

  if (mul2tanwby2 == 0 || r_d == 0) {
    return false;
  }

  // Calculate undistorted radius of point.
  double r_u;
  if (fabs(r_d * w_) <= kMaxValidAngle) {
    r_u = tan(r_d * w_) / (r_d * mul2tanwby2);
  } else {
    return false;
  }

  *pointUndistorted = pointDistorted * r_u;

  return true;
}

bool FovDistortion::undistort(const Eigen::Vector2d& /*pointDistorted*/,
                              Eigen::Vector2d* /*pointUndistorted*/,
                              Eigen::Matrix2d* /*pointJacobian*/) const {
  throw std::runtime_error("FOV undistort with pointJacobian not implemented!");
  return false;
}

}  // namespace cameras
}  // namespace okvis


/**
 * @brief Defines functions for sumproduct algorithm
 * @file sumproduct.hpp
 * @author Vikas Dhiman
 */
#pragma once
#include "cartesian_product.hpp"
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/construct.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/range/any_range.hpp>
#include <boost/iterator/iterator_categories.hpp>
#include <boost/random.hpp>
#include <boost/graph/random.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/foreach.hpp>
#include <sstream>
#include "utility.hpp"
#include "sumproduct_traits.hpp"

namespace occgrid {

/**
 * \brief normalizes values in `map` over InputIterator to sum up to one
 */
// TODO: just use a transform iterator over a map to just accept a Iterator
template<typename InputIterator, typename PropertyMap>
void normalize(InputIterator first, InputIterator last, PropertyMap& map) {
  typedef typename boost::property_traits<PropertyMap>::value_type value_type;
  value_type normalizer(0);
  for (;first != last; ++first)
    normalizer += map[*first];

  for (;first != last; ++first) {
    map[*first] /= normalizer;
    assert(! isnan(map[*first]));
  }
}

namespace detail {
  /**
   * @brief Computes belief message over variable to factor edge.
   *
   * @type FactorGraph: A adjacency list graph.
   * @type MessageValues: A property map denoting messages on graph edges per
   * variable state.
   * @type SampleSpaceMap: Map prodviding sample space of each variable in the
   * graph node.
   * @type FactorMap: Map providing factor function for each factor node.
   *
   * @param e: the edge
   * @param fg: the graph
   * @param msgs: Messages over directed edges for each state of connected
   * variables
   * @param cdmap: Sample space map.
   * @param fmap: Factor map
   * @param xv: The fixed state of source variable node
   * @param v2f_edge_tag: For tag dispatch
   */
  template<typename FactorGraph, typename MessageValues,
    typename SampleSpaceMap,
    typename FactorMap>
  typename MessageValues::value_type
    belief(
      typename boost::graph_traits<FactorGraph>::edge_descriptor e,
      const FactorGraph &fg, MessageValues &msgs,
      SampleSpaceMap &cdmap, FactorMap& fmap,
      const typename SampleSpaceMap::value_type::first_type::value_type &xv,
      v2f_edge_tag) {
    // Short alias
    typedef typename boost::graph_traits<FactorGraph>::vertex_descriptor vertex_descriptor;
    // Deduce real type
    typedef typename MessageValues::value_type Real;

    vertex_descriptor var = source(e, fg), factor = target(e, fg);
    // \prod_{h \in n(var) \ factor} msgs_{h -> var(var = xv)}
    Real prod (1);
    BOOST_FOREACH(vertex_descriptor h, adjacent_vertices(var, fg)) {
      if (h != factor)
        prod *= msgs[typename MessageValues::key_type(h, var, xv)];
    }
    return prod;
  }

  /**
   * @brief Compute a single term inside the summary function.
   *
   * f(X) \prod_{y in n(factor) except var} msgs[y, factor, (y = X_y)]
   *
   * where X is the assignment
   *
   * @type FactorGraph: A adjacency list graph.
   * @type MessageValues: A property map denoting messages on graph edges per
   * variable state.
   * graph node.
   * @type FactorMap: Map providing factor function for each factor node.
   *
   * @param g: the graph
   * @param msgs: Messages over directed edges for each state of connected
   * variables
   * @param fmap: Factor map
   * @param e: the edge
   */
  template<typename FactorGraph,
    typename MessageValues,
    typename FactorMap>
  struct belief_from_var_assignment
  : public std::unary_function<
    typename FactorMap::value_type::argument_type,
    typename MessageValues::value_type >
  {
    typedef typename boost::graph_traits<FactorGraph>::vertex_descriptor vertex_descriptor;
    typedef typename FactorMap::value_type::argument_type argument_type;
    typedef typename MessageValues::value_type result_type;
    belief_from_var_assignment(const FactorGraph& g,
        const MessageValues& msgs,
        const FactorMap& fmap,
        const typename boost::graph_traits<FactorGraph>::edge_descriptor e)
      : msgs_(msgs), e_(e), factor_(source(e, g)), var_(target(e, g)),
      func_(get(fmap, factor_)), nbrs_(adjacent_vertices(factor_, g)) {

      }
    result_type operator()(argument_type assign) {
      result_type prod = func_(assign);
      BOOST_FOREACH(vertex_descriptor nbr, nbrs_) {
        if (nbr != var_)
          prod *= msgs_[typename MessageValues::key_type(nbr, factor_, assign[nbr])];
      }
      return prod;
    }
    private:
    const MessageValues& msgs_;
    const typename boost::graph_traits<FactorGraph>::edge_descriptor e_;
    const typename boost::graph_traits<FactorGraph>::vertex_descriptor factor_, var_;
    const typename boost::property_traits<FactorMap>::value_type func_;
    typedef typename boost::graph_traits<FactorGraph>::adjacency_iterator adjacency_iterator;
    std::pair<adjacency_iterator, adjacency_iterator> nbrs_;
  };

  /**
   * @brief Computes belief message over factor to variable edge.
   *
   * @type FactorGraph: A adjacency list graph.
   * @type MessageValues: A property map denoting messages on graph edges per
   * variable state.
   * @type SampleSpaceMap: Map prodviding sample space of each variable in the
   * graph node.
   * @type FactorMap: Map providing factor function for each factor node.
   *
   * @param e: the edge
   * @param fg: the graph
   * @param msgs: Messages over directed edges for each state of connected
   * variables
   * @param cdmap: Sample space map.
   * @param fmap: Factor map
   * @param xv: The fixed state of source variable node
   * @param v2f_edge_tag: For tag dispatch
   */
  template<typename FactorGraph,
    typename MessageValues,
    typename SampleSpaceMap,
    typename FactorMap>
  typename MessageValues::value_type
  belief(
      typename boost::graph_traits<FactorGraph>::edge_descriptor e,
      const FactorGraph &fg,
      MessageValues &msgs,
      SampleSpaceMap &cdmap,
      FactorMap& fmap,
      //const typename Assignment::value_type &xv
      const typename SampleSpaceMap::value_type::first_type::value_type &xv,
      f2v_edge_tag
      ) {
    typedef typename boost::graph_traits<FactorGraph>::vertex_descriptor vertex_descriptor;
    typedef typename MessageValues::value_type Real;
    typedef typename boost::graph_traits<FactorGraph>::adjacency_iterator adjacency_iterator;
    typedef typename SampleSpaceMap::value_type::first_type sample_space_iterator;
    typedef typename FactorMap::value_type::argument_type const_assignment_ref_type;
    typedef typename boost::remove_reference<const_assignment_ref_type>::type const_assignment_type;
    typedef typename boost::remove_const<const_assignment_type>::type assignment_type;
    namespace bl = boost::lambda;
    using boost::source;
    using boost::target;
    using boost::adjacent_vertices;
    vertex_descriptor factor = source(e, fg), var = target(e, fg);
    BOOST_AUTO_TPL(nbrs, adjacent_vertices(factor, fg));
    // Functor to compute belief from a given assignment of neigboring
    // variables.
    // f(X) \prod_{y in n(factor) except var} msgs[y, factor, (y = X_y)]
    const belief_from_var_assignment<FactorGraph, MessageValues, FactorMap> bfv(fg, msgs,
        fmap, e);
    BOOST_AUTO_TPL(func, get(fmap, factor));
    // Marginalize bfv for var = xv over all nbrs
    Real sum = summaryOf<Real, adjacency_iterator, SampleSpaceMap,
         assignment_type>( bfv, nbrs.first, nbrs.second, cdmap, var, xv);
    return sum;
  }
} // namespace detail

/// Dispatch belief depending on edge type
template<typename FactorGraph, typename MessageValues,
  typename SampleSpaceMap,
  typename FactorMap,
  typename IsFactorMap>
typename MessageValues::value_type
belief(
    typename boost::graph_traits<FactorGraph>::edge_descriptor e,
    const FactorGraph &fg,
    MessageValues &msgs,
    SampleSpaceMap &cdmap,
    FactorMap& fmap,
    IsFactorMap& is_factor,
    const typename SampleSpaceMap::value_type::first_type::value_type &xv)
{
  typedef typename boost::graph_traits<FactorGraph>::vertex_descriptor vertex_descriptor;
  vertex_descriptor u, v;
  using boost::incident;
  boost::tie(u, v) = incident(e, fg);
  if (get(is_factor, u) && (! get(is_factor, v)))
    return detail::belief<FactorGraph, MessageValues, SampleSpaceMap, FactorMap>(
        e, fg, msgs, cdmap, fmap, xv, f2v_edge_tag());
  else if (( ! get(is_factor, u) ) && get(is_factor, v))
    return detail::belief<FactorGraph, MessageValues, SampleSpaceMap, FactorMap>(
        e, fg, msgs, cdmap, fmap, xv, v2f_edge_tag());
  else {
    std::stringstream ss;
    ss << "Not a factor graph. Following edge is invalid:" << u << " - " << v;
    throw std::logic_error(ss.str());
  }
}

/// for each state in sample space compute belief message over edge e
/// normalize the belief messages
template<typename FactorGraph, typename MessageValues,
  typename SampleSpaceMap,
  typename FactorMap,
  typename IsFactorMap>
void foreach_sample_space(
    typename boost::graph_traits<FactorGraph>::edge_descriptor e,
    const FactorGraph &fg,
    MessageValues &msgs,
    SampleSpaceMap &cdmap,
    FactorMap& fmap,
    IsFactorMap& is_factor)
{
    typedef typename boost::graph_traits<FactorGraph>::vertex_descriptor vertex_descriptor;
    typedef typename MessageValues::value_type Real;
    typedef typename SampleSpaceMap::value_type::first_type sample_space_iterator;

    vertex_descriptor factor = source(e, fg), var = target(e, fg);
    sample_space_iterator cd, cd_end;
    boost::tie(cd, cd_end) = get(cdmap, var);
    for (boost::tie(cd, cd_end) = get(cdmap, var); cd != cd_end; ++cd) {
      Real sum = belief(e, fg, msgs, cdmap, fmap, is_factor, *cd);
      assert( ! isnan(sum));
      msgs[typename MessageValues::key_type(factor, var, *cd)] = sum;
    }

    // normalize
    boost::tie(cd, cd_end) = get(cdmap, var);

    typedef typename MessageValues::key_type msg_key_type;
    namespace bl = boost::lambda;
    BOOST_AUTO_TPL(ctmkt,
          bl::bind(bl::constructor<msg_key_type>(), factor, var, boost::lambda::_1));
    normalize(make_trans_it(cd, ctmkt),
              make_trans_it(cd_end, ctmkt),
              msgs);
}

/**
 * @brief Visitor class for sumproduct algorithm
 */
template<typename FactorGraph, typename MessageValues,
  typename SampleSpaceMap,
  typename FactorMap,
  typename IsFactorMap>
struct sumproduct_visitor : public boost::base_visitor<sumproduct_visitor<FactorGraph, MessageValues, SampleSpaceMap, FactorMap, IsFactorMap> > {
private:
  MessageValues &msgs_;
  SampleSpaceMap &cdmap_;
  FactorMap& fmap_;
  IsFactorMap& isfactor_;

public:
  typedef boost::on_examine_edge event_filter;
  sumproduct_visitor(MessageValues &msgs,
    SampleSpaceMap &cdmap, FactorMap& fmap,
    IsFactorMap& isfactor)
    : msgs_(msgs), cdmap_(cdmap), fmap_(fmap), isfactor_(isfactor) { }

  void operator()(typename boost::graph_traits<FactorGraph>::edge_descriptor e,
      const FactorGraph& fg) {
    //std::cout << "Called for edge" << source(e, fg) << "-" << target(e, fg) << std::endl;
    foreach_sample_space<FactorGraph, MessageValues, SampleSpaceMap, FactorMap, IsFactorMap>(e, fg, msgs_, cdmap_, fmap_, isfactor_);
  }
};

/// Compute marginal of the given node u for value xv
template <typename FactorGraph, typename MessageValues, typename SampleSpaceType>
typename boost::property_traits<MessageValues>::value_type
marginal(const FactorGraph& fg,
    const MessageValues& msgs,
    typename boost::graph_traits<FactorGraph>::vertex_descriptor u,
    SampleSpaceType xv
    ) {
  typedef typename boost::graph_traits<FactorGraph>::vertex_descriptor vertex_descriptor;
  typedef typename boost::property_traits<MessageValues>::value_type value_type;
  value_type prod(1);
  BOOST_FOREACH(vertex_descriptor v, adjacent_vertices(u, fg))
    prod *= msgs[typename MessageValues::key_type(u, v, xv)];
  return prod;
}

namespace detail {
  template <typename G, typename RandomNumGen>
  typename boost::graph_traits<G>::edge_descriptor
  random_edge(G& g, RandomNumGen& gen, boost::vertex_list_graph_tag, boost::incidence_graph_tag) {
    typedef typename boost::graph_traits<G>::vertex_descriptor vertex_descriptor;
    typedef typename boost::graph_traits<G>::degree_size_type degree_size_type;
    typedef typename boost::graph_traits<G>::out_edge_iterator out_edge_iterator;

    using boost::random_vertex;
    vertex_descriptor v = random_vertex(g, gen);
    BOOST_AUTO(od, out_degree(v, g));
    // keep trying unless we get a vertex with neigbours
    while (od <= 1) {
      v = random_vertex(g, gen);
      od = out_degree(v, g);
    }

    boost::uniform_int<> distrib(0,  od - 1);
    boost::variate_generator<RandomNumGen&, boost::uniform_int<> > rand_gen(gen, distrib);
    degree_size_type n = rand_gen();

    out_edge_iterator i = out_edges(v, g).first;
    return *(boost::next(i, n));
  }

  template <typename G, typename RandomNumGen>
  typename boost::graph_traits<G>::edge_descriptor
  random_edge(G& g, RandomNumGen& gen, boost::edge_list_graph_tag, boost::edge_list_graph_tag) {
    return boost::random_edge(g, gen);
  }
} // namespace detail

/// Random edge iterator over edge list graph or (vertex list and adjacency list graph)
template <typename G, typename RandomNumGen>
typename boost::graph_traits<G>::edge_descriptor
random_edge(G& g, RandomNumGen& gen) {
  typename boost::graph_traits<G>::traversal_category category;
  return detail::random_edge(g, gen, category, category);
}

/** @brief Random edge traversal with visitors */
template<typename FactorGraph, typename Visitors>
void random_edge_traversal(const FactorGraph& g, Visitors& visitor, std::size_t max_iter, double max_clock) {
  boost::mt19937 gen;
  //std::cout << "n:" << n << std::endl;
  clock_t st = clock();
  for (std::size_t i = 0; i < max_iter && clock() < (max_clock - st); ++i)
    invoke_visitors(visitor, occgrid::random_edge(g, gen), g, boost::on_examine_edge());
}

/** \brief Total edge traversal over a vertex list and adjacency list graph */
template<typename FactorGraph, typename Visitors>
void total_edge_traversal(const FactorGraph& g, Visitors& visitor, size_t sweeps = 2) {
  boost::mt19937 gen;
  //std::cout << "n:" << n << std::endl;
  typedef typename boost::graph_traits<FactorGraph>::edge_descriptor edge_descriptor;
  typedef typename boost::graph_traits<FactorGraph>::vertex_descriptor vertex_descriptor;
  for (size_t i = 0; i < sweeps; ++i) {
    BOOST_FOREACH(vertex_descriptor v, vertices(g)) {
      BOOST_FOREACH(edge_descriptor e, out_edges(v, g)) {
        invoke_visitors(visitor, e, g, boost::on_examine_edge());
      }
    }
  }
}

/** \brief Single i algoirthm traversal for sum product algorithm over trees */
template<typename FactorGraph, typename Visitors>
void single_i_algorithm_traversal(const FactorGraph& g, Visitors& visitor) {
  typedef typename boost::graph_traits<FactorGraph>::adjacency_iterator adjacency_iterator;
  typedef typename boost::graph_traits<FactorGraph>::vertex_iterator vertex_iterator;
  typedef typename boost::graph_traits<FactorGraph>::vertex_descriptor vertex_descriptor;
  typedef typename boost::graph_traits<FactorGraph>::degree_size_type degree_size_type;
  typedef typename boost::graph_traits<FactorGraph>::out_edge_iterator out_edge_iterator;
  typedef typename boost::graph_traits<FactorGraph>::edge_descriptor edge_descriptor;
  typedef typename std::pair<vertex_descriptor, vertex_descriptor> vertex_pair;
  typedef typename boost::unordered_map<vertex_pair, bool> MessageSentType;
  typedef typename MessageSentType::const_iterator vpair_iterator;
  vertex_iterator v, v_end;

  // The is the number of incoming edges from a vertex has NOT received any
  // message so far
  boost::unordered_map<vertex_descriptor, degree_size_type> message_degree;

  // On each "directed"* edge, the message will be sent only once. This map
  // keeps a track whether the message has been sent or not
  // *(that's why we use a pair of vertices instead of edge descriptor)
  MessageSentType message_sent;

  // Keeps a collection of vertices for next iteration
  std::vector<vertex_descriptor> degree_one_or_less_vertices;

  // Initialize degrees for each vertex
  for (boost::tie(v, v_end) = vertices(g); v != v_end; ++v) {
    degree_size_type d = out_degree(*v, g);
    message_degree[*v] = d;
    if (d == 1)
      degree_one_or_less_vertices.push_back(*v);
  }

  typedef typename std::vector<vertex_descriptor>::const_iterator d_one_iterator;
  //int count = 1;
  while (degree_one_or_less_vertices.size() > 0) {
    std::vector<vertex_descriptor> degree_one_or_less_vertices_new;
    //std::cout << "Step " << count++ << "+++++++++++++++++++++++++" << std::endl;
    for (d_one_iterator it(degree_one_or_less_vertices.begin());
        it != degree_one_or_less_vertices.end();
        ++it)
    {
      // When the message_degree of a vertex is 1, it means that we have
      // messages available from all sides but one.
      // - In that case we find the "incoming edge" from where this vertex
      //   did NOT receive a message  and send the message in that direction.
      //   There should be only one such edge.
      // - Once the message_degree of a vertex is 0, it means that the
      //   vertex has received messages from all sides and is free to send
      //   to message to all "outgoing edges" that have NOT been sent messages
      //   so far.
      out_edge_iterator e, e_end;
      std::vector<edge_descriptor> filtered_edges;
      for (boost::tie(e, e_end) = out_edges(*it, g); e != e_end; ++e) {
        vertex_descriptor t = target(*e, g);

        assert(message_degree[*it] == 0 || message_degree[*it] == 1);
        vpair_iterator vpair = (message_degree[*it] == 1)
          ? message_sent.find(std::make_pair(t, *it))  // incoming edge
          : message_sent.find(std::make_pair(*it, t)); // outgoing edge

        if (vpair == message_sent.end())
          filtered_edges.push_back(*e);
      }

      // Send messages along these edges and update the data structures
      for (typename std::vector<edge_descriptor>::const_iterator
          chosen_edge(filtered_edges.begin());
          chosen_edge != filtered_edges.end(); ++chosen_edge)
      {
          vertex_descriptor t = target(*chosen_edge, g);

          // if message available this edge is already processed
          // std::cout << "examining edge:" << *it << " -> " << t << std::endl;
          message_sent[incident(*chosen_edge, g)] = true; // outgoing edge

          // prepair for next iteration
          message_degree[t] -= 1;
          if (message_degree[t] == 0 || message_degree[t] == 1)
            degree_one_or_less_vertices_new.push_back(t);

          // Actually send message
          // visitor(*chosen_edge, g); // outgoing edge
          invoke_visitors(visitor, *chosen_edge, g, boost::on_examine_edge());
      }
    }
    swap(degree_one_or_less_vertices_new, degree_one_or_less_vertices);
  }
}

} // namespace occgrid


/*
 * polllayer.cpp
 *
 * This is polling layer
 *
 * MAX mode:
 *  Feed-forward:
 *    poll_result(l, i, j) = max(a(l-1, k, l) where k,l in (i, i + filter_size), (j, j + filter_size)
 *
 *  Back propagation:
 *    gradient(C, a(l)) = gradient(C, a(l+1)) for the position of max() for feedforward
 *
 * AVG mode:
 *  Feed-forward:
 *    poll_result(l, i, j) = avg(a(l-1, k, l) where k,l in (i, i + filter_size), (j, j + filter_size)
 *  Back propagation:
 *    gradient(C, a(l)) = gradient(C, a(l+1)) for each cell in submatrix
 *
 * ALL:
 *  Feed-forward:
 *    z(l) = w * poll_result(l) + b
 *    a(l) = activation(z(l))
 *
 * Back propagation (see FullyConnected layer):
 *    delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
 *    dC/db(l) = elem_sum(delta(l))
 *    dC/dw(l) = elem_sum(elem_prod(poll_result(a(l-1) * delta(l)))
 */
#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/random.h"
#include "core/functions.h"
#include "contlayer.h"
#include "polllayer.h"

using namespace std;
using namespace boost;
using namespace yann;

namespace yann {

////////////////////////////////////////////////////////////////////////////////////////////////
//
// PollingLayer_Context implementation
//
class PollingLayer_Context :
    public Layer::Context
{
  typedef Layer::Context Base;

  friend class PollingLayer;

public:
  PollingLayer_Context(const MatrixSize & output_size, const MatrixSize & batch_size) :
    Base(output_size, batch_size)
  {
    _poll_zz.resizeLike(get_output());
    _zz.resizeLike(get_output());
  }
  PollingLayer_Context(const RefVectorBatch & output) :
    Base(output)
  {
    _poll_zz.resizeLike(get_output());
    _zz.resizeLike(get_output());
  }

protected:
  VectorBatch _poll_zz;
  VectorBatch _zz;
}; // class PollingLayer_Context

////////////////////////////////////////////////////////////////////////////////////////////////
//
// PollingLayer_TrainingContext implementation
//
class PollingLayer_TrainingContext :
  public PollingLayer_Context
{
  typedef PollingLayer_Context Base;

  friend class PollingLayer;

public:
  PollingLayer_TrainingContext(
      const MatrixSize & output_size,
      const MatrixSize & batch_size,
      const unique_ptr<Layer::Updater> & updater) :
    Base(output_size, batch_size),
    _delta_ww(0),
    _delta_bb(0),
    _ww_updater(updater->copy()),
    _bb_updater(updater->copy())
  {
    _delta.resizeLike(_zz);
    _sigma_derivative_zz.resizeLike(_zz);

    _ww_updater->init(1, 1);
    _bb_updater->init(1, 1);
  }

  PollingLayer_TrainingContext(
     const RefVectorBatch & output,
     const unique_ptr<Layer::Updater> & updater) :
    Base(output),
    _delta_ww(0),
    _delta_bb(0),
    _ww_updater(updater->copy()),
    _bb_updater(updater->copy())
  {
    _delta.resizeLike(_zz);
    _sigma_derivative_zz.resizeLike(_zz);

    _ww_updater->init(1, 1);
    _bb_updater->init(1, 1);
  }

  // Layer::Context  overwrites
  virtual void start_epoch()
  {
    YANN_SLOW_CHECK(_ww_updater);
    YANN_SLOW_CHECK(_bb_updater);

    Base::start_epoch();

    _ww_updater->start_epoch();
    _bb_updater->start_epoch();
  }

  virtual void reset_state()
  {
    YANN_SLOW_CHECK(_ww_updater);
    YANN_SLOW_CHECK(_bb_updater);

    Base::reset_state();

    _delta_ww = 0;
    _delta_bb = 0;

    _ww_updater->reset();
    _bb_updater->reset();
  }

private:
  Value _delta_ww;
  Value _delta_bb;

  VectorBatch _delta;
  VectorBatch _sigma_derivative_zz;

  unique_ptr<Layer::Updater> _ww_updater;
  unique_ptr<Layer::Updater> _bb_updater;
}; // class PollingLayer_TrainingContext

}; // namespace yann

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::PollingLayer implementation
//
MatrixSize yann::PollingLayer::get_output_rows(const MatrixSize & input_rows, const MatrixSize & filter_size)
{
  return input_rows / filter_size;
}
MatrixSize yann::PollingLayer::get_output_cols(const MatrixSize & input_cols, const MatrixSize & filter_size)
{
  return input_cols / filter_size;
}
MatrixSize yann::PollingLayer::get_output_size(const MatrixSize & input_rows, const MatrixSize & input_cols,
                                  const MatrixSize & filter_size)
{
  return get_output_rows(input_rows, filter_size) * get_output_cols(input_cols, filter_size);
}

// Polling layer: each conv layer sends output to the corresponding poll layer
unique_ptr<ParallelLayer> yann::PollingLayer::create_poll_parallel_layer(
    const size_t & frames_num,
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const MatrixSize & filter_size,
    enum Mode mode,
    const std::unique_ptr<ActivationFunction> & activation_function)
{
  YANN_CHECK_GT(frames_num, 0);

  auto poll_container_layer = make_unique<ParallelLayer>(frames_num);
  YANN_CHECK(poll_container_layer);
  for(auto ii = frames_num; ii > 0; --ii) {
    auto poll_layer = make_unique<PollingLayer>(
        input_rows,
        input_cols,
        filter_size,
        mode);
    YANN_CHECK(poll_layer);
    if(activation_function) {
      poll_layer->set_activation_function(activation_function);
    }

    poll_container_layer->append_layer(std::move(poll_layer));
  }

  return poll_container_layer;
}

yann::PollingLayer::PollingLayer(const MatrixSize & input_rows,
                                 const MatrixSize & input_cols,
                                 const MatrixSize & filter_size,
                                 enum Mode mode) :
    _input_rows(input_rows),
    _input_cols(input_cols),
    _filter_size(filter_size),
    _mode(mode),
    _ww(0),
    _bb(0),
    _activation_function(new SigmoidFunction())
{
  YANN_CHECK_GE(input_rows, filter_size);
  YANN_CHECK_GE(input_cols, filter_size);
}

yann::PollingLayer::~PollingLayer()
{
}

void yann::PollingLayer::set_activation_function(
    const unique_ptr<ActivationFunction> & activation_function)
{
  YANN_CHECK(activation_function);
  _activation_function = activation_function->copy();
}

void yann::PollingLayer::set_values(
    const Value & ww, const Value & bb)
{
  _ww = ww;
  _bb = bb;
}

MatrixSize yann::PollingLayer::get_output_rows() const
{
  return get_output_rows(_input_rows, _filter_size);
}

MatrixSize yann::PollingLayer::get_output_cols() const
{
  return get_output_cols(_input_cols, _filter_size);
}

// Layer overwrites
std::string yann::PollingLayer::get_name() const
{
  return "PollingLayer";
}

string yann::PollingLayer::get_info() const
{
  ostringstream oss;

  oss << Base::get_info();

  // assume that all layers are the same, print only one
  oss << " activation: " << _activation_function->get_info()
      << ", input rows: " << _input_rows
      << ", input cols: " << _input_cols
      << ", filter: " << _filter_size
      << ", output rows: " << get_output_rows()
      << ", output cols: " << get_output_cols()
  ;
  switch(_mode) {
  case PollMode_Max:
    oss << ", mode: Max";
    break;
  case PollMode_Avg:
    oss << ", mode: Avg";
    break;
  }
  return oss.str();
}

bool yann::PollingLayer::is_valid() const
{
  if(!Base::is_valid()) {
    return false;
  }
  if(!_activation_function) {
    return false;
  }
  return true;
}

bool yann::PollingLayer::is_equal(const Layer & other, double tolerance) const
{
  if(!Base::is_equal(other, tolerance)) {
    return false;
  }
  auto the_other = dynamic_cast<const PollingLayer*>(&other);
  if(the_other == nullptr) {
    return false;
  }
  if(_input_rows != the_other->_input_rows) {
    return false;
  }
  if(_input_cols != the_other->_input_cols) {
    return false;
  }
  if(_filter_size != the_other->_filter_size) {
    return false;
  }
  if(_mode != the_other->_mode) {
    return false;
  }
  // TOOD: add deep compare
  if(_activation_function->get_info() != the_other->_activation_function->get_info()) {
    return false;
  }
  if(fabs(_ww - the_other->_ww) >= tolerance) {
    return false;
  }
  if(fabs(_bb - the_other->_bb) >= tolerance) {
    return false;
  }
  return true;
}

MatrixSize yann::PollingLayer::get_input_size() const
{
  return _input_rows * _input_cols;
}

MatrixSize yann::PollingLayer::get_output_size() const
{
  return get_output_size(_input_rows, _input_cols, _filter_size);
}

unique_ptr<Layer::Context> yann::PollingLayer::create_context(
    const MatrixSize & batch_size) const
{
  YANN_CHECK(is_valid());
  return make_unique<PollingLayer_Context>(get_output_size(), batch_size);
}
unique_ptr<Layer::Context> yann::PollingLayer::create_context(
    const RefVectorBatch & output) const
{
  YANN_CHECK(is_valid());
  return make_unique<PollingLayer_Context>(output);
}
unique_ptr<Layer::Context> yann::PollingLayer::create_training_context(
    const MatrixSize & batch_size,
    const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  return make_unique<PollingLayer_TrainingContext>(
      get_output_size(), batch_size, updater);
}
unique_ptr<Layer::Context> yann::PollingLayer::create_training_context(
    const RefVectorBatch & output,
    const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  return make_unique<PollingLayer_TrainingContext>(output, updater);
}

void yann::PollingLayer::poll(
    const RefConstMatrix & input,
    const MatrixSize & filter_size,
    enum Mode mode,
    RefMatrix output)
{
  YANN_CHECK_GT(filter_size, 0);
  YANN_CHECK_EQ(output.rows(), get_output_rows(input.rows(), filter_size));
  YANN_CHECK_EQ(output.cols(), get_output_cols(input.cols(), filter_size));

  // Rows/cols order should change if matrix layout is change from RowMajor to ColMajor for perf reasons
  for(MatrixSize in_ii = 0, out_ii = 0; out_ii < output.rows(); in_ii += filter_size, ++out_ii) {
    for(MatrixSize in_jj = 0, out_jj = 0; out_jj < output.cols(); in_jj += filter_size, ++out_jj) {
      switch(mode) {
        case PollMode_Max:
          output(out_ii, out_jj) = input.block(in_ii, in_jj, filter_size, filter_size).maxCoeff();
          break;
        case PollMode_Avg:
          output(out_ii, out_jj) = input.block(in_ii, in_jj, filter_size, filter_size).sum() / (filter_size * filter_size);
          break;
      }
    }
  }
}

void yann::PollingLayer::poll(
    const RefConstVectorBatch & input,
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const MatrixSize & filter_size,
    enum Mode mode,
    RefVectorBatch output)
{
  YANN_CHECK_GT(filter_size, 0);
  YANN_CHECK_LE(filter_size, input_rows);
  YANN_CHECK_LE(filter_size, input_cols);
  YANN_CHECK_GT(get_batch_size(input), 0);
  YANN_CHECK_EQ(get_batch_item_size(input), input_rows * input_cols);
  YANN_CHECK_EQ(get_batch_size(output), get_batch_size(input));

  const auto batch_size = get_batch_size(input);
  const auto output_rows = get_output_rows(input_rows, filter_size);
  const auto output_cols = get_output_cols(input_cols, filter_size);
  YANN_CHECK_EQ(get_batch_item_size(output), output_rows * output_cols);

  // ATTENTION: this code operates on raw Matrix.data() and might be broken
  // if Matrix.data() layout changes
  for(MatrixSize ii = 0; ii < batch_size; ++ii) {
    MapConstMatrix in(get_batch(input, ii).data(), input_rows, input_cols);
    MapMatrix out(get_batch(output, ii).data(), output_rows, output_cols);
    poll(in, filter_size, mode, out);
  }
}

//
// MAX mode:
//   gradient(C, a(l)) = gradient(C, a(l+1)) for the position of max() for feedforward
//
// AVG mode:
//   gradient(C, a(l)) = gradient(C, a(l+1)) for each cell in sub-matrix
void yann::PollingLayer::poll_gradient_backprop(
    const RefConstMatrix & gradient_output,
    const RefConstVectorBatch & input,
    const MatrixSize & filter_size,
    enum Mode mode,
    RefMatrix gradient_input)
{
  YANN_CHECK_EQ(gradient_output.rows(), get_output_rows(input.rows(), filter_size));
  YANN_CHECK_EQ(gradient_output.cols(), get_output_cols(input.cols(), filter_size));
  YANN_CHECK(is_same_size(gradient_input, input));

  // Rows/cols order should change if matrix layout is change from RowMajor to ColMajor for perf reasons
  MatrixSize ii, jj;
  for(MatrixSize in_ii = 0, out_ii = 0; out_ii < gradient_output.rows(); in_ii += filter_size, ++out_ii) {
    for(MatrixSize in_jj = 0, out_jj = 0; out_jj < gradient_output.cols(); in_jj += filter_size, ++out_jj) {
      Value val = gradient_output(out_ii, out_jj);
      switch(mode) {
        case PollMode_Max:
          input.block(in_ii, in_jj, filter_size, filter_size).maxCoeff(&ii, &jj);
          gradient_input(in_ii + ii, in_jj + jj) += val;
          break;
        case PollMode_Avg:
          gradient_input.block(in_ii, in_jj, filter_size, filter_size).array() += val;
          break;
      }
    }
  }
}

void yann::PollingLayer::poll_gradient_backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstVectorBatch & input,
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const MatrixSize & filter_size,
    enum Mode mode,
    RefVectorBatch gradient_input)
{
  YANN_CHECK(is_same_size(input, gradient_input));
  YANN_CHECK_EQ(get_batch_size(input), get_batch_size(gradient_output));

  const auto batch_size = get_batch_size(input);
  const auto output_rows = get_output_rows(input_rows, filter_size);
  const auto output_cols = get_output_cols(input_cols, filter_size);

  gradient_input.setZero();
  for(MatrixSize ii = 0; ii < batch_size; ++ii) {
    MapMatrix gradient_in(get_batch(gradient_input, ii).data(), input_rows, input_cols);
    MapConstMatrix in(get_batch(input, ii).data(), input_rows, input_cols);
    MapConstMatrix gradient_out(get_batch(gradient_output, ii).data(), output_rows, output_cols);
    poll_gradient_backprop(gradient_out, in, filter_size, mode, gradient_in);
  }
}

void yann::PollingLayer::feedforward(
    const RefConstVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  auto ctx = dynamic_cast<PollingLayer_Context *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  BOOST_VERIFY(is_same_size(ctx->_zz, ctx->get_output()));

  // z(l) = poll(a(l-1))*w(l) + b(l)
  poll(input, _input_rows, _input_cols, _filter_size, _mode,  ctx->_poll_zz);
  ctx->_zz.array() = _ww *  ctx->_poll_zz.array() + _bb;

  // a(l) = activation(z(l))
  _activation_function->f(ctx->_zz, ctx->get_output(), mode);
}

void yann::PollingLayer::feedforward(
    const RefConstSparseVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  throw runtime_error("PollingLayer::feedforward() is not implemented for sparse vectors");
}

void yann::PollingLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  auto ctx = dynamic_cast<PollingLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_GT(get_batch_size(gradient_output), 0);
  YANN_CHECK_EQ(get_batch_item_size(gradient_output), get_output_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());

  // just to make it easier to read
  const auto & poll_zz = ctx->_poll_zz;
  const auto & zz = ctx->_zz;
  auto & sigma_derivative_zz = ctx->_sigma_derivative_zz;
  auto & delta = ctx->_delta;
  auto & delta_ww = ctx->_delta_ww;
  auto & delta_bb = ctx->_delta_bb;

  // delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
  YANN_CHECK(is_same_size(zz, sigma_derivative_zz));
  YANN_CHECK(is_same_size(zz, gradient_output));
  YANN_CHECK(is_same_size(zz, delta));
  _activation_function->derivative(zz, sigma_derivative_zz);
  delta.array() = gradient_output.array() * sigma_derivative_zz.array();

  // update deltas
  // dC/dw(l) = elem_sum(elem_prod(poll_result(a(l-1)) * delta(l)))
  // dC/db(l) = elem_sum(delta(l))
  YANN_CHECK(is_same_size(poll_zz, delta));
  delta_ww += (poll_zz.array() * delta.array()).sum();
  delta_bb += delta.array().sum();

  // we don't need to calculate the gradient(C, a(l)) for the "first" layer (actual inputs)
  if(gradient_input) {
    poll_gradient_backprop(
        gradient_output, input, _input_rows, _input_cols,
        _filter_size, _mode, *gradient_input);
  }
}

void yann::PollingLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstSparseVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  throw runtime_error("PollingLayer::backprop() is not implemented for sparse vectors");
}

void yann::PollingLayer::init(enum InitMode mode, boost::optional<InitContext> init_context)
{
  switch (mode) {
  case InitMode_Zeros:
    _ww = 0;
    _bb = 0;
    break;
  case InitMode_Random:
    {
      unique_ptr<RandomGenerator> gen01 = RandomGenerator::normal_distribution(0, 1,
          init_context ? optional<Value>(init_context->seed()) : boost::none);
      gen01->generate(_ww);
      gen01->generate(_bb);
    }
    break;
  }
}

void yann::PollingLayer::update(Context * context, const size_t & tests_num)
{
  auto ctx = dynamic_cast<PollingLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(ctx->_ww_updater);
  YANN_CHECK(ctx->_bb_updater);

  ctx->_ww_updater->update(ctx->_delta_ww, tests_num, _ww);
  ctx->_bb_updater->update(ctx->_delta_bb, tests_num, _bb);
}

// the format is (w:<weight>,b:<bias>)
void yann::PollingLayer::read(std::istream & is)
{
  Base::read(is);

  read_char(is, '(');
  read_object(is, "w", _ww);
  read_char(is, ',');
  read_object(is, "b", _bb);
  read_char(is, ')');
}

// the format is (w:<weight>,b:<bias>)
void yann::PollingLayer::write(std::ostream & os) const
{
  Base::write(os);

  os << "(";
  write_object(os, "w", _ww);
  os << ",";
  write_object(os, "b", _bb);
  os << ")";
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/traversable/traverse_mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/applicative.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>
#include <boost/hana/detail/minimal/traversable.hpp>
using namespace boost::hana;

template <typename mcd>
void test() {
    constexpr auto traversable = detail::minimal::traversable<mcd>;
    constexpr auto applicative = detail::minimal::applicative<>;
    constexpr auto comparable = detail::minimal::comparable<>;

    BOOST_HANA_CONSTEXPR_ASSERT(
        sequence<detail::minimal::Applicative<>>(traversable(applicative(comparable(1))))
            ==
        applicative(traversable(comparable(1)))
    );
}

int main() {
    test<Traversable::traverse_mcd>();
}


#include <iostream>
#include <utility>
#include <algorithm>
#include <cstdlib>
#include <string>
#include <vector>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/graph_utility.hpp>

#include <boost/python.hpp>
#include <boost/python/numpy.hpp>

using namespace boost;
namespace py = boost::python;
namespace np = boost::python::numpy;

typedef adjacency_list<vecS, vecS, undirectedS> Graph;
typedef property_map<Graph, vertex_index_t>::type IndexMap;
typedef graph_traits<Graph>::vertex_iterator VertexIter;
typedef std::vector<int> IntVec;
typedef std::map<IntVec, int> VecIntMap;

enum class RemovalStrategy { random, targeted};

void print_vector(IntVec& vec_to_print) {
    for (IntVec::iterator it=vec_to_print.begin(); it!=vec_to_print.end(); ++it)
        std::cout << ' ' << *it;
    std::cout << std::endl;
}

void populate_index_labels(Graph& g, IntVec& index_labels) {
    for (auto v : boost::make_iterator_range(vertices(g))) {
       index_labels[v] = v;
    }
}

void generate_random_sequence(Graph& g, IntVec& removal_sequence, int N) {
    populate_index_labels(g, removal_sequence);
    std::random_shuffle (removal_sequence.begin(), removal_sequence.end());
}

void generate_targeted_sequence(Graph& g, const IntVec& node_degrees, IntVec& removal_sequence, int N) {
    populate_index_labels(g, removal_sequence);

    IntVec random_sequence(N);
    generate_random_sequence(g, random_sequence, N);

    std::sort(
        removal_sequence.begin(),
        removal_sequence.end(),
        [node_degrees, random_sequence](int a, int b) {
            return (node_degrees[a] > node_degrees[b]) || (node_degrees[a] == node_degrees[b] && random_sequence[a] > random_sequence[b]);
        }
    );
}

void generate_removal_sequence(RemovalStrategy strat, Graph& g, const IntVec& node_degrees, IntVec& removal_sequence, int N) {
    switch(strat) {
        case RemovalStrategy::random: return generate_random_sequence(g,removal_sequence, N);
        case RemovalStrategy::targeted: return generate_targeted_sequence(g, node_degrees, removal_sequence, N);
    }
}

bool should_cache_ncc(RemovalStrategy strat) {
    switch(strat) {
        case RemovalStrategy::random: return false;
        case RemovalStrategy::targeted: return true;
    }
}

double compute_critical_fraction(RemovalStrategy strat, Graph& g, const IntVec& node_degrees, VecIntMap& ncc_cache, int N, unsigned base_graph_hash, int sim_num, int random_seed) {
    int crit_removed = 0;
    std::srand (unsigned ( (base_graph_hash * sim_num * random_seed) ) );
    IntVec component(N);
    IntVec removal_sequence(N);
    generate_removal_sequence(strat, g, node_degrees, removal_sequence, N);

    for(int i=0; i<N-1; ++i) {
        int node_to_remove = removal_sequence[i];

        clear_vertex(node_to_remove, g);
        int nodes_removed = i+1;

        int num = -1;
        if(should_cache_ncc(strat)) {
            IntVec seq_so_far = IntVec(removal_sequence.begin(), removal_sequence.begin() + i + 1);

            auto search = ncc_cache.find(seq_so_far);
            if (search != ncc_cache.end()) {
                num = search->second;
            } else {
                num = connected_components(g, &component[0]) - nodes_removed;
                ncc_cache.insert(std::make_pair(seq_so_far, num));
            }
        }
        else {
            num = connected_components(g, &component[0]) - nodes_removed;
        }

        if (num > 1) {
            crit_removed = nodes_removed;
            break;
        }
        if (i == N-2) {
            crit_removed = N;
            break;
        }
    }

    double p = ((double) crit_removed) / ((double) N);
    return p;
}

double exp_critical_fraction(RemovalStrategy strat, int N, int M, np::ndarray const& edge_list, int num_sims, unsigned base_graph_hash, int random_seed) {
    int* input_ptr = reinterpret_cast<int*>(edge_list.get_data());

    Graph g;
    int first_node, second_node;
    int input_size = M * 2;

    for (int i = 0; i < input_size-1; i+=2) {
        first_node = *(input_ptr + i);
        second_node = *(input_ptr + i + 1);
        add_edge(first_node, second_node, g);
    }

    IntVec node_degrees(N);
    for (auto v : boost::make_iterator_range(vertices(g))) {
        node_degrees[v] = out_degree(v, g);
    }

    double cf_sum = 0;

    VecIntMap ncc_cache;
    for (int sim_num = 0; sim_num < num_sims; ++sim_num) {
        Graph g_copy;
        copy_graph(g, g_copy);
        double cf = compute_critical_fraction(strat, g_copy, node_degrees, ncc_cache, N, base_graph_hash, sim_num, random_seed);
        cf_sum += cf;
    }

    double exp_cf = cf_sum / ((double) num_sims);
    return exp_cf;
}

double critical_fraction_random(int N, int M, np::ndarray const& edge_list, int num_sims, unsigned base_graph_hash, int random_seed)
{
    return exp_critical_fraction(RemovalStrategy::random, N, M, edge_list, num_sims, base_graph_hash, random_seed);
}

double critical_fraction_targeted(int N, int M, np::ndarray const& edge_list, int num_sims, unsigned base_graph_hash, int random_seed)
{
    return exp_critical_fraction(RemovalStrategy::targeted, N, M, edge_list, num_sims, base_graph_hash, random_seed);
}

BOOST_PYTHON_MODULE(objective_functions_ext)
{
    Py_Initialize();
    np::initialize();

    def("critical_fraction_random", critical_fraction_random);
    def("critical_fraction_targeted", critical_fraction_targeted);
}


#ifndef FAST_GICP_FAST_GICP_MP_IMPL_HPP
#define FAST_GICP_FAST_GICP_MP_IMPL_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <pcl/registration/registration.h>

#include <sophus/so3.hpp>
#include <fast_gicp/so3/so3.hpp>
#include <fast_gicp/opt/gauss_newton.hpp>
#include <fast_gicp/gicp/experimental/fast_gicp_mp.hpp>
#include <atomic>

namespace fast_gicp {

template<typename PointSource, typename PointTarget>
FastGICPMultiPoints<PointSource, PointTarget>::FastGICPMultiPoints() {
#ifdef _OPENMP
  num_threads_ = omp_get_max_threads();
#else
  num_threads_ = 1;
#endif

  k_correspondences_ = 20;
  reg_name_ = "FastGICPMultiPoints";
  max_iterations_ = 64;
  rotation_epsilon_ = 1e-5;
  transformation_epsilon_ = 1e-5;
  // corr_dist_threshold_ = 1.0;
  regularization_method_ = PLANE;
  corr_dist_threshold_ = std::numeric_limits<float>::max();
  neighbor_search_radius_ = 0.5;
}

template<typename PointSource, typename PointTarget>
FastGICPMultiPoints<PointSource, PointTarget>::~FastGICPMultiPoints() {}

template<typename PointSource, typename PointTarget>
void FastGICPMultiPoints<PointSource, PointTarget>::setRotationEpsilon(double eps) {
  rotation_epsilon_ = eps;
}

template<typename PointSource, typename PointTarget>
void FastGICPMultiPoints<PointSource, PointTarget>::setNumThreads(int n) {
  num_threads_ = n;

#ifdef _OPENMP
  if(n == 0) {
    num_threads_ = omp_get_max_threads();
  }
#endif
}

template<typename PointSource, typename PointTarget>
void FastGICPMultiPoints<PointSource, PointTarget>::setCorrespondenceRandomness(int k) {
  k_correspondences_ = k;
}

template<typename PointSource, typename PointTarget>
void FastGICPMultiPoints<PointSource, PointTarget>::setRegularizationMethod(RegularizationMethod method) {
  regularization_method_ = method;
}

template<typename PointSource, typename PointTarget>
void FastGICPMultiPoints<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
  pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
  calculate_covariances(cloud, source_kdtree, source_covs);
}

template<typename PointSource, typename PointTarget>
void FastGICPMultiPoints<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
  pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
  calculate_covariances(cloud, target_kdtree, target_covs);
}

template<typename PointSource, typename PointTarget>
void FastGICPMultiPoints<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
  Eigen::Matrix<float, 6, 1> x0;
  x0.head<3>() = Sophus::SO3f(guess.template block<3, 3>(0, 0)).log();
  x0.tail<3>() = guess.template block<3, 1>(0, 3);

  // if(x0.head<3>().norm() < 1e-2) {
  //   x0.head<3>() = (Eigen::Vector3f::Random()).normalized() * 1e-2;
  // }

  converged_ = false;
  GaussNewton<double, 6> solver;

  for(int i = 0; i < max_iterations_; i++) {
    nr_iterations_ = i;

    update_correspondences(x0);
    Eigen::MatrixXf J;
    Eigen::VectorXf loss = loss_ls(x0, &J);

    Eigen::Matrix<float, 6, 1> delta = solver.delta(loss.cast<double>(), J.cast<double>()).cast<float>();

    x0.head<3>() = (Sophus::SO3f::exp(-delta.head<3>()) * Sophus::SO3f::exp(x0.head<3>())).log();
    x0.tail<3>() -= delta.tail<3>();

    if(is_converged(delta)) {
      converged_ = true;
      break;
    }
  }

  final_transformation_.setIdentity();
  final_transformation_.template block<3, 3>(0, 0) = Sophus::SO3f::exp(x0.head<3>()).matrix();
  final_transformation_.template block<3, 1>(0, 3) = x0.tail<3>();

  pcl::transformPointCloud(*input_, output, final_transformation_);
}

template<typename PointSource, typename PointTarget>
bool FastGICPMultiPoints<PointSource, PointTarget>::is_converged(const Eigen::Matrix<float, 6, 1>& delta) const {
  double accum = 0.0;
  Eigen::Matrix3f R = Sophus::SO3f::exp(delta.head<3>()).matrix() - Eigen::Matrix3f::Identity();
  Eigen::Vector3f t = delta.tail<3>();

  Eigen::Matrix3f r_delta = 1.0 / rotation_epsilon_ * R.array().abs();
  Eigen::Vector3f t_delta = 1.0 / transformation_epsilon_ * t.array().abs();

  return std::max(r_delta.maxCoeff(), t_delta.maxCoeff()) < 1;
}

template<typename PointSource, typename PointTarget>
void FastGICPMultiPoints<PointSource, PointTarget>::update_correspondences(const Eigen::Matrix<float, 6, 1>& x) {
  Eigen::Matrix4f trans = Eigen::Matrix4f::Identity();
  trans.block<3, 3>(0, 0) = Sophus::SO3f::exp(x.head<3>()).matrix();
  trans.block<3, 1>(0, 3) = x.tail<3>();

  correspondences.resize(input_->size());
  sq_distances.resize(input_->size());

#pragma omp parallel for num_threads(num_threads_)
  for(int i = 0; i < input_->size(); i++) {
    PointTarget pt;
    pt.getVector4fMap() = trans * input_->at(i).getVector4fMap();

    std::vector<int> k_indices;
    std::vector<float> k_sq_dists;
    target_kdtree.radiusSearch(pt, neighbor_search_radius_, k_indices, k_sq_dists);

    if(k_indices.empty()) {
    //  target_kdtree.nearestKSearch(pt, 1, k_indices, k_sq_dists);
    }

    correspondences[i] = k_indices;
    sq_distances[i] = k_sq_dists;
  }
}

template<typename PointSource, typename PointTarget>
Eigen::VectorXf FastGICPMultiPoints<PointSource, PointTarget>::loss_ls(const Eigen::Matrix<float, 6, 1>& x, Eigen::MatrixXf* J) const {
  Eigen::Matrix4f trans = Eigen::Matrix4f::Identity();
  trans.block<3, 3>(0, 0) = Sophus::SO3f::exp(x.head<3>()).matrix();
  trans.block<3, 1>(0, 3) = x.tail<3>();

  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> losses(input_->size());
  // use row-major arrangement for ease of repacking
  std::vector<Eigen::Matrix<float, 3, 6, Eigen::RowMajor>, Eigen::aligned_allocator<Eigen::Matrix<float, 3, 6, Eigen::RowMajor>>> Js(input_->size());

  std::atomic_int count(0);

#pragma omp parallel for num_threads(num_threads_)
  for(int i = 0; i < correspondences.size(); i++) {
    int source_index = i;
    const auto& mean_A = input_->at(source_index).getVector4fMap();
    const auto& cov_A = source_covs[source_index];

    const auto& k_indices = correspondences[i];
    const auto& k_sq_dists = sq_distances[i];
    if(k_indices.empty()) {
      continue;
    }

    double sum_w = 0.0;
    Eigen::Vector4d sum_mean_B = Eigen::Vector4d::Zero();
    Eigen::Matrix4d sum_cov_B = Eigen::Matrix4d::Zero();
    for(int j = 0; j < k_indices.size(); j++) {
      double w = 1 - std::sqrt(k_sq_dists[j]) / neighbor_search_radius_;
      w = std::max(1e-3, std::min(1.0, w));
      sum_w += w;

      int target_index = k_indices[j];

      sum_mean_B += w * target_->at(target_index).getVector4fMap().template cast<double>();
      sum_cov_B += w * target_covs[target_index].template cast<double>();
    }

    Eigen::Vector4f mean_B = (sum_mean_B / sum_w).template cast<float>();
    Eigen::Matrix4f cov_B = (sum_cov_B / sum_w).template cast<float>();

    Eigen::Vector4f transed_mean_A = trans * mean_A;
    Eigen::Vector4f d = mean_B - transed_mean_A;
    Eigen::Matrix4f RCR = cov_B + trans * cov_A * trans.transpose();
    RCR(3, 3) = 1;

    Eigen::Matrix4f RCR_inv = RCR.inverse();
    Eigen::Vector4f RCRd = RCR_inv * d;

    Eigen::Matrix<float, 4, 6> dtdx0 = Eigen::Matrix<float, 4, 6>::Zero();
    dtdx0.block<3, 3>(0, 0) = skew(transed_mean_A.head<3>());
    dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3f::Identity();

    Eigen::Matrix<float, 4, 6> jlossexp = RCR_inv * dtdx0;

    int n = count++;
    losses[n] = RCRd.head<3>();
    Js[n] = jlossexp.block<3, 6>(0, 0);
  }

  int final_size = count;
  Eigen::VectorXf loss = Eigen::Map<Eigen::VectorXf>(losses.front().data(), final_size * 3);
  *J = Eigen::Map<Eigen::MatrixXf>(Js.front().data(), 6, final_size * 3).transpose();

  return loss;
}

template<typename PointSource, typename PointTarget>
template<typename PointT>
bool FastGICPMultiPoints<PointSource, PointTarget>::calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances) {
  kdtree.setInputCloud(cloud);
  covariances.resize(cloud->size());

#pragma omp parallel for num_threads(num_threads_)
  for(int i = 0; i < cloud->size(); i++) {
    std::vector<int> k_indices;
    std::vector<float> k_sq_distances;
    kdtree.nearestKSearch(cloud->at(i), k_correspondences_, k_indices, k_sq_distances);

    Eigen::Matrix<float, 4, -1> data(4, k_correspondences_);

    for(int j = 0; j < k_indices.size(); j++) {
      data.col(j) = cloud->at(k_indices[j]).getVector4fMap();
    }

    data.colwise() -= data.rowwise().mean().eval();
    Eigen::Matrix4f cov = data * data.transpose();

    if(regularization_method_ == FROBENIUS) {
      double lambda = 1e-3;
      Eigen::Matrix3d C = cov.block<3, 3>(0, 0).cast<double>() + lambda * Eigen::Matrix3d::Identity();
      Eigen::Matrix3d C_inv = C.inverse();
      covariances[i].setZero();
      covariances[i].template block<3, 3>(0, 0) = (C_inv / C_inv.norm()).inverse().cast<float>();
    } else {
      Eigen::JacobiSVD<Eigen::Matrix3f> svd(cov.block<3, 3>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV);
      Eigen::Vector3f values;

      switch(regularization_method_) {
        default:
          std::cerr << "here must not be reached" << std::endl;
          abort();
        case PLANE:
          values = Eigen::Vector3f(1, 1, 1e-3);
          break;
        case MIN_EIG:
          values = svd.singularValues().array().max(1e-3);
          break;
        case NORMALIZED_MIN_EIG:
          values = svd.singularValues() / svd.singularValues().maxCoeff();
          values = values.array().max(1e-3);
          break;
      }

      covariances[i].setZero();
      covariances[i].template block<3, 3>(0, 0) = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose();
    }
  }

  return true;
}

}  // namespace fast_gicp

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// 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 CRYPTO3_BLOCK_CIPHERS_DETAIL_SHACAL_FUNCTIONS_HPP
#define CRYPTO3_BLOCK_CIPHERS_DETAIL_SHACAL_FUNCTIONS_HPP

#include <boost/crypto3/detail/basic_functions.hpp>

namespace boost {
    namespace crypto3 {
        namespace block {
            namespace detail {

                /*!
                 * @brief
                 * @tparam WordBits
                 *
                 * Implemented directly from the standard as found at
                 * http://csrc.nist.gov/publications/fips/fips180-2/fips180-2.pdf
                 * Specifically, subsection 4.1
                 */
                template<std::size_t WordBits>
                struct basic_shacal_functions : public ::boost::crypto3::detail::basic_functions<WordBits> {
                    typedef typename ::boost::crypto3::detail::basic_functions<WordBits>::word_type word_type;

                    static word_type Ch(word_type x, word_type y, word_type z) {
                        return (x & y) ^ (~x & z);
                    }

                    static word_type Maj(word_type x, word_type y, word_type z) {
                        return (x & y) ^ (x & z) ^ (y & z);
                    }
                };

                struct shacal_functions : public basic_shacal_functions<32> {
                    static word_type Parity(word_type x, word_type y, word_type z) {
                        return x ^ y ^ z;
                    }

                    static word_type f(unsigned t, word_type x, word_type y, word_type z) {
                        if (t < 40) {
                            if (t < 20) {
                                return Ch(x, y, z);
                            }
                        } else {
                            if (t < 60) {
                                return Maj(x, y, z);
                            }
                        }
                        return Parity(x, y, z);
                    }
                };

                typedef shacal_functions shacal0_functions;
                typedef shacal_functions shacal1_functions;

                template<std::size_t WordBits>
                struct shacal2_functions;

                template<>
                struct shacal2_functions<32> : public basic_shacal_functions<32> {
                    static word_type Sigma_0(word_type x) {
                        return rotr<2>(x) ^ rotr<13>(x) ^ rotr<22>(x);
                    }

                    static word_type Sigma_1(word_type x) {
                        return rotr<6>(x) ^ rotr<11>(x) ^ rotr<25>(x);
                    }

                    static word_type sigma_0(word_type x) {
                        return rotr<7>(x) ^ rotr<18>(x) ^ shr<3>(x);
                    }

                    static word_type sigma_1(word_type x) {
                        return rotr<17>(x) ^ rotr<19>(x) ^ shr<10>(x);
                    }
                };

                template<>
                struct shacal2_functions<64> : public basic_shacal_functions<64> {
                    static word_type Sigma_0(word_type x) {
                        return rotr<28>(x) ^ rotr<34>(x) ^ rotr<39>(x);
                    }

                    static word_type Sigma_1(word_type x) {
                        return rotr<14>(x) ^ rotr<18>(x) ^ rotr<41>(x);
                    }

                    static word_type sigma_0(word_type x) {
                        return rotr<1>(x) ^ rotr<8>(x) ^ shr<7>(x);
                    }

                    static word_type sigma_1(word_type x) {
                        return rotr<19>(x) ^ rotr<61>(x) ^ shr<6>(x);
                    }
                };

            }    // namespace detail
        }        // namespace block
    }            // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_BLOCK_CIPHERS_DETAIL_SHACAL_FUNCTIONS_HPP


#pragma once

#include <atomic>
#include <bitset>
#include <functional>
#include <numeric>
#include <stdexcept>
#include <vector>

#include <dtl/dtl.hpp>
#include <dtl/bits.hpp>
#include <dtl/math.hpp>
#include <dtl/simd.hpp>
#include <dtl/filter/blocked_bloomfilter/vector_helper.hpp>

#include "immintrin.h"

#include <boost/integer/static_min_max.hpp>

namespace dtl {

// Specialization for k = 8 with manual unrolling to improve IPC.
template<
    typename key_t,               // the key type
    typename word_t,              // the word type
    u32 s,                        // the numbers of sectors (must be a power of two)

    template<typename Ty, u32 i> class hasher,      // the hash function family to use
    typename hash_value_t,        // the hash value type

    u32 hash_fn_idx,              // current hash function index (used for recursion)
    u32 remaining_hash_bit_cnt    // the number of remaining hash bits (used for recursion)
>
struct word_block<key_t, word_t, s, 8, hasher, hash_value_t, hash_fn_idx, remaining_hash_bit_cnt, 8> {

  //===----------------------------------------------------------------------===//
  // Static part
  //===----------------------------------------------------------------------===//
  static constexpr u32 k = 8; // Specialization for k = 8
  static constexpr u32 remaining_k_cnt = k;

  static_assert(dtl::is_power_of_two(s), "Parameter 's' must be a power of two.");

  static constexpr u32 word_bitlength = sizeof(word_t) * 8;
  static constexpr u32 word_bitlength_mask = word_bitlength - 1;

  static constexpr u32 hash_value_bitlength = sizeof(hash_value_t) * 8;

  static constexpr u32 sector_bitlength = word_bitlength / s;
  static constexpr u32 sector_bitlength_log2 = dtl::ct::log_2_u32<sector_bitlength>::value;
  static constexpr word_t sector_mask() { return static_cast<word_t>(sector_bitlength) - 1; }

  static_assert(sector_bitlength >= 8, "A sector must be at least one byte in size.");

  static constexpr u32 hash_bit_cnt_per_k = sector_bitlength_log2;
  static constexpr u32 k_cnt_per_hash_value = ((sizeof(hash_value_t) * 8) / hash_bit_cnt_per_k) ; // consider -1 to respect hash fn weakness in the low order bits
  static constexpr u32 k_cnt_per_sector = k / s;

  static constexpr u32 current_k = k - remaining_k_cnt;

  static constexpr u1 rehash = remaining_hash_bit_cnt < hash_bit_cnt_per_k;
  static constexpr u32 remaining_hash_bit_cnt_after_rehash = rehash ? hash_value_bitlength : remaining_hash_bit_cnt;
  //===----------------------------------------------------------------------===//

  __forceinline__ __unroll_loops__ __host__ __device__
  static void
  which_bits(const key_t key, hash_value_t& hash_val, word_t& word) noexcept {

    // First k. - Set one bit in the given word; rehash if necessary
    static constexpr u32 hash_fn_idx_0 = hash_fn_idx;
    static constexpr u1 rehash_0 = remaining_hash_bit_cnt < hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_after_rehash_0 = rehash_0 ? hash_value_bitlength : remaining_hash_bit_cnt;
    const hash_value_t hash_val_0 = rehash_0 ? hasher<key_t, hash_fn_idx_0>::hash(key) : hash_val;
    constexpr u32 sector_idx_0 = /*current_k = */ 0  / k_cnt_per_sector;
    constexpr u32 shift_0 = remaining_hash_bit_cnt_after_rehash_0 - hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_0 = shift_0;

    // Second k. - Set one bit in the given word; rehash if necessary
    static constexpr u32 hash_fn_idx_1 = rehash_0 ? hash_fn_idx_0 + 1 : hash_fn_idx_0;
    static constexpr u1 rehash_1 = remaining_hash_bit_cnt_0 < hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_after_rehash_1 = rehash_1 ? hash_value_bitlength : remaining_hash_bit_cnt_0;
    const hash_value_t hash_val_1 = rehash_1 ? hasher<key_t, hash_fn_idx_1>::hash(key) : hash_val_0;
    constexpr u32 sector_idx_1 = /*current_k = */ 1  / k_cnt_per_sector;
    constexpr u32 shift_1 = remaining_hash_bit_cnt_after_rehash_1 - hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_1 = shift_1;

    // Third k. - Set one bit in the given word; rehash if necessary
    static constexpr u32 hash_fn_idx_2 = rehash_1 ? hash_fn_idx_1 + 1 : hash_fn_idx_1;
    static constexpr u1 rehash_2 = remaining_hash_bit_cnt_1 < hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_after_rehash_2 = rehash_2 ? hash_value_bitlength : remaining_hash_bit_cnt_1;
    const hash_value_t hash_val_2 = rehash_2 ? hasher<key_t, hash_fn_idx_2>::hash(key) : hash_val_1;
    constexpr u32 sector_idx_2 = /*current_k = */ 2  / k_cnt_per_sector;
    constexpr u32 shift_2 = remaining_hash_bit_cnt_after_rehash_2 - hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_2 = shift_2;

    // Forth k. - Set one bit in the given word; rehash if necessary
    static constexpr u32 hash_fn_idx_3 = rehash_2 ? hash_fn_idx_2 + 1 : hash_fn_idx_2;
    static constexpr u1 rehash_3 = remaining_hash_bit_cnt_2 < hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_after_rehash_3 = rehash_3 ? hash_value_bitlength : remaining_hash_bit_cnt_2;
    const hash_value_t hash_val_3 = rehash_3 ? hasher<key_t, hash_fn_idx_3>::hash(key) : hash_val_2;
    constexpr u32 sector_idx_3 = /*current_k = */ 3  / k_cnt_per_sector;
    constexpr u32 shift_3 = remaining_hash_bit_cnt_after_rehash_3 - hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_3 = shift_3;

    // Fifth k. - Set one bit in the given word; rehash if necessary
    static constexpr u32 hash_fn_idx_4 = rehash_3 ? hash_fn_idx_3 + 1 : hash_fn_idx_3;
    static constexpr u1 rehash_4 = remaining_hash_bit_cnt_3 < hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_after_rehash_4 = rehash_4 ? hash_value_bitlength : remaining_hash_bit_cnt_3;
    const hash_value_t hash_val_4 = rehash_4 ? hasher<key_t, hash_fn_idx_4>::hash(key) : hash_val_3;
    constexpr u32 sector_idx_4 = /*current_k = */ 4  / k_cnt_per_sector;
    constexpr u32 shift_4 = remaining_hash_bit_cnt_after_rehash_4 - hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_4 = shift_4;

    // Sixth k. - Set one bit in the given word; rehash if necessary
    static constexpr u32 hash_fn_idx_5 = rehash_4 ? hash_fn_idx_4 + 1 : hash_fn_idx_4;
    static constexpr u1 rehash_5 = remaining_hash_bit_cnt_4 < hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_after_rehash_5 = rehash_5 ? hash_value_bitlength : remaining_hash_bit_cnt_4;
    const hash_value_t hash_val_5 = rehash_5 ? hasher<key_t, hash_fn_idx_5>::hash(key) : hash_val_4;
    constexpr u32 sector_idx_5 = /*current_k = */ 5  / k_cnt_per_sector;
    constexpr u32 shift_5 = remaining_hash_bit_cnt_after_rehash_5 - hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_5 = shift_5;

    // Seventh k. - Set one bit in the given word; rehash if necessary
    static constexpr u32 hash_fn_idx_6 = rehash_5 ? hash_fn_idx_5 + 1 : hash_fn_idx_5;
    static constexpr u1 rehash_6 = remaining_hash_bit_cnt_5 < hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_after_rehash_6 = rehash_6 ? hash_value_bitlength : remaining_hash_bit_cnt_5;
    const hash_value_t hash_val_6 = rehash_6 ? hasher<key_t, hash_fn_idx_6>::hash(key) : hash_val_5;
    constexpr u32 sector_idx_6 = /*current_k = */ 6  / k_cnt_per_sector;
    constexpr u32 shift_6 = remaining_hash_bit_cnt_after_rehash_6 - hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_6 = shift_6;

    // Eighth k. - Set one bit in the given word; rehash if necessary
    static constexpr u32 hash_fn_idx_7 = rehash_6 ? hash_fn_idx_6 + 1 : hash_fn_idx_6;
    static constexpr u1 rehash_7 = remaining_hash_bit_cnt_6 < hash_bit_cnt_per_k;
    static constexpr u32 remaining_hash_bit_cnt_after_rehash_7 = rehash_7 ? hash_value_bitlength : remaining_hash_bit_cnt_6;
    const hash_value_t hash_val_7 = rehash_7 ? hasher<key_t, hash_fn_idx_7>::hash(key) : hash_val_6;
    constexpr u32 sector_idx_7 = /*current_k = */ 7  / k_cnt_per_sector;
    constexpr u32 shift_7 = remaining_hash_bit_cnt_after_rehash_7 - hash_bit_cnt_per_k;

    u32 bit_idx_0 = ((hash_val_0 >> shift_0) & sector_mask()) + (sector_idx_0 * sector_bitlength);
    const word_t word_0 = word_t(1) << bit_idx_0;
    u32 bit_idx_1 = ((hash_val_1 >> shift_1) & sector_mask()) + (sector_idx_1 * sector_bitlength);
    const word_t word_1 = word_t(1) << bit_idx_1;
    u32 bit_idx_2 = ((hash_val_2 >> shift_2) & sector_mask()) + (sector_idx_2 * sector_bitlength);
    const word_t word_2 = word_t(1) << bit_idx_2;
    u32 bit_idx_3 = ((hash_val_3 >> shift_3) & sector_mask()) + (sector_idx_3 * sector_bitlength);
    const word_t word_3 = word_t(1) << bit_idx_3;
    u32 bit_idx_4 = ((hash_val_4 >> shift_4) & sector_mask()) + (sector_idx_4 * sector_bitlength);
    const word_t word_4 = word_t(1) << bit_idx_4;
    u32 bit_idx_5 = ((hash_val_5 >> shift_5) & sector_mask()) + (sector_idx_5 * sector_bitlength);
    const word_t word_5 = word_t(1) << bit_idx_5;
    u32 bit_idx_6 = ((hash_val_6 >> shift_6) & sector_mask()) + (sector_idx_6 * sector_bitlength);
    const word_t word_6 = word_t(1) << bit_idx_6;
    u32 bit_idx_7 = ((hash_val_7 >> shift_7) & sector_mask()) + (sector_idx_7 * sector_bitlength);
    const word_t word_7 = word_t(1) << bit_idx_7;

    // Reduce
    const word_t word_01 = word_0 | word_1;
    const word_t word_23 = word_2 | word_3;
    const word_t word_45 = word_4 | word_5;
    const word_t word_67 = word_6 | word_7;
    const word_t word_0123 = word_01 | word_23;
    const word_t word_4567 = word_45 | word_67;
    word |= word_0123 | word_4567;
  }
  //===----------------------------------------------------------------------===//

  template<u64 n>
  __forceinline__ __unroll_loops__ __host__ __device__
  static void
  which_bits(const vec<key_t, n>& keys,
             vec<hash_value_t, n>& hash_vals,
             vec<word_t, n>& words) noexcept {

    // Typedef vector types
    using key_vt = vec<key_t, n>;
    using hash_value_vt = vec<hash_value_t, n>;
    using word_vt = vec<word_t, n>;

    hash_vals = rehash ? hasher<key_vt, hash_fn_idx>::hash(keys) : hash_vals;
    const hash_value_t remaining_hash_bit_cnt_after_rehash = rehash ? hash_value_bitlength : remaining_hash_bit_cnt;

    // Set one bit in the given word; rehash if necessary
    constexpr u32 sector_idx = current_k / k_cnt_per_sector;
    constexpr u32 shift = remaining_hash_bit_cnt_after_rehash - hash_bit_cnt_per_k;
    hash_value_vt bit_idxs = sector_idx == 0 ? ((hash_vals >> shift) & sector_mask())
                                             : ((hash_vals >> shift) & sector_mask()) + (sector_idx * sector_bitlength);
    words |= word_vt(1) << internal::vector_convert<hash_value_t, word_t, n>::convert(bit_idxs);

    // Recurse
    word_block<key_t, word_t, s, k,
        hasher, hash_value_t,
        (rehash ? hash_fn_idx + 1 : hash_fn_idx), // increment the hash function index
        remaining_hash_bit_cnt_after_rehash >= hash_bit_cnt_per_k ? remaining_hash_bit_cnt_after_rehash - hash_bit_cnt_per_k : 0, // the number of remaining hash bits
        remaining_k_cnt - 1> // decrement the remaining k counter
    ::which_bits(keys, hash_vals, words);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // The number of required hash functions.
  //===----------------------------------------------------------------------===//
  static constexpr u32 hash_fn_idx_end =
      word_block<key_t, word_t, s, k,
          hasher, hash_value_t,
          (rehash ? hash_fn_idx + 1 : hash_fn_idx), // increment the hash function index
          remaining_hash_bit_cnt_after_rehash >= hash_bit_cnt_per_k ? remaining_hash_bit_cnt_after_rehash - hash_bit_cnt_per_k : 0, // the number of remaining hash bits
          remaining_k_cnt - 1> // decrement the remaining k counter
      ::hash_fn_idx_end;
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // The number of required hash bits.
  //===----------------------------------------------------------------------===//
  static constexpr u32 remaining_hash_bits =
      word_block<key_t, word_t, s, k,
          hasher, hash_value_t,
          (rehash ? hash_fn_idx + 1 : hash_fn_idx), // increment the hash function index
          remaining_hash_bit_cnt_after_rehash >= hash_bit_cnt_per_k ? remaining_hash_bit_cnt_after_rehash - hash_bit_cnt_per_k : 0, // the number of remaining hash bits
          remaining_k_cnt - 1> // decrement the remaining k counter
      ::remaining_hash_bits;
  //===----------------------------------------------------------------------===//

};

} // namespace dtl


///////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2021.                        //
//  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 DECWIDE_T_2021_02_24_HPP_
  #define DECWIDE_T_2021_02_24_HPP_

  #include <boost/math/bindings/detail/big_lanczos.hpp>
  #include <boost/math/policies/policy.hpp>
  #include <boost/version.hpp>

  #include <math/wide_decimal/decwide_t.h>

  namespace boost { namespace math {

  namespace policies {

  // Specialization of the precision structure.
  template<const std::int32_t MyDigits10,
           typename LimbType,
           typename AllocatorType,
           typename InternalFloatType,
           typename ExponentType,
           typename ThisPolicy>
  struct precision<::math::wide_decimal::decwide_t<MyDigits10, LimbType, AllocatorType, InternalFloatType, ExponentType>, ThisPolicy>
  {
    using local_wide_decimal_type =
      ::math::wide_decimal::decwide_t<MyDigits10, LimbType, AllocatorType, InternalFloatType, ExponentType>;

    using precision_type = typename ThisPolicy::precision_type;

    using local_digits_2 = digits2<(((long long) std::numeric_limits<local_wide_decimal_type>::digits10 + 1LL) * 1000LL) / 301LL>;

    using type = typename std::conditional<((local_digits_2::value <= precision_type::value) || (precision_type::value <= 0)),
                                            local_digits_2,
                                            precision_type>::type;
  };

  } // namespace boost::math::policies

  namespace constants { namespace detail {

  template <class T>
  struct constant_pi;

  template<const std::int32_t MyDigits10,
           typename LimbType,
           typename AllocatorType,
           typename InternalFloatType,
           typename ExponentType>
  struct constant_pi<::math::wide_decimal::decwide_t<MyDigits10, LimbType, AllocatorType, InternalFloatType, ExponentType>>
  {
  public:
    using result_type =
      ::math::wide_decimal::decwide_t<MyDigits10, LimbType, AllocatorType, InternalFloatType, ExponentType>;

  private:
    static result_type my_compute()
    {
      // Adapted from Boost.Math.Constants (see file calculate_constants.hpp).
      using std::ldexp;
      using std::sqrt;

      result_type result;
      result_type a = 1;
      result_type b;
      result_type A(a);
      result_type B = 0.5F;
      result_type D = 0.25F;

      result_type lim = std::numeric_limits<result_type>::epsilon();

      unsigned k = 1;

      do
      {
        result = A + B;
        result = ldexp(result, -2);
        b = sqrt(B);
        a += b;
        a = ldexp(a, -1);
        A = a * a;
        B = A - result;
        B = ldexp(B, 1);
        result = A - B;
        bool neg = boost::math::sign(result) < 0;
        if(neg)
           result = -result;
        if(result <= lim)
           break;
        if(neg)
           result = -result;
        result = ldexp(result, k - 1);
        D -= result;
        ++k;
        lim = ldexp(lim, 1);
      }
      while(true);

      result = B / D;
      return result;
    }

  public:
    #if (BOOST_VERSION <= 107200)
    template<int N> static const result_type& get(const mpl::int_<N>&)
    #elif (BOOST_VERSION <= 107500)
    template<int N> static const result_type& get(const boost::integral_constant<int, N>&)
    #else
    template<int N> static const result_type& get(const std::integral_constant<int, N>&)
    #endif // BOOST_VERSION
    {
      static result_type result;
      static bool        has_init = false;

      if(has_init == false)
      {
        has_init = true;

        result = my_compute();
      }

      return result;
    }

    #if (BOOST_VERSION <= 107200)
    static inline const result_type get(const mpl::int_<0>&)
    #elif (BOOST_VERSION <= 107500)
    static inline const result_type get(const boost::integral_constant<int, 0>&)
    #else
    static inline const result_type get(const std::integral_constant<int, 0>&)
    #endif // BOOST_VERSION
    {
      return my_compute();
    }
  };

  } } // namespace boost::math::constants::detail

  #if (BOOST_VERSION <= 107600)
  namespace lanczos {

  template <typename T, typename Policy>
  struct lanczos;

  template<const std::int32_t MyDigits10,
           typename LimbType,
           typename AllocatorType,
           typename InternalFloatType,
           typename ExponentType,
           typename FftFloatType,
           typename Policy>
  struct lanczos<::math::wide_decimal::decwide_t<MyDigits10,
                                                 LimbType,
                                                 AllocatorType,
                                                 InternalFloatType,
                                                 ExponentType,
                                                 FftFloatType>,
                 Policy>
  {
    using local_backend_type =
      ::math::wide_decimal::decwide_t<MyDigits10,
                                      LimbType,
                                      AllocatorType,
                                      InternalFloatType,
                                      ExponentType,
                                      FftFloatType>;

    using precision_type =
      typename boost::math::policies::precision<local_backend_type, Policy>::type;

    using type =
      typename std::conditional<precision_type::value && (precision_type::value <= 73),
                                lanczos13UDT,
                                typename std::conditional<precision_type::value && (precision_type::value <= 122),
                                                          lanczos22UDT,
                                                          undefined_lanczos>::type>::type;
  };

  } // namespace boost::math::lanczos
  #endif // BOOST_VERSION

  } } // namespace boost::math

#endif // DECWIDE_T_2021_02_24_HPP_


/***
* Copyright 2017 Marc Stevens <marc@marc-stevens.nl>, Dan Shumow <danshu@microsoft.com>
* Distributed under the MIT Software License.
* See accompanying file LICENSE.txt or copy at
* https://opensource.org/licenses/MIT
***/

#include <iostream>
#include <iomanip>
#include <vector>

#include <boost/cstdint.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random.hpp>
#include <boost/progress.hpp>
#include <boost/timer.hpp>
#include <boost/array.hpp>

#include "ubc_check_test.h"
#include "test_simd.h"

#include "test_simd.cpp"

#include "../../lib/simd_sse128.h"

#ifdef INCLUDE_SSE128_TEST
int test_ubc_check_sse128()
{
	return test_ubc_check_simd<SIMD_WORD, ubc_check_sse128>("_sse128");
}
#endif


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2007.

//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_STATS_FIND_LOCATION_HPP
#define BOOST_STATS_FIND_LOCATION_HPP

#include <boost/math/distributions/fwd.hpp> // for all distribution signatures.
#include <boost/math/distributions/complement.hpp>
#include <boost/math/policies/policy.hpp>
#include <boost/math/tools/traits.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/policies/error_handling.hpp>
// using boost::math::policies::policy;
// using boost::math::complement; // will be needed by users who want complement,
// but NOT placed here to avoid putting it in global scope.

namespace boost
{
  namespace math
  {
  // Function to find location of random variable z
  // to give probability p (given scale)
  // Applies to normal, lognormal, extreme value, Cauchy, (and symmetrical triangular),
  // enforced by static_assert below.

    template <class Dist, class Policy>
    inline
      typename Dist::value_type find_location( // For example, normal mean.
      typename Dist::value_type z, // location of random variable z to give probability, P(X > z) == p.
      // For example, a nominal minimum acceptable z, so that p * 100 % are > z
      typename Dist::value_type p, // probability value desired at x, say 0.95 for 95% > z.
      typename Dist::value_type scale, // scale parameter, for example, normal standard deviation.
      const Policy& pol
      )
    {
      static_assert(::boost::math::tools::is_distribution<Dist>::value, "The provided distribution does not meet the conceptual requirements of a distribution.");
      static_assert(::boost::math::tools::is_scaled_distribution<Dist>::value, "The provided distribution does not meet the conceptual requirements of a scaled distribution.");
      static const char* function = "boost::math::find_location<Dist, Policy>&, %1%)";

      if(!(boost::math::isfinite)(p) || (p < 0) || (p > 1))
      {
       return policies::raise_domain_error<typename Dist::value_type>(
           function, "Probability parameter was %1%, but must be >= 0 and <= 1!", p, pol);
      }
      if(!(boost::math::isfinite)(z))
      {
       return policies::raise_domain_error<typename Dist::value_type>(
           function, "z parameter was %1%, but must be finite!", z, pol);
      }
      if(!(boost::math::isfinite)(scale))
      {
       return policies::raise_domain_error<typename Dist::value_type>(
           function, "scale parameter was %1%, but must be finite!", scale, pol);
      }

      //cout << "z " << z << ", p " << p << ",  quantile(Dist(), p) "
      //  << quantile(Dist(), p) << ", quan * scale " << quantile(Dist(), p) * scale << endl;
      return z - (quantile(Dist(), p) * scale);
    } // find_location

    template <class Dist>
    inline // with default policy.
      typename Dist::value_type find_location( // For example, normal mean.
      typename Dist::value_type z, // location of random variable z to give probability, P(X > z) == p.
      // For example, a nominal minimum acceptable z, so that p * 100 % are > z
      typename Dist::value_type p, // probability value desired at x, say 0.95 for 95% > z.
      typename Dist::value_type scale) // scale parameter, for example, normal standard deviation.
    { // Forward to find_location with default policy.
       return (find_location<Dist>(z, p, scale, policies::policy<>()));
    } // find_location

    // So the user can start from the complement q = (1 - p) of the probability p,
    // for example, l = find_location<normal>(complement(z, q, sd));

    template <class Dist, class Real1, class Real2, class Real3>
    inline typename Dist::value_type find_location( // Default policy.
      complemented3_type<Real1, Real2, Real3> const& c)
    {
      static const char* function = "boost::math::find_location<Dist, Policy>&, %1%)";

      typename Dist::value_type p = c.param1;
      if(!(boost::math::isfinite)(p) || (p < 0) || (p > 1))
      {
       return policies::raise_domain_error<typename Dist::value_type>(
           function, "Probability parameter was %1%, but must be >= 0 and <= 1!", p, policies::policy<>());
      }
      typename Dist::value_type z = c.dist;
      if(!(boost::math::isfinite)(z))
      {
       return policies::raise_domain_error<typename Dist::value_type>(
           function, "z parameter was %1%, but must be finite!", z, policies::policy<>());
      }
      typename Dist::value_type scale = c.param2;
      if(!(boost::math::isfinite)(scale))
      {
       return policies::raise_domain_error<typename Dist::value_type>(
           function, "scale parameter was %1%, but must be finite!", scale, policies::policy<>());
      }
       // cout << "z " << c.dist << ", quantile (Dist(), " << c.param1 << ") * scale " << c.param2 << endl;
       return z - quantile(Dist(), p) * scale;
    } // find_location complement

    template <class Dist, class Real1, class Real2, class Real3, class Real4>
    inline typename Dist::value_type find_location( // Explicit policy.
      complemented4_type<Real1, Real2, Real3, Real4> const& c)
    {
      static const char* function = "boost::math::find_location<Dist, Policy>&, %1%)";

      typename Dist::value_type p = c.param1;
      if(!(boost::math::isfinite)(p) || (p < 0) || (p > 1))
      {
       return policies::raise_domain_error<typename Dist::value_type>(
           function, "Probability parameter was %1%, but must be >= 0 and <= 1!", p, c.param3);
      }
      typename Dist::value_type z = c.dist;
      if(!(boost::math::isfinite)(z))
      {
       return policies::raise_domain_error<typename Dist::value_type>(
           function, "z parameter was %1%, but must be finite!", z, c.param3);
      }
      typename Dist::value_type scale = c.param2;
      if(!(boost::math::isfinite)(scale))
      {
       return policies::raise_domain_error<typename Dist::value_type>(
           function, "scale parameter was %1%, but must be finite!", scale, c.param3);
      }
       // cout << "z " << c.dist << ", quantile (Dist(), " << c.param1 << ") * scale " << c.param2 << endl;
       return z - quantile(Dist(), p) * scale;
    } // find_location complement

  } // namespace boost
} // namespace math

#endif // BOOST_STATS_FIND_LOCATION_HPP


#include <gtest/gtest.h>
#include "test_complex_helper.hpp"
#include "pitts_fixed_tensor3_combine.hpp"
#include <Eigen/Dense>
#include <complex>

template<typename T>
class PITTS_FixedTensor3_combine : public ::testing::Test
{
  public:
    using Type = T;
};

using TestTypes = ::testing::Types<double, std::complex<double>>;
TYPED_TEST_CASE(PITTS_FixedTensor3_combine, TestTypes);

TYPED_TEST(PITTS_FixedTensor3_combine, N_equals_one)
{
  using Type = TestFixture::Type;
  using FixedTensor3 = PITTS::FixedTensor3<Type,1>;
  constexpr auto eps = 1.e-10;

  FixedTensor3 t3a(5,3);
  for(int i = 0; i < 5; i++)
    for(int j = 0; j < 3; j++)
      t3a(i,0,j) = 100 + i*10 + j;

  FixedTensor3 t3b(3,7);
  for(int i = 0; i < 3; i++)
    for(int j = 0; j < 7; j++)
      t3b(i,0,j) = 10 + i*0.5 + j*0.337;

  using Matrix = Eigen::Matrix<Type, Eigen::Dynamic, Eigen::Dynamic>;
  using Map = Eigen::Map<Matrix>;
  auto mapT3a = Map(&t3a(0,0,0), 5, 3);
  auto mapT3b = Map(&t3b(0,0,0), 3, 7);

  Matrix refResult = mapT3a * mapT3b;

  const auto t3c = combine(t3a, t3b, false);
  const auto t3c_swapped = combine(t3a, t3b, true);
  ASSERT_EQ(5,t3c.r1());
  ASSERT_EQ(1,t3c.n());
  ASSERT_EQ(7,t3c.r2());

  for(int i = 0; i < 5; i++)
    for(int j = 0; j < 7; j++)
    {
      EXPECT_NEAR(refResult(i,j), t3c(i,0,j), eps);
      // no difference due to transposing the second dimension
      EXPECT_NEAR(refResult(i,j), t3c_swapped(i,0,j), eps);
    }
}

TYPED_TEST(PITTS_FixedTensor3_combine, N_equals_2)
{
  using Type = TestFixture::Type;
  using FixedTensor3 = PITTS::FixedTensor3<Type,2>;
  constexpr auto eps = 1.e-10;

  FixedTensor3 t3a(5,3);
  for(int i = 0; i < 5; i++)
    for(int j = 0; j < 3; j++)
      for(int k = 0; k < 2; k++)
        t3a(i,k,j) = 100 + i*10 + j - k * 0.77777;

  FixedTensor3 t3b(3,7);
  for(int i = 0; i < 3; i++)
    for(int j = 0; j < 7; j++)
      for(int k = 0; k < 2; k++)
        t3b(i,k,j) = 10 + i*0.5 + j*0.337 + k * 0.027;

  using Matrix = Eigen::Matrix<Type, Eigen::Dynamic, Eigen::Dynamic>;
  using Map = Eigen::Map<Matrix>;
  auto mapT3a = Map(&t3a(0,0,0), 10, 3);
  auto mapT3b = Map(&t3b(0,0,0), 3, 14);

  Matrix refResult = mapT3a * mapT3b;

  const auto t3c = combine(t3a, t3b);
  ASSERT_EQ(5,t3c.r1());
  ASSERT_EQ(4,t3c.n());
  ASSERT_EQ(7,t3c.r2());

  for(int i = 0; i < 5; i++)
    for(int j = 0; j < 7; j++)
    {
      EXPECT_NEAR(refResult(i+0,2*j+0), t3c(i,0,j), eps);
      EXPECT_NEAR(refResult(i+5,2*j+0), t3c(i,1,j), eps);
      EXPECT_NEAR(refResult(i+0,2*j+1), t3c(i,2,j), eps);
      EXPECT_NEAR(refResult(i+5,2*j+1), t3c(i,3,j), eps);
    }
}

TYPED_TEST(PITTS_FixedTensor3_combine, N_equals_2_swapped)
{
  using Type = TestFixture::Type;
  using FixedTensor3 = PITTS::FixedTensor3<Type,2>;
  constexpr auto eps = 1.e-8;

  FixedTensor3 t3a(5,3);
  for(int i = 0; i < 5; i++)
    for(int j = 0; j < 3; j++)
      for(int k = 0; k < 2; k++)
        t3a(i,k,j) = 100 + i*10 + j - k * 0.77777;

  FixedTensor3 t3b(3,7);
  for(int i = 0; i < 3; i++)
    for(int j = 0; j < 7; j++)
      for(int k = 0; k < 2; k++)
        t3b(i,k,j) = 10 + i*0.5 + j*0.337 + k * 0.027;

  using Matrix = Eigen::Matrix<Type, Eigen::Dynamic, Eigen::Dynamic>;
  using Map = Eigen::Map<Matrix>;
  auto mapT3a = Map(&t3a(0,0,0), 10, 3);
  auto mapT3b = Map(&t3b(0,0,0), 3, 14);

  Matrix refResult = mapT3a * mapT3b;

  const auto t3c = combine(t3a, t3b, true);
  ASSERT_EQ(5,t3c.r1());
  ASSERT_EQ(4,t3c.n());
  ASSERT_EQ(7,t3c.r2());

  for(int i = 0; i < 5; i++)
    for(int j = 0; j < 7; j++)
    {
      EXPECT_NEAR(refResult(i+0,2*j+0), t3c(i,0,j), eps);
      EXPECT_NEAR(refResult(i+5,2*j+0), t3c(i,2,j), eps);
      EXPECT_NEAR(refResult(i+0,2*j+1), t3c(i,1,j), eps);
      EXPECT_NEAR(refResult(i+5,2*j+1), t3c(i,3,j), eps);
    }
}


//
// Copyright (c) 2019 INRIA
//

#include "pinocchio/math/tensor.hpp"
#include "pinocchio/multibody/model.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <iostream>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_emulate_tensors)
{
  typedef double Scalar;
  typedef pinocchio::Tensor<Scalar,3> Tensor;

  const Eigen::DenseIndex x_dim = 6, y_dim = 20, z_dim = 20;
  Tensor tensor1(x_dim,y_dim,z_dim);

  BOOST_CHECK(tensor1.size() == x_dim * y_dim * z_dim);
  BOOST_CHECK(tensor1.dimension(0) == x_dim);
  BOOST_CHECK(tensor1.dimension(1) == y_dim);
  BOOST_CHECK(tensor1.dimension(2) == z_dim);

  Scalar * data = tensor1.data();
  for(Eigen::DenseIndex k = 0; k < tensor1.size(); ++k)
    data[k] = (Scalar)k;

  for(Eigen::DenseIndex k = 0; k < z_dim; ++k)
  {
    for(Eigen::DenseIndex j = 0; j < y_dim; ++j)
    {
      for(Eigen::DenseIndex i = 0; i < x_dim; ++i)
      {
        BOOST_CHECK(tensor1(i,j,k) == (Scalar)(i + j*x_dim + k*(x_dim*y_dim)));
      }
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * EigenDef.hpp
 *
 *  Created on: Oct 27, 2016
 *      Author: wyan
 */

#ifndef EIGENDEF_HPP_
#define EIGENDEF_HPP_

#define EIGEN_DONT_PARALLELIZE  // 1 thread for any eigen stuff
#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <vector>
#include <cmath>

/*
 *
 * Do not use auto with Eigen
 *
 * Eigen defaults to column-major storage.
 * Mat(i,j) is not affected by row major or column major storage format
 *
 * */

// unaligned fixed size vectors
using Evec2 = Eigen::Matrix<double, 2, 1, Eigen::DontAlign>;
using Evec3 = Eigen::Matrix<double, 3, 1, Eigen::DontAlign>;
using Evec4 = Eigen::Matrix<double, 4, 1, Eigen::DontAlign>;
using Evec6 = Eigen::Matrix<double, 6, 1, Eigen::DontAlign>;

// unaligned fixed size matrices
using Emat2 = Eigen::Matrix<double, 2, 2, Eigen::DontAlign>;
using Emat3 = Eigen::Matrix<double, 3, 3, Eigen::DontAlign>;
using Emat4 = Eigen::Matrix<double, 4, 4, Eigen::DontAlign>;
using Emat6 = Eigen::Matrix<double, 6, 6, Eigen::DontAlign>;

// unaligned quaternion
using Equatn = Eigen::Quaternion<double, Eigen::DontAlign>;

// aligned fixed size vectors
using EAvec2 = Eigen::Vector2d;
using EAvec3 = Eigen::Vector3d;
using EAvec4 = Eigen::Vector4d;
using EAvec6 = Eigen::Matrix<double, 6, 1>;

// aligned fixed size matrices
using EAmat2 = Eigen::Matrix2d;
using EAmat3 = Eigen::Matrix3d;
using EAmat4 = Eigen::Matrix4d;
using EAmat6 = Eigen::Matrix<double, 6, 6>;

// aligned quaternion
using EAquatn = Eigen::Quaternion<double>;

// dynamic size types
using Evec = Eigen::VectorXd;
using Espmat = Eigen::SparseMatrix<double>;                         // default to column major
using Emat = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>; // default to column major

#endif /* EIGENDEF_HPP_ */


#pragma once

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <merely3d/types.hpp>

namespace merely3d
{
    class Camera
    {
    public:
        Camera();

        Eigen::Vector3f position() const;
        Eigen::Vector3f direction() const;
        Eigen::Vector3f up() const;
        Eigen::Vector3f right() const;

        Eigen::Quaternionf orientation() const;
        Eigen::Affine3f transform() const;

        void set_position(const Eigen::Vector3f & position);

        template <typename IntoRotation>
        void set_orientation(const IntoRotation & orientation);

        void look_in(const Eigen::Vector3f & direction,
                     const Eigen::Vector3f & up);

    private:
        Eigen::Vector3f      _position;
        UnalignedQuaternionf _orientation;
    };

    inline Camera::Camera()
        : _position(Eigen::Vector3f::Zero()),
          _orientation(Eigen::Quaternionf::Identity())
    {

    }

    inline Eigen::Vector3f Camera::position() const
    {
        return _position;
    }

    inline Eigen::Vector3f Camera::direction() const
    {
        return _orientation * Eigen::Vector3f(0.0, 0.0, -1.0);
    }

    inline Eigen::Vector3f Camera::up() const
    {
        return _orientation * Eigen::Vector3f(0.0, 1.0, 0.0);
    }

    inline Eigen::Vector3f Camera::right() const
    {
        return _orientation * Eigen::Vector3f(1.0, 0.0, 0.0);
    }

    inline Eigen::Quaternionf Camera::orientation() const
    {
        return _orientation;
    }

    inline Eigen::Affine3f Camera::transform() const
    {
        return Eigen::Translation3f(_position) * _orientation;
    }

    inline void Camera::set_position(const Eigen::Vector3f & position)
    {
        _position = position;
    }

    template <typename IntoRotation>
    inline void Camera::set_orientation(const IntoRotation & orientation)
    {
        _orientation = orientation;
    }

    inline void Camera::look_in(const Eigen::Vector3f & direction,
                         const Eigen::Vector3f & up)
    {
        using Eigen::Vector3f;
        using Eigen::Matrix3f;

        const Vector3f d = direction.normalized();
        Vector3f u = up - up.dot(d) * d;

        if (u.isZero(0.0))
        {
            u = Vector3f(0.0, 1.0, 0.0);
        }
        else
        {
            u.normalize();
        }

        const Vector3f r = d.cross(u);

        // The matrix that rotates [r0, u0, d0] into [r, u, d]
        // is defined below
        Matrix3f rotation;
        rotation << r, u, -d;

        _orientation = rotation;
    }
}