/**
 * @file BayesianNetwork.hpp
 * @brief Implementation of the BayesianNetwork functions.
 * @author Ankit Srivastava <asrivast@gatech.edu>
 *
 * Copyright 2020 Georgia Institute of Technology
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#ifndef DETAIL_BAYESIANNETWORK_HPP_
#define DETAIL_BAYESIANNETWORK_HPP_

#include "utils/Logging.hpp"

#include <boost/graph/copy.hpp>
#include <boost/graph/tiernan_all_cycles.hpp>

template <typename Var>
/**
  * @brief Helper class that implements the anti-parallel edge filter functionality.
 */
class BayesianNetwork<Var>::AntiParallelEdgeFilter {
public:
  AntiParallelEdgeFilter(
  ) : m_graph(nullptr)
  {
  }

  AntiParallelEdgeFilter(
    const GraphImpl& g
  ) : m_graph(&g)
  {
  }

  template <typename EdgeDescriptor>
  bool
  operator()(
    const EdgeDescriptor& e
  ) const
  {
    auto source = boost::source(e, *m_graph);
    auto target = boost::target(e, *m_graph);
    return !boost::edge(target, source, *m_graph).second;
  }

private:
  const GraphImpl* m_graph;
}; // class AntiParallelEdgeFilter

template <typename Var>
/**
 * @brief Helper class for counting all the simple cycles that
 *        an edge is part of.
 */
class BayesianNetwork<Var>::EdgeCycleCounter {
public:
  EdgeCycleCounter(
    const Graph<BidirectionalAdjacencyList, VertexLabel, Var>& graph,
    std::unordered_map<Edge, size_t, typename Edge::Hash>& counts
  ) : m_graph(graph),
      m_counts(counts)
  {
  }

  template <typename Path, typename DirectedGraph>
  void
  cycle(
    const Path& p,
    const DirectedGraph& dg
  )
  {
    using IndexMap = typename boost::property_map<DirectedGraph, boost::vertex_index_t>::const_type;
    IndexMap indices = boost::get(boost::vertex_index, dg);
    auto u = p.begin();
    auto v = u+1;
    while (v != p.end()) {
      auto e = m_graph.getEdge(boost::get(indices, *u), boost::get(indices, *v));
      if (m_counts.find(e) == m_counts.end()) {
        m_counts[e] = 0;
      }
      m_counts[e] += 1;
      ++u;
      ++v;
    }
    v = p.begin();
    auto e = m_graph.getEdge(boost::get(indices, *u), boost::get(indices, *v));
    if (m_counts.find(e) == m_counts.end()) {
      m_counts[e] = 0;
    }
    m_counts[e] += 1;
  }

private:
  const Graph<BidirectionalAdjacencyList, VertexLabel, Var>& m_graph;
  std::unordered_map<Edge, size_t, typename Edge::Hash>& m_counts;
}; // class EdgeCycleCounter

template <typename Var>
/**
 * @brief Constructs empty network with given labels as vertices.
 */
BayesianNetwork<Var>::BayesianNetwork(
  const std::vector<std::string>& varLabels
) : Graph<BidirectionalAdjacencyList, VertexLabel, Var>(varLabels),
    m_directed(this->filterAntiParallelEdges())
{
}

template <typename Var>
/**
 * @brief Adds a directed or undirected edge between the vertices.
 */
void
BayesianNetwork<Var>::addEdge(
  const Var source,
  const Var target,
  const bool undirected
)
{
  this->addEdge(source, target);
  if (undirected) {
    this->addEdge(target, source);
  }
}

template <typename Var>
/**
 * @brief Returns a filtered view of the current graph with the anti-parallel edges removed.
 */
typename BayesianNetwork<Var>::FilteredGraph
BayesianNetwork<Var>::filterAntiParallelEdges(
) const
{
  AntiParallelEdgeFilter bef(this->m_graph);
  boost::filtered_graph<decltype(this->m_graph), AntiParallelEdgeFilter> fg(this->m_graph, bef);
  return FilteredGraph(std::move(fg), this->m_idVertexMap);
}

template <typename Var>
/**
 * @brief Orients the edges of the network in accordance with v-structures.
 *
 * @param vStructures A tuple with all the discovered v-structures and the corresponding p-values.
 */
void
BayesianNetwork<Var>::applyVStructures(
  std::vector<std::tuple<double, Var, Var, Var>>&& vStructures
)
{
  // First sort the v-structures in the ascending order of the p-values
  std::sort(vStructures.begin(), vStructures.end());
  for (const auto& vs : vStructures) {
    auto y = this->wrap(this->m_idVertexMap.at(std::get<1>(vs)));
    auto x = this->wrap(this->m_idVertexMap.at(std::get<2>(vs)));
    auto z = this->wrap(this->m_idVertexMap.at(std::get<3>(vs)));
    // First check if the reverse edges still exist
    if (!this->edgeExists(y, x) || !this->edgeExists(z, x)) {
      LOG_MESSAGE(warning, "* Could not apply v-structure %s -> %s <- %s (p-value = % g)",
                           y.property().label, x.property().label, z.property().label, std::get<0>(vs));
      LOG_MESSAGE_IF(!this->edgeExists(y, x), debug, "* %s - %s has already been oriented in the opposite direction", y.property().label, x.property().label);
      LOG_MESSAGE_IF(!this->edgeExists(z, x), debug, "* %s - %s has already been oriented in the opposite direction", x.property().label, z.property().label);
      continue;
    }
    LOG_MESSAGE(info, "+ Applying the v-structure %s -> %s <- %s (p-value = %g)",
                      y.property().label, x.property().label, z.property().label, std::get<0>(vs));
    this->removeEdge(x, y);
    this->removeEdge(x, z);
  }
}

template <typename Var>
/**
 * @brief Function which checks if the network has directed cycles.
 */
bool
BayesianNetwork<Var>::hasDirectedCycles(
) const
{
  return m_directed.hasCycles();
}

template <typename Var>
/**
 * @brief Counts the number of simple cycles that each edge is part of.
 */
std::unordered_map<typename BayesianNetwork<Var>::Edge, size_t, typename BayesianNetwork<Var>::Edge::Hash>
BayesianNetwork<Var>::countEdgeCycles(
) const
{
  // Copy the directed view of the graph to a directed graph
  typename DirectedGraph<VertexLabel, Var>::Impl dg;
  boost::copy_graph(*m_directed, dg);
  // Record all the counts
  std::unordered_map<Edge, size_t, typename Edge::Hash> counts;
  EdgeCycleCounter ecc(*this, counts);
  boost::tiernan_all_cycles(dg, ecc);
  return counts;
}

template <typename Var>
/**
 * @brief Function which breaks directed cycles in the network by reversing
 *        the direction of the edge which is part of most cycles.
 */
void
BayesianNetwork<Var>::breakDirectedCycles(
)
{
  Edge e;
  auto maxCount = 0u;
  for (const auto& cc : this->countEdgeCycles()) {
    if (cc.second > maxCount) {
      e = cc.first;
      maxCount = cc.second;
    }
  }
  if (maxCount > 0) {
    auto source = *e.source();
    auto target = *e.target();
    // Reverse the edge
    LOG_MESSAGE(info, "* Reversing the direction of edge %s -> %s", e.source().property().label, e.target().property().label);
    this->removeEdge(source, target);
    this->addEdge(target, source);
  }
}

template <typename Var>
/**
 * @brief Function which orients an edge, if it doesn't create directed cycles.
 *
 * @param e The edge to be removed.
 *
 * @returns true if any changes were made, otherwise returns false.
 */
bool
BayesianNetwork<Var>::removeEdgeAcyclic(
  Edge&& e
)
{
  auto source = e.source();
  auto target = e.target();
  this->removeEdge(std::move(e));
  if (m_directed.hasCycles(*source)) {
    this->addEdge(source, target);
    return false;
  }
  return true;
}

template <typename Var>
/**
 * @brief Function which checks if the undirected edge Y - Z can be
 *        oriented as Y -> Z to prevent new unshielded colliders.
 */
bool
BayesianNetwork<Var>::unshieldedColliderRule(
  const Vertex& y,
  const Vertex& z
) const
{
  if (m_directed.wrap(*y).inDegree() == 0) {
    return false;
  }
  bool potential = false;
  // Examine all the edges incoming into Y for a potential X
  for (const auto inY : m_directed.wrap(*y).inEdges()) {
    auto x = inY.source();
    // Check if an X exists such that no edge exists between X and Z
    if (!(this->edgeExists(*z, *x) || this->edgeExists(*x, *z))) {
      potential = true;
      break;
    }
  }
  return potential;
}

template <typename Var>
/**
 * @brief Function which checks if the undirected edge X - Z can be
 *        oriented as X -> Z to prevent directed cycles.
 */
bool
BayesianNetwork<Var>::acyclicityRule(
  const Vertex& x,
  const Vertex& z
) const
{
  // Therefore, try to apply the rule by setting the source as Z and the target as X
  bool orientEdge = false;
  // Iterate over all the outgoing neighbors of X for a potential Y
  for (const auto y : m_directed.wrap(*x).outNeighbors()) {
    if (m_directed.edgeExists(*y, *z)) {
      // Orient this edge as X -> Z
      // ...as long as it does not create an immorality
      orientEdge = true;
      break;
    }
  }
  bool immorality = false;
  if (orientEdge) {
    for (const auto inZ : m_directed.wrap(*z).inEdges()) {
      auto w = inZ.source();
      // Check if there is an edge between the source of another incoming edge into Z and X
      if (!(this->edgeExists(*x, *w) || this->edgeExists(*w, *x))) {
        // If no such edge exists, then the rule can not be applied because an immorality will be created
        immorality = true;
        break;
      }
    }
  }
  return (orientEdge && !immorality);
}

template <typename Var>
/**
 * @brief Function which checks if the undirected edge X - Z can be
 *        oriented as X -> Z by applying the hybrid rule.
 */
bool
BayesianNetwork<Var>::hybridRule(
  const Vertex& x,
  const Vertex& z
) const
{
  auto countY = 0u;
  // Iterate over all the incoming neighbors of Z for potential Y
  for (const auto inZ : m_directed.wrap(*z).inEdges()) {
    auto y = inZ.source();
    // Check if an undirected edge exists between X and Y
    if (this->edgeExists(*x, *y) && this->edgeExists(*y, *x)) {
      ++countY;
    }
  }
  // The rule can be applied only if at least two Ys were found
  return (countY >= 2);
}

template <typename Var>
/**
 * @brief Top level function for orienting edges using Meek's rules.
 *
 * @returns true if any changes were made, otherwise returns false.
 */
bool
BayesianNetwork<Var>::applyMeekRules(
)
{
  bool changed = false;
  auto isCollider = [] (const Vertex& v) { return (v.inDegree() > v.outDegree()) &&
                                                  (v.inDegree() - v.outDegree() > 1); };
  // Iterate over all the undirected edges
  for (auto e : this->antiParallelEdges()) {
    // Check if the anti-parallel edge still exists
    if (!e.hasAntiParallel()) {
      continue;
    }
    // See if this direction of the anti-parallel edge can be removed
    // Therefore, check if the rules can be applied to the reverse direction
    auto source = e.source();
    auto target = e.target();
    if (isCollider(source) && isCollider(target)) {
      LOG_MESSAGE_IF(*source < *target, info, "* Fixing edge %s - %s because of conflicting v-structures", source.property().label, target.property().label);
      continue;
    }
    if (this->unshieldedColliderRule(target, source)) { // Apply Meek's Rule 1
      if (this->removeEdgeAcyclic(std::move(e))) {
        LOG_MESSAGE(info, "* Directing edge %s -> %s (R1: unshielded colliders)", target.property().label, source.property().label);
        changed = true;
      }
    }
    else if (this->acyclicityRule(target, source)) { // Apply Meek's Rule 2
      if (this->removeEdgeAcyclic(std::move(e))) {
        LOG_MESSAGE(info, "* Directing edge %s -> %s (R2: acyclicity)", target.property().label, source.property().label);
        changed = true;
      }
    }
    else if (this->hybridRule(target, source)) { // Apply Meek's Rule 3
      if (this->removeEdgeAcyclic(std::move(e))) {
        LOG_MESSAGE(info, "* Directing edge %s -> %s (R3: hybrid)", target.property().label, source.property().label);
        changed = true;
      }
    }
  }
  return changed;
}

template <typename Var>
/**
 * @brief Top level function for writing the network in graphviz format.
 *
 * @param fileName Name of the file to which the network should be written.
 */
void
BayesianNetwork<Var>::writeGraphviz(
  const std::string& fileName
) const
{
  std::ofstream out(fileName);
  // Check if there are any directed edges in the graph
  // If not, write the graph as an undirected graph
  auto directed = false;
  for (const auto e : m_directed.edges()) {
    std::ignore = e;
    directed = true;
    break;
  }
  out << (directed ? "digraph" : "graph") << " {" << std::endl;
  for (const auto v : this->vertices()) {
    out << "  ";
    out << boost::escape_dot_string(v.property().label);
    out << " ;" << std::endl;
  }
  auto delimiter = directed ? " -> " : " -- ";
  for (const auto e : this->edges()) {
    bool write = false;
    if (directed && !this->edgeExists(e.target(), e.source())) {
      out << "  edge [dir=forward] ";
      write = true;
    }
    else if (e.source() < e.target()) {
      out << "  edge [dir=none] ";
      write = true;
    }
    if (write) {
      out << boost::escape_dot_string(e.source().property().label);
      out << delimiter;
      out << boost::escape_dot_string(e.target().property().label);
      out << " ;" << std::endl;
    }
  }
  out << "}" << std::endl;
}

#endif // DETAIL_BAYESIANNETWORK_HPP


//---------------------------------------------------------------------------//
//!
//! \file   Utility_AtomicMassUnit.hpp
//! \author Alex Robinson
//! \brief  The atomic mass unit
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_ATOMIC_MASS_UNIT_HPP
#define UTILITY_ATOMIC_MASS_UNIT_HPP

// Boost Includes
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/conversion.hpp>

namespace Utility{

namespace Units{

//! The atomic mass base unit
struct AtomicMassBaseUnit : public boost::units::base_unit<AtomicMassBaseUnit,boost::units::mass_dimension,4>
{
  static const char* name() { return "atomic mass unit"; }
  static const char* symbol() { return "amu"; }
};

//! The atomic mass unit
typedef AtomicMassBaseUnit::unit_type AtomicMass;

BOOST_UNITS_STATIC_CONSTANT( amu, AtomicMass );

} // end Units namespace

} // end Utility namespace

// From codata 2014
BOOST_UNITS_DEFINE_CONVERSION_FACTOR( Utility::Units::AtomicMassBaseUnit, boost::units::si::mass, double, 1.660539040e-27 );
BOOST_UNITS_DEFINE_CONVERSION_FACTOR( boost::units::si::mass, Utility::Units::AtomicMassBaseUnit, double, 1.0/1.660539040e-27 );

BOOST_UNITS_DEFAULT_CONVERSION( Utility::Units::AtomicMassBaseUnit, boost::units::si::mass );

#endif // end UTILITY_ATOMIC_MASS_UNIT_HPP

//---------------------------------------------------------------------------//
// end Utility_AtomicMassUnit.hpp
//---------------------------------------------------------------------------//


#include <string>
#include <unordered_map>
#include <vector>
#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <sudoku/dimensions.h>
#include <sudoku/fork.h>
#include <sudoku/formatter.h>
#include <sudoku/grid.h>
#include <sudoku/solver.h>
#include <sudoku/square.h>
#include "util.h"

// Each test case represents the number of peers to request.
std::vector<size_t> testCases_fork9x9{ 1, 2, 3, 6, 8, 16 };

BOOST_DATA_TEST_CASE(fork9x9, testCases_fork9x9)
{
    // Initialize a solver and fork it!
    const size_t numPeers = sample;
    sudoku::square::Dimensions dims(3);
    std::vector<sudoku::CellValue> cellValues(dims.getCellCount(), 0);
    sudoku::Grid grid(dims, std::move(cellValues));
    auto peers = sudoku::fork(std::move(grid), numPeers);

    // Test that we obtained the requested number of peers.
    // For some sudokus, this may not be possible, but an empty
    // 9x9 sudoku should not have a problem.
    BOOST_REQUIRE_EQUAL(peers.size(), numPeers);

    // Use a hashmap of (solution) -> (# occurances) to determine
    // if all solutions are unique.
    std::unordered_map<std::string, size_t> solutions;
    std::string formatString(dims.getCellCount(), '0');
    sudoku::Formatter fmt(dims, formatString);

    // Each peer should all be solvable.
    for (auto& peer : peers) {
        sudoku::Solver solver(std::move(peer));
        BOOST_REQUIRE(solver.computeNextSolution());
        solutions[fmt.format(solver.getCellValues())]++;
    }

    // Each peer's solution should be unique from all the others.
    // Verify number of solution occurances is 1 for all solutions.
    for (auto it = solutions.begin(); it != solutions.end(); ++it) {
        size_t solutionOcurrances = it->second;
        BOOST_CHECK(solutionOcurrances == 1);
    }
}

BOOST_AUTO_TEST_CASE(Solver_oneCellRemaining_fork)
{
    sudoku::square::Dimensions dims(2);
    std::vector<sudoku::CellValue> cellValues{
        1, 2, 3, 4,
        3, 4, 1, 2,
        2, 1, 4, 3,
        4, 3, 2, 0 // last remaining cell. Should be set to 1.
    };
    sudoku::Grid grid(dims, cellValues);
    auto peers = sudoku::fork(grid, 3);

    // Even though we requested 3 peers, there was only one
    // remaining value. This means we expect 1 peer.
    BOOST_REQUIRE_EQUAL(peers.size(), 1);

    // We want computeNextSolution() to produce the solution
    // obtained during fork() instead of popping the last
    // guess and continuing (which would yield no more
    // solutions in this case).
    sudoku::Solver solver(peers[0]);
    BOOST_REQUIRE(solver.computeNextSolution());
    std::vector<sudoku::CellValue> expectedSolution = {
        1, 2, 3, 4,
        3, 4, 1, 2,
        2, 1, 4, 3,
        4, 3, 2, 1
    };
    BOOST_REQUIRE_EQUAL_VECTORS(expectedSolution, solver.getCellValues());

    // The next attempt to compute a solution should find no
    // more solutions.
    BOOST_REQUIRE(!solver.computeNextSolution());
}

BOOST_AUTO_TEST_CASE(Solver_fork_peerInitializedWithSolution)
{
    // In this example, the last remaining cell has multiple
    // available values (since there are no groups). This will
    // cause fork() to produce 4 peers, where each is initialized
    // with a complete solution.
    sudoku::Dimensions dims(4, 4, {});
    std::vector<sudoku::CellValue> cellValues{1, 2, 3, 0};
    sudoku::Grid grid(dims, cellValues);
    //sudoku::Solver solver(grid);
    auto peers = sudoku::fork(grid, 4);
    BOOST_REQUIRE_EQUAL(peers.size(), 4);
    for (const auto& peer : peers) {
        sudoku::Solver solver(peer);
        BOOST_CHECK(solver.computeNextSolution());
    }
}


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015, 2016.
// Modifications copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

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

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

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

#include "test_envelope.hpp"

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/adapted/std_pair_as_segment.hpp>
#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P>
void test_2d()
{
    test_envelope
("POINT(1 1)", 1, 1, 1, 1);
    test_envelope<bg::model::linestring
 >("LINESTRING(1 1,2 2)", 1, 2, 1, 2);
    test_envelope<bg::model::polygon
 >("POLYGON((1 1,1 3,3 3,3 1,1 1))", 1, 3, 1, 3);

    test_envelope<bg::model::ring
 >("POLYGON((1 1,1 3,3 3,3 1,1 1))", 1, 3, 1, 3);
    test_envelope<bg::model::box
 >("BOX(1 1,3 3)", 1, 3, 1, 3);

    // Triangle, closed and open, and CCW.
    // Note that for the envelope algorithm,
    // these combinations should theoretically not differ
    test_envelope<bg::model::ring
 >("POLYGON((4 1,0 7,7 9,4 1))", 0, 7, 1, 9);
    test_envelope<bg::model::ring
 >("POLYGON((4 1,0 7,7 9))", 0, 7, 1, 9);
    test_envelope<bg::model::ring
 >("POLYGON((4 1,7 9,0 7,4 1))", 0, 7, 1, 9);
    test_envelope<bg::model::ring
 >("POLYGON((4 1,7 9,0 7))", 0, 7, 1, 9);

    typedef std::pair
 segment_type;
    test_envelope<segment_type>("SEGMENT(1 1,3 3)", 1, 3, 1, 3);
}

template <typename P>
void test_3d()
{
    test_envelope
("POINT(1 2 3)", 1, 1, 2, 2, 3, 3);
    test_envelope
("POINT(3 2 1)", 3, 3, 2, 2, 1, 1);
    test_envelope<bg::model::linestring
 >("LINESTRING(1 1 1,2 2 2)", 1, 2, 1, 2, 1, 2);
    test_envelope<bg::model::box
 >("BOX(1 1 1,3 3 3)", 1, 3, 1, 3, 1, 3);
}

template <typename Geometry>
void test_empty_geometry(std::string const& wkt)
{
    typedef typename bg::coordinate_type<Geometry>::type ct;
    ct high_val = boost::numeric::bounds<ct>::highest();
    ct low_val = boost::numeric::bounds<ct>::lowest();

    test_envelope<Geometry>(wkt, high_val, low_val, high_val, low_val);
}

template <typename P>
void test_empty()
{
    test_empty_geometry<bg::model::linestring
 >("LINESTRING()");
    test_empty_geometry<bg::model::ring
 >("POLYGON(())");

    test_empty_geometry<bg::model::polygon
 >("POLYGON(())");

    test_empty_geometry<bg::model::multi_point
 >("MULTIPOINT()");

    test_empty_geometry
        <
            bg::model::multi_linestring<bg::model::linestring
 >
        >("MULTILINESTRING()");

    test_empty_geometry
        <
            bg::model::multi_polygon<bg::model::polygon
 >
        >("MULTIPOLYGON()");
}

template <typename P>
void test_invalid()
{
    // polygon with empty exterior and interior rings
    test_empty_geometry<bg::model::polygon
 >("POLYGON((),(),())");

    // polygon with empty interior rings
    test_envelope
        <
            bg::model::polygon

        >("POLYGON((1 2,1 20,22 20,22 2,1 2),(),())",
          1, 22, 2, 20);

    // another polygon with empty interior rings
    test_envelope
        <
            bg::model::polygon

        >("POLYGON((1 2,1 20,22 20,22 2,1 2),(),(3 4,19 4,19 18,3 18,3 4),())",
          1, 22, 2, 20);

    // polygon with empty exterior ring
    test_envelope
        <
            bg::model::polygon

        >("POLYGON((),(),(3 4,19 4,19 18,3 18,3 4),())",
          3, 19, 4, 18);

    // another polygon with empty exterior ring
    test_envelope
        <
            bg::model::polygon

        >("POLYGON((),(),(3 4,19 4,19 18,3 18,3 4),(4 5,18 5,18 17,4 17,4 5))",
          3, 19, 4, 18);

    // yet one more polygon with empty exterior ring
    test_envelope
        <
            bg::model::polygon

        >("POLYGON((),(),(4 5,18 5,18 17,4 17,4 5),(3 4,19 4,19 18,3 18,3 4))",
          3, 19, 4, 18);

    // multilinestring with empty linestrings
    test_empty_geometry
        <
            bg::model::multi_linestring<bg::model::linestring
 >
        >("MULTILINESTRING((),(),())");

    // multilinestring with empty and non-empty linestrings
    test_envelope
        <
            bg::model::multi_linestring<bg::model::linestring
 >
        >("MULTILINESTRING((),(10 20),())", 10, 10, 20, 20);

    // multipolygon with empty polygon
    test_empty_geometry
        <
            bg::model::multi_polygon<bg::model::polygon
 >
        >("MULTIPOLYGON((()))");

    // multipolygon with many empty polygons
    test_empty_geometry
        <
            bg::model::multi_polygon<bg::model::polygon
 >
        >("MULTIPOLYGON(((),(),()),(()),((),(),(),(),()))");

    // multipolygon with empty polygons and non-empty (valid) polygon
    test_envelope
        <
            bg::model::multi_polygon<bg::model::polygon
 >
        >("MULTIPOLYGON(((),(),()),((10 30,10 40,20 30,10 30)),\
          ((),(),()),(()))", 10, 20, 30, 40);

    // multipolygon with empty polygons and non-empty (valid) polygon
    // that has an interior ring
    test_envelope
        <
            bg::model::multi_polygon<bg::model::polygon
 >
        >("MULTIPOLYGON(((),(),()),(()),\
          ((1 2,1 20,22 20,22 2,1 2),(3 4,19 4,19 18,3 18,3 4)),(()))",
          1, 22, 2, 20);

    // multipolygon with empty polygons and non-empty (invalid) polygon
    // that has an interior ring but empty exterior ring
    test_envelope
        <
            bg::model::multi_polygon<bg::model::polygon
 >
        >("MULTIPOLYGON(((),(),()),(()),((),(3 4,19 4,19 18,3 18,3 4)),(()))",
          3, 19, 4, 18);

    // multipolygon with empty polygons and non-empty (invalid) polygon
    // that has an interior ring but empty exterior ring
    test_envelope
        <
            bg::model::multi_polygon<bg::model::polygon
 >
        >("MULTIPOLYGON(((),(),()),((),(),(3 4,19 4,19 18,3 18,3 4),()),(()))",
          3, 19, 4, 18);

    // multipolygon with empty polygons and non-empty (invalid) polygon
    // that has two non-empty interior rings but empty exterior ring
    test_envelope
        <
            bg::model::multi_polygon<bg::model::polygon
 >
        >("MULTIPOLYGON(((),(),()),\
          ((),(),(3 4,19 4,19 18,3 18,3 4),(4 5,18 5,18 17,4 17,4 5),()),\
          (()))",
          3, 19, 4, 18);
}

int test_main(int, char* [])
{
    //test_2d<int[2]>();
    //test_2d<float[2]>();
    //test_2d<double[2]>();
    test_2d<boost::tuple<float, float> >();
    test_2d<bg::model::d2::point_xy<int> >();
    test_2d<bg::model::d2::point_xy<float> >();
    test_2d<bg::model::d2::point_xy<double> >();

    test_3d<test::test_point>();
    test_3d<boost::tuple<int, int, int> >();

    test_empty<boost::tuple<float, float> >();
    test_empty<bg::model::d2::point_xy<int> >();
    test_empty<bg::model::d2::point_xy<float> >();
    test_empty<bg::model::d2::point_xy<double> >();

    test_invalid<boost::tuple<float, float> >();
    test_invalid<bg::model::d2::point_xy<int> >();
    test_invalid<bg::model::d2::point_xy<float> >();
    test_invalid<bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_2d<bg::model::d2::point_xy<ttmath_big> >();
    test_3d<boost::tuple<ttmath_big, ttmath_big, ttmath_big> >();
#endif

    return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// Test only compilability

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    A.change_dim(5, 5); A= 0.0;
    {
	mtl::mat::inserter<Matrix>   ins(A);
	ins[0][0] << 7; ins[1][1] << 8; ins[1][3] << 2; ins[1][4] << 3;
	ins[2][2] << 2; ins[3][3] << 4; ins[4][4] << 9;
    }

    double xa[] = {1, 2, 3, 4, 5};
    mtl::dense_vector<double> x(xa), b;

    const Matrix B(A);

    b= trans(A) * x;

    typedef mtl::transposed_view<Matrix>       trans_type;
    typedef mtl::transposed_view<const Matrix> ctrans_type;

#if 0
    cout << "Type of A" << typeid(typename mtl::ashape::ashape<Matrix>::type).name() << "\n";
    cout << "Type of B" << typeid(typename mtl::ashape::ashape<const Matrix>::type).name() << "\n";

    cout << "Type of trans(A)" << typeid(typename mtl::ashape::ashape<mtl::transposed_view<Matrix> >::type).name() << "\n";
    cout << "Type of trans(B)" << typeid(typename mtl::ashape::ashape<mtl::transposed_view<const Matrix> >::type).name() << "\n";
#endif

    cout << name <<":\n Type of enable_if_matrix<trans(A)> " << typeid(typename mtl::traits::enable_if_matrix<trans_type>::type).name() << "\n";
    cout << "Type of enable_if_matrix<trans(B)> " << typeid(typename mtl::traits::enable_if_matrix<ctrans_type>::type).name() << "\n";

    b= B * x;
    b= trans(B) * x;
}

int main(int, char**)
{
    using namespace mtl;
    dense2D<double>                                      dr;
    dense2D<double, mat::parameters<col_major> >      dc;
    morton_dense<double, recursion::morton_z_mask>       mzd;
    morton_dense<double, recursion::doppled_2_row_mask>  d2r;
    compressed2D<double>                                 cr;
    compressed2D<double, mat::parameters<col_major> > cc;
#if 0
    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
#endif
    test(cr, "Compressed row major");
    test(cc, "Compressed column major");

    return 0;
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include <boost/graph/metric_tsp_approx.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct get_tsp_approx
{
    template <class Graph, class WeightMap, class IntType>
    void operator()(Graph& g, size_t src, WeightMap weights,
                    vector<IntType>& tour) const
    {
        back_insert_iterator<vector<IntType> > back_it(tour);
        metric_tsp_approx_tour_from_vertex(g, vertex(src, g), weights,
                                           back_it);
    }
};

vector<int32_t> get_tsp(GraphInterface& gi, size_t src, boost::any weight_map)
{
    vector<int32_t> tour;

    typedef UnityPropertyMap<size_t,GraphInterface::edge_t> cweight_t;

    if (weight_map.empty())
        weight_map = cweight_t();

    typedef mpl::push_back<edge_scalar_properties, cweight_t>::type
        weight_maps;

    run_action<graph_tool::detail::never_directed>()
        (gi, std::bind(get_tsp_approx(), std::placeholders::_1, src,
                       std::placeholders::_2, std::ref(tour)),
         weight_maps())(weight_map);

    return tour;
}


/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder(s) nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

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

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <math.h>
#include <stdio.h>
#include <ctime>
#include <time.h>

#include "CollisionAvoidance.h"

using namespace std;
using namespace Eigen;
using namespace KDL;

//////////////////////////////////	BOX

Eigen::VectorXd box(3);
////////////////////////////////////////////////

/////////////////////////////////  CONTACT POINT

Eigen::VectorXd cp(7);
////////////////////////////////////////////////

int main (int argc, char **argv)
{

	ros::init(argc, argv, "collision_avoidance");	// ROS node
	ros::NodeHandle nh;

	string relative_path_file_in;
	string file_name_in;

	string relative_path_file_out;
	string file_name_out;

	string relative_path_file_hand;
	string file_name_hand;

	nh.param<std::string>("file_name_in", relative_path_file_in, "/box_test/test" );
	nh.param<std::string>("file_name_out", relative_path_file_out, "/box_estimate/" );
	nh.param<std::string>("file_name_hand_point", relative_path_file_hand, "/hand_point/hand_points");

	////////////////////////////////////// load the data_base /////////////////////////////////////
	std::string path = ros::package::getPath("grasp_learning");

	file_name_in = path + relative_path_file_in;
	ifstream file_in(file_name_in);

	std::cout << "file: " << file_name_in.c_str() << " is " << (file_in.is_open() == true ? "already" : "not") << " open" << std::endl;
	if(!file_in.is_open())
	return 0;

	file_name_hand = path + relative_path_file_hand;
	ifstream file_hand(file_name_hand);

	std::cout << "file: " << file_name_hand.c_str() << " is " << (file_hand.is_open() == true ? "already" : "not") << " open" << std::endl;
	if(!file_hand.is_open())
	return 0;

  file_name_out = path + relative_path_file_out;

  ofstream file_output; //output file
  std::string name = "box_ca_test";
  file_output.open( file_name_out + name, ofstream::app);

  ofstream file_output_klampt; //output file for KLAMPT
  std::string name_ = "box_ca_test.csv";
  file_output_klampt.open( file_name_out + name_, ofstream::app);
  ///////////////////////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////// TAKE HAND POINT

  std::vector<Eigen::VectorXd> hp; //collision point?

  for(std::string line; getline( file_hand, line, '\n' ); )
	{
		std::vector<double> values_inline;
    std::istringstream iss_line(line);
    for(std::string value; getline(iss_line, value, ' ' ); )
    	values_inline.push_back(stod(value));

    Eigen::VectorXd t(7);

    double x = values_inline[0];
    double y = values_inline[1];
    double z = values_inline[2];

    double qx = values_inline[3];
    double qy = values_inline[4];
    double qz = values_inline[5];
  	double qw = values_inline[6];

    t(0) = x;
    t(1) = y;
    t(2) = z;
    t(3) = qx;
    t(4) = qy;
    t(5) = qz;
    t(6) = qw;

    hp.push_back(t);
  }

  ////////////////////////////////////////////////////////////////////////////// /////////////////

  ////////////////////////////////////////////////////////////////////////////// TAKE GRASP POINT

  for(std::string line; getline( file_in, line, '\n' ); )
	{
		std::vector<double> values_inline;
    std::istringstream iss_line(line);
    for(std::string value; getline(iss_line, value, ' ' ); )
    	values_inline.push_back(stod(value));

    box(0) = values_inline[0];
    box(1) = values_inline[1];
    box(2) = values_inline[2];

    cp(0) = values_inline[3];
    cp(1) = values_inline[4];
    cp(2) = values_inline[5];
    cp(3) = values_inline[6];
    cp(4) = values_inline[7];
    cp(5) = values_inline[8];
    cp(6) = values_inline[9];

    if(CollisionAvoidance(box, cp, hp))
    {
    	file_output<<line<<endl;
    	file_output_klampt<<box(0)<<','<<box(1)<<','<<box(2)<<','<<cp(0)<<','<<cp(1)<<','<<cp(2)<<','<<cp(3)<<','<<cp(4)<<','<<cp(5)<<','<<cp(6)<<endl;
    }
  }

  ////////////////////////////////////////////////////////////////////////////// /////////////////

  cout << "-------------------------------------------------" << endl;
  cout << " You can see your result in this folder : " << (file_name_out + name) << endl;
  cout << "-------------------------------------------------" << endl;

	ros::spinOnce();
	return 0;
}


#define BOOST_TEST_MODULE key_test_suite
#include <boost/test/included/unit_test.hpp>

#include <iostream>

#include <crypto/Digest.h>
#include <crypto/PrivateKey.h>
#include <crypto/PublicKey.h>
#include <crypto/Signature.h>

using namespace ultrainio;
using namespace std;

// Private/Public Key
BOOST_AUTO_TEST_SUITE(key_test_suite)
    BOOST_AUTO_TEST_CASE(generate) {
        PrivateKey privateKey;
        PublicKey publicKey;
        PrivateKey::generate(publicKey, privateKey);
        BOOST_CHECK(privateKey.isValid());
        BOOST_CHECK(publicKey.isValid());
    }

    BOOST_AUTO_TEST_CASE(sign) {
        PrivateKey privateKey;
        PublicKey publicKey;
        PrivateKey::generate(publicKey, privateKey);
        Digest digest(std::string("hash test"));
        Signature signature = privateKey.sign(digest);
        BOOST_CHECK(signature.isValid());
        BOOST_CHECK(publicKey.verify(signature, digest));
    }

    BOOST_AUTO_TEST_CASE(verify) {
        PrivateKey privateKey;
        PublicKey publicKey;
        PrivateKey::generate(publicKey, privateKey);
        PrivateKey sk1;
        PublicKey pk1;
        PrivateKey::generate(pk1, sk1);
        Digest digest(std::string("hash test"));
        Signature signature = privateKey.sign(digest);
        BOOST_CHECK(!pk1.verify(signature, digest));
    }

    BOOST_AUTO_TEST_CASE(verifyKeyPair) {
        PrivateKey privateKey;
        PublicKey publicKey;
        PrivateKey::generate(publicKey, privateKey);
        PrivateKey sk1;
        PublicKey pk1;
        PrivateKey::generate(pk1, sk1);
        BOOST_CHECK(PrivateKey::verifyKeyPair(publicKey, privateKey));
        BOOST_CHECK(PrivateKey::verifyKeyPair(pk1, sk1));

        BOOST_CHECK(!PrivateKey::verifyKeyPair(pk1, privateKey));
        BOOST_CHECK(!PrivateKey::verifyKeyPair(publicKey, sk1));

        std::string priHexStr(privateKey);
        std::string pubHexStr(publicKey);
        BOOST_CHECK(PrivateKey::verifyKeyPair(PrivateKey(priHexStr).getPublicKey(), PrivateKey(priHexStr)));
    }

    BOOST_AUTO_TEST_CASE(verifyKeyPair_genesis) {
        PrivateKey privateKey("5079f570cde7801c70a19fb2c7e292d09923218f2684c8a1121c2da7a02a5dc3369c31f242bfc5093815511e4a4eda297f4b8772a7ff98f7806ce7a80ffffb35");
        PublicKey publicKey("369c31f242bfc5093815511e4a4eda297f4b8772a7ff98f7806ce7a80ffffb35");
        BOOST_CHECK(PrivateKey::verifyKeyPair(publicKey, privateKey));
    }

    BOOST_AUTO_TEST_CASE(operator_case) {
        PrivateKey sk;
        PublicKey pk;
        PrivateKey::generate(pk, sk);
        std::string pkHexStr = std::string(pk);
        PublicKey pk1(pkHexStr);
        BOOST_CHECK(pk == pk1);
        PrivateKey sk2;
        PublicKey pk2;
        PrivateKey::generate(pk2, sk2);
        BOOST_CHECK(pk != pk2);
    }

BOOST_AUTO_TEST_SUITE_END()


/*
@copyright Louis Dionne 2015
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/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/first.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/second.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/product.hpp>
namespace hana = boost::hana;
using hana::test::ct_eq;

int main() {
    // first, second
    {
        hana::pair<ct_eq<1>, ct_eq<2>> p;
        BOOST_HANA_CONSTANT_CHECK(hana::equal(hana::first(p), ct_eq<1>{}));
        BOOST_HANA_CONSTANT_CHECK(hana::equal(hana::second(p), ct_eq<2>{}));
    }

    hana::test::TestProduct<hana::pair_tag>{hana::make_tuple(
        ct_eq<3>{}, ct_eq<4>{}, ct_eq<5>{}
    )};
}


/* ************************************************************************
 * Copyright 2015 Vratis, Ltd.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * ************************************************************************ */

#include <gtest/gtest.h>
#include <vector>
#include <string>
#include <boost/program_options.hpp>
#include <iomanip>

#include "resources/clsparse_environment.h"
#include "resources/csr_matrix_environment.h"

//boost ublas
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/blas.hpp>

// ULP calculation
#include <boost/math/special_functions/next.hpp>

clsparseControl ClSparseEnvironment::control = NULL;
cl_command_queue ClSparseEnvironment::queue = NULL;
cl_context ClSparseEnvironment::context = NULL;
//cl_uint ClSparseEnvironment::N = 1024;
static cl_bool extended_precision = false;
static cl_bool explicit_zeroes = true;

namespace po = boost::program_options;
namespace uBLAS = boost::numeric::ublas;

template <typename T>
class Blas2 : public ::testing::Test
{

    using CLSE = ClSparseEnvironment;
    using CSRE = CSREnvironment;

public:

    void SetUp()
    {
        clsparseInitScalar(&gAlpha);
        clsparseInitScalar(&gBeta);

        clsparseInitVector(&gX);
        clsparseInitVector(&gY);

        hAlpha = T(CSRE::alpha);
        hBeta = T(CSRE::beta);

        hX = uBLAS::vector<T>(CSRE::n_cols, 1);
        hY = uBLAS::vector<T>(CSRE::n_rows, 2);

        cl_int status;

        gX.values = clCreateBuffer(CLSE::context,
                                   CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
                                   hX.size() * sizeof(T), hX.data().begin(),
                                   &status);
        gX.num_values = hX.size();
        ASSERT_EQ(CL_SUCCESS, status);

        gY.values = clCreateBuffer(CLSE::context,
                                   CL_MEM_WRITE_ONLY | CL_MEM_COPY_HOST_PTR,
                                   hY.size() * sizeof(T), hY.data().begin(),
                                   &status);
        gY.num_values = hY.size();
        ASSERT_EQ(CL_SUCCESS, status);

        gAlpha.value = clCreateBuffer(CLSE::context,
                                      CL_MEM_READ_ONLY| CL_MEM_COPY_HOST_PTR,
                                      sizeof(T), &hAlpha, &status);
        ASSERT_EQ(CL_SUCCESS, status);

        gBeta.value = clCreateBuffer(CLSE::context,
                                     CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
                                     sizeof(T), &hBeta, &status);
        ASSERT_EQ(CL_SUCCESS, status);
    }

    void TearDown()
    {
        ::clReleaseMemObject(gAlpha.value);
        ::clReleaseMemObject(gBeta.value);

        ::clReleaseMemObject(gX.values);
        ::clReleaseMemObject(gY.values);

        clsparseInitScalar(&gAlpha);
        clsparseInitScalar(&gBeta);

        clsparseInitVector(&gX);
        clsparseInitVector(&gY);

    }

    // Knuth's Two-Sum algorithm, which allows us to add together two floating
    // point numbers and exactly transform the answer into a sum and a
    // rounding error.
    // Inputs: x and y, the two inputs to be added together.
    // In/Out: *sumk_err, which is incremented (by reference) -- holds the
    //         error value as a result of the 2sum calculation.
    // Returns: The non-corrected sum of inputs x and y.
    T two_sum(T x, T y, T *sumk_err)
    {
        // We use this 2Sum algorithm to perform a compensated summation,
        // which can reduce the cumulative rounding errors in our SpMV
        // summation. Our compensated summation is based on the SumK algorithm
        // (with K==2) from Ogita, Rump, and Oishi, "Accurate Sum and Dot
        // Product" in SIAM J. on Scientific Computing 26(6) pp 1955-1988,
        // Jun. 2005.
        T sumk_s = x + y;
        T bp = sumk_s - x;
        (*sumk_err) += ((x - (sumk_s - bp)) + (y - bp));
        return sumk_s;
    }

    void test_csrmv()
    {
        clsparseStatus status;
        cl_int cl_status;

        clsparseEnableExtendedPrecision(CLSE::control, extended_precision);

        if (typeid(T) == typeid(cl_float) )
        {
            status = clsparseScsrmv(&gAlpha, &CSRE::csrSMatrix, &gX,
                                    &gBeta, &gY, CLSE::control);

            ASSERT_EQ(clsparseSuccess, status);

            float* vals = (float*)&CSRE::ublasSCsr.value_data()[0];
            clsparseIdx_t* rows = &CSRE::ublasSCsr.index1_data()[0];
            clsparseIdx_t* cols = &CSRE::ublasSCsr.index2_data()[0];
            for ( clsparseIdx_t row = 0; row < CSRE::n_rows; row++)
            {
                // Summation done at a higher precision to decrease
                // summation errors from rounding.
                hY[row] *= hBeta;
                clsparseIdx_t row_end = rows[row + 1];
                double temp_sum;
                temp_sum = hY[row];
                for ( clsparseIdx_t i = rows[row]; i < rows[row + 1]; i++)
                {
                    // Perform: hY[row] += hAlpha * vals[i] * hX[cols[i]];
                    temp_sum += hAlpha * vals[i] * hX[cols[i]];
                }
                hY[row] = temp_sum;
            }

            T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gY.values,
                                                       CL_TRUE, CL_MAP_READ,
                                                       0, gY.num_values * sizeof(T),
                                                       0, nullptr, nullptr, &cl_status);
            ASSERT_EQ(CL_SUCCESS, cl_status);

            uint64_t max_ulps = 0;
            uint64_t min_ulps = UINT64_MAX;
            uint64_t total_ulps = 0;
            for (size_t i = 0; i < hY.size(); i++)
            {
                long long int intDiff = (long long int)boost::math::float_distance(hY[i], host_result[i]);
                intDiff = llabs(intDiff);
                total_ulps += intDiff;
                if (max_ulps < intDiff)
                    max_ulps = intDiff;
                if (min_ulps > intDiff)
                    min_ulps = intDiff;
                // Debug printouts.
                //std::cout << "Row " << i << " Float Ulps: " << intDiff << std::endl;
                //std::cout.precision(9);
                //std::cout << "\tFloat hY[" << i << "] = " << std::scientific << hY[i] << " (0x" << std::hex << *(uint32_t *)&hY[i] << "), " << std::dec;
                //std::cout << "host_result[" << i << "] = " << std::scientific << host_result[i] << " (0x" << std::hex << *(uint32_t *)&host_result[i] << ")" << std::dec << std::endl;
            }
#ifndef NDEBUG
            if (extended_precision)
            {
                std::cout << "Float Min ulps: " << min_ulps << std::endl;
                std::cout << "Float Max ulps: " << max_ulps << std::endl;
                std::cout << "Float Total ulps: " << total_ulps << std::endl;
                std::cout << "Float Average ulps: " << (double)total_ulps/(double)hY.size() <<  " (Size: " << hY.size() << ")" << std::endl;
            }
#endif

            for (size_t i = 0; i < hY.size(); i++)
            {
                double compare_val = 0.;
                if (extended_precision)
                {
                    // The limit here is somewhat weak because some GPUs don't
                    // support correctly rounded denorms in SPFP mode.
                    if (boost::math::isnormal(hY[i]))
                        compare_val = fabs(hY[i]*1e-3);
                }
                else
                {
                    if (boost::math::isnormal(hY[i]))
                        compare_val = fabs(hY[i]*0.1);
                }
                if (compare_val < 10*FLT_EPSILON)
                    compare_val = 10*FLT_EPSILON;
                ASSERT_NEAR(hY[i], host_result[i], compare_val);
            }

            cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gY.values,
                                                  host_result, 0, nullptr, nullptr);
            ASSERT_EQ(CL_SUCCESS, cl_status);
        }

        if (typeid(T) == typeid(cl_double) )
        {
            status = clsparseDcsrmv(&gAlpha, &CSRE::csrDMatrix, &gX,
                                    &gBeta, &gY, CLSE::control);

            ASSERT_EQ(clsparseSuccess, status);

            double* vals = (double*)&CSRE::ublasDCsr.value_data()[0];
            clsparseIdx_t* rows = &CSRE::ublasDCsr.index1_data()[0];
            clsparseIdx_t* cols = &CSRE::ublasDCsr.index2_data()[0];
            for ( clsparseIdx_t row = 0; row < CSRE::n_rows; row++)
            {
                // Summation done using a compensated summation to decrease
                // summation errors from rounding. This allows us to get
                // smaller errors without requiring quad precision support.
                // This method is like performing summation at quad precision and
                // casting down to double in the end.
                hY[row] *= hBeta;
                clsparseIdx_t row_end = rows[row + 1];
                double temp_sum;
                temp_sum = hY[row];
                T sumk_err = 0.;
                for ( clsparseIdx_t i = rows[row]; i < rows[row + 1]; i++)
                {
                    // Perform: hY[row] += hAlpha * vals[i] * hX[cols[i]];
                    temp_sum = two_sum(temp_sum, hAlpha*vals[i]*hX[cols[i]], &sumk_err);
                }
                hY[row] = temp_sum + sumk_err;
            }

            T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gY.values,
                                                       CL_TRUE, CL_MAP_READ,
                                                       0, gY.num_values * sizeof(T),
                                                       0, nullptr, nullptr, &cl_status);
            ASSERT_EQ(CL_SUCCESS, cl_status);

            uint64_t max_ulps = 0;
            uint64_t min_ulps = ULLONG_MAX;
            uint64_t total_ulps = 0;
            for (size_t i = 0; i < hY.size(); i++)
            {
                long long int intDiff = (long long int)boost::math::float_distance(hY[i], host_result[i]);
                intDiff = llabs(intDiff);
                total_ulps += intDiff;
                if (max_ulps < intDiff)
                    max_ulps = intDiff;
                if (min_ulps > intDiff)
                    min_ulps = intDiff;
                // Debug printouts.
                //std::cout << "Row " << i << " Double Ulps: " << intDiff << std::endl;
                //std::cout.precision(17);
                //std::cout << "\tDouble hY[" << i << "] = " << std::scientific << hY[i] << " (0x" << std::hex << *(uint64_t *)&hY[i] << "), " << std::dec;
                //std::cout << "host_result[" << i << "] = " << std::scientific << host_result[i] << " (0x" << std::hex << *(uint64_t *)&host_result[i] << ")" << std::dec << std::endl;
            }
            if (extended_precision)
            {
#ifndef NDEBUG
                std::cout << "Double Min ulps: " << min_ulps << std::endl;
                std::cout << "Double Max ulps: " << max_ulps << std::endl;
                std::cout << "Double Total ulps: " << total_ulps << std::endl;
                std::cout << "Double Average ulps: " << (double)total_ulps/(double)hY.size() <<  " (Size: " << hY.size() << ")" << std::endl;
#endif

                for (size_t i = 0; i < hY.size(); i++)
                {
                    double compare_val = fabs(hY[i]*1e-14);
                    if (compare_val < 10*DBL_EPSILON)
                        compare_val = 10*DBL_EPSILON;
                    ASSERT_NEAR(hY[i], host_result[i], compare_val);
                }
            }
            else
            {
                for (size_t i = 0; i < hY.size(); i++)
                {
                    double compare_val = 0.;
                    if (boost::math::isnormal(hY[i]))
                        compare_val = fabs(hY[i]*0.1);
                    if (compare_val < 10*DBL_EPSILON)
                        compare_val = 10*DBL_EPSILON;
                    ASSERT_NEAR(hY[i], host_result[i], compare_val);
                }
            }

            cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gY.values,
                                                  host_result, 0, nullptr, nullptr);
            ASSERT_EQ(CL_SUCCESS, cl_status);
        }
        // Reset output buffer for next test.
        ::clReleaseMemObject(gY.values);
        clsparseInitVector(&gY);
        gY.values = clCreateBuffer(CLSE::context,
                CL_MEM_WRITE_ONLY | CL_MEM_COPY_HOST_PTR,
                hY.size() * sizeof(T), hY.data().begin(),
                &cl_status);
        gY.num_values = hY.size();
        ASSERT_EQ(CL_SUCCESS, cl_status);
    }

    uBLAS::vector<T> hX;
    uBLAS::vector<T> hY;

    cldenseVector gX;
    cldenseVector gY;

    T hAlpha;
    T hBeta;

    clsparseScalar gAlpha;
    clsparseScalar gBeta;

};

// CSRMV Tests might give you a false ASSERT_NEAR depends on the matrix.
// We should calculate relative error instead of ASSERT_NEAR.

typedef ::testing::Types<cl_float, cl_double> TYPES;
TYPED_TEST_CASE(Blas2, TYPES);

TYPED_TEST(Blas2, csrmv_adaptive)
{
    this->test_csrmv();
}

TYPED_TEST(Blas2, csrmv_vector)
{
    // To call csrmv vector we need to artificially get rid of the rowBlocks data
    using CSRE = CSREnvironment;

    clsparseCsrMetaDelete( &CSRE::csrSMatrix );
    clsparseCsrMetaDelete( &CSRE::csrDMatrix );

    this->test_csrmv();

    // After calling the kernel we need to recreate the rowBlocks data for
    // later use.

    clsparseStatus status;
    clsparseMetaSizeResult sizeResult;
    sizeResult = clsparseCsrMetaSize( &CSRE::csrSMatrix, CLSE::control );
    ASSERT_EQ(clsparseSuccess, sizeResult.status );

    sizeResult = clsparseCsrMetaSize( &CSRE::csrDMatrix, CLSE::control );
    ASSERT_EQ(clsparseSuccess, sizeResult.status );

    status = clsparseCsrMetaCreate(&CSRE::csrSMatrix, CLSE::control );
    ASSERT_EQ (clsparseSuccess, status);
    status = clsparseCsrMetaCreate(&CSRE::csrDMatrix, CLSE::control );
    ASSERT_EQ (clsparseSuccess, status);
}

int main (int argc, char* argv[])
{

    using CLSE = ClSparseEnvironment;
    using CSRE = CSREnvironment;

    std::string path;
    double alpha;
    double beta;
    std::string platform;
    cl_platform_type pID;
    cl_uint dID;

    po::options_description desc("Allowed options");

    desc.add_options()
            ("help,h", "Produce this message.")
            ("path,p", po::value(&path)->required(), "Path to matrix in mtx format.")
            ("platform,l", po::value(&platform)->default_value("AMD"),
             "OpenCL platform: AMD, NVIDIA or INTEL.")
            ("device,d", po::value(&dID)->default_value(0),
             "Device id within platform.")
            ("alpha,a", po::value(&alpha)->default_value(1.0),
             "Alpha parameter for eq: \n\ty = alpha * M * x + beta * y")
            ("beta,b", po::value(&beta)->default_value(1.0),
             "Beta parameter for eq: \n\ty = alpha * M * x + beta * y")
            ("extended,e", po::bool_switch()->default_value(false),
             "Use compensated summation to improve accuracy by emulating extended precision.")
            ("no_zeroes,z", po::bool_switch()->default_value(false),
             "Disable reading explicit zeroes from the input matrix market file.");

    po::variables_map vm;
    po::parsed_options parsed =
            po::command_line_parser( argc, argv ).options( desc ).allow_unregistered( ).run( );

    try {
        po::store(parsed, vm);
        if (vm.count("help"))
        {
            std::cout << desc << std::endl;
            return 0;
        }
        po::notify(vm);
    }
    catch (po::error& error)
    {
        std::cerr << "Parsing command line options..." << std::endl;
        std::cerr << "Error: " << error.what() << std::endl;
        std::cerr << desc << std::endl;
        return false;
    }

    std::vector< std::string > to_pass_further =
            po::collect_unrecognized( parsed.options, po::include_positional );

    //check platform
    if(vm.count("platform"))
    {
        if ("AMD" == platform)
        {
            pID = AMD;
        }
        else if ("NVIDIA" == platform)
        {
            pID = NVIDIA;
        }
        else if ("INTEL" == platform)
        {
            pID = INTEL;
        }
        else
        {

            std::cout << "The platform option is missing or is ill defined!\n";
            std::cout << "Given [" << platform << "]" << std::endl;
            platform = "AMD";
            pID = AMD;
            std::cout << "Setting [" << platform << "] as default" << std::endl;
        }

    }

    if (vm["extended"].as<bool>())
        extended_precision = true;
    if (vm["no_zeroes"].as<bool>())
        explicit_zeroes = false;

    ::testing::InitGoogleTest(&argc, argv);
    //order does matter!
    ::testing::AddGlobalTestEnvironment( new CLSE(pID, dID));
    ::testing::AddGlobalTestEnvironment( new CSRE(path, alpha, beta,
                                                  CLSE::queue, CLSE::context, explicit_zeroes));

    return RUN_ALL_TESTS();

}


#include <cradle/imaging/geometry.hpp>
#include <cradle/imaging/blend.hpp>
#include <cradle/imaging/view_transforms.hpp>
#include <cradle/math/interpolate.hpp>
#include <cradle/common.hpp>
#include <boost/range/metafunctions.hpp>
#include <boost/range/functions.hpp>
#include <vector>
#include <algorithm>
#include <cradle/imaging/utilities.hpp>

namespace cradle {

namespace impl {

    // Check that all the slices in the given range are consistent.
    template<unsigned N, class T, class SP>
    void check_slice_consistency(
        std::vector<image_slice<N,T,SP> > const& slices)
    {
        image_slice<N,T,SP> const& slice0 = slices.front();
        for (typename std::vector<image_slice<N,T,SP> >::const_iterator
            i = slices.begin(); i != slices.end(); ++i)
        {
            if (i->content.size != slice0.content.size)
            {
                throw cradle::exception(
                    "image dimensions is inconsistent across slices");
            }
            if (!same_spatial_mapping(i->content, slice0.content))
            {
                throw cradle::exception(
                    "spatial mapping is inconsistent across slices");
            }
            if (!same_value_mapping(i->content, slice0.content))
            {
                throw cradle::exception(
                    "value mapping is inconsistent across slices");
            }
            if (i->content.units != slice0.content.units)
            {
                throw cradle::exception(
                    "value units are inconsistent across slices");
            }
            if (i->axis != slice0.axis)
            {
                throw cradle::exception(
                    "slice axis is inconsistent across slices");
            }
        }
    }

    template<unsigned N, class T, class SP>
    struct image_slice_comparison
    {
        bool operator()(image_slice<N,T,SP> const& a,
            image_slice<N,T,SP> const& b) const
        { return a.position < b.position; }
    };

    template<unsigned N, class T, class SP>
    std::vector<image_slice<N,T,SP> >
    sort_slices(std::vector<image_slice<N,T,SP> > const& slices)
    {
        std::vector<image_slice<N,T,SP> > sorted_slices = slices;
        sort(sorted_slices.begin(), sorted_slices.end(),
            image_slice_comparison<N,T,SP>());
        return sorted_slices;
    }

    template<unsigned N, class T, class SP>
    image<N + 1,T,shared>
    merge_sorted_slices(
        std::vector<image_slice<N,T,SP> > const& slices,
        regular_grid<1,double> const& interpolation_grid)
    {
        assert(!slices.empty());
        if (slices.empty())
        {
            throw cradle::exception(
                "merge_slices() called with empty slice list");
        }

        image_slice<N,T,SP> const& slice0 = slices.front();

        unsigned axis = slice0.axis;

        // Compute the dimensions for the merged image.
        vector<N + 1,unsigned> dimensions = unslice(
            slice0.content.size, axis, interpolation_grid.n_points[0]);

        image<N + 1,T,unique> tmp;
        create_image(tmp, dimensions);

        // Set the properties for the merged image...

        // value mapping
        copy_value_mapping(tmp, slice0.content);

        tmp.units = slice0.content.units;

        // spatial mapping
        tmp.origin = unslice(slice0.content.origin, axis,
            interpolation_grid.p0[0] - interpolation_grid.spacing[0] / 2);
        for (unsigned i = 0; i < N; ++i)
        {
            unsigned j = i < axis ? i : i + 1;
            tmp.axes[j] = unslice(slice0.content.axes[i], axis, 0);
        }
        tmp.axes[axis] = unslice(
            uniform_vector<N,double>(0), axis,
            interpolation_grid.spacing[0]);

        // While interpolating, prev_real_slice is the real slice immediately
        // before our interpolation position.  next_real_slice is the slice
        // immediately after.
        typename std::vector<image_slice<N,T,SP> >::const_iterator
            prev_real_slice = slices.begin();
        typename std::vector<image_slice<N,T,SP> >::const_iterator
            next_real_slice = slices.begin();
        double interpolated_position = interpolation_grid.p0[0];

        // Interpolate.
        for (unsigned i = 0; i < interpolation_grid.n_points[0]; ++i)
        {
            // Get a view of the merged slice.
            image_slice<N,T,view> merged_slice =
                sliced_view(cast_storage_type<view>(tmp), axis, i);

            // Update the real slice pointers if necessary.
            while (interpolated_position > next_real_slice->position)
            {
                prev_real_slice = next_real_slice;
                ++next_real_slice;
                if (next_real_slice == slices.end())
                {
                    next_real_slice = prev_real_slice;
                    break;
                }
            }

            // This is true before the first real slice and after the last one.
            if (next_real_slice == prev_real_slice)
            {
                copy_pixels(merged_slice.content, next_real_slice->content);
            }
            // Otherwise, we are in between two real slices, so interpolate.
            else
            {
                double fraction =
                    (next_real_slice->position - interpolated_position) /
                    (next_real_slice->position - prev_real_slice->position);
                // Don't extrapolate!
                fraction = clamp(fraction, 0., 1.);
                raw_blend_images(merged_slice.content,
                    prev_real_slice->content, next_real_slice->content,
                    fraction);
            }

            // Increment the interpolated position.
            interpolated_position += interpolation_grid.spacing[0];
        }

        return share(tmp);
    }
}

template<unsigned N, class T, class SP>
image<N + 1,T,shared>
merge_slices(
    check_in_interface& check_in,
    progress_reporter_interface& progress_reporter,
        std::vector<image_slice<N,T,SP> > const& slices)
{
    impl::check_slice_consistency(slices);

        auto sorted_slices = impl::sort_slices(slices);

        progress_reporter(0.2f);

    auto interpolation_grid =
        compute_interpolation_grid(
            map(
                [](image_slice<N,T,SP> const& slice) -> double
                { return slice.position; },
                sorted_slices),
            4.);

        progress_reporter(0.8f);

    return impl::merge_sorted_slices(sorted_slices, interpolation_grid);
}

template<unsigned N, class T, class SP>
image<N + 1,T,shared>
merge_slices(
    std::vector<image_slice<N,T,SP> > const& slices,
    regular_grid<1,double> const& interpolation_grid)
{
    impl::check_slice_consistency(slices);

    auto sorted_slices = impl::sort_slices(slices);

    return impl::merge_sorted_slices(sorted_slices, interpolation_grid);
}

namespace impl {

    template<unsigned N, class SP>
    struct merge_images_fn
    {
        image<N + 1,variant,shared> merged_image;
        std::vector<image_slice<N,variant,SP> > const* slices;
        template<class Pixel>
        void operator()(Pixel)
        {
            std::size_t n_slices = slices->size();
            std::vector<image_slice<N,Pixel,const_view> > views(n_slices);
            for (std::size_t i = 0; i != n_slices; ++i)
            {
                image_slice<N,variant,SP> const& src = (*slices)[i];
                image_slice<N,Pixel,const_view>& dst = views[i];
                dst.content =
                    cast_image<image<N,Pixel,const_view> >(src.content);
                copy_slice_properties(dst, src);
            }
            merged_image = as_variant(merge_slices(views));
        }
    };
}

template<unsigned N, class SP>
image<N + 1,variant,shared>
merge_slices(
    check_in_interface& check_in,
    progress_reporter_interface& progress_reporter,
    std::vector<image_slice<N,variant,SP> > const& slices)
{
    assert(!slices.empty());
    if (slices.empty())
    {
        throw cradle::exception(
            "merge_slices() called with empty slice list");
    }

    progress_reporter(0.5f);

    impl::merge_images_fn<N,SP> fn;
    fn.slices = &slices;
    dispatch_gray_variant(slices.front().content.pixels.type_info, fn);
    return fn.merged_image;
}

}


/*
 * SpanInterval.cpp
 *
 *  Created on: Mar 30, 2011
 *      Author: Joe
 */

#include <algorithm>
#include <exception>
#include <stdexcept>
#include <string>
#include <sstream>
#include <boost/optional.hpp>
#include <iostream>
#include <list>
#include <iterator>
#include "Interval.h"
#include "SpanInterval.h"
#include "Log.h"

/*
SpanInterval::SpanInterval(unsigned int smallest, unsigned int largest)
    : maxInterval_.start()(smallest), maxInterval_.end()(largest) {
}
*/

boost::optional<SpanInterval> SpanInterval::satisfiesRelation(Interval::INTERVAL_RELATION relation, const SpanInterval& universe) const {
    if (!universe.isLiquid()) throw std::invalid_argument("SpanInterval::satisfiesRelation() - universe variable is not liquid!  technically this can be supported, but currently is not.");
    unsigned int neg_inf = universe.start().start();
    unsigned int pos_inf = universe.start().finish();

    // to make it easy on ourselves, normalize
    if (!normalize()) { return boost::optional<SpanInterval>(); }
    SpanInterval n = normalize().get();

    // easy names - [[i,j], [k,l]]
    const unsigned int i = n.start().start();
    const unsigned int j = n.start().finish();
    const unsigned int k = n.finish().start();
    const unsigned int l = n.finish().finish();

    switch (relation) {
        case Interval::EQUALS:
            return n;
        case Interval::LESSTHAN:
            if (k == pos_inf || k == pos_inf-1) return boost::optional<SpanInterval>();
            return SpanInterval(k+2, pos_inf, neg_inf, pos_inf).normalize();
        case Interval::GREATERTHAN:
            if (j == neg_inf || j == neg_inf+1) return boost::optional<SpanInterval>();
            return SpanInterval(neg_inf, pos_inf, neg_inf, j-2).normalize();
        case Interval::MEETS:
            if (k == pos_inf)
                return boost::optional<SpanInterval>();
            if (l == pos_inf)
                return SpanInterval(k+1, pos_inf, neg_inf, pos_inf).normalize();
            return SpanInterval(k+1, l+1, neg_inf, pos_inf).normalize();
        case Interval::UMEETS:
            if (l == pos_inf)
                return boost::optional<SpanInterval>();
            return SpanInterval(l+1, l+1, neg_inf, pos_inf).normalize();
        case Interval::MEETSI:
            if (j == neg_inf)
                return boost::optional<SpanInterval>();
            if (i == neg_inf)
                return SpanInterval(neg_inf, pos_inf, neg_inf, j-1).normalize();
            return SpanInterval(neg_inf, pos_inf, i-1, j-1).normalize();
        case Interval::UMEETSI:
            if (i == neg_inf)
                return boost::optional<SpanInterval>();
            return SpanInterval(neg_inf, pos_inf, i-1, i-1).normalize();
        case Interval::OVERLAPS:
            if (k == pos_inf || i == pos_inf) return boost::optional<SpanInterval>();
            if (i == k) {
                // special case here
                if (k >= pos_inf-1 ) return boost::optional<SpanInterval>();
                return SpanInterval(i+1, l, k+2, pos_inf).normalize();
            }
            return SpanInterval(i+1, l, k+1, pos_inf).normalize();
        case Interval::OVERLAPSI:
            if (j == neg_inf || l == neg_inf) return boost::optional<SpanInterval>();
            if (j == l) {
                // special case here
                if (j <= neg_inf+1) return boost::optional<SpanInterval>();
                return SpanInterval(neg_inf, j-2, i, l-1).normalize();
            }
            return SpanInterval(neg_inf, j-1, i, l-1).normalize();
        case Interval::STARTS:
            if (k == pos_inf) return boost::optional<SpanInterval>();
            return SpanInterval(i, j, k+1, pos_inf).normalize();
        case Interval::STARTSI:
            if (l == neg_inf) return boost::optional<SpanInterval>();
            return SpanInterval(i, j, neg_inf, l-1).normalize();
        case Interval::FINISHES:
            if (j == neg_inf) return boost::optional<SpanInterval>();
            return SpanInterval(neg_inf, j-1, k, l).normalize();
        case Interval::FINISHESI:
            if (i == pos_inf) return boost::optional<SpanInterval>();
            return SpanInterval(i+1, pos_inf, k, l).normalize();
        case Interval::DURING:                                              // TODO: BUG!  try during on [1:3]
            if (j == neg_inf || k == pos_inf)
                return boost::optional<SpanInterval>();
            return SpanInterval(neg_inf, j-1, k+1, pos_inf).normalize();
        case Interval::DURINGI:
            if (i == pos_inf || l == neg_inf)
                return boost::optional<SpanInterval>();
            return SpanInterval(i+1, pos_inf, neg_inf, l-1).normalize();
        default:
            std::runtime_error e("SpanInterval::siSatisfying() not implemented for relation!");
            throw e;
    }

    //return result;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/pow2.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/four.hpp>

STF_CASE_TPL (" pow2",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::pow2;
  using iT = bd::as_integer_t<T>;
  using r_t = decltype(pow2(T(), iT()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(pow2(bs::Inf<T>(),  2), bs::Inf<r_t>());
  STF_EQUAL(pow2(bs::Minf<T>(), 2), bs::Minf<r_t>());
  STF_IEEE_EQUAL(pow2(bs::Nan<T>(),  2), bs::Nan<r_t>());
  STF_EQUAL(pow2(bs::Inf<T>(),  T(2.5)), bs::Inf<r_t>());
  STF_EQUAL(pow2(bs::Minf<T>(), T(2.5)), bs::Minf<r_t>());
  STF_IEEE_EQUAL(pow2(bs::Nan<T>(),  T(2.5)), bs::Nan<r_t>());
  STF_EQUAL(pow2(bs::Mone<T>(), 2), -bs::Four<r_t>());
  STF_EQUAL(pow2(bs::One<T>(),  2), bs::Four<r_t>());
  STF_EQUAL(pow2(bs::Zero<T>(), 2), bs::Zero<r_t>());
  STF_EQUAL(pow2(bs::Mone<T>(), T(2.5)), -bs::Four<r_t>());
  STF_EQUAL(pow2(bs::One<T>(),  T(2.5)), bs::Four<r_t>());
  STF_EQUAL(pow2(bs::Zero<T>(), T(2.5)), bs::Zero<r_t>());
}


#include "half_edge.h"
#include <Eigen/Geometry>
#include <iostream>
#include <functional>

VertexBundle::VertexBundle(int new_vi, int new_tci) : vi( new_vi ), tci( new_tci ) {}
bool operator==( const VertexBundle& lhs, const VertexBundle& rhs )
{
    return lhs.vi == rhs.vi && lhs.tci == rhs.tci;
}
bool operator!=( const VertexBundle& lhs, const VertexBundle& rhs )
{
    return !( lhs == rhs );
}

HalfEdge::HalfEdge(int a, int b) : fi(a), ki(b) {}

Bundle get_half_edge_bundle(
    int e,
    Eigen::MatrixXi & E,
    Eigen::MatrixXi & EF,
    Eigen::MatrixXi & EI,
    Eigen::MatrixXi & F,
    Eigen::MatrixXi & FTC
    )
{
    Bundle result;
    result.reserve(2);

    for( int side = 0; side < 2; ++side ) {

        const int face_index = EF( e, side );
        const int opposite_vertex = EI( e, side );

        // Normally F and FTC have the same number of faces.
        // For meshes with boundaries, though, they won't.
        // The outside-of-the-mesh side of the boundary
        // edge will be connected with new faces to a vertex at infinity.
        // These new faces exist in F, but not FT.
        // In case someone accesses one of these edges to infinity,
        // return a bogus texture coordinate (-1).
        // UPDATE: They now do have the same number of faces;
        //         FTC is also augmented with a vertex to infinity.

        HalfEdge he( face_index, opposite_vertex );
        int v1 = F  ( face_index, (opposite_vertex + 1)%3);
        int t1 = FTC( face_index, (opposite_vertex + 1)%3);
        /*
        int t1 = face_index < FTC.rows()
               ? FTC( face_index, (opposite_vertex + 1)%3)
               : -1
               ;
        */
        int v2 = F  ( face_index, (opposite_vertex + 2)%3);
        int t2 = FTC( face_index, (opposite_vertex + 2)%3);
        /*
        int t2 = face_index < FTC.rows()
               ? FTC( face_index, (opposite_vertex + 2)%3)
               : -1
               ;
        */
        he.p[0] = VertexBundle(v1, t1);
        he.p[1] = VertexBundle(v2, t2);

        result.push_back( he );
    }

    return result;
}

/*
BundleKey bundle_key(
	const int e,
	const Eigen::MatrixXi & E)
{
	assert( E.cols() == 2 );
	assert( e < E.rows() );
	int v1 = E(e,0);
	int v2 = E(e,1);
	if( v1 > v2 ) 	std::swap(v1,v2);
	return std::make_pair(v1,v2);
}
*/

void print_bundle( const Bundle & b )
{
	using namespace std;
	for(int i=0; i<b.size(); i++) {
		cout << "Half Edge #: " << i << endl;
		cout << "vi: " << b[i].p[0].vi << " ti: " << b[i].p[0].tci << endl;
		cout << "vi: " << b[i].p[1].vi << " ti: " << b[i].p[1].tci << endl;
		cout << "fi: " << b[i].fi << endl;
		cout << "ki: " << b[i].ki << endl;
	}
}

bool contains_edge( const EdgeMap& edges, int v1, int v2 )
{
    assert( v1 != v2 );
    assert( v1 >= 0 );
    assert( v2 >= 0 );

    const bool result1 = edges.count( v1 ) && edges.at( v1 ).count( v2 );
    const bool result2 = edges.count( v2 ) && edges.at( v2 ).count( v1 );
    assert( result1 == result2 );
    return result1;
}

void collapse_edge( EdgeMap& edges, int vertex_to_remove, int vertex_collapsing_into )
{
    assert( contains_edge( edges, vertex_to_remove, vertex_collapsing_into ) );

    // This should be true from the way we use it.
    assert( vertex_collapsing_into < vertex_to_remove );

    // For every neighbor of `vertex_to_remove`, replace it with `vertex_collapsing_into`.
    const std::unordered_set< int > neighbors( edges[ vertex_to_remove ] );
    for( const auto& n : neighbors ) {
        edges[ n ].erase( vertex_to_remove );
        edges[ n ].insert( vertex_collapsing_into );
    }

    // Add every neighbor of `vertex_to_remove` to `vertex_collapsing_into`.
    edges[ vertex_collapsing_into ].insert( neighbors.begin(), neighbors.end() );

    // We have now added `vertex_collapsing_into` as a neighbor of itself. Remove it.
    assert( edges[ vertex_collapsing_into ].count( vertex_collapsing_into ) );
    edges[ vertex_collapsing_into ].erase( vertex_collapsing_into );

    // Finally, erase `vertex_to_remove` from `edges`.
    edges.erase( vertex_to_remove );
}
void rename_vertex( EdgeMap& edges, int old_vertex_name, int new_vertex_name )
{
    // We must already know about old_vertex_name.
    assert( edges.count( old_vertex_name ) );
    // We must not already know about new_vertex_name.
    assert( !edges.count( new_vertex_name ) );

    // For every neighbor of `old_vertex_name`, replace it with `new_vertex_name`.
    const std::unordered_set< int > neighbors( edges[ old_vertex_name ] );
    for( const auto& n : neighbors ) {
        edges[ n ].erase( old_vertex_name );
        edges[ n ].insert( new_vertex_name );
    }

    // Move the neighbors of `old_vertex_name` to `new_vertex_name`.
    edges[ new_vertex_name ] = neighbors;
    edges.erase( old_vertex_name );
}
void insert_edge( EdgeMap& edges, int v1, int v2 )
{
    assert( !contains_edge( edges, v1, v2 ) );
    edges[ v1 ].insert( v2 );
    edges[ v2 ].insert( v1 );
}


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

// **  NOTE: Parts of this file were borrowed or inspired from the Ceres   **
// ** Solver library. The Ceres license is included here for completeness. **
//
//
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2015 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)

// edited by Steffen Urban 2019, January
// removed SuiteSparse dependency
// use Eigens LDLT

#include "theia/math/matrix/sparse_cholesky_llt.h"

#include <glog/logging.h>
#include <Eigen/Core>
#include <Eigen/SparseCore>

// UF_long is deprecated but SuiteSparse_long is only available in
// newer versions of SuiteSparse. So for older versions of
// SuiteSparse, we define SuiteSparse_long to be the same as UF_long,
// which is what recent versions of SuiteSparse do anyways.
#ifndef SuiteSparse_long
#define SuiteSparse_long UF_long
#endif

namespace theia {

// A class for performing the choleksy decomposition of a sparse matrix using
// CHOLMOD from SuiteSparse. This allows us to utilize the supernodal algorithms
// which are not included with Eigen. The interface is meant to mimic the Eigen
// linear solver interface except that it is not templated and requires sparse
// matrices.
SparseCholeskyLLt::SparseCholeskyLLt(const Eigen::SparseMatrix<double>& mat)
    : is_factorization_ok_(false),
      is_analysis_ok_(false),
      info_(Eigen::Success) {
  Compute(mat);
}

SparseCholeskyLLt::SparseCholeskyLLt()
    : is_factorization_ok_(false),
      is_analysis_ok_(false),
      info_(Eigen::Success) {}

SparseCholeskyLLt::~SparseCholeskyLLt() {}

void SparseCholeskyLLt::AnalyzePattern(const Eigen::SparseMatrix<double>& mat) {
  solver_.analyzePattern(mat);
  info_ = solver_.info();
  if (info_ == Eigen::Success)
    is_analysis_ok_ = true;
  else
    is_analysis_ok_ = false;
  is_factorization_ok_ = false;
  info_ = Eigen::Success;
}

void SparseCholeskyLLt::Factorize(const Eigen::SparseMatrix<double>& mat) {
  solver_.factorize(mat);

  info_ = solver_.info();
  if (info_ == Eigen::Success)
    is_factorization_ok_ = true;
  else
    is_factorization_ok_ = false;
}

void SparseCholeskyLLt::Compute(const Eigen::SparseMatrix<double>& mat) {
  solver_.compute(mat);
  info_ = solver_.info();
  if (info_ == Eigen::Success) {
    is_factorization_ok_ = true;
    is_analysis_ok_ = true;
  }
}

Eigen::ComputationInfo SparseCholeskyLLt::Info() { return info_; }

// Using the cholesky decomposition, solve for x that minimizes
//    lhs * x = rhs
// where lhs is the factorized matrix.
Eigen::VectorXd SparseCholeskyLLt::Solve(const Eigen::VectorXd& rhs) {
  CHECK(is_analysis_ok_) << "Cannot call Solve() because symbolic analysis "
                            "of the matrix (i.e. AnalyzePattern()) failed!";
  CHECK(is_factorization_ok_)
      << "Cannot call Solve() because numeric factorization "
         "of the matrix (i.e. Factorize()) failed!";

  Eigen::VectorXd solution;
  solution = solver_.solve(rhs);
  return solution;
}

}  // namespace theia


#include <boost/graph/isomorphism.hpp>


#include <boost/numeric/odeint/stepper/euler.hpp>


#include "cloudproc.hpp"
#include "convexdecomp.hpp"
using namespace pcl;
using namespace std;
using namespace Eigen;
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>
#include "utils/clock.hpp"
#include <boost/format.hpp>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/planar_region.h>
#include <pcl/segmentation/organized_multi_plane_segmentation.h>
#include <pcl/segmentation/organized_connected_component_segmentation.h>
using namespace cloudproc;
typedef pcl::PointXYZRGBA PointT;
int main(int argc, char** argv) {
  if (argc != 2) {
    printf("usage: %s inputfile.pcd\n", argv[0]);
    abort();
  }

  PointCloud<PointT>::Ptr cloud = readPCD<PointT>(argv[1]);

  pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
  ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
  ne.setMaxDepthChangeFactor(0.03f);
  ne.setNormalSmoothingSize(20.0f);

  pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
  mps.setMinInliers(10);
  mps.setAngularThreshold(0.017453 * 2.0); //3 degrees
  mps.setDistanceThreshold(0.02); //2cm

   pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
   ne.setInputCloud (cloud);
   ne.compute (*normal_cloud);

   std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; mps.setInputNormals (normal_cloud);
   mps.setInputCloud (cloud);
   //mps.segmentAndRefine (regions);

   visualization::PCLVisualizer  viewer ("3d Viewer");
   viewer.setBackgroundColor (0,0,0);

   char name[1024];
   unsigned char red [6] = {255,   0,   0, 255, 255,   0};
   unsigned char grn [6] = {  0, 255,   0, 255,   0, 255};
   unsigned char blu [6] = {  0,   0, 255,   0, 255, 255};

   pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
   for (size_t i = 0; i < regions.size (); i++)
   {
     Eigen::Vector3f centroid = regions[i].getCentroid ();
     Eigen::Vector4f model = regions[i].getCoefficients ();
     pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
     pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
                                        centroid[1] + (0.5f * model[1]),
                                        centroid[2] + (0.5f * model[2]));
     sprintf (name, "normal_%zu", i);
     viewer.addArrow (pt2, pt1, 1.0, 0, 0, false, name);

     contour->points = regions[i].getContour ();
     sprintf (name, "plane_%02zu", i);
     pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
     viewer.addPointCloud (contour, color, name);
     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
   }
   viewer.spin();

}


/**
 * \file
 *
 * \author Nicholas J. Curtis
 * \date 04/29/2016
 *
 * \brief Defines an interface for boost's runge_kutta_fehlberg78 solver
 *
*/

//wrapper code
#include "rk78_typedefs.hpp"

//boost includes
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
extern "C" {
#include "solver.h"
}

#ifdef GENERATE_DOCS
namespace rk78 {
#endif

extern std::vector<state_type*> state_vectors;
extern std::vector<rhs_eval*> evaluators;
extern std::vector<controller> controllers;

extern "C" void intDriver(const int, const double, const double, const double*, double*);

#ifdef STIFFNESS_MEASURE
    extern std::vector<double> max_stepsize;
    controlled_step_result test_step(int index, const state_type& y, const double t, state_type& y_out, const double dt)
    {
        try
        {
            double t_copy = t;
            double dt_copy = dt;
            return controllers[index]->try_step(*evaluators[index], y, t_copy, y_out, dt_copy);
        }
        catch(...)
        {
            return fail;
        }
    }
#endif

/**
 * \brief Integration driver for the CPU integrators
 * \param[in]       NUM         the number of IVPs to solve
 * \param[in]       t           the current IVP time
 * \param[in]       t_end       the time to integrate the IVP to
 * \param[in]       pr_global   the pressure value for the IVPs
 * \param[in, out]  y_global    the state vectors
 *
 * The integration driver for the RK78 solver
 */
void intDriver (const int NUM, const double t, const double t_end,
                const double *pr_global, double *y_global)
{
    #ifdef STIFFNESS_MEASURE
    max_stepsize.clear();
    max_stepsize.resize(NUM, 0.0);
    #endif

	int tid = 0;
#ifdef STIFFNESS_MEASURE
    #pragma omp parallel for shared(state_vectors, evaluators, controllers, max_stepsize) private(tid)
#else
	#pragma omp parallel for shared(state_vectors, evaluators, controllers) private(tid)
#endif
    for (tid = 0; tid < NUM; ++tid) {
    	int index = omp_get_thread_num();

        // local array with initial values
        state_type& vec = *state_vectors[index];
        evaluators[index]->set_state_var(pr_global[tid]);

        // load local array with initial values from global array
        for (int i = 0; i < NSP; i++)
        {
            vec[i] = y_global[tid + i * NUM];
        }

#ifndef STIFFNESS_MEASURE
        integrate_adaptive(controllers[index],
            *evaluators[index], vec, t, t_end, t_end - t);
#else
        double tol = 1e-15;
        state_type y_copy(vec);
        //do a binary search to find the maximum stepsize
        double left_size = 1.0;
        while (test_step(index, vec, t, y_copy, left_size) == success)
        {
            left_size *= 10.0;
        }
        double right_size = 1e-20;
        while (test_step(index, vec, t, y_copy, right_size) == fail)
        {
            right_size /= 10.0;
        }
        double delta = 1.0;
        double mid = 0;
        while (delta > tol) {
            mid = (left_size + right_size) / 2.0;
            controlled_step_result result = test_step(index, vec, t, y_copy, mid);
            if (result == fail) {
                //mid becomes the new left
                delta = fabs(left_size - mid) / left_size;
                left_size = mid;
            }
            else{
                delta = fabs(right_size - mid) / right_size;
                right_size = mid;
            }
        }
        max_stepsize[tid] = mid;
#endif

        // update global array with integrated values
        for (int i = 0; i < NSP; i++)
        {
            y_global[tid + i * NUM] = vec[i];
        }

    }
}

#ifdef GENERATE_DOCS
}
#endif


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_CENTROID_CARTESIAN_HPP
#define BOOST_GEOMETRY_STRATEGIES_CENTROID_CARTESIAN_HPP

#include <boost/geometry/strategies/cartesian/centroid_average.hpp>
#include <boost/geometry/strategies/cartesian/centroid_bashein_detmer.hpp>
#include <boost/geometry/strategies/cartesian/centroid_weighted_length.hpp>

#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/centroid/services.hpp>

#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace centroid
{

template <typename CalculationType = void>
struct cartesian
    : public strategies::detail::cartesian_base
{
    template <typename Geometry>
    static auto centroid(Geometry const&,
                         std::enable_if_t
                            <
                                util::is_pointlike<Geometry>::value
                            > * = nullptr)
    {
        return strategy::centroid::average<>();
    }

    template <typename Geometry>
    static auto centroid(Geometry const&,
                         std::enable_if_t
                            <
                                util::is_polylinear<Geometry>::value
                            > * = nullptr)
    {
        return strategy::centroid::weighted_length<void, void, CalculationType>();
    }

    template <typename Geometry>
    static auto centroid(Geometry const&,
                         std::enable_if_t
                            <
                                util::is_polygonal<Geometry>::value
                             // TODO: This condition was used for the legacy default strategy
                             // && geometry::dimension<Geometry>::value == 2
                            > * = nullptr)
    {
        return strategy::centroid::bashein_detmer<void, void, CalculationType>();
    }

    // TODO: Box and Segment should have proper strategies.
    template <typename Geometry, typename Point>
    static auto centroid(Geometry const&, Point const&,
                         std::enable_if_t
                            <
                                util::is_segment<Geometry>::value
                             || util::is_box<Geometry>::value
                            > * = nullptr)
    {
        return strategy::centroid::not_applicable_strategy();
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, cartesian_tag>
{
    using type = strategies::centroid::cartesian<>;
};

template <typename PC, typename PG>
struct strategy_converter<strategy::centroid::average<PC, PG> >
{
    static auto get(strategy::centroid::average<PC, PG> const&)
    {
        return strategies::centroid::cartesian<>();
    }
};

template <typename PC, typename PG>
struct strategy_converter<strategy::centroid::weighted_length<PC, PG> >
{
    static auto get(strategy::centroid::weighted_length<PC, PG> const&)
    {
        return strategies::centroid::cartesian<>();
    }
};

template <typename PC, typename PG, typename CT>
struct strategy_converter<strategy::centroid::bashein_detmer<PC, PG, CT> >
{
    static auto get(strategy::centroid::bashein_detmer<PC, PG, CT> const&)
    {
        return strategies::centroid::cartesian<CT>();
    }
};

} // namespace services

}} // namespace strategies::centroid

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CENTROID_CARTESIAN_HPP


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <functional>
#include <mutex>
#include <Eigen/Geometry>

#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
    #include <omp.h>
#endif

#ifndef SIMPLE_HAUSDORFF_DISTANCE_HPP
#define SIMPLE_HAUSDORFF_DISTANCE_HPP

namespace simple_hausdorff_distance
{
    class SimpleHausdorffDistance
    {
    private:

        SimpleHausdorffDistance() {}

        static inline size_t GetNumOMPThreads(void)
        {
#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
        #if defined(_OPENMP)
            size_t num_threads = 0;
            #pragma omp parallel
            {
                num_threads = (size_t)omp_get_num_threads();
            }
            return num_threads;
        #else
            return 1;
        #endif
#else
            return 1;
#endif
        }

    public:

        template<typename FirstDatatype, typename SecondDatatype, typename FirstAllocator=std::allocator<FirstDatatype>, typename SecondAllocator=std::allocator<SecondDatatype>>
        static double ComputeDistance(const std::vector<FirstDatatype, FirstAllocator>& first_distribution, const std::vector<SecondDatatype, SecondAllocator>& second_distribution, const std::function<double(const FirstDatatype&, const SecondDatatype&)>& distance_fn)
        {
            // Compute the Hausdorff distance - the "maximum minimum" distance
            std::vector<double> per_thread_storage(GetNumOMPThreads(), 0.0);
#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
            #pragma omp parallel for
#endif
            for (size_t idx = 0; idx < first_distribution.size(); idx++)
            {
                const FirstDatatype& first = first_distribution[idx];
                double minimum_distance = INFINITY;
                for (size_t jdx = 0; jdx < second_distribution.size(); jdx++)
                {
                    const SecondDatatype& second = second_distribution[jdx];
                    const double current_distance = distance_fn(first, second);
                    if (current_distance < minimum_distance)
                    {
                        minimum_distance = current_distance;
                    }
                }
#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
                #if defined(_OPENMP)
                const size_t current_thread_id = (size_t)omp_get_thread_num();
                #else
                const size_t current_thread_id = 0;
                #endif
#else
                const size_t current_thread_id = 0;
#endif
                if (minimum_distance > per_thread_storage[current_thread_id])
                {
                    per_thread_storage[current_thread_id] = minimum_distance;
                }
            }
            double maximum_minimum_distance = 0.0;
            for (size_t idx = 0; idx < per_thread_storage.size(); idx++)
            {
                const double temp_minimum_distance = per_thread_storage[idx];
                if (temp_minimum_distance > maximum_minimum_distance)
                {
                    maximum_minimum_distance = temp_minimum_distance;
                }
            }
            return maximum_minimum_distance;
        }
    };
}
#endif // SIMPLE_HAUSDORFF_DISTANCE_HPP